CMakeLists.txt

@@ -1,21 +1,8 @@
 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(
-        SRCS ${srcs}
+        SRC_DIRS "."
-        INCLUDE_DIRS "." "LinearAlgebra"
+        INCLUDE_DIRS "."
-        REQUIRES esp_netif esp_wifi
     )
 else() 
     project(RoboidControl)
@@ -41,6 +28,14 @@ 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

@@ -1,157 +0,0 @@
-#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

@@ -1,30 +0,0 @@
-#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

@@ -1,100 +0,0 @@
-#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

@@ -1,6 +0,0 @@
-#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();
+  AngleOf<T>();
 
   /// @brief An zero value angle
   const static AngleOf<T> zero;
@@ -209,7 +209,7 @@ class AngleOf {
  private:
   T value;
 
-  AngleOf(T rawValue);
+  AngleOf<T>(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();
+  DirectionOf<T>();
   /// @brief Create a new direction
   /// @param horizontal The horizontal angle
   /// @param vertical The vertical angle.
-  DirectionOf(AngleOf<T> horizontal, AngleOf<T> vertical);
+  DirectionOf<T>(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,207 +1,6 @@
 #include "Matrix.h"
-#include <iostream>
 
-#pragma region Matrix1
+template <> MatrixOf<float>::MatrixOf(unsigned int rows, unsigned int cols) {
-
-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;
@@ -215,8 +14,7 @@ MatrixOf<float>::MatrixOf(unsigned int rows, unsigned int cols) {
   this->data = new float[matrixSize]{0.0f};
 }
 
-template <>
+template <> MatrixOf<float>::MatrixOf(Vector3 v) : MatrixOf(3, 1) {
-MatrixOf<float>::MatrixOf(Vector3 v) : MatrixOf(3, 1) {
   Set(0, 0, v.Right());
   Set(1, 0, v.Up());
   Set(2, 0, v.Forward());
@@ -224,8 +22,7 @@ MatrixOf<float>::MatrixOf(Vector3 v) : MatrixOf(3, 1) {
 
 template <>
 void MatrixOf<float>::Multiply(const MatrixOf<float> *m1,
-                               const MatrixOf<float>* m2,
+                               const MatrixOf<float> *m2, MatrixOf<float> *r) {
-                               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;
@@ -250,8 +47,7 @@ Vector3 MatrixOf<float>::Multiply(const MatrixOf<float>* m, Vector3 v) {
   return r;
 }
 
-template <typename T>
+template <typename T> Vector3 MatrixOf<T>::operator*(const Vector3 v) const {
-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]{};

LinearAlgebra/Matrix.h

@@ -1,102 +1,10 @@
 #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 {
@@ -208,6 +116,6 @@ class MatrixOf {
 };
 
 }  // namespace LinearAlgebra
-//using namespace LinearAlgebra;
+using namespace LinearAlgebra;
 
 #endif

LinearAlgebra/Quaternion.cpp

@@ -6,7 +6,6 @@
 #include <float.h>
 #include <math.h>
 #include "Angle.h"
-#include "Matrix.h"
 #include "Vector3.h"
 
 void CopyQuat(const Quat& q1, Quat& q2) {
@@ -98,28 +97,6 @@ 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,8 +34,6 @@ typedef struct Quat {
 
 namespace LinearAlgebra {
 
-class Matrix2;
-
 /// <summary>
 /// A quaternion
 /// </summary>
@@ -91,8 +89,6 @@ 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();
+  SphericalOf<T>();
-  SphericalOf(float distance, AngleOf<T> horizontal, AngleOf<T> vertical);
+  SphericalOf<T>(float distance, AngleOf<T> horizontal, AngleOf<T> vertical);
-  SphericalOf(float distance, DirectionOf<T> direction);
+  SphericalOf<T>(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();
+  SwingTwistOf<T>();
-  SwingTwistOf(DirectionOf<T> swing, AngleOf<T> twist);
+  SwingTwistOf<T>(DirectionOf<T> swing, AngleOf<T> twist);
-  SwingTwistOf(AngleOf<T> horizontal, AngleOf<T> vertical, AngleOf<T> twist);
+  SwingTwistOf<T>(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 {
- public:
+ protected:
   /// <summary>
   /// The right axis of the vector
   /// </summary>

LinearAlgebra/test/Matrix_test.cc

@@ -1,90 +1,10 @@
 #if GTEST
 #include <gtest/gtest.h>
-#include <math.h>
 #include <limits>
+#include <math.h>
 
 #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);

LocalParticipant.cpp

@@ -3,7 +3,6 @@
 #include "Thing.h"
 
 #include "Arduino/ArduinoParticipant.h"
-#include "EspIdf/EspIdfParticipant.h"
 
 #if defined(_WIN32) || defined(_WIN64)
 #include <winsock2.h>
@@ -25,6 +24,8 @@
 
 namespace RoboidControl {
 
+// LocalParticipant::LocalParticipant() {}
+
 LocalParticipant::LocalParticipant(int port) {
   this->ipAddress = "0.0.0.0";
   this->port = port;
@@ -32,10 +33,10 @@ LocalParticipant::LocalParticipant(int port) {
     this->isIsolated = true;
 }
 
-LocalParticipant::LocalParticipant(const char* ipAddress,
+LocalParticipant::LocalParticipant(const char* ipAddress, int port) {
-                                   int port,
+  this->ipAddress = "0.0.0.0";  // ipAddress;  // maybe this is not needed
-                                   int localPort)
+                                // anymore, keeping it to "0.0.0.0"
-    : Participant("127.0.0.1", localPort) {
+  this->port = port;
   if (this->port == 0)
     this->isIsolated = true;
   else
@@ -54,7 +55,7 @@ void LocalParticipant::begin() {
   if (this->isIsolated)
     return;
 
-  SetupUDP(this->port, this->remoteSite->ipAddress, this->remoteSite->port);
+  SetupUDP(this->port, this->ipAddress, this->port);
 }
 
 void LocalParticipant::SetupUDP(int localPort,
@@ -72,17 +73,22 @@ 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)
@@ -127,10 +133,6 @@ 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
 }
 
@@ -194,12 +196,6 @@ 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
 }
 
@@ -237,12 +233,6 @@ 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
 }
 
@@ -258,11 +248,11 @@ void LocalParticipant::ReceiveData(unsigned char packetSize,
       this->GetParticipant(senderIpAddress, senderPort);
   if (remoteParticipant == nullptr) {
     remoteParticipant = this->AddParticipant(senderIpAddress, senderPort);
-    // std::cout << "New sender " << senderIpAddress << ":" << senderPort
+    // std::cout << "New sender " << sender_ipAddress << ":" << sender_port
     //           << "\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);
@@ -314,8 +304,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 Site Id " << (int)this->networkId
+  // std::cout << this->name << ": process NetworkId [" << (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";
@@ -326,26 +316,9 @@ 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;
@@ -363,20 +336,17 @@ void LocalParticipant::Process(Participant* sender, NameMsg* msg) {
 #endif
     thingName[nameLength] = '\0';
     thing->name = thingName;
-    // std::cout << "thing name = " << thing->name << " length = " <<
+    // std::cout << "thing name = " << thing->name << " length = " << nameLength
-    // 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);

LocalParticipant.h

@@ -11,9 +11,7 @@
 #include "Participant.h"
 
 #if !defined(NO_STD)
-#include <functional>
 #include <list>
-// #include <unordered_map>
 #endif
 
 #if defined(_WIN32) || defined(_WIN64)
@@ -53,9 +51,7 @@ 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,
+  LocalParticipant(const char* ipAddress, int port = 7681);
-                   int port = 7681,
-                   int localPort = 7681);
   // Note to self: one cannot specify the port used by the local participant
   // now!!
 
@@ -90,11 +86,10 @@ class LocalParticipant : public Participant {
 
 #if defined(__unix__) || defined(__APPLE__)
   int sock;
-#elif defined(_WIN32) || defined(_WIN64)
+#endif
   sockaddr_in remote_addr;
   sockaddr_in server_addr;
   sockaddr_in broadcast_addr;
-#endif
 
 #endif
 
@@ -140,35 +135,6 @@ 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.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

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,20 +45,19 @@ 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 = 
+      newThing = new Thing(sender, msg->networkId, msg->thingId,
-//         new Thing(sender, msg->networkId, msg->thingId,
+                           (Thing::Type)msg->thingType);
-//                            (Thing::Type)msg->thingType);
+#endif
-// #endif
   }
 }
 

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

Things/TemperatureSensor.cpp

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