Refactoring

CMakeLists.txt

@@ -1,8 +1,21 @@
 cmake_minimum_required(VERSION 3.13)  # CMake version check
+
+file(GLOB srcs 
+    *.cpp
+    Things/*.cpp
+    Messages/*.cpp
+    Arduino/*.cpp
+    Posix/*.cpp
+    Windows/*.cpp
+    EspIdf/*.cpp
+    LinearAlgebra/*.cpp
+)
+
 if(ESP_PLATFORM)
     idf_component_register(
-        SRC_DIRS "."
+        SRCS ${srcs}
-        INCLUDE_DIRS "."
+        INCLUDE_DIRS "." "LinearAlgebra"
+        REQUIRES esp_netif esp_wifi
     )
 else() 
     project(RoboidControl)
@@ -28,14 +41,6 @@ else()
         .
         LinearAlgebra
     )
-    file(GLOB srcs 
-        *.cpp
-        Things/*.cpp
-        Messages/*.cpp
-        Arduino/*.cpp
-        Posix/*.cpp
-        Windows/*.cpp
-    )
     add_library(RoboidControl STATIC ${srcs})
     
     enable_testing()

EspIdf/EspIdfParticipant.cpp

@@ -0,0 +1,157 @@
+#include "EspIdfParticipant.h"
+
+#include "esp_wifi.h"
+
+namespace RoboidControl {
+namespace EspIdf {
+
+void LocalParticipant::Setup(int localPort,
+                             const char* remoteIpAddress,
+                             int remotePort) {
+#if defined(IDF_VER)
+  GetBroadcastAddress();
+
+  wifi_ap_record_t ap_info;
+  esp_err_t result = esp_wifi_sta_get_ap_info(&ap_info);
+  if (result != ESP_OK) {
+    std::cout << "No network available!\n";
+    return;
+  }
+
+  // Create a UDP socket
+  sockfd = socket(AF_INET, SOCK_DGRAM, 0);
+  if (sockfd < 0) {
+    std::cout << "Unable to create UDP socket: errno " << errno << "\n";
+    vTaskDelete(NULL);
+    return;
+  }
+
+  // Set up the server address structure
+  struct sockaddr_in local_addr;
+  memset(&local_addr, 0, sizeof(local_addr));
+  local_addr.sin_family = AF_INET;
+  local_addr.sin_port = htons(this->port);
+  local_addr.sin_addr.s_addr =
+      htonl(INADDR_ANY);  // Listen on all available network interfaces
+
+      
+  // Bind the socket to the address and port
+  if (bind(sockfd, (struct sockaddr*)&local_addr, sizeof(local_addr)) < 0) {
+    std::cout << "Unable to bind UDP socket: errno " << errno << "\n";
+    close(sockfd);
+    vTaskDelete(NULL);
+    return;
+  }
+
+  // struct sockaddr_in dest_addr;
+  memset(dest_addr.sin_zero, 0, sizeof(dest_addr.sin_zero));
+  dest_addr.sin_family = AF_INET;
+  dest_addr.sin_port = htons(this->remoteSite->port);
+  inet_pton(AF_INET, this->remoteSite->ipAddress, &dest_addr.sin_addr.s_addr);
+
+
+  std::cout << "Wifi sync started local " << this->port << ", remote "
+            << this->remoteSite->ipAddress << ":" << this->remoteSite->port
+            << "\n";
+#endif
+}
+
+void LocalParticipant::GetBroadcastAddress() {
+  esp_netif_ip_info_t ip_info;
+  esp_netif_t* esp_netif = esp_netif_get_handle_from_ifkey("WIFI_STA_DEF");
+
+  // Get IP information (IP address, netmask, gateway)
+  if (esp_netif_get_ip_info(esp_netif, &ip_info) != ESP_OK) {
+    std::cout << "Failed to get IP info\n";
+    return;
+  }
+
+  ip_addr_t broadcast_addr = {};
+  broadcast_addr.u_addr.ip4.addr =
+      (ip_info.ip.addr & ip_info.netmask.addr) | ~ip_info.netmask.addr;
+
+  this->broadcastIpAddress = new char[16];  // IPv4 address can have a max of 15
+                                            // characters + null  terminator
+  snprintf(this->broadcastIpAddress, 16, IPSTR,
+           IP2STR(&broadcast_addr.u_addr.ip4));
+  std::cout << "Broadcast address: " << this->broadcastIpAddress << "\n";
+}
+
+void LocalParticipant::Receive() {
+#if defined(IDF_VER)
+  struct pollfd fds;
+  fds.fd = sockfd;
+  fds.events = POLLIN;  // We're looking for data available to read
+
+  // Use poll() with a timeout of 0 to return immediately
+  int ret = poll(&fds, 1, 0);
+  if (ret == -1) {
+    std::cout << "poll() error\n";
+    return;
+  }
+  socklen_t addr_len = sizeof(this->src_addr);
+
+  while (ret > 0 && fds.revents & POLLIN) {
+    int packetSize = recvfrom(sockfd, buffer, sizeof(buffer) - 1, 0,
+                              (struct sockaddr*)&this->src_addr, &addr_len);
+    if (packetSize < 0) {
+      std::cout << "recvfrom() error\n";
+      return;
+    }
+
+    char sender_ipAddress[16];
+    inet_ntoa_r(this->src_addr.sin_addr, sender_ipAddress, INET_ADDRSTRLEN);
+    unsigned int sender_port = ntohs(this->src_addr.sin_port);
+
+    ReceiveData(packetSize, sender_ipAddress, sender_port);
+
+    int ret = poll(&fds, 1, 0);
+    if (ret == -1) {
+      std::cout << "poll() error\n";
+      return;
+    }
+  }
+
+#endif
+}
+
+bool LocalParticipant::Send(Participant* remoteParticipant, int bufferSize) {
+#if defined(IDF_VER)
+  std::cout << "Sending to " << remoteParticipant->ipAddress << ":"
+            << remoteParticipant->port << "\n";
+
+  int err = sendto(sockfd, buffer, bufferSize, 0, (struct sockaddr*)&dest_addr,
+               sizeof(dest_addr));
+  if (errno != 0)
+    std::cout << "Send error " << err << " or " << errno << "\n";
+
+#endif
+  return true;
+}
+
+bool LocalParticipant::Publish(IMessage* msg) {
+#if defined(IDF_VER)
+  int bufferSize = msg->Serialize((char*)this->buffer);
+  if (bufferSize <= 0)
+    return true;
+
+  struct sockaddr_in dest_addr;
+  dest_addr.sin_family = AF_INET;
+  dest_addr.sin_port = htons(this->port);
+  inet_pton(AF_INET, this->broadcastIpAddress, &dest_addr.sin_addr.s_addr);
+  int err = sendto(sockfd, buffer, bufferSize, 0, (struct sockaddr*)&dest_addr,
+                   sizeof(dest_addr));
+  if (err != 0)
+    std::cout << "Publish error\n";
+  //   udp.beginPacket(this->broadcastIpAddress, this->remotePort);
+  //   udp.write((unsigned char*)buffer, bufferSize);
+  //   udp.endPacket();
+
+  //   std::cout << "Publish to " << this->broadcastIpAddress << ":"
+  //             << this->remotePort << "\n";
+#endif
+  return true;
+};
+
+}  // namespace EspIdf
+}  // namespace RoboidControl

EspIdf/EspIdfParticipant.h

@@ -0,0 +1,30 @@
+#pragma once
+
+#include "../LocalParticipant.h"
+
+#include "lwip/sockets.h"
+
+namespace RoboidControl {
+namespace EspIdf {
+
+class LocalParticipant : public RoboidControl::LocalParticipant {
+ public:
+  void Setup(int localPort, const char* remoteIpAddress, int remotePort);
+  void Receive();
+  bool Send(Participant* remoteParticipant, int bufferSize);
+  bool Publish(IMessage* msg);
+
+ protected:
+  // const char* remoteIpAddress = nullptr;
+  // unsigned short remotePort = 0;
+  char* broadcastIpAddress = nullptr;
+
+  int sockfd;
+  struct sockaddr_in dest_addr;
+  struct sockaddr_in src_addr;
+
+  void GetBroadcastAddress();
+};
+
+}  // namespace EspIdf
+}  // namespace RoboidControl

EspIdf/EspIdfUtils.cpp

@@ -0,0 +1,100 @@
+#include "EspIdfUtils.h"
+
+#if defined(IDF_VER)
+#include <iostream>
+// #include "esp_event.h"
+// #include "esp_log.h"
+#include "esp_netif.h"
+#include "esp_wifi.h"
+// #include "lwip/inet.h"
+// #include "lwip/ip_addr.h"
+#include "string.h"
+
+const char* hotspotSSID = "Roboid";
+const char* hotspotPassword = "alchemy7000";
+
+esp_netif_t* wifi_netif = nullptr;
+// Semaphore to signal Wi-Fi connection status
+// SemaphoreHandle_t wifi_semaphore;
+static bool wifi_connected = false;
+
+static void wifi_event_handler(void* arg,
+                               esp_event_base_t event_base,
+                               int32_t event_id,
+                               void* event_data) {
+  if (event_base == WIFI_EVENT) {
+    if (event_id == WIFI_EVENT_STA_START)
+      esp_wifi_connect();
+    else if (event_id == WIFI_EVENT_STA_DISCONNECTED)
+      esp_wifi_connect();
+  } else if (event_base == IP_EVENT) {
+    if (event_id == IP_EVENT_STA_GOT_IP) {
+      // ip_event_got_ip_t* event = (ip_event_got_ip_t*)event_data;
+      // const char* ipaddr = IP2STR(&event->ip_info.ip);
+      wifi_connected = true;
+      // xSemaphoreGive(wifi_semaphore);  // Signal that connection is
+      // established
+    }
+  }
+}
+
+bool StartWifi(const char* wifiSsid, const char* wifiPassword) {
+  std::cout << "Connecting to WiFi " << wifiSsid << "\n";
+
+  esp_netif_init();
+  esp_event_loop_create_default();
+
+  wifi_netif = esp_netif_create_default_wifi_sta();
+  wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT();
+  esp_wifi_init(&cfg);
+
+  esp_event_handler_register(WIFI_EVENT, ESP_EVENT_ANY_ID, &wifi_event_handler,
+                             NULL);
+  esp_event_handler_register(IP_EVENT, IP_EVENT_STA_GOT_IP, &wifi_event_handler,
+                             NULL);
+
+  wifi_config_t wifi_config = {};
+  strncpy((char*)wifi_config.sta.ssid, wifiSsid, strlen(wifiSsid) + 1);
+  strncpy((char*)wifi_config.sta.password, wifiPassword,
+          strlen(wifiPassword) + 1);
+
+  esp_wifi_set_mode(WIFI_MODE_STA);
+  esp_wifi_set_config(WIFI_IF_STA, &wifi_config);
+  esp_wifi_start();
+
+  // Wait for connection with a timeout of 10 seconds
+  TickType_t xLastWakeTime = xTaskGetTickCount();
+  bool success = false;
+  for (int i = 0; i < 20; i++) {  // 20 iterations, each 500ms
+    if (wifi_connected) {
+      success = true;
+      std::cout << " Connected.\n";
+      break;
+    }
+    std::cout << ".";
+    fflush(stdout);  // Ensure output is printed immediately
+    vTaskDelayUntil(&xLastWakeTime, pdMS_TO_TICKS(500));  // Wait 500ms
+  }
+
+  if (wifi_connected) {
+    esp_netif_ip_info_t ip_info = {};
+    esp_netif_t* esp_netif = esp_netif_get_handle_from_ifkey("WIFI_STA_DEF");
+
+    // Get IP information (IP address, netmask, gateway)
+    if (esp_netif_get_ip_info(esp_netif, &ip_info) != ESP_OK) {
+      std::cout << "Failed to get IP info\n";
+      return false;
+    }
+
+    // Convert the IP address to string format using inet_ntoa
+    char ip_str[16];  // IPv4 address can have a max of 15 characters + null
+                      // terminator
+    snprintf(ip_str, sizeof(ip_str), IPSTR, IP2STR(&ip_info.ip));
+    std::cout << "IP address = " << ip_str << "\n";
+  } else
+    std::cout << "\nCould not connect to home network.\n";
+
+  return success;
+}
+
+#endif

EspIdf/EspIdfUtils.h

@@ -0,0 +1,6 @@
+#pragma once
+#if defined(IDF_VER)
+
+bool StartWifi(const char *wifiSsid, const char *wifiPassword);
+
+#endif

LinearAlgebra/Angle.h

@@ -21,7 +21,7 @@ template <typename T>
 class AngleOf {
  public:
   /// @brief Create a new angle with a zero value
-  AngleOf<T>();
+  AngleOf();
 
   /// @brief An zero value angle
   const static AngleOf<T> zero;
@@ -209,7 +209,7 @@ class AngleOf {
  private:
   T value;
 
-  AngleOf<T>(T rawValue);
+  AngleOf(T rawValue);
 };
 
 using AngleSingle = AngleOf<float>;

LinearAlgebra/Direction.h

@@ -30,11 +30,11 @@ class DirectionOf {
   AngleOf<T> vertical;
 
   /// @brief Create a new direction with zero angles
-  DirectionOf<T>();
+  DirectionOf();
   /// @brief Create a new direction
   /// @param horizontal The horizontal angle
   /// @param vertical The vertical angle.
-  DirectionOf<T>(AngleOf<T> horizontal, AngleOf<T> vertical);
+  DirectionOf(AngleOf<T> horizontal, AngleOf<T> vertical);
 
   /// @brief Convert the direction into a carthesian vector
   /// @return The carthesian vector corresponding to this direction.

LinearAlgebra/Matrix.cpp

@@ -1,6 +1,207 @@
 #include "Matrix.h"
+#include <iostream>
 
-template <> MatrixOf<float>::MatrixOf(unsigned int rows, unsigned int cols) {
+#pragma region Matrix1
+
+Matrix1::Matrix1(int size) : size(size) {
+  if (this->size == 0)
+    data = nullptr;
+  else {
+    this->data = new float[size]();
+    this->externalData = false;
+  }
+}
+
+Matrix1::Matrix1(float* data, int size)
+    : data(data), size(size) {
+  this->externalData = true;
+}
+
+Matrix1 LinearAlgebra::Matrix1::FromQuaternion(Quaternion q) {
+  Matrix1 r = Matrix1(4);
+  float* data = r.data;
+  data[0] = q.x;
+  data[1] = q.y;
+  data[2] = q.z;
+  data[3] = q.w;
+  return r;
+}
+
+Quaternion LinearAlgebra::Matrix1::ToQuaternion() {
+  return Quaternion(
+    this->data[0],
+    this->data[1],
+    this->data[2],
+    this->data[3]
+  );
+}
+
+// Matrix1
+#pragma endregion
+
+#pragma region Matrix2
+
+Matrix2::Matrix2() {}
+
+Matrix2::Matrix2(int nRows, int nCols) : nRows(nRows), nCols(nCols) {
+  this->nValues = nRows * nCols;
+  if (this->nValues == 0)
+    data = nullptr;
+  else {
+    this->data = new float[nValues]();
+    this->externalData = false;
+  }
+}
+
+Matrix2::Matrix2(float* data, int nRows, int nCols)
+    : nRows(nRows), nCols(nCols), data(data) {
+  this->nValues = nRows * nCols;
+  this->externalData = true;
+}
+
+Matrix2::~Matrix2() {
+  if (data != nullptr && !this->externalData)
+    delete[] data;
+}
+
+// Move constructor
+Matrix2::Matrix2(Matrix2&& other) noexcept
+    : nRows(other.nRows),
+      nCols(other.nCols),
+      nValues(other.nValues),
+      data(other.data) {
+  other.data = nullptr;  // Set the other object's pointer to nullptr to avoid
+                         // double deletion
+}
+
+// Move assignment operator
+Matrix2& Matrix2::operator=(Matrix2&& other) noexcept {
+  if (this != &other) {
+    delete[] data;  // Clean up current data
+    nRows = other.nRows;
+    nCols = other.nCols;
+    nValues = other.nValues;
+    data = other.data;
+    other.data = nullptr;  // Avoid double deletion
+  }
+  return *this;
+}
+
+Matrix2 Matrix2::Zero(int nRows, int nCols) {
+  Matrix2 r = Matrix2(nRows, nCols);
+  for (int ix = 0; ix < r.nValues; ix++)
+    r.data[ix] = 0;
+  return r;
+}
+
+Matrix2 Matrix2::Identity(int size) {
+  return Diagonal(1, size);
+}
+
+Matrix2 Matrix2::Diagonal(float f, int size) {
+  Matrix2 r = Matrix2(size, size);
+  float* data = r.data;
+  int valueIx = 0;
+  for (int ix = 0; ix < size; ix++) {
+    data[valueIx] = f;
+    valueIx += size + 1;
+  }
+  return r;
+}
+
+Matrix2 Matrix2::SkewMatrix(const Vector3& v) {
+  Matrix2 r = Matrix2(3, 3);
+  float* data = r.data;
+  data[0 * 3 + 1] = -v.z;  // result(0, 1)
+  data[0 * 3 + 2] = v.y;   // result(0, 2)
+  data[1 * 3 + 0] = v.z;   // result(1, 0)
+  data[1 * 3 + 2] = -v.x;  // result(1, 2)
+  data[2 * 3 + 0] = -v.y;  // result(2, 0)
+  data[2 * 3 + 1] = v.x;   // result(2, 1)
+  return r;
+}
+
+Matrix2 LinearAlgebra::Matrix2::operator-() const {
+  Matrix2 r = Matrix2(this->nRows, this->nCols);
+  for (int ix = 0; ix < r.nValues; ix++)
+    r.data[ix] = -this->data[ix];
+  return r;
+}
+
+Matrix2 LinearAlgebra::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;
+}
+
+Matrix2 LinearAlgebra::Matrix2::operator*(const Matrix2& B) const {
+  Matrix2 r = Matrix2(this->nRows, B.nCols);
+
+  int ACols = this->nCols;
+  int BCols = B.nCols;
+  int ARows = this->nRows;
+  // int BRows = B.nRows;
+
+  for (int i = 0; i < ARows; ++i) {
+    // Pre-compute row offsets
+    int ARowOffset = i * ACols;  // ARowOffset is constant for each row of A
+    int BColOffset = i * BCols;  // BColOffset is constant for each row of B
+    for (int j = 0; j < BCols; ++j) {
+      float sum = 0;
+      std::cout << " 0";
+      int BIndex = j;
+      for (int k = 0; k < ACols; ++k) {
+        std::cout << " + " << this->data[ARowOffset + k] << " * "
+                  << B.data[BIndex];
+        sum += this->data[ARowOffset + k] * B.data[BIndex];
+        BIndex += BCols;
+      }
+      r.data[BColOffset + j] = sum;
+      std::cout << " = " << sum << " ix: " << BColOffset + j << "\n";
+    }
+  }
+  return r;
+}
+
+void LinearAlgebra::Matrix2::SetSlice(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)];
+  }
+}
+
+/// @brief Compute the Omega matrix of a 3D vector
+/// @param v The vector
+/// @return 4x4 Omega matrix
+Matrix2 LinearAlgebra::Matrix2::Omega(const Vector3& v) {
+  Matrix2 r = Matrix2::Zero(4, 4);
+  r.SetSlice(0, 3, 0, 3, -Matrix2::SkewMatrix(v));
+
+  // set last row to -v
+  int ix = 3 * 4;
+  r.data[ix++] = -v.x;
+  r.data[ix++] = -v.y;
+  r.data[ix] = -v.z;
+
+  // Set last column to v
+  ix = 3;
+  r.data[ix += 4] = v.x;
+  r.data[ix += 4] = v.y;
+  r.data[ix] = v.z;
+  return r;
+}
+
+// Matrix2
+#pragma endregion
+
+template <>
+MatrixOf<float>::MatrixOf(unsigned int rows, unsigned int cols) {
   if (rows <= 0 || cols <= 0) {
     this->rows = 0;
     this->cols = 0;
@@ -14,15 +215,17 @@ template <> MatrixOf<float>::MatrixOf(unsigned int rows, unsigned int cols) {
   this->data = new float[matrixSize]{0.0f};
 }
 
-template <> MatrixOf<float>::MatrixOf(Vector3 v) : MatrixOf(3, 1) {
+template <>
+MatrixOf<float>::MatrixOf(Vector3 v) : MatrixOf(3, 1) {
   Set(0, 0, v.Right());
   Set(1, 0, v.Up());
   Set(2, 0, v.Forward());
 }
 
 template <>
-void MatrixOf<float>::Multiply(const MatrixOf<float> *m1,
+void MatrixOf<float>::Multiply(const MatrixOf<float>* m1,
-                               const MatrixOf<float> *m2, MatrixOf<float> *r) {
+                               const MatrixOf<float>* m2,
+                               MatrixOf<float>* r) {
   for (unsigned int rowIx1 = 0; rowIx1 < m1->rows; rowIx1++) {
     for (unsigned int colIx2 = 0; colIx2 < m2->cols; colIx2++) {
       unsigned int rDataIx = colIx2 * m2->cols + rowIx1;
@@ -37,7 +240,7 @@ void MatrixOf<float>::Multiply(const MatrixOf<float> *m1,
 }
 
 template <>
-Vector3 MatrixOf<float>::Multiply(const MatrixOf<float> *m, Vector3 v) {
+Vector3 MatrixOf<float>::Multiply(const MatrixOf<float>* m, Vector3 v) {
   MatrixOf<float> v_m = MatrixOf<float>(v);
   MatrixOf<float> r_m = MatrixOf<float>(3, 1);
 
@@ -47,10 +250,11 @@ Vector3 MatrixOf<float>::Multiply(const MatrixOf<float> *m, Vector3 v) {
   return r;
 }
 
-template <typename T> Vector3 MatrixOf<T>::operator*(const Vector3 v) const {
+template <typename T>
-  float *vData = new float[3]{v.Right(), v.Up(), v.Forward()};
+Vector3 MatrixOf<T>::operator*(const Vector3 v) const {
+  float* vData = new float[3]{v.Right(), v.Up(), v.Forward()};
   MatrixOf<float> v_m = MatrixOf<float>(3, 1, vData);
-  float *rData = new float[3]{};
+  float* rData = new float[3]{};
   MatrixOf<float> r_m = MatrixOf<float>(3, 1, rData);
 
   Multiply(this, &v_m, &r_m);

LinearAlgebra/Matrix.h

@@ -1,10 +1,102 @@
 #ifndef MATRIX_H
 #define MATRIX_H
 
+#include "Quaternion.h"
 #include "Vector3.h"
 
 namespace LinearAlgebra {
 
+/// @brief A 1-dimensional matrix or vector of arbitrary size
+class Matrix1 {
+ public:
+  float* data = nullptr;
+  int size = 0;
+
+  Matrix1(int size);
+  Matrix1(float* data, int size);
+
+  static Matrix1 FromQuaternion(Quaternion q);
+  Quaternion ToQuaternion();
+
+ private:
+  bool externalData = true;
+};
+
+/// @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();
+
+  static Matrix2 Zero(int nRows, int nCols);
+
+  static Matrix2 Identity(int size);
+
+  static Matrix2 Diagonal(float f, int size);
+
+  static Matrix2 SkewMatrix(const Vector3& v);
+
+  Matrix2 operator-() const;
+
+  /// @brief Add a matrix to this matrix
+  /// @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& m) const;
+  friend Matrix2 operator*(const Matrix2& m, float f) {
+    Matrix2 r = Matrix2(m.nRows, m.nCols);
+    for (int ix = 0; ix < r.nValues; ix++)
+      r.data[ix] = m.data[ix] * f;
+    return r;
+  }
+  friend Matrix2 operator*(float f, const Matrix2& m) {
+    Matrix2 r = Matrix2(m.nRows, m.nCols);
+    for (int ix = 0; ix < r.nValues; ix++)
+      r.data[ix] = f * m.data[ix];
+    return r;
+  }
+
+  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*(float f, const Matrix2& v) {
+  //   Matrix2 r = Matrix2(v.nRows, v.nCols);
+  //   for (int ix = 0; ix < r.nValues; ix++)
+  //     r.data[ix] = f * v.data[ix];
+  //   return r;
+  // }
+
+  void SetSlice(int rowStart,
+                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);
+
+ private:
+  bool externalData = true;
+};
+
 /// @brief Single precision float matrix
 template <typename T>
 class MatrixOf {
@@ -116,6 +208,6 @@ class MatrixOf {
 };
 
 }  // namespace LinearAlgebra
-using namespace LinearAlgebra;
+//using namespace LinearAlgebra;
 
 #endif

LinearAlgebra/Quaternion.cpp

@@ -6,6 +6,7 @@
 #include <float.h>
 #include <math.h>
 #include "Angle.h"
+#include "Matrix.h"
 #include "Vector3.h"
 
 void CopyQuat(const Quat& q1, Quat& q2) {
@@ -97,6 +98,28 @@ 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;
+  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);
+  data[1 * 3 + 0] = 2 * (x * y + w * z);
+  data[1 * 3 + 1] = 1 - 2 * (x * x + z * z);
+  data[1 * 3 + 2] = 2 * (y * z - w * x);
+  data[2 * 3 + 0] = 2 * (x * z - w * y);
+  data[2 * 3 + 1] = 2 * (y * z + w * x);
+  data[2 * 3 + 2] = 1 - 2 * (x * x + y * y);
+
+  return r;
+}
+
 Quaternion Quaternion::operator*(const Quaternion& r2) const {
   return Quaternion(
       this->x * r2.w + this->y * r2.z - this->z * r2.y + this->w * r2.x,

LinearAlgebra/Quaternion.h

@@ -34,6 +34,8 @@ typedef struct Quat {
 
 namespace LinearAlgebra {
 
+class Matrix2;
+
 /// <summary>
 /// A quaternion
 /// </summary>
@@ -89,6 +91,8 @@ struct Quaternion : Quat {
   /// The euler angles performed in the order: Z, X, Y
   static Vector3 ToAngles(const Quaternion& q);
 
+  Matrix2 ToRotationMatrix();
+
   /// <summary>
   /// Rotate a vector using this quaterion
   /// </summary>

LinearAlgebra/Spherical.h

@@ -24,9 +24,9 @@ class SphericalOf {
   /// @brief The direction of the vector
   DirectionOf<T> direction;
 
-  SphericalOf<T>();
+  SphericalOf();
-  SphericalOf<T>(float distance, AngleOf<T> horizontal, AngleOf<T> vertical);
+  SphericalOf(float distance, AngleOf<T> horizontal, AngleOf<T> vertical);
-  SphericalOf<T>(float distance, DirectionOf<T> direction);
+  SphericalOf(float distance, DirectionOf<T> direction);
 
   /// @brief Create spherical vector without using AngleOf type. All given
   /// angles are in degrees

LinearAlgebra/SwingTwist.h

@@ -21,9 +21,9 @@ class SwingTwistOf {
   DirectionOf<T> swing;
   AngleOf<T> twist;
 
-  SwingTwistOf<T>();
+  SwingTwistOf();
-  SwingTwistOf<T>(DirectionOf<T> swing, AngleOf<T> twist);
+  SwingTwistOf(DirectionOf<T> swing, AngleOf<T> twist);
-  SwingTwistOf<T>(AngleOf<T> horizontal, AngleOf<T> vertical, AngleOf<T> twist);
+  SwingTwistOf(AngleOf<T> horizontal, AngleOf<T> vertical, AngleOf<T> twist);
 
   static SwingTwistOf<T> Degrees(float horizontal,
                                  float vertical = 0,

LinearAlgebra/Vector3.h

@@ -14,7 +14,7 @@ extern "C" {
 /// This is a C-style implementation
 /// This uses the right-handed coordinate system.
 typedef struct Vec3 {
- protected:
+ public:
   /// <summary>
   /// The right axis of the vector
   /// </summary>

LinearAlgebra/test/Matrix_test.cc

@@ -1,10 +1,90 @@
 #if GTEST
 #include <gtest/gtest.h>
-#include <limits>
 #include <math.h>
+#include <limits>
 
 #include "Matrix.h"
 
+TEST(Matrix2, Zero) {
+      // Test case 1: 2x2 zero matrix
+      Matrix2 zeroMatrix = Matrix2::Zero(2, 2);
+      EXPECT_TRUE(zeroMatrix.nRows == 2);
+      EXPECT_TRUE(zeroMatrix.nCols == 2);
+      for (int i = 0; i < zeroMatrix.nValues; ++i) {
+        EXPECT_TRUE(zeroMatrix.data[i] == 0.0f);
+      }
+      std::cout << "Test case 1 passed: 2x2 zero matrix\n";
+  
+      // Test case 2: 3x3 zero matrix
+      zeroMatrix = Matrix2::Zero(3, 3);
+      EXPECT_TRUE(zeroMatrix.nRows == 3);
+      EXPECT_TRUE(zeroMatrix.nCols == 3);
+      for (int i = 0; i < zeroMatrix.nValues; ++i) {
+        EXPECT_TRUE(zeroMatrix.data[i] == 0.0f);
+      }
+      std::cout << "Test case 2 passed: 3x3 zero matrix\n";
+  
+      // Test case 3: 1x1 zero matrix
+      zeroMatrix = Matrix2::Zero(1, 1);
+      EXPECT_TRUE(zeroMatrix.nRows == 1);
+      EXPECT_TRUE(zeroMatrix.nCols == 1);
+      EXPECT_TRUE(zeroMatrix.data[0] == 0.0f);
+      std::cout << "Test case 3 passed: 1x1 zero matrix\n";
+  
+      // Test case 4: 0x0 matrix (edge case)
+      zeroMatrix = Matrix2::Zero(0, 0);
+      EXPECT_TRUE(zeroMatrix.nRows == 0);
+      EXPECT_TRUE(zeroMatrix.nCols == 0);
+      EXPECT_TRUE(zeroMatrix.data == nullptr);
+      std::cout << "Test case 4 passed: 0x0 matrix\n";
+}
+
+TEST(Matrix2, Multiplication) {
+  // Test 1: Multiplying two 2x2 matrices
+  float dataA[] = {1, 2, 3, 4};
+  float dataB[] = {5, 6, 7, 8};
+  Matrix2 A(dataA, 2, 2);
+  Matrix2 B(dataB, 2, 2);
+
+  Matrix2 result = A * B;
+
+  float expectedData[] = {19, 22, 43, 50};
+  for (int i = 0; i < 4; ++i) 
+    EXPECT_TRUE(result.data[i] == expectedData[i]);  
+  std::cout << "Test 1 passed: 2x2 matrix multiplication.\n";
+
+  
+  // Test 2: Multiplying a 3x2 matrix with a 2x3 matrix
+  float dataC[] = {1, 2, 3, 4, 5, 6};
+  float dataD[] = {7, 8, 9, 10, 11, 12};
+  Matrix2 C(dataC, 3, 2);
+  Matrix2 D(dataD, 2, 3);
+
+  Matrix2 result2 = C * D;
+
+  float expectedData2[] = {27, 30, 33, 61, 68, 75, 95, 106, 117};
+  for (int i = 0; i < 9; ++i)
+    EXPECT_TRUE(result2.data[i] == expectedData2[i]);
+  std::cout << "Test 2 passed: 3x2 * 2x3 matrix multiplication.\n";
+  
+  // Test 3: Multiplying with a zero matrix
+  Matrix2 zeroMatrix = Matrix2::Zero(2, 2);
+  Matrix2 result3 = A * zeroMatrix;
+
+  for (int i = 0; i < 4; ++i)
+    EXPECT_TRUE(result3.data[i] == 0);
+  std::cout << "Test 3 passed: Multiplication with zero matrix.\n";
+  
+  // Test 4: Multiplying with an identity matrix
+  Matrix2 identityMatrix = Matrix2::Identity(2);
+  Matrix2 result4 = A * identityMatrix;
+
+  for (int i = 0; i < 4; ++i)
+    EXPECT_TRUE(result4.data[i] == A.data[i]);
+  std::cout << "Test 4 passed: Multiplication with identity matrix.\n";
+  
+}
+
 TEST(MatrixSingle, Init) {
   // zero
   MatrixOf<float> m0 = MatrixOf<float>(0, 0);

Participants/ParticipantUDP.cpp

@@ -3,6 +3,7 @@
 #include "Thing.h"
 
 #include "Arduino/ArduinoParticipant.h"
+#include "EspIdf/EspIdfParticipant.h"
 
 #if defined(_WIN32) || defined(_WIN64)
 #include <winsock2.h>
@@ -24,8 +25,6 @@
 
 namespace RoboidControl {
 
-// LocalParticipant::LocalParticipant() {}
-
 LocalParticipant::LocalParticipant(int port) {
   this->ipAddress = "0.0.0.0";
   this->port = port;
@@ -33,10 +32,10 @@ LocalParticipant::LocalParticipant(int port) {
     this->isIsolated = true;
 }
 
-LocalParticipant::LocalParticipant(const char* ipAddress, int port) {
+LocalParticipant::LocalParticipant(const char* ipAddress,
-  this->ipAddress = "0.0.0.0";  // ipAddress;  // maybe this is not needed
+                                   int port,
-                                // anymore, keeping it to "0.0.0.0"
+                                   int localPort)
-  this->port = port;
+    : Participant("127.0.0.1", localPort) {
   if (this->port == 0)
     this->isIsolated = true;
   else
@@ -55,7 +54,7 @@ void LocalParticipant::begin() {
   if (this->isIsolated)
     return;
 
-  SetupUDP(this->port, this->ipAddress, this->port);
+  SetupUDP(this->port, this->remoteSite->ipAddress, this->remoteSite->port);
 }
 
 void LocalParticipant::SetupUDP(int localPort,
@@ -73,22 +72,17 @@ void LocalParticipant::SetupUDP(int localPort,
   Arduino::LocalParticipant* thisArduino =
       static_cast<Arduino::LocalParticipant*>(this);
   thisArduino->Setup(localPort, remoteIpAddress, remotePort);
+#elif defined(IDF_VER)
+  EspIdf::LocalParticipant* thisEspIdf =
+      static_cast<EspIdf::LocalParticipant*>(this);
+  thisEspIdf->Setup(localPort, remoteIpAddress, remotePort);
 #endif
   this->connected = true;
 }
 
 void LocalParticipant::Update(unsigned long currentTimeMs) {
-  if (currentTimeMs == 0) {
+  if (currentTimeMs == 0)
     currentTimeMs = Thing::GetTimeMs();
-    // #if defined(ARDUINO)
-    //     currentTimeMs = millis();
-    // #elif defined(__unix__) || defined(__APPLE__)
-    //     auto now = std::chrono::steady_clock::now();
-    //     auto ms =
-    //     std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch());
-    //     currentTimeMs = static_cast<unsigned long>(ms.count());
-    // #endif
-  }
 
   if (this->isIsolated == false) {
     if (this->connected == false)
@@ -133,6 +127,10 @@ void LocalParticipant::ReceiveUDP() {
   Arduino::LocalParticipant* thisArduino =
       static_cast<Arduino::LocalParticipant*>(this);
   thisArduino->Receive();
+#elif defined(IDF_VER)
+  EspIdf::LocalParticipant* thisEspIdf =
+      static_cast<EspIdf::LocalParticipant*>(this);
+  thisEspIdf->Receive();
 #endif
 }
 
@@ -196,6 +194,12 @@ bool LocalParticipant::Send(Participant* remoteParticipant, IMessage* msg) {
   Arduino::LocalParticipant* thisArduino =
       static_cast<Arduino::LocalParticipant*>(this);
   return thisArduino->Send(remoteParticipant, bufferSize);
+#elif defined(IDF_VER)
+  EspIdf::LocalParticipant* thisEspIdf =
+      static_cast<EspIdf::LocalParticipant*>(this);
+  return thisEspIdf->Send(remoteParticipant, bufferSize);
+#else
+  return false;
 #endif
 }
 
@@ -233,6 +237,12 @@ bool LocalParticipant::Publish(IMessage* msg) {
   Arduino::LocalParticipant* thisArduino =
       static_cast<Arduino::LocalParticipant*>(this);
   return thisArduino->Publish(msg);
+#elif defined(IDF_VER)
+  EspIdf::LocalParticipant* thisEspIdf =
+      static_cast<EspIdf::LocalParticipant*>(this);
+  return thisEspIdf->Publish(msg);
+#else
+  return false;
 #endif
 }
 
@@ -248,11 +258,11 @@ void LocalParticipant::ReceiveData(unsigned char packetSize,
       this->GetParticipant(senderIpAddress, senderPort);
   if (remoteParticipant == nullptr) {
     remoteParticipant = this->AddParticipant(senderIpAddress, senderPort);
-    // std::cout << "New sender " << sender_ipAddress << ":" << sender_port
+    // std::cout << "New sender " << senderIpAddress << ":" << senderPort
     //           << "\n";
-    // std::cout << "New remote participant " << remoteParticipant->ipAddress
+    std::cout << "New remote participant " << remoteParticipant->ipAddress
-    //           << ":" << remoteParticipant->port << " "
+              << ":" << remoteParticipant->port << " "
-    //           << (int)remoteParticipant->networkId << "\n";
+              << (int)remoteParticipant->networkId << "\n";
   }
 
   ReceiveData(packetSize, remoteParticipant);
@@ -304,8 +314,8 @@ void LocalParticipant::ReceiveData(unsigned char bufferSize,
 void LocalParticipant::Process(Participant* sender, ParticipantMsg* msg) {}
 
 void LocalParticipant::Process(Participant* sender, SiteMsg* msg) {
-  // std::cout << this->name << ": process NetworkId [" << (int)this->networkId
+  std::cout << this->name << ": process Site Id " << (int)this->networkId
-  //           << "/" << (int)msg->networkId << "]\n";
+            << "->" << (int)msg->networkId << "\n";
   if (this->networkId != msg->networkId) {
     this->networkId = msg->networkId;
     // std::cout << this->things.size() << " things\n";
@@ -316,9 +326,26 @@ void LocalParticipant::Process(Participant* sender, SiteMsg* msg) {
 
 void LocalParticipant::Process(Participant* sender, InvestigateMsg* msg) {}
 
-void LocalParticipant::Process(Participant* sender, ThingMsg* msg) {}
+void LocalParticipant::Process(Participant* sender, ThingMsg* msg) {
+  std::cout << this->name << ": process Thing [" << (int)msg->networkId << "/"
+            << (int)msg->thingId << "]\n";
+#if !defined(NO_STD)
+  auto thingMsgProcessor = thingMsgProcessors.find(msg->thingType);
+  //Thing* newThing;
+  if (thingMsgProcessor != thingMsgProcessors.end()) {  // found item
+    //newThing = 
+    thingMsgProcessor->second(sender, msg->networkId, msg->thingId);
+  } else
+    //newThing = 
+    new Thing(sender, msg->networkId, msg->thingId,
+                         (Thing::Type)msg->thingType);
+#endif
+}
 
 void LocalParticipant::Process(Participant* sender, NameMsg* msg) {
+  std::cout << this->name << ": process Name [" << (int)msg->networkId << "/"
+            << (int)msg->thingId << "]\n";
+
   Thing* thing = sender->Get(msg->networkId, msg->thingId);
   if (thing != nullptr) {
     int nameLength = msg->nameLength;
@@ -336,17 +363,20 @@ void LocalParticipant::Process(Participant* sender, NameMsg* msg) {
 #endif
     thingName[nameLength] = '\0';
     thing->name = thingName;
-    // std::cout << "thing name = " << thing->name << " length = " << nameLength
+    // std::cout << "thing name = " << thing->name << " length = " <<
+    // nameLength
     //           << "\n";
   }
 }
 
-void LocalParticipant::Process(Participant* sender, PoseMsg* msg) {}
+void LocalParticipant::Process(Participant* sender, PoseMsg* msg) {
+  std::cout << this->name << ": process Pose [" << (int)this->networkId << "/"
+            << (int)msg->networkId << "]\n";
+}
 
 void LocalParticipant::Process(Participant* sender, BinaryMsg* msg) {
-  // std::cout << this->name << ": process Binary [" << (int)this->networkId <<
+  std::cout << this->name << ": process Binary [" << (int)this->networkId << "/"
-  // "/"
+            << (int)msg->networkId << "]\n";
-  //           << (int)msg->networkId << "]\n";
   Thing* thing = sender->Get(msg->networkId, msg->thingId);
   if (thing != nullptr)
     thing->ProcessBinary(msg->bytes);

Participants/ParticipantUDP.h

@@ -11,7 +11,9 @@
 #include "Participant.h"
 
 #if !defined(NO_STD)
+#include <functional>
 #include <list>
+// #include <unordered_map>
 #endif
 
 #if defined(_WIN32) || defined(_WIN64)
@@ -51,7 +53,9 @@ class LocalParticipant : public Participant {
   /// @brief Create a participant which will try to connect to a site.
   /// @param ipAddress The IP address of the site
   /// @param port The port used by the site
-  LocalParticipant(const char* ipAddress, int port = 7681);
+  LocalParticipant(const char* ipAddress,
+                   int port = 7681,
+                   int localPort = 7681);
   // Note to self: one cannot specify the port used by the local participant
   // now!!
 
@@ -86,10 +90,11 @@ class LocalParticipant : public Participant {
 
 #if defined(__unix__) || defined(__APPLE__)
   int sock;
-#endif
+#elif defined(_WIN32) || defined(_WIN64)
   sockaddr_in remote_addr;
   sockaddr_in server_addr;
   sockaddr_in broadcast_addr;
+#endif
 
 #endif
 
@@ -135,6 +140,35 @@ class LocalParticipant : public Participant {
   virtual void Process(Participant* sender, NameMsg* msg);
   virtual void Process(Participant* sender, PoseMsg* msg);
   virtual void Process(Participant* sender, BinaryMsg* msg);
+
+#if !defined(NO_STD)
+ public:
+  using ThingConstructor = std::function<Thing*(Participant* participant,
+                                                unsigned char networkId,
+                                                unsigned char thingId)>;
+
+  template <typename ThingClass>
+  void Register(unsigned char thingType) {
+    thingMsgProcessors[thingType] = [](Participant* participant,
+                                       unsigned char networkId,
+                                       unsigned char thingId) {
+      return new ThingClass(participant, networkId, thingId);
+    };
+  };
+
+  template <typename ThingClass>
+  void Register2(unsigned char thingType, ThingConstructor f) {
+    thingMsgProcessors[thingType] = [f](Participant* participant,
+                                       unsigned char networkId,
+                                       unsigned char thingId) {
+      return f(participant, networkId, thingId);
+    };
+  };
+
+ protected:
+  std::unordered_map<unsigned char, ThingConstructor> thingMsgProcessors;
+
+#endif
 };
 
 }  // namespace RoboidControl

Participants/SiteServer.cpp

@@ -25,7 +25,7 @@ SiteServer::SiteServer(int port) {
   SetupUDP(port, ipAddress, 0);
 
 #if !defined(NO_STD)
-  Register<TemperatureSensor>((unsigned char)Thing::Type::TemperatureSensor);
+  //Register<TemperatureSensor>((unsigned char)Thing::Type::TemperatureSensor);
 #endif
 }
 
@@ -45,19 +45,20 @@ void SiteServer::Process(Participant* sender, SiteMsg* msg) {}
 void SiteServer::Process(Participant* sender, ThingMsg* msg) {
   Thing* thing = sender->Get(msg->networkId, msg->thingId);
   if (thing == nullptr) {
-#if defined(NO_STD)
+// #if defined(NO_STD)
     new Thing(sender, msg->networkId, msg->thingId,
               (Thing::Type)msg->thingType);
-#else
+// #else
-    auto thingMsgProcessor = thingMsgProcessors.find(msg->thingType);
+//     auto thingMsgProcessor = thingMsgProcessors.find(msg->thingType);
-    Thing* newThing;
+//     //Thing* newThing;
-    if (thingMsgProcessor != thingMsgProcessors.end())  // found item
+//     if (thingMsgProcessor != thingMsgProcessors.end())  // found item
-      newThing =
+//       //newThing =
-          thingMsgProcessor->second(sender, msg->networkId, msg->thingId);
+//           thingMsgProcessor->second(sender, msg->networkId, msg->thingId);
-    else
+//     else
-      newThing = new Thing(sender, msg->networkId, msg->thingId,
+//       //newThing = 
-                           (Thing::Type)msg->thingType);
+//         new Thing(sender, msg->networkId, msg->thingId,
-#endif
+//                            (Thing::Type)msg->thingType);
+// #endif
   }
 }
 

Participants/SiteServer.h

@@ -0,0 +1,46 @@
+#pragma once
+
+#include "LocalParticipant.h"
+
+#if !defined(NO_STD)
+#include <functional>
+#include <memory>
+#include <unordered_map>
+#endif
+
+namespace RoboidControl {
+
+/// @brief A participant is device which can communicate with other participants
+class SiteServer : public LocalParticipant {
+ public:
+  SiteServer(int port = 7681);
+
+  // virtual void Update(unsigned long currentTimeMs = 0) override;
+
+// #if !defined(NO_STD)
+//   template <typename ThingClass>
+//   void Register(unsigned char thingType) {
+//     thingMsgProcessors[thingType] = [](Participant* participant,
+//                                        unsigned char networkId,
+//                                        unsigned char thingId) {
+//       return new ThingClass(participant, networkId, thingId);
+//     };
+//   };
+// #endif
+
+ protected:
+  unsigned long nextPublishMe = 0;
+
+  virtual void Process(Participant* sender, ParticipantMsg* msg) override;
+  virtual void Process(Participant* sender, SiteMsg* msg) override;
+  virtual void Process(Participant* sender, ThingMsg* msg) override;
+
+// #if !defined(NO_STD)
+//   using ThingConstructor = std::function<Thing*(Participant* participant,
+//                                                 unsigned char networkId,
+//                                                 unsigned char thingId)>;
+//   std::unordered_map<unsigned char, ThingConstructor> thingMsgProcessors;
+// #endif
+};
+
+}  // namespace RoboidControl

SiteServer.h

@@ -1,46 +0,0 @@
-#pragma once
-
-#include "LocalParticipant.h"
-
-#if !defined(NO_STD)
-#include <functional>
-#include <memory>
-#include <unordered_map>
-#endif
-
-namespace RoboidControl {
-
-/// @brief A participant is device which can communicate with other participants
-class SiteServer : public LocalParticipant {
- public:
-  SiteServer(int port = 7681);
-
-  // virtual void Update(unsigned long currentTimeMs = 0) override;
-
-#if !defined(NO_STD)
-  template <typename ThingClass>
-  void Register(unsigned char thingType) {
-    thingMsgProcessors[thingType] = [](Participant* participant,
-                                       unsigned char networkId,
-                                       unsigned char thingId) {
-      return new ThingClass(participant, networkId, thingId);
-    };
-  };
-#endif
-
- protected:
-  unsigned long nextPublishMe = 0;
-
-  virtual void Process(Participant* sender, ParticipantMsg* msg) override;
-  virtual void Process(Participant* sender, SiteMsg* msg) override;
-  virtual void Process(Participant* sender, ThingMsg* msg) override;
-
-#if !defined(NO_STD)
-  using ThingConstructor = std::function<Thing*(Participant* participant,
-                                                unsigned char networkId,
-                                                unsigned char thingId)>;
-  std::unordered_map<unsigned char, ThingConstructor> thingMsgProcessors;
-#endif
-};
-
-}  // namespace RoboidControl

Things/TemperatureSensor.cpp

@@ -4,10 +4,6 @@
 
 namespace RoboidControl {
 
-// TemperatureSensor::TemperatureSensor() : Thing(Type::TemperatureSensor) {}
-
-// TemperatureSensor::TemperatureSensor() : Thing(Type::TemperatureSensor) {}
-
 TemperatureSensor::TemperatureSensor(Participant* participant,
                                      unsigned char networkId,
                                      unsigned char thingId)