Cleanup

Arduino/ArduinoParticipant.cpp

@@ -88,7 +88,7 @@ void ParticipantUDP::Receive() {
 #endif
 }
 
-bool ParticipantUDP::SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize) {
+bool ParticipantUDP::SendTo(ParticipantSocket* remoteParticipant, int bufferSize) {
 #if defined(ARDUINO) && defined(HAS_WIFI)
   // std::cout << "Sending to:\n " << remoteParticipant->ipAddress << ":"
   // << remoteParticipant->port << "\n";

Arduino/ArduinoParticipant.h

@@ -9,7 +9,7 @@ class ParticipantUDP : public ParticipantUDPGeneric {
  public:
   void Setup();
   void Receive();
-  bool SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize);
+  bool SendTo(ParticipantSocket* remoteParticipant, int bufferSize);
   bool Publish(IMessage* msg);
 
  protected:

EspIdf/EspIdfParticipant.cpp

@@ -182,7 +182,7 @@ bool ParticipantUDP::Send(IMessage* msg) {
   return this->SendTo(this->remoteSite, bufferSize);
 }
 
-bool ParticipantUDP::SendTo(RemoteParticipantUDP* remoteParticipant,
+bool ParticipantUDP::SendTo(ParticipantSocket* remoteParticipant,
                             int bufferSize) {
 #if defined(IDF_VER)
   uint16_t port = ntohs(dest_addr.sin_port);

EspIdf/EspIdfParticipant.h

@@ -23,7 +23,7 @@ class ParticipantUDP : public ParticipantUDPGeneric {
 
   void Setup(int localPort, const char* remoteIpAddress, int remotePort);
   void Receive();
-  bool SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize);
+  bool SendTo(ParticipantSocket* remoteParticipant, int bufferSize);
   bool Publish(IMessage* msg);
 
   // bool SendTest();

LinearAlgebra/Matrix.cpp

@@ -1,14 +1,21 @@
 #include "Matrix.h"
-#if !defined(NO_STD) 
+#if !defined(NO_STD)
 #include <iostream>
 #endif
 
+#include "esp_log.h"
+#include "esp_system.h"
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+
 namespace LinearAlgebra {
 
 #pragma region Matrix1
 
+Matrix1::Matrix1() {}
+
 Matrix1::Matrix1(int size) : size(size) {
-  if (this->size == 0)
+  if (this->size <= 0)
     data = nullptr;
   else {
     this->data = new float[size]();
@@ -16,10 +23,63 @@ Matrix1::Matrix1(int size) : size(size) {
   }
 }
 
-Matrix1::Matrix1(float* data, int size) : data(data), size(size) {
+Matrix1::Matrix1(int size, float* data) : data(data), size(size) {
   this->externalData = true;
 }
 
+Matrix1::Matrix1(const Matrix1& m) : size(m.size) {
+  if (this->size <= 0)
+    this->data = nullptr;
+  else {
+    this->data = new float[this->size];
+
+    for (int ix = 0; ix < this->size; ++ix)
+      this->data[ix] = m.data[ix];
+  }
+}
+
+Matrix1::~Matrix1() {
+  delete[] data;
+}
+
+unsigned int Matrix1::Size() const {
+  return this->size;
+}
+
+Matrix1 Matrix1::FromVector3(Vector3 v) {
+  Matrix1 r(3);
+  r.data[0] = v.x;
+  r.data[1] = v.y;
+  r.data[2] = v.z;
+  return r;
+}
+
+Matrix1 Matrix1::Zero(unsigned int size) {
+  Matrix1 r(size);
+  r.Fill(0);
+  return r;
+}
+
+void Matrix1::Fill(float value) {
+  for (int ix = 0; ix < this->size; ix++)
+    this->data[ix] = value;
+}
+
+void Matrix1::Fill(float value, unsigned int start, unsigned int stop) {
+  if (start > stop || start > this->size)
+    return;
+
+  if (stop > this->size)
+    stop = this->size;
+
+  for (unsigned int ix = start; ix < stop; ix++)
+    this->data[ix] = value;
+}
+
+Vector3 Matrix1::ToVector3() {
+  return Vector3(this->data[0], this->data[1], this->data[2]);
+}
+
 Matrix1 LinearAlgebra::Matrix1::FromQuaternion(Quaternion q) {
   Matrix1 r = Matrix1(4);
   float* data = r.data;
@@ -34,6 +94,40 @@ Quaternion LinearAlgebra::Matrix1::ToQuaternion() {
   return Quaternion(this->data[0], this->data[1], this->data[2], this->data[3]);
 }
 
+Matrix1 Matrix1::Slice(int start, int stop) {
+  if (start < 0 || stop >= this->size)
+    std::cerr << "Slice index out of range." << std::endl;
+
+  Matrix1 result(stop - start);
+  int resultIx = 0;
+  for (int ix = start; ix < stop; ix++)
+    result.data[resultIx++] = this->data[ix];
+
+  return result;
+}
+
+void Matrix1::UpdateSlice(int start, int stop, const Matrix1& source) const {
+  if (start > stop || start > this->size || start > source.size)
+    return;
+
+  if (stop > this->size)
+    stop = this->size;
+  if (stop > source.size)
+    stop = source.size;
+
+  unsigned int sourceIx = 0;
+  for (int ix = start; ix < stop; ix++, sourceIx++) {
+    this->data[ix] = source.data[sourceIx];
+  }
+}
+
+const float& Matrix1::operator()(int ix) const {
+  return data[ix];
+}
+float& Matrix1::operator()(int ix) {
+  return data[ix];
+}
+
 // Matrix1
 #pragma endregion
 
@@ -43,15 +137,19 @@ Matrix2::Matrix2() {}
 
 Matrix2::Matrix2(int nRows, int nCols) : nRows(nRows), nCols(nCols) {
   this->nValues = nRows * nCols;
-  if (this->nValues == 0)
+  // printf("Allocate %d\n", this->nValues);
+
+  if (this->nValues <= 0) {
     this->data = nullptr;
-  else {
-    this->data = new float[this->nValues];
     this->externalData = false;
+    return;
   }
+
+  this->data = new float[this->nValues];
+  this->externalData = false;
 }
 
-Matrix2::Matrix2(float* data, int nRows, int nCols)
+Matrix2::Matrix2(int nRows, int nCols, float* data, float autoDestruct)
     : nRows(nRows), nCols(nCols), data(data) {
   this->nValues = nRows * nCols;
   this->externalData = true;
@@ -88,39 +186,43 @@ Matrix2& Matrix2::operator=(const Matrix2& m) {
 }
 
 Matrix2::~Matrix2() {
-  if (!this->externalData)
+  if (!this->externalData) {
+    // printf("Deallocate %d\n", this->nValues);
     delete[] data;
+  }
 }
 
-Matrix2 Matrix2::Clone() const {
+unsigned int Matrix2::NRows() {
-  Matrix2 r = Matrix2(this->nRows, this->nCols);
+  return this->nRows;
-  for (int ix = 0; ix < this->nValues; ++ix)
+}
-    r.data[ix] = this->data[ix];
+
-  return r;
+unsigned int Matrix2::NCols() {
+  return this->nCols;
 }
 
 // Move constructor
-Matrix2::Matrix2(Matrix2&& other) noexcept
+// Matrix2::Matrix2(Matrix2&& other) noexcept
-    : nRows(other.nRows),
+//     : nRows(other.nRows),
-      nCols(other.nCols),
+//       nCols(other.nCols),
-      nValues(other.nValues),
+//       nValues(other.nValues),
-      data(other.data) {
+//       data(other.data) {
-  other.data = nullptr;  // Set the other object's pointer to nullptr to avoid
+//   other.data = nullptr;  // Set the other object's pointer to nullptr to
-                         // double deletion
+//   avoid
-}
+//                          // double deletion
+// }
 
 // Move assignment operator
-Matrix2& Matrix2::operator=(Matrix2&& other) noexcept {
+// Matrix2& Matrix2::operator=(Matrix2&& other) noexcept {
-  if (this != &other) {
+//   if (this != &other) {
-    delete[] data;  // Clean up current data
+//     delete[] data;  // Clean up current data
-    nRows = other.nRows;
+//     nRows = other.nRows;
-    nCols = other.nCols;
+//     nCols = other.nCols;
-    nValues = other.nValues;
+//     nValues = other.nValues;
-    data = other.data;
+//     data = other.data;
-    other.data = nullptr;  // Avoid double deletion
+//     other.data = nullptr;  // Avoid double deletion
-  }
+//   }
-  return *this;
+//   return *this;
-}
+// }
 
 Matrix2 Matrix2::Zero(int nRows, int nCols) {
   Matrix2 r = Matrix2(nRows, nCols);
@@ -129,9 +231,16 @@ Matrix2 Matrix2::Zero(int nRows, int nCols) {
   return r;
 }
 
-void Matrix2::Clear() {
+const float& Matrix2::operator()(int row, int col) const {
+  return data[row * this->nCols + col];
+}
+float& Matrix2::operator()(int row, int col) {
+  return data[row * this->nCols + col];
+}
+
+void Matrix2::Fill(float value) {
   for (int ix = 0; ix < this->nValues; ix++)
-    this->data[ix] = 0;
+    this->data[ix] = value;
 }
 
 Matrix2 Matrix2::Identity(int size) {
@@ -186,13 +295,30 @@ Matrix2 LinearAlgebra::Matrix2::operator+(const Matrix2& v) const {
   return r;
 }
 
-Matrix2 Matrix2::operator+=(const Matrix2& v) {
+Matrix2& Matrix2::operator+=(const Matrix2& v) {
   for (int ix = 0; ix < this->nValues; ix++)
     this->data[ix] += v.data[ix];
   return *this;
 }
 
-Matrix2 LinearAlgebra::Matrix2::operator*(const Matrix2& B) const {
+Matrix2 Matrix2::operator-(const Matrix2& v) const {
+  Matrix2 r = Matrix2(this->nRows, this->nCols);
+  for (int ix = 0; ix < r.nValues; ix++)
+    r.data[ix] = this->data[ix] - v.data[ix];
+  return r;
+}
+
+Matrix1 operator*(const Matrix2& m, const Matrix1& v) {
+  Matrix1 r = Matrix1(m.nRows);
+  for (int rowIx = 0; rowIx < m.nRows; rowIx++) {
+    int mRowIx = rowIx * m.nCols;
+    for (int colIx = 0; colIx < m.nCols; colIx++)
+      r(rowIx) += m.data[mRowIx + colIx] * v(rowIx);
+  }
+  return r;
+}
+
+Matrix2 Matrix2::operator*(const Matrix2& B) const {
   Matrix2 r = Matrix2(this->nRows, B.nCols);
 
   int ACols = this->nCols;
@@ -221,6 +347,43 @@ Matrix2 LinearAlgebra::Matrix2::operator*(const Matrix2& B) const {
   return r;
 }
 
+Matrix2 Matrix2::GetRows(int from, int to) {
+  if (from < 0 || to >= this->nRows)
+    std::cerr << "Slice index out of range." << std::endl;
+
+  Matrix2 result = Matrix2(to - from, this->nCols);
+  int resultRowIx = 0;
+  for (int rowIx = from; rowIx < to; rowIx++) {
+    for (int colIx = 0; colIx < this->nCols; colIx++)
+      result.data[resultRowIx * result.nCols + colIx] =
+          this->data[rowIx * this->nCols + colIx];
+
+    resultRowIx++;
+  }
+
+  return result;
+}
+
+Vector3 Matrix2::GetRow3(int rowIx) {
+  int cellIx = rowIx * this->nCols;
+  Vector3 row(this->data[cellIx + 0], this->data[cellIx + 1],
+              this->data[cellIx + 2]);
+  return row;
+}
+
+void Matrix2::SetRow(int rowIx, Matrix1 source) {
+  int cellIx = rowIx * this->nCols;
+  for (int ix = 0; ix < source.Size(); ix++)
+    this->data[cellIx + ix] = source(ix);
+}
+
+void Matrix2::SetRow3(int rowIx, Vector3 v) {
+  int cellIx = rowIx * this->nCols;
+  this->data[cellIx + 0] = v.x;
+  this->data[cellIx + 1] = v.y;
+  this->data[cellIx + 2] = v.z;
+}
+
 Matrix2 Matrix2::Slice(int rowStart, int rowStop, int colStart, int colStop) {
   Matrix2 r = Matrix2(rowStop - rowStart, colStop - colStart);
 
@@ -234,22 +397,18 @@ Matrix2 Matrix2::Slice(int rowStart, int rowStop, int colStart, int colStop) {
   return r;
 }
 
+void Matrix2::UpdateSlice(int rowStart, int rowStop, const Matrix2& m) const {
+  UpdateSlice(rowStart, rowStop, 0, this->nCols, m);
+}
+
 void Matrix2::UpdateSlice(int rowStart,
                           int rowStop,
                           int colStart,
                           int colStop,
                           const Matrix2& m) const {
-  // for (int i = rowStart; i < rowStop; i++) {
-  //   for (int j = colStart; j < colStop; j++)
-  //     this->data[i * this->nCols + j] =
-  //         m.data[(i - rowStart) * m.nCols + (j - colStart)];
-  // }
-
-  int rRowDataIx = rowStart * this->nCols;
   int mRowDataIx = 0;
   for (int rowIx = rowStart; rowIx < rowStop; rowIx++) {
-    rRowDataIx = rowIx * this->nCols;
+    int rRowDataIx = rowIx * this->nCols;
-    // rRowDataIx += this->nCols;
     mRowDataIx += m.nCols;
     for (int colIx = colStart; colIx < colStop; colIx++) {
       this->data[rRowDataIx + colIx] = m.data[mRowDataIx + (colIx - colStart)];
@@ -257,6 +416,91 @@ void Matrix2::UpdateSlice(int rowStart,
   }
 }
 
+Matrix2 Matrix2::Inverse() {
+  int n = this->nRows;
+  // // Create an identity matrix of the same size as the original matrix
+  Matrix2 augmentedMatrix(n, 2 * n);
+  for (int i = 0; i < n; i++) {
+    for (int j = 0; j < n; j++) {
+      augmentedMatrix.data[i * this->nCols + j] =
+          this->data[i * this->nCols + j];
+      augmentedMatrix.data[i * this->nCols + (j + n)] =
+          (i == j) ? 1 : 0;  // identity matrix;
+    }
+  }
+
+  // Perform Gaussian elimination
+  for (int i = 0; i < n; i++) {
+    // Find the pivot row
+    float pivot = augmentedMatrix.data[i * augmentedMatrix.nCols + i];
+    if (abs(pivot) < 1e-10)  // Check for singular matrix
+      std::cerr << "Matrix is singular and cannot be inverted." << std::endl;
+
+    // Normalize the pivot row
+    for (int j = 0; j < 2 * n; j++)
+      augmentedMatrix.data[i * augmentedMatrix.nCols + j] /= pivot;
+
+    // Eliminate the column below the pivot
+    for (int j = i + 1; j < n; j++) {
+      float factor = augmentedMatrix.data[j * augmentedMatrix.nCols + i];
+      for (int k = 0; k < 2 * n; k++)
+        augmentedMatrix.data[j * augmentedMatrix.nCols + k] -=
+            factor * augmentedMatrix.data[i * augmentedMatrix.nCols + k];
+    }
+  }
+
+  // Back substitution
+  for (int i = n - 1; i >= 0; i--) {
+    // Eliminate the column above the pivot
+    for (int j = i - 1; j >= 0; j--) {
+      float factor = augmentedMatrix.data[j * augmentedMatrix.nCols + i];
+      for (int k = 0; k < 2 * n; k++)
+        augmentedMatrix.data[j * augmentedMatrix.nCols + k] -=
+            factor * augmentedMatrix.data[i * augmentedMatrix.nCols + k];
+    }
+  }
+
+  // Extract the inverse matrix from the augmented matrix
+  Matrix2 inverse(n, n);
+  for (int i = 0; i < n; i++) {
+    for (int j = 0; j < n; j++)
+      inverse.data[i * inverse.nCols + j] =
+          augmentedMatrix.data[i * augmentedMatrix.nCols + j + n];
+  }
+  return inverse;
+}
+
+Matrix2 Matrix2::DeleteRows(int rowStart, int rowStop) {
+  Matrix2 r = Matrix2(this->nRows - (rowStop - rowStart), this->nCols);
+
+  int resultRowIx = 0;
+  for (int i = 0; i < this->nRows; i++) {
+    if (i >= rowStart && i < rowStop)
+      continue;
+
+    for (int j = 0; j < this->nCols; j++)
+      r.data[resultRowIx * r.nCols + j] = this->data[i * this->nCols + j];
+
+    resultRowIx++;
+  }
+  return r;
+}
+
+Matrix2 Matrix2::DeleteColumns(int colStart, int colStop) {
+  Matrix2 r = Matrix2(this->nRows, this->nCols - (colStop - colStart));
+
+  for (int i = 0; i < this->nRows; i++) {
+    int resultColIx = 0;
+    for (int j = 0; j < this->nCols; j++) {
+      if (j >= colStart && j < colStop)
+        continue;
+
+      r.data[i * r.nCols + resultColIx++] = this->data[i * this->nCols + j];
+    }
+  }
+  return r;
+}
+
 /// @brief Compute the Omega matrix of a 3D vector
 /// @param v The vector
 /// @return 4x4 Omega matrix

LinearAlgebra/Matrix.h

@@ -9,15 +9,36 @@ namespace LinearAlgebra {
 /// @brief A 1-dimensional matrix or vector of arbitrary size
 class Matrix1 {
  public:
-  float* data = nullptr;
+  Matrix1();
-  int size = 0;
-
   Matrix1(int size);
-  Matrix1(float* data, int size);
+  Matrix1(int size, float* data);
+  Matrix1(const Matrix1& m);  // Make a clone
+
+  ~Matrix1();
+
+  unsigned int Size() const;
+
+  static Matrix1 Zero(unsigned int size);
+  void Fill(float value);
+  void Fill(float value, unsigned int start, unsigned int stop);
+
+  static Matrix1 FromVector3(Vector3 v);
+  Vector3 ToVector3();
 
   static Matrix1 FromQuaternion(Quaternion q);
   Quaternion ToQuaternion();
 
+  const float& operator()(int ix) const;
+  float& operator()(int ix);
+
+
+  Matrix1 Slice(int start, int stop);
+  void UpdateSlice(int start, int stop, const Matrix1& m) const;
+
+ protected:
+  float* data = nullptr;
+  int size = 0;
+
  private:
   bool externalData = true;
 };
@@ -25,31 +46,47 @@ class Matrix1 {
 /// @brief A 2-dimensional matrix of arbitrary size
 class Matrix2 {
  public:
-  int nRows = 0;
-  int nCols = 0;
-  int nValues = 0;
-  float* data = nullptr;
-
   Matrix2();
   Matrix2(int nRows, int nCols);
-  Matrix2(float* data, int nRows, int nCols);
+  Matrix2(int nRows, int nCols, float* data, float autoDestruct = false);
   Matrix2(const Matrix2& m);
   Matrix2& operator=(const Matrix2& other);
 
   ~Matrix2();
 
-  Matrix2 Clone() const;
+  unsigned int NRows();
+  unsigned int NCols();
 
   static Matrix2 Zero(int nRows, int nCols);
-  void Clear();
+  void Fill(float value);
 
   static Matrix2 Identity(int size);
-
   static Matrix2 Diagonal(float f, int size);
-
   static Matrix2 SkewMatrix(const Vector3& v);
 
   Matrix2 Transpose() const;
+  Matrix2 Inverse();
+
+  const float& operator()(int row, int col) const;
+  float& operator()(int row, int col);
+
+  Matrix2 GetRows(int from, int to);
+  Vector3 GetRow3(int rowIx);
+
+  void SetRow(int rowIx, Matrix1 source);
+  void SetRow3(int rowIx, Vector3 v);
+
+  Matrix2 Slice(int rawStart, int rowStop, int colStart, int colStop);
+
+  void UpdateSlice(int rowStart, int rowStop, const Matrix2& m) const;
+  void UpdateSlice(int rowStart,
+                   int rowStop,
+                   int colStart,
+                   int colStop,
+                   const Matrix2& m) const;
+
+  Matrix2 DeleteRows(int rowStart, int rowStop);
+  Matrix2 DeleteColumns(int colStart, int colStop);
 
   Matrix2 operator-() const;
 
@@ -57,7 +94,9 @@ class Matrix2 {
   /// @param m The matrix to add to this matrix
   /// @return The result of the addition
   Matrix2 operator+(const Matrix2& v) const;
-  Matrix2 operator+=(const Matrix2& v);
+  Matrix2& operator+=(const Matrix2& v);
+
+  Matrix2 operator-(const Matrix2& v) const;
 
   Matrix2 operator*(const Matrix2& m) const;
   friend Matrix2 operator*(const Matrix2& m, float f) {
@@ -73,15 +112,7 @@ class Matrix2 {
     return r;
   }
 
-  friend Matrix1 operator*(const Matrix2& m, const Matrix1& v) {
+  friend Matrix1 operator*(const Matrix2& m, const Matrix1& v);
-    Matrix1 r = Matrix1(m.nRows);
-    for (int rowIx = 0; rowIx < m.nRows; rowIx++) {
-      int mRowIx = rowIx * m.nCols;
-      for (int colIx = 0; colIx < m.nCols; colIx++)
-        r.data[rowIx] += m.data[mRowIx + colIx] * v.data[rowIx];
-    }
-    return r;
-  }
 
   friend Matrix2 operator/(const Matrix2& m, float f) {
     Matrix2 r = Matrix2(m.nRows, m.nCols);
@@ -96,20 +127,19 @@ class Matrix2 {
     return r;
   }
 
-  Matrix2 Slice(int rawStart, int rowStop, int colStart, int colStop);
+    //  move constructor and move assignment operator
-
+  // Matrix2(Matrix2&& other) noexcept;
-  void UpdateSlice(int rowStart,
+  // Matrix2& operator=(Matrix2&& other) noexcept;
-                int rowStop,
-                int colStart,
-                int colStop,
-                const Matrix2& m) const;
-  // private:
-  //  move constructor and move assignment operator
-  Matrix2(Matrix2&& other) noexcept;
-  Matrix2& operator=(Matrix2&& other) noexcept;
 
   static Matrix2 Omega(const Vector3& v);
 
+ public:
+  int nRows = 0;
+  int nCols = 0;
+protected:
+  int nValues = 0;
+  float* data = nullptr;
+
  private:
   bool externalData = true;
 };

LinearAlgebra/Quaternion.cpp

@@ -99,14 +99,12 @@ Vector3 Quaternion::ToAngles(const Quaternion& q1) {
 }
 
 Matrix2 LinearAlgebra::Quaternion::ToRotationMatrix() {
-  Matrix2 r = Matrix2(3, 3);
-
   float x = this->x;
   float y = this->y;
   float z = this->z;
   float w = this->w;
 
-  float* data = r.data;
+  float* data = new float[9];  // r.data;
   data[0 * 3 + 0] = 1 - 2 * (y * y + z * z);
   data[0 * 3 + 1] = 2 * (x * y - w * z);
   data[0 * 3 + 2] = 2 * (x * z + w * y);
@@ -117,6 +115,8 @@ Matrix2 LinearAlgebra::Quaternion::ToRotationMatrix() {
   data[2 * 3 + 1] = 2 * (y * z + w * x);
   data[2 * 3 + 2] = 1 - 2 * (x * x + y * y);
 
+  Matrix2 r = Matrix2(3, 3, data, true);
+
   return r;
 }
 
@@ -168,6 +168,10 @@ Quaternion Quaternion::Inverse(Quaternion r) {
   return Quaternion(-r.x / n, -r.y / n, -r.z / n, r.w / n);
 }
 
+Quaternion Quaternion::Reflect(Quaternion q) {
+  return Quaternion(-q.x, -q.y, -q.z, q.w);
+}
+
 Quaternion Quaternion::LookRotation(const Vector3& forward) {
   Vector3 up = Vector3(0, 1, 0);
   return LookRotation(forward, up);

LinearAlgebra/Quaternion.h

@@ -126,6 +126,8 @@ struct Quaternion : Quat {
   /// needed</param> <returns>The inverted quaternion</returns>
   static Quaternion Inverse(Quaternion quaternion);
 
+  static Quaternion Reflect(Quaternion q);
+
   /// <summary>
   /// A rotation which looks in the given direction
   /// </summary>

LinearAlgebra/Vector2.cpp

@@ -180,3 +180,13 @@ Vector2 Vector2::Lerp(const Vector2& v1, const Vector2& v2, float f) {
   Vector2 v = v1 + (v2 - v1) * f;
   return v;
 }
+
+
+#pragma region Vector2Of
+
+template <typename T>
+Vector2Of<T>::Vector2Of() {
+  this->horizontal = 0;
+  this->vertical = 0;
+}
+#pragma endregion Vector2Of

LinearAlgebra/Vector2.h

@@ -201,6 +201,17 @@ struct Vector2 : Vec2 {
   static Vector2 Lerp(const Vector2& v1, const Vector2& v2, float f);
 };
 
+template <typename T>
+struct Vector2Of {
+ public:
+  Vector2Of();
+
+  T horizontal = 0;
+  T vertical = 0;
+};
+
+using Vector2Int = Vector2Of<int>;
+
 }  // namespace LinearAlgebra
 using namespace LinearAlgebra;
 

Participant.cpp

@@ -19,34 +19,9 @@ void Participant::ReplaceLocalParticipant(Participant& newParticipant) {
 
 Participant::Participant() {
   Thing::CreateRoot(this);
-  //this->Add(this->root);
 }
 
-/*
+Participant::~Participant() {}
-Participant::Participant(const char* ipAddress, int port) {
-  Thing::CreateRoot(this);
-  //this->Add(this->root);
-
-  // make a copy of the ip address string
-  int addressLength = (int)strlen(ipAddress);
-  int stringLength = addressLength + 1;
-  char* addressString = new char[stringLength];
-#if defined(_WIN32) || defined(_WIN64)
-  strncpy_s(addressString, stringLength, ipAddress,
-            addressLength);  // Leave space for null terminator
-#else
-  strncpy(addressString, ipAddress, addressLength);
-#endif
-  addressString[addressLength] = '\0';
-
-  this->ipAddress = addressString;
-  this->port = port;
-}
-*/
-Participant::~Participant() {
-  // registry.Remove(this);
-  // delete[] this->ipAddress;
-}
 
 void Participant::Update(bool recurse) {
   for (Thing* thing : this->things) {
@@ -56,48 +31,20 @@ void Participant::Update(bool recurse) {
 }
 
 bool Participant::Send(IMessage* msg) {
-    std::cout << "sending message " << (static_cast<int>(this->buffer[0]) & 0xff)
+  std::cout << "sending message " << (static_cast<int>(this->buffer[0]) & 0xff)
-            << " to base Participant without communcation support " << std::endl;
+            << " to base Participant without communcation support "
+            << std::endl;
 
   return true;
 }
-/*
-bool Participant::Send(IMessage* msg) {
-  int bufferSize = msg->Serialize(this->buffer);
-  if (bufferSize <= 0)
-    return true;
-
-  // std::cout << "send msg " << (static_cast<int>(this->buffer[0]) & 0xff)
-  //           << " to " << this->ipAddress << std::endl;
-
-#if defined(_WIN32) || defined(_WIN64)
-  Windows::ParticipantUDP* thisWindows =
-      static_cast<Windows::ParticipantUDP*>(this);
-  return thisWindows->SendTo(this, bufferSize);
-#elif defined(__unix__) || defined(__APPLE__)
-  Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
-  return thisPosix->SendTo(this, bufferSize);
-#elif defined(ARDUINO)
-  Arduino::ParticipantUDP* thisArduino =
-      static_cast<Arduino::ParticipantUDP*>(this);
-  return thisArduino->SendTo(this, bufferSize);
-#elif defined(IDF_VER)
-  EspIdf::ParticipantUDP* thisEspIdf =
-      static_cast<EspIdf::ParticipantUDP*>(this);
-  return thisEspIdf->SendTo(this, bufferSize);
-#else
-  return false;
-#endif
-}
-*/
 
 Thing* Participant::Get(unsigned char networkId, unsigned char thingId) {
   for (Thing* thing : this->things) {
     if (thing->owner->networkId == networkId && thing->id == thingId)
       return thing;
   }
-  std::cout << "Could not find thing " //<< this->ipAddress << ":" << this->port
+  std::cout << "Could not find thing " << "[" << (int)networkId << ": "
-            << "[" << (int)thingId << "]\n";
+            << (int)thingId << "]\n";
   return nullptr;
 }
 
@@ -119,9 +66,8 @@ void Participant::Add(Thing* thing, bool checkId) {
     thing->id = highestIx + 1;
     this->things.push_back(thing);
 #endif
-    std::cout << "Add thing with generated ID " 
+    std::cout << "Add thing with generated ID "
-    //<< this->ipAddress << ":" << this->port
+              << "[" << (int)networkId << ": " << (int)thing->id << "]\n";
-    << "[" << (int)thing->id << "]\n";
   } else {
     Thing* foundThing = Get(thing->owner->networkId, thing->id);
     if (foundThing == nullptr) {
@@ -130,13 +76,11 @@ void Participant::Add(Thing* thing, bool checkId) {
 #else
       this->things.push_back(thing);
 #endif
-      std::cout << "Add thing " << //this->ipAddress << ":" << this->port <<
+      std::cout << "Add thing [" << (int)networkId << ": " << (int)thing->id
-      "["
+                << "]\n";
-                << (int)thing->id << "]\n";
     } else {
-      std::cout << "Did not add, existing thing " 
+      std::cout << "Did not add, existing thing "
-      //<< this->ipAddress << ":" << this->port
+                << "[" << (int)networkId << ": " << (int)thing->id << "]\n";
-       << "[" << (int)thing->id << "]\n";
     }
   }
 }
@@ -164,88 +108,4 @@ void Participant::Remove(Thing* thing) {
 
 #pragma endregion
 
-// #pragma region ParticipantRegistry
-
-// /*
-// Participant* ParticipantRegistry::Get(const char* ipAddress,
-//                                       unsigned int port) {
-// #if !defined(NO_STD)
-//   for (Participant* participant : ParticipantRegistry::participants) {
-//     if (participant == nullptr)
-//       continue;
-//     if (strcmp(participant->ipAddress, ipAddress) == 0 &&
-//         participant->port == port) {
-//       // std::cout << "found participant " << participant->ipAddress << ":"
-//       //           << (int)participant->port << std::endl;
-//       return participant;
-//     }
-//   }
-//   std::cout << "Could not find participant " << ipAddress << ":" << (int)port
-//             << std::endl;
-// #endif
-//   return nullptr;
-// }
-// */
-// Participant* ParticipantRegistry::Get(unsigned char participantId) {
-// #if !defined(NO_STD)
-//   for (Participant* participant : ParticipantRegistry::participants) {
-//     if (participant == nullptr)
-//       continue;
-//     if (participant->networkId == participantId)
-//       return participant;
-//   }
-//   std::cout << "Could not find participant " << (int)participantId << std::endl;
-// #endif
-//   return nullptr;
-// }
-
-// // Participant* ParticipantRegistry::Add(const char* ipAddress,
-// //                                       unsigned int port) {
-// //   Participant* participant = new Participant(ipAddress, port);
-// //   Add(participant);
-// //   return participant;
-// // }
-
-// void ParticipantRegistry::Add(Participant* participant) {
-//   Participant* foundParticipant =
-//     Get(participant->networkId);
-//       //Get(participant->ipAddress, participant->port);
-
-//   if (foundParticipant == nullptr) {
-// #if defined(NO_STD)
-//     // this->things[this->thingCount++] = thing;
-// #else
-//     ParticipantRegistry::participants.push_back(participant);
-// #endif
-//     //   std::cout << "Add participant " << participant->ipAddress << ":"
-//     //             << participant->port << "[" << (int)participant->networkId
-//     //             << "]\n";
-//     //   std::cout << "participants " <<
-//     //   ParticipantRegistry::participants.size()
-//     //             << "\n";
-//     // } else {
-//     //   std::cout << "Did not add, existing participant " <<
-//     //   participant->ipAddress
-//     //             << ":" << participant->port << "[" <<
-//     //             (int)participant->networkId
-//     //             << "]\n";
-//   }
-// }
-
-// void ParticipantRegistry::Remove(Participant* participant) {
-//   // participants.remove(participant);
-// }
-
-// #if defined(NO_STD)
-// Participant** ParticipantRegistry::GetAll() const {
-//   return ParticipantRegistry::participants;
-// }
-// #else
-// const std::list<Participant*>& ParticipantRegistry::GetAll() const {
-//   return ParticipantRegistry::participants;
-// }
-// #endif
-
-// #pragma endregion ParticipantRegistry
-
 }  // namespace RoboidControl

Participant.h

@@ -7,55 +7,6 @@ namespace RoboidControl {
 
 constexpr int MAX_THING_COUNT = 256;
 
-/*
-/// @brief class which manages all known participants
-class ParticipantRegistry {
- public:
-  /// @brief Retrieve a participant by its address
-  /// @param ipAddress The IP address of the participant
-  /// @param port The port number of the participant
-  /// @return The participant or a nullptr when it could not be found
-  //Participant* Get(const char* ipAddress, unsigned int port);
-  /// @brief Retrieve a participant by its network ID
-  /// @param networkID The network ID of the participant
-  /// @return The participant or a nullptr when it could not be found
-  Participant* Get(unsigned char networkID);
-
-  /// @brief Add a participant with the given details
-  /// @param ipAddress The IP address of the participant
-  /// @param port The port number of the participant
-  /// @return The added participant
-  //Participant* Add(const char* ipAddress, unsigned int port);
-
-  /// @brief Add a participant
-  /// @param participant The participant to add
-  void Add(Participant* participant);
-
-  /// @brief Remove a participant
-  /// @param participant The participant to remove
-  void Remove(Participant* participant);
-
- private:
-#if defined(NO_STD)
- public:
-  Participant** GetAll() const;
-  int count = 0;
-
- private:
-  Participant** participants;
-#else
- public:
-  /// @brief Get all participants
-  /// @return All participants
-  const std::list<Participant*>& GetAll() const;
-
- private:
-  /// @brief The list of known participants
-  std::list<Participant*> participants;
-#endif
-};
-*/
-
 /// @brief A participant is a device which manages things.
 /// It can communicate with other participant to synchronise the state of
 /// things. This class is used to register the things the participant is
@@ -137,12 +88,6 @@ class Participant {
 
 #pragma endregion Send
 
-// #pragma region Participant Registry
-
-//  public:
-//   static ParticipantRegistry registry;
-
-// #pragma endregion Participant Registry
 };
 
 }  // namespace RoboidControl

Participants/ParticipantSocket.cpp

@@ -0,0 +1,24 @@
+#include "ParticipantSocket.h"
+
+#include <string.h>
+
+namespace RoboidControl {
+
+ParticipantSocket::ParticipantSocket(const char* ipAddress, int port) {
+  // make a copy of the ip address string
+  int addressLength = (int)strlen(ipAddress);
+  int stringLength = addressLength + 1;
+  char* addressString = new char[stringLength];
+#if defined(_WIN32) || defined(_WIN64)
+  strncpy_s(addressString, stringLength, ipAddress,
+            addressLength);  // Leave space for null terminator
+#else
+  strncpy(addressString, ipAddress, addressLength);
+#endif
+  addressString[addressLength] = '\0';
+
+  this->ipAddress = addressString;
+  this->port = port;
+}
+
+}

Participants/ParticipantSocket.h

@@ -0,0 +1,28 @@
+#pragma once
+
+#include "Participant.h"
+
+namespace RoboidControl {
+
+class ParticipantSocket : public Participant {
+ public:
+ /// @brief Create a new participant with the given communcation info
+  /// @param ipAddress The IP address of the participant
+  /// @param port The UDP port of the participant
+  /// @remarks This does not belong here, it should move to ParticipantUDP or
+  /// something like that in the future
+  ParticipantSocket(const char* ipAddress, int port);
+
+  /// @brief The Ip Address of a participant.
+  /// @remarks This does not belong here, it should move to ParticipantUDP or
+  /// something like that in the future
+  const char* ipAddress = "0.0.0.0";
+  /// @brief The port number for UDP communication with the participant.
+  /// @remarks This does not belong here, it should move to ParticipantUDP or
+  /// something like that in the future
+  unsigned int port = 0;
+
+  bool Send(IMessage* msg) override;
+};
+
+}

Participants/ParticipantUDP.cpp

@@ -12,112 +12,11 @@
 #include "Things/DistanceSensor.h"
 #include "Things/TouchSensor.h"
 
-#include <string.h>
+// #include <string.h>
 
 namespace RoboidControl {
 
-#pragma region ParticipantRegistry
+bool ParticipantSocket::Send(IMessage* msg) {
-
-ParticipantRegistry ParticipantUDPGeneric::registry;
-
-RemoteParticipantUDP* ParticipantRegistry::Get(const char* ipAddress,
-                                               unsigned int port) {
-#if !defined(NO_STD)
-  for (RemoteParticipantUDP* participant : ParticipantRegistry::participants) {
-    if (participant == nullptr)
-      continue;
-    if (strcmp(participant->ipAddress, ipAddress) == 0 &&
-        participant->port == port) {
-      // std::cout << "found participant " << participant->ipAddress << ":"
-      //           << (int)participant->port << std::endl;
-      return participant;
-    }
-  }
-  std::cout << "Could not find participant " << ipAddress << ":" << (int)port
-            << std::endl;
-#endif
-  return nullptr;
-}
-
-RemoteParticipantUDP* ParticipantRegistry::Get(unsigned char participantId) {
-#if !defined(NO_STD)
-  for (RemoteParticipantUDP* participant : ParticipantRegistry::participants) {
-    if (participant == nullptr)
-      continue;
-    if (participant->networkId == participantId)
-      return participant;
-  }
-  std::cout << "Could not find participant " << (int)participantId << std::endl;
-#endif
-  return nullptr;
-}
-
-RemoteParticipantUDP* ParticipantRegistry::Add(const char* ipAddress,
-                                               unsigned int port) {
-  RemoteParticipantUDP* participant = new RemoteParticipantUDP(ipAddress, port);
-  Add(participant);
-  return participant;
-}
-
-void ParticipantRegistry::Add(RemoteParticipantUDP* participant) {
-  Participant* foundParticipant = Get(participant->networkId);
-  // Get(participant->ipAddress, participant->port);
-
-  if (foundParticipant == nullptr) {
-#if defined(NO_STD)
-    // this->things[this->thingCount++] = thing;
-#else
-    ParticipantRegistry::participants.push_back(participant);
-#endif
-    //   std::cout << "Add participant " << participant->ipAddress << ":"
-    //             << participant->port << "[" << (int)participant->networkId
-    //             << "]\n";
-    //   std::cout << "participants " <<
-    //   ParticipantRegistry::participants.size()
-    //             << "\n";
-    // } else {
-    //   std::cout << "Did not add, existing participant " <<
-    //   participant->ipAddress
-    //             << ":" << participant->port << "[" <<
-    //             (int)participant->networkId
-    //             << "]\n";
-  }
-}
-
-void ParticipantRegistry::Remove(RemoteParticipantUDP* participant) {
-  // participants.remove(participant);
-}
-
-#if defined(NO_STD)
-RemoteParticipantUDP** ParticipantRegistry::GetAll() const {
-  return ParticipantRegistry::participants;
-}
-#else
-const std::list<RemoteParticipantUDP*>& ParticipantRegistry::GetAll() const {
-  return ParticipantRegistry::participants;
-}
-#endif
-
-#pragma endregion ParticipantRegistry
-
-RemoteParticipantUDP::RemoteParticipantUDP(const char* ipAddress, int port) {
-  // make a copy of the ip address string
-  int addressLength = (int)strlen(ipAddress);
-  int stringLength = addressLength + 1;
-  char* addressString = new char[stringLength];
-#if defined(_WIN32) || defined(_WIN64)
-  strncpy_s(addressString, stringLength, ipAddress,
-            addressLength);  // Leave space for null terminator
-#else
-  strncpy(addressString, ipAddress, addressLength);
-#endif
-  addressString[addressLength] = '\0';
-
-  this->ipAddress = addressString;
-  this->port = port;
-}
-
-bool RemoteParticipantUDP::Send(IMessage* msg) {
   // No message is actually sent, because this class has no networking
   // implementation
   return false;
@@ -126,7 +25,7 @@ bool RemoteParticipantUDP::Send(IMessage* msg) {
 #pragma region Init
 
 ParticipantUDPGeneric::ParticipantUDPGeneric(int port)
-    : RemoteParticipantUDP("127.0.0.1", port) {
+    : ParticipantSocket("127.0.0.1", port) {
   this->name = "ParticipantUDP";
   this->remoteSite = nullptr;
   if (this->port == 0)
@@ -145,12 +44,12 @@ ParticipantUDPGeneric::ParticipantUDPGeneric(int port)
 ParticipantUDPGeneric::ParticipantUDPGeneric(const char* ipAddress,
                                              int port,
                                              int localPort)
-    : RemoteParticipantUDP("127.0.0.1", localPort) {
+    : ParticipantSocket("127.0.0.1", localPort) {
   this->name = "ParticipantUDP";
   if (this->port == 0)
     this->isIsolated = true;
   else
-    this->remoteSite = new RemoteParticipantUDP(ipAddress, port);
+    this->remoteSite = new ParticipantSocket(ipAddress, port);
   registry.Add(this);
 
   this->root = Thing::LocalRoot();  // Participant::LocalParticipant->root;
@@ -196,7 +95,7 @@ void ParticipantUDPGeneric::Update(bool recurse) {
       this->nextPublishMe = currentTimeMs + this->publishInterval;
     }
 
-    //this->ReceiveUDP();
+    this->ReceiveUDP();
   }
 
   UpdateMyThings();
@@ -208,7 +107,7 @@ void ParticipantUDPGeneric::UpdateMyThings() {
     if (thing == nullptr)  // || thing->GetParent() != nullptr)
       continue;
 
-      // Why don't we do recursive?
+    // Why don't we do recursive?
     // Because when a thing creates a thing in the update,
     // that new thing is not sent out (because of hierarchyChanged)
     // before it is updated itself: it is immediatedly updated!
@@ -311,8 +210,7 @@ void ParticipantUDPGeneric::ReceiveData(unsigned char packetSize,
                                         unsigned int senderPort) {
   // std::cout << "Receive data from " << senderIpAddress << ":" << senderPort
   //           << std::endl;
-  RemoteParticipantUDP* sender =
+  ParticipantSocket* sender = this->registry.Get(senderIpAddress, senderPort);
-      this->registry.Get(senderIpAddress, senderPort);
   if (sender == nullptr) {
     sender = this->registry.Add(senderIpAddress, senderPort);
 #if !defined(NO_STD)
@@ -325,7 +223,7 @@ void ParticipantUDPGeneric::ReceiveData(unsigned char packetSize,
 }
 
 void ParticipantUDPGeneric::ReceiveData(unsigned char bufferSize,
-                                        RemoteParticipantUDP* sender) {
+                                        ParticipantSocket* sender) {
   unsigned char msgId = this->buffer[0];
   // std::cout << "receive msg " << (int)msgId << "\n";
   // std::cout << "  buffer size = " <<(int) bufferSize << "\n";
@@ -398,7 +296,7 @@ void ParticipantUDPGeneric::ReceiveData(unsigned char bufferSize,
 #endif
 }
 
-void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
+void ParticipantUDPGeneric::Process(ParticipantSocket* sender,
                                     ParticipantMsg* msg) {
 #if defined(DEBUG)
   std::cout << this->name << ": Process ParticipantMsg " << (int)msg->networkId
@@ -406,7 +304,7 @@ void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
 #endif
 }
 
-void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
+void ParticipantUDPGeneric::Process(ParticipantSocket* sender,
                                     NetworkIdMsg* msg) {
 #if defined(DEBUG)
   std::cout << this->name << ": process NetworkIdMsg " << (int)this->networkId
@@ -422,7 +320,7 @@ void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
   }
 }
 
-void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
+void ParticipantUDPGeneric::Process(ParticipantSocket* sender,
                                     InvestigateMsg* msg) {
 #if defined(DEBUG)
   std::cout << this->name << ": Process InvestigateMsg [" << (int)msg->networkId
@@ -430,16 +328,15 @@ void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
 #endif
 }
 
-void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
+void ParticipantUDPGeneric::Process(ParticipantSocket* sender, ThingMsg* msg) {
-                                    ThingMsg* msg) {
 #if defined(DEBUG)
   std::cout << this->name << ": process ThingMsg [" << (int)msg->networkId
             << "/" << (int)msg->thingId << "] " << (int)msg->thingType << " "
             << (int)msg->parentId << "\n";
 #endif
-  RemoteParticipantUDP* owner = registry.Get(msg->networkId);
+  ParticipantSocket* owner = registry.Get(msg->networkId);
   if (owner == nullptr) {
-    owner = new RemoteParticipantUDP(sender->ipAddress, sender->port);
+    owner = new ParticipantSocket(sender->ipAddress, sender->port);
     owner->networkId = msg->networkId;
     registry.Add(owner);
   }
@@ -461,7 +358,7 @@ void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
     thing->SetParent(nullptr);
 }
 
-Thing* ParticipantUDPGeneric::ProcessNewThing(RemoteParticipantUDP* owner,
+Thing* ParticipantUDPGeneric::ProcessNewThing(ParticipantSocket* owner,
                                               ThingMsg* msg,
                                               bool isRemote) {
   switch (msg->thingType) {
@@ -476,8 +373,7 @@ Thing* ParticipantUDPGeneric::ProcessNewThing(RemoteParticipantUDP* owner,
   }
 }
 
-void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
+void ParticipantUDPGeneric::Process(ParticipantSocket* sender, NameMsg* msg) {
-                                    NameMsg* msg) {
 #if defined(DEBUG)
   std::cout << this->name << ": process NameMsg [" << (int)msg->networkId << "/"
             << (int)msg->thingId << "] ";
@@ -509,7 +405,7 @@ void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
 #endif
 }
 
-void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
+void ParticipantUDPGeneric::Process(ParticipantSocket* sender,
                                     ModelUrlMsg* msg) {
 #if defined(DEBUG)
   std::cout << this->name << ": process ModelUrlMsg [" << (int)msg->networkId
@@ -517,8 +413,7 @@ void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
 #endif
 }
 
-void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
+void ParticipantUDPGeneric::Process(ParticipantSocket* sender, PoseMsg* msg) {
-                                    PoseMsg* msg) {
 #if !defined(DEBUG) && !defined(NO_STD)
   std::cout << this->name << ": process PoseMsg [" << (int)this->networkId
             << "/" << (int)msg->networkId << "] " << (int)msg->poseType << "\n";
@@ -541,8 +436,7 @@ void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
     thing->SetAngularVelocity(msg->angularVelocity);
 }
 
-void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
+void ParticipantUDPGeneric::Process(ParticipantSocket* sender, BinaryMsg* msg) {
-                                    BinaryMsg* msg) {
 #if defined(DEBUG)
   std::cout << this->name << ": process BinaryMsg [" << (int)msg->networkId
             << "/" << (int)msg->thingId << "]\n";
@@ -564,15 +458,14 @@ void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
   }
 }
 
-void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
+void ParticipantUDPGeneric::Process(ParticipantSocket* sender, TextMsg* msg) {
-                                    TextMsg* msg) {
 #if defined(DEBUG)
   std::cout << this->name << ": process TextMsg " << (int)msg->textLength << " "
             << (int)msg->text << "\n";
 #endif
 }
 
-void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
+void ParticipantUDPGeneric::Process(ParticipantSocket* sender,
                                     DestroyMsg* msg) {
 #if defined(DEBUG)
   std::cout << this->name << ": process Destroy [" << (int)msg->networkId << "/"
@@ -587,4 +480,87 @@ void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
 // Receive
 #pragma endregion
 
+#pragma region Registry
+
+ParticipantUDP::Registry ParticipantUDPGeneric::registry;
+
+ParticipantSocket* ParticipantUDP::Registry::Get(const char* ipAddress, unsigned int port) {
+#if !defined(NO_STD)
+  for (ParticipantSocket* participant : Registry::participants) {
+    if (participant == nullptr)
+      continue;
+    if (strcmp(participant->ipAddress, ipAddress) == 0 &&
+        participant->port == port) {
+      // std::cout << "found participant " << participant->ipAddress << ":"
+      //           << (int)participant->port << std::endl;
+      return participant;
+    }
+  }
+  std::cout << "Could not find participant " << ipAddress << ":" << (int)port
+            << std::endl;
+#endif
+  return nullptr;
+}
+
+ParticipantSocket* ParticipantUDP::Registry::Get(unsigned char participantId) {
+#if !defined(NO_STD)
+  for (ParticipantSocket* participant : Registry::participants) {
+    if (participant == nullptr)
+      continue;
+    if (participant->networkId == participantId)
+      return participant;
+  }
+  std::cout << "Could not find participant " << (int)participantId << std::endl;
+#endif
+  return nullptr;
+}
+
+ParticipantSocket* ParticipantUDP::Registry::Add(const char* ipAddress, unsigned int port) {
+  ParticipantSocket* participant = new ParticipantSocket(ipAddress, port);
+  Add(participant);
+  return participant;
+}
+
+void ParticipantUDP::Registry::Add(ParticipantSocket* participant) {
+  Participant* foundParticipant = Get(participant->networkId);
+  // Get(participant->ipAddress, participant->port);
+
+  if (foundParticipant == nullptr) {
+#if defined(NO_STD)
+    // this->things[this->thingCount++] = thing;
+#else
+    Registry::participants.push_back(participant);
+#endif
+    //   std::cout << "Add participant " << participant->ipAddress << ":"
+    //             << participant->port << "[" << (int)participant->networkId
+    //             << "]\n";
+    //   std::cout << "participants " <<
+    //   Registry::participants.size()
+    //             << "\n";
+    // } else {
+    //   std::cout << "Did not add, existing participant " <<
+    //   participant->ipAddress
+    //             << ":" << participant->port << "[" <<
+    //             (int)participant->networkId
+    //             << "]\n";
+  }
+}
+
+void ParticipantUDP::Registry::Remove(ParticipantSocket* participant) {
+  // participants.remove(participant);
+}
+
+#if defined(NO_STD)
+RemoteParticipantUDP** Registry::GetAll() const {
+  return Registry::participants;
+}
+#else
+const std::list<ParticipantSocket*>& ParticipantUDP::Registry::GetAll() const {
+  return Registry::participants;
+}
+#endif
+
+// Registry
+#pragma endregion Registry
+
 }  // namespace RoboidControl

Participants/ParticipantUDP.h

@@ -10,7 +10,8 @@
 #include "Messages/PoseMsg.h"
 #include "Messages/TextMsg.h"
 #include "Messages/ThingMsg.h"
-#include "Participant.h"
+
+#include "ParticipantSocket.h"
 
 #if !defined(NO_STD)
 #include <functional>
@@ -31,74 +32,6 @@ namespace RoboidControl {
 
 constexpr int MAX_SENDER_COUNT = 256;
 
-class RemoteParticipantUDP : public Participant {
- public:
- /// @brief Create a new participant with the given communcation info
-  /// @param ipAddress The IP address of the participant
-  /// @param port The UDP port of the participant
-  /// @remarks This does not belong here, it should move to ParticipantUDP or
-  /// something like that in the future
-  RemoteParticipantUDP(const char* ipAddress, int port);
-
-  /// @brief The Ip Address of a participant.
-  /// @remarks This does not belong here, it should move to ParticipantUDP or
-  /// something like that in the future
-  const char* ipAddress = "0.0.0.0";
-  /// @brief The port number for UDP communication with the participant.
-  /// @remarks This does not belong here, it should move to ParticipantUDP or
-  /// something like that in the future
-  unsigned int port = 0;
-
-  bool Send(IMessage* msg) override;
-};
-
-/// @brief class which manages all known participants
-class ParticipantRegistry {
- public:
-  /// @brief Retrieve a participant by its address
-  /// @param ipAddress The IP address of the participant
-  /// @param port The port number of the participant
-  /// @return The participant or a nullptr when it could not be found
-  RemoteParticipantUDP* Get(const char* ipAddress, unsigned int port);
-  /// @brief Retrieve a participant by its network ID
-  /// @param networkID The network ID of the participant
-  /// @return The participant or a nullptr when it could not be found
-  RemoteParticipantUDP* Get(unsigned char networkID);
-
-  /// @brief Add a participant with the given details
-  /// @param ipAddress The IP address of the participant
-  /// @param port The port number of the participant
-  /// @return The added participant
-  RemoteParticipantUDP* Add(const char* ipAddress, unsigned int port);
-
-  /// @brief Add a participant
-  /// @param participant The participant to add
-  void Add(RemoteParticipantUDP* participant);
-
-  /// @brief Remove a participant
-  /// @param participant The participant to remove
-  void Remove(RemoteParticipantUDP* participant);
-
- private:
-#if defined(NO_STD)
- public:
-  RemoteParticipantUDP** GetAll() const;
-  int count = 0;
-
- private:
-  RemoteParticipantUDP** participants;
-#else
- public:
-  /// @brief Get all participants
-  /// @return All participants
-  const std::list<RemoteParticipantUDP*>& GetAll() const;
-
- private:
-  /// @brief The list of known participants
-  std::list<RemoteParticipantUDP*> participants;
-#endif
-};
-
 /// @brief A participant using UDP communication
 /// A local participant is the local device which can communicate with
 /// other participants It manages all local things and communcation with other
@@ -111,7 +44,7 @@ class ParticipantRegistry {
 /// participant is created which can be obtained using
 /// RoboidControl::IsolatedParticipant::Isolated().
 /// @sa RoboidControl::Thing::Thing()
-class ParticipantUDPGeneric : public RemoteParticipantUDP {
+class ParticipantUDPGeneric : public ParticipantSocket {
 #pragma region Init
 
  public:
@@ -139,7 +72,7 @@ class ParticipantUDPGeneric : public RemoteParticipantUDP {
   bool isIsolated = false;
 
   /// @brief The remote site when this participant is connected to a site
-  RemoteParticipantUDP* remoteSite = nullptr;
+  ParticipantSocket* remoteSite = nullptr;
 
   /// The interval in milliseconds for publishing (broadcasting) data on the
   /// local network
@@ -192,33 +125,82 @@ class ParticipantUDPGeneric : public RemoteParticipantUDP {
   void ReceiveData(unsigned char bufferSize,
                    char* senderIpAddress,
                    unsigned int senderPort);
-  void ReceiveData(unsigned char bufferSize, RemoteParticipantUDP* remoteParticipant);
+  void ReceiveData(unsigned char bufferSize,
+                   ParticipantSocket* remoteParticipant);
 
-  virtual void SetupUDP(int localPort, const char* remoteIpAddress, int remotePort) = 0;
+  virtual void SetupUDP(int localPort,
+                        const char* remoteIpAddress,
+                        int remotePort) = 0;
 
   virtual void ReceiveUDP() = 0;
 
-  virtual void Process(RemoteParticipantUDP* sender, ParticipantMsg* msg);
+  virtual void Process(ParticipantSocket* sender, ParticipantMsg* msg);
-  virtual void Process(RemoteParticipantUDP* sender, NetworkIdMsg* msg);
+  virtual void Process(ParticipantSocket* sender, NetworkIdMsg* msg);
-  virtual void Process(RemoteParticipantUDP* sender, InvestigateMsg* msg);
+  virtual void Process(ParticipantSocket* sender, InvestigateMsg* msg);
 
-  virtual void Process(RemoteParticipantUDP* sender, ThingMsg* msg);
+  virtual void Process(ParticipantSocket* sender, ThingMsg* msg);
-  virtual Thing* ProcessNewThing(RemoteParticipantUDP* sender,
+  virtual Thing* ProcessNewThing(ParticipantSocket* sender,
                                  ThingMsg* msg,
                                  bool isRemote);
 
-  virtual void Process(RemoteParticipantUDP* sender, NameMsg* msg);
+  virtual void Process(ParticipantSocket* sender, NameMsg* msg);
-  virtual void Process(RemoteParticipantUDP* sender, ModelUrlMsg* msg);
+  virtual void Process(ParticipantSocket* sender, ModelUrlMsg* msg);
-  virtual void Process(RemoteParticipantUDP* sender, PoseMsg* msg);
+  virtual void Process(ParticipantSocket* sender, PoseMsg* msg);
-  virtual void Process(RemoteParticipantUDP* sender, BinaryMsg* msg);
+  virtual void Process(ParticipantSocket* sender, BinaryMsg* msg);
-  virtual void Process(RemoteParticipantUDP* sender, TextMsg* msg);
+  virtual void Process(ParticipantSocket* sender, TextMsg* msg);
-  virtual void Process(RemoteParticipantUDP* sender, DestroyMsg* msg);
+  virtual void Process(ParticipantSocket* sender, DestroyMsg* msg);
 
 #pragma endregion Receive
 
-public:
+ public:
-  static ParticipantRegistry registry;
+  /// @brief class which manages all known participants
+  class Registry {
+   public:
+    /// @brief Retrieve a participant by its address
+    /// @param ipAddress The IP address of the participant
+    /// @param port The port number of the participant
+    /// @return The participant or a nullptr when it could not be found
+    ParticipantSocket* Get(const char* ipAddress, unsigned int port);
+    /// @brief Retrieve a participant by its network ID
+    /// @param networkID The network ID of the participant
+    /// @return The participant or a nullptr when it could not be found
+    ParticipantSocket* Get(unsigned char networkID);
 
+    /// @brief Add a participant with the given details
+    /// @param ipAddress The IP address of the participant
+    /// @param port The port number of the participant
+    /// @return The added participant
+    ParticipantSocket* Add(const char* ipAddress, unsigned int port);
+
+    /// @brief Add a participant
+    /// @param participant The participant to add
+    void Add(ParticipantSocket* participant);
+
+    /// @brief Remove a participant
+    /// @param participant The participant to remove
+    void Remove(ParticipantSocket* participant);
+
+   private:
+#if defined(NO_STD)
+   public:
+    ParticipantSocket** GetAll() const;
+    int count = 0;
+
+   private:
+    ParticipantSocket** participants;
+#else
+   public:
+    /// @brief Get all participants
+    /// @return All participants
+    const std::list<ParticipantSocket*>& GetAll() const;
+
+   private:
+    /// @brief The list of known participants
+    std::list<ParticipantSocket*> participants;
+#endif
+  };
+
+  static Registry registry;
 };
 
 }  // namespace RoboidControl

Participants/SiteServer.cpp

@@ -56,7 +56,7 @@ void SiteServer::UpdateMyThings() {
 
 #pragma region Receive
 
-void SiteServer::Process(RemoteParticipantUDP* sender, ParticipantMsg* msg) {
+void SiteServer::Process(ParticipantSocket* sender, ParticipantMsg* msg) {
   if (msg->networkId != sender->networkId) {
     // std::cout << this->name << " received New Client -> " <<
     // sender->ipAddress
@@ -67,9 +67,9 @@ void SiteServer::Process(RemoteParticipantUDP* sender, ParticipantMsg* msg) {
   }
 }
 
-void SiteServer::Process(RemoteParticipantUDP* sender, NetworkIdMsg* msg) {}
+void SiteServer::Process(ParticipantSocket* sender, NetworkIdMsg* msg) {}
 
-void SiteServer::Process(RemoteParticipantUDP* sender, ThingMsg* msg) {
+void SiteServer::Process(ParticipantSocket* sender, ThingMsg* msg) {
   Thing* thing = sender->Get(msg->networkId, msg->thingId);
   if (thing == nullptr) {
     // new Thing(sender, (Thing::Type)msg->thingType, msg->thingId);

Participants/SiteServer.h

@@ -33,9 +33,9 @@ class SiteServer : public ParticipantUDPGeneric {
  protected:
   unsigned long nextPublishMe = 0;
 
-  virtual void Process(RemoteParticipantUDP* sender, ParticipantMsg* msg) override;
+  virtual void Process(ParticipantSocket* sender, ParticipantMsg* msg) override;
-  virtual void Process(RemoteParticipantUDP* sender, NetworkIdMsg* msg) override;
+  virtual void Process(ParticipantSocket* sender, NetworkIdMsg* msg) override;
-  virtual void Process(RemoteParticipantUDP* sender, ThingMsg* msg) override;
+  virtual void Process(ParticipantSocket* sender, ThingMsg* msg) override;
 
 #pragma endregion Receive
 

Posix/PosixParticipant.cpp

@@ -90,7 +90,7 @@ void ParticipantUDP::Receive() {
 #endif
 }
 
-bool ParticipantUDP::SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize) {
+bool ParticipantUDP::SendTo(ParticipantSocket* remoteParticipant, int bufferSize) {
 #if defined(__unix__) || defined(__APPLE__)
   // std::cout << "Send to " << remoteParticipant->ipAddress << ":" << ntohs(remoteParticipant->port)
   //           << "\n";

Posix/PosixParticipant.h

@@ -9,7 +9,7 @@ class ParticipantUDP : public ParticipantUDPGeneric {
  public:
   void Setup(int localPort, const char* remoteIpAddress, int remotePort);
   void Receive();
-  bool SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize);
+  bool SendTo(ParticipantSocket* remoteParticipant, int bufferSize);
   bool Publish(IMessage* msg);
 
  protected:

Windows/WindowsParticipant.cpp

@@ -102,7 +102,7 @@ void ParticipantUDP::Receive() {
 #endif // _WIN32 || _WIN64
 }
 
-bool ParticipantUDP::SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize) {
+bool ParticipantUDP::SendTo(ParticipantSocket* remoteParticipant, int bufferSize) {
 #if defined(_WIN32) || defined(_WIN64)
   char ip_str[INET_ADDRSTRLEN];
   inet_ntop(AF_INET, &(remote_addr.sin_addr), ip_str, INET_ADDRSTRLEN);

Windows/WindowsParticipant.h

@@ -9,7 +9,7 @@ class ParticipantUDP : public ParticipantUDPGeneric {
  public:
   void Setup(int localPort, const char* remoteIpAddress, int remotePort);
   void Receive();
-  bool SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize);
+  bool SendTo(ParticipantSocket* remoteParticipant, int bufferSize);
   bool Publish(IMessage* msg);
 
  protected: