Merge commit '726f761d1ce08e886c6ec1808f2c427490b397f1' into RoboidControl

Arduino/ArduinoParticipant.cpp

@@ -19,7 +19,7 @@
 namespace RoboidControl {
 namespace Arduino {
 
-void LocalParticipant::Setup(int localPort,
+void ParticipantUDP::Setup(int localPort,
                              const char* remoteIpAddress,
                              int remotePort) {
 #if defined(ARDUINO) && defined(HAS_WIFI)
@@ -44,7 +44,7 @@ void LocalParticipant::Setup(int localPort,
 #endif
 }
 
-void LocalParticipant::GetBroadcastAddress() {
+void ParticipantUDP::GetBroadcastAddress() {
 #if defined(ARDUINO) && defined(HAS_WIFI)
   IPAddress broadcastAddress = WiFi.localIP();
   broadcastAddress[3] = 255;
@@ -56,7 +56,7 @@ void LocalParticipant::GetBroadcastAddress() {
 #endif
 }
 
-void LocalParticipant::Receive() {
+void ParticipantUDP::Receive() {
 #if defined(ARDUINO) && defined(HAS_WIFI)
   int packetSize = udp.parsePacket();
   while (packetSize > 0) {
@@ -86,7 +86,7 @@ void LocalParticipant::Receive() {
 #endif
 }
 
-bool LocalParticipant::Send(Participant* remoteParticipant, int bufferSize) {
+bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
 #if defined(ARDUINO) && defined(HAS_WIFI)
   // std::cout << "Sending to:\n " << remoteParticipant->ipAddress << ":"
   // << remoteParticipant->port << "\n";
@@ -106,7 +106,7 @@ bool LocalParticipant::Send(Participant* remoteParticipant, int bufferSize) {
   return true;
 }
 
-bool LocalParticipant::Publish(IMessage* msg) {
+bool ParticipantUDP::Publish(IMessage* msg) {
 #if defined(ARDUINO) && defined(HAS_WIFI)
   int bufferSize = msg->Serialize((char*)this->buffer);
   if (bufferSize <= 0)

Arduino/ArduinoParticipant.h

@@ -1,6 +1,6 @@
 #pragma once
 
-#include "../LocalParticipant.h"
+#include "Participants/ParticipantUDP.h"
 
 #if defined(HAS_WIFI)
 #include <WiFiUdp.h>
@@ -9,7 +9,7 @@
 namespace RoboidControl {
 namespace Arduino {
 
-class LocalParticipant : public RoboidControl::LocalParticipant {
+class ParticipantUDP : public RoboidControl::ParticipantUDP {
  public:
   void Setup(int localPort, const char* remoteIpAddress, int remotePort);
   void Receive();

CMakeLists.txt

@@ -1,8 +1,22 @@
 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
+    Participants/*.cpp
+)
+
 if(ESP_PLATFORM)
     idf_component_register(
-        SRC_DIRS "."
-        INCLUDE_DIRS "."
+        SRCS ${srcs}
+        INCLUDE_DIRS "." "LinearAlgebra"
+        REQUIRES esp_netif esp_wifi
     )
 else() 
     project(RoboidControl)
@@ -28,14 +42,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,166 @@
+#include "EspIdfParticipant.h"
+
+#if defined(IDF_VER)
+#include "esp_wifi.h"
+#endif
+
+namespace RoboidControl {
+namespace EspIdf {
+
+void ParticipantUDP::Setup(int localPort,
+                           const char* remoteIpAddress,
+                           int remotePort) {
+#if defined(IDF_VER)
+  std::cout << "Set up UDP\n";
+  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
+  this->sockfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
+  if (this->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(this->sockfd, (struct sockaddr*)&local_addr, sizeof(local_addr)) <
+      0) {
+    std::cout << "Unable to bind UDP socket: errno " << errno << "\n";
+    close(sockfd);
+    vTaskDelete(NULL);
+    return;
+  }
+
+  // Initialize the dest_addr structure
+  memset(&this->dest_addr, 0,
+         sizeof(this->dest_addr));  // Clear the entire structure
+  this->dest_addr.sin_family = AF_INET;
+  this->dest_addr.sin_port = htons(this->remoteSite->port);
+  inet_pton(AF_INET, this->remoteSite->ipAddress,
+            &this->dest_addr.sin_addr.s_addr);
+
+  std::cout << "Wifi sync started local " << this->port << ", remote "
+            << this->remoteSite->ipAddress << ":" << this->remoteSite->port
+            << "\n";
+#endif  // IDF_VER
+}
+
+void ParticipantUDP::GetBroadcastAddress() {
+#if defined(IDF_VER)
+  // SOMEHOW, THIS FUNCTION RESULTS IN MEMORY CORRUPION...
+
+  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;
+
+  snprintf(this->broadcastIpAddress, INET_ADDRSTRLEN, IPSTR,
+           IP2STR(&broadcast_addr.u_addr.ip4));
+  std::cout << "Broadcast address: " << this->broadcastIpAddress << "\n";
+#endif  // IDF_VER
+}
+
+void ParticipantUDP::Receive() {
+#if defined(IDF_VER)
+  struct pollfd fds[1];
+  fds[0].fd = sockfd;
+  fds[0].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;
+  }
+
+  // char buffer[1024];
+  struct sockaddr_in source_addr;
+
+  socklen_t addr_len = sizeof(source_addr);
+  char sender_ipAddress[INET_ADDRSTRLEN];
+
+  while (ret > 0 && fds[0].revents & POLLIN) {
+    int packetSize = recvfrom(this->sockfd, buffer, sizeof(buffer) - 1, 0,
+                              (struct sockaddr*)&source_addr, &addr_len);
+    if (packetSize < 0) {
+      std::cout << "recvfrom() error\n";
+      return;
+    } else if (packetSize == 0) {
+      break;
+    }
+
+    // std::cout << "receiving " << packetSize << " bytes, msgId " <<
+    // (int)this->buffer[0] << "\n";
+    inet_ntoa_r(source_addr.sin_addr, sender_ipAddress, INET_ADDRSTRLEN);
+    unsigned int sender_port = ntohs(source_addr.sin_port);
+
+    ReceiveData(packetSize, sender_ipAddress, sender_port);
+
+    ret = poll(fds, 1, 0);
+    if (ret == -1) {
+      std::cout << "poll() error\n";
+      return;
+    }
+  }
+  // std::cout << "no more messages\n";
+
+#endif  // IDF_VER
+}
+
+bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
+#if defined(IDF_VER)
+  // std::cout << "Sending to " << remoteParticipant->ipAddress << ":"
+  //           << remoteParticipant->port << "\n";
+
+  int err = sendto(this->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 ParticipantUDP::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";
+#endif
+  return true;
+};
+
+}  // namespace EspIdf
+}  // namespace RoboidControl

EspIdf/EspIdfParticipant.h

@@ -0,0 +1,32 @@
+#pragma once
+
+#include "Participants/ParticipantUDP.h"
+
+#if defined(IDF_VER)
+#include "lwip/sockets.h"
+#endif
+
+namespace RoboidControl {
+namespace EspIdf {
+
+class ParticipantUDP : public RoboidControl::ParticipantUDP {
+ public:
+  void Setup(int localPort, const char* remoteIpAddress, int remotePort);
+  void Receive();
+  bool Send(Participant* remoteParticipant, int bufferSize);
+  bool Publish(IMessage* msg);
+
+ protected:
+#if defined(IDF_VER)
+  char broadcastIpAddress[INET_ADDRSTRLEN];
+
+  int sockfd;
+  struct sockaddr_in dest_addr;
+  // struct sockaddr_in src_addr;
+#endif
+
+  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,284 @@
 #include "Matrix.h"
+#include <iostream>
 
-template <> MatrixOf<float>::MatrixOf(unsigned int rows, unsigned int cols) {
+namespace LinearAlgebra {
+
+#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)
+    this->data = nullptr;
+  else {
+    this->data = new float[this->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(const Matrix2& m)
+    : nRows(m.nRows), nCols(m.nCols), nValues(m.nValues) {
+  if (this->nValues == 0)
+    this->data = nullptr;
+  else {
+    this->data = new float[this->nValues];
+    std::copy(m.data, m.data + nValues, this->data);
+  }
+}
+
+Matrix2& Matrix2::operator=(const Matrix2& m) {
+  if (this != &m) {
+    delete[] this->data;  // Free the current memory
+
+    this->nRows = m.nRows;
+    this->nCols = m.nCols;
+    this->nValues = m.nValues;
+    if (this->nValues == 0)
+      this->data = nullptr;
+    else {
+      this->data = new float[this->nValues];
+      std::copy(m.data, m.data + this->nValues, this->data);
+    }
+  }
+  return *this;
+}
+
+Matrix2::~Matrix2() {
+  if (!this->externalData)
+    delete[] data;
+}
+
+Matrix2 Matrix2::Clone() const {
+  Matrix2 r = Matrix2(this->nRows, this->nCols);
+  std::copy(this->data, this->data + this->nValues, r.data);
+  return r;
+}
+
+// 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;
+}
+
+void Matrix2::Clear() {
+  for (int ix = 0; ix < this->nValues; ix++)
+    this->data[ix] = 0;
+}
+
+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 Matrix2::Transpose() const {
+  Matrix2 r = Matrix2(this->nCols, this->nRows);
+
+  for (uint rowIx = 0; rowIx < this->nRows; rowIx++) {
+    for (uint colIx = 0; colIx < this->nCols; colIx++)
+      r.data[colIx * this->nCols + rowIx] =
+          this->data[rowIx * this->nCols + colIx];
+  }
+  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 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 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;
+}
+
+Matrix2 Matrix2::Slice(int rowStart, int rowStop, int colStart, int colStop) {
+  Matrix2 r = Matrix2(rowStop - rowStart, colStop - colStart);
+
+  int resultRowIx = 0;
+  int resultColIx = 0;
+  for (int i = rowStart; i < rowStop; i++) {
+    for (int j = colStart; j < colStop; j++)
+      r.data[resultRowIx * r.nCols + resultColIx] =
+          this->data[i * this->nCols + j];
+  }
+  return r;
+}
+
+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;
+    // rRowDataIx += this->nCols;
+    mRowDataIx += m.nCols;
+    for (int colIx = colStart; colIx < colStop; colIx++) {
+      this->data[rRowDataIx + colIx] = m.data[mRowDataIx + (colIx - 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.UpdateSlice(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
+
+}  // namespace LinearAlgebra
+
+template <>
+MatrixOf<float>::MatrixOf(unsigned int rows, unsigned int cols) {
   if (rows <= 0 || cols <= 0) {
     this->rows = 0;
     this->cols = 0;
@@ -14,15 +292,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,
-                               const MatrixOf<float> *m2, MatrixOf<float> *r) {
+void MatrixOf<float>::Multiply(const MatrixOf<float>* m1,
+                               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 +317,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 +327,11 @@ Vector3 MatrixOf<float>::Multiply(const MatrixOf<float> *m, Vector3 v) {
   return r;
 }
 
-template <typename T> Vector3 MatrixOf<T>::operator*(const Vector3 v) const {
-  float *vData = new float[3]{v.Right(), v.Up(), v.Forward()};
+template <typename T>
+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,119 @@
 #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(const Matrix2& m);
+  Matrix2& operator=(const Matrix2& other);
+
+  ~Matrix2();
+
+  Matrix2 Clone() const;
+
+  static Matrix2 Zero(int nRows, int nCols);
+  void Clear();
+
+  static Matrix2 Identity(int size);
+
+  static Matrix2 Diagonal(float f, int size);
+
+  static Matrix2 SkewMatrix(const Vector3& v);
+
+  Matrix2 Transpose() const;
+
+  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& v);
+
+  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/(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;
+  }
+
+  Matrix2 Slice(int rawStart, int rowStop, int colStart, int colStop);
+
+  void UpdateSlice(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 +225,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

@@ -21,17 +21,12 @@ class SphericalOf {
   /// @brief The distance in meters
   /// @remark The distance should never be negative
   float distance;
-  /// @brief The angle in the horizontal plane in degrees, clockwise rotation
-  /// @details The angle is automatically normalized to -180 .. 180
-  // AngleOf<T> horizontal;
-  /// @brief The angle in the vertical plane in degrees. Positive is upward.
-  /// @details The angle is automatically normalized to -180 .. 180
-  // AngleOf<T> vertical;
+  /// @brief The direction of the vector
   DirectionOf<T> direction;
 
-  SphericalOf<T>();
-  SphericalOf<T>(float distance, AngleOf<T> horizontal, AngleOf<T> vertical);
-  SphericalOf<T>(float distance, DirectionOf<T> direction);
+  SphericalOf();
+  SphericalOf(float distance, AngleOf<T> horizontal, AngleOf<T> vertical);
+  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<T>(DirectionOf<T> swing, AngleOf<T> twist);
-  SwingTwistOf<T>(AngleOf<T> horizontal, AngleOf<T> vertical, AngleOf<T> twist);
+  SwingTwistOf();
+  SwingTwistOf(DirectionOf<T> swing, 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);

Messages/BinaryMsg.cpp

@@ -2,33 +2,45 @@
 
 namespace RoboidControl {
 
-BinaryMsg::BinaryMsg(char* buffer) {
-  unsigned char ix = 1;
-  this->networkId = buffer[ix++];
-  this->thingId = buffer[ix++];
-  this->bytes = buffer + ix;  // This is only valid because the code ensures the the msg
-                              // lifetime is shorter than the buffer lifetime...
-}
-
 BinaryMsg::BinaryMsg(unsigned char networkId, Thing* thing) {
   this->networkId = networkId;
   this->thingId = thing->id;
   this->thing = thing;
+  unsigned char ix = BinaryMsg::length;
+  this->dataLength = this->thing->GenerateBinary(this->data, &ix);
 }
 
-BinaryMsg::~BinaryMsg() {}
+BinaryMsg::BinaryMsg(char* buffer) {
+  unsigned char ix = 1;
+  this->networkId = buffer[ix++];
+  this->thingId = buffer[ix++];
+  this->dataLength = buffer[ix++];
+  char* data = new char[this->dataLength];
+  for (int i = 0; i < this->dataLength; i++)
+    data[i] = buffer[ix++];
+  this->data = data;
+}
+
+BinaryMsg::~BinaryMsg() {
+  delete[] this->data;
+}
 
 unsigned char BinaryMsg::Serialize(char* buffer) {
-  unsigned char ix = this->length;
-  this->thing->GenerateBinary(buffer, &ix);
-  if (ix <= this->length)  // in this case, no data is actually sent
+  // unsigned char ix = this->length;
+  //  this->dataLength = this->thing->GenerateBinary(buffer, &ix);
+  if (this->dataLength <= 0)  // in this case, no data is actually sent
     return 0;
 
-  buffer[0] = this->id;
-  buffer[1] = this->networkId;
-  buffer[2] = this->thingId;
-  return ix;
+#if defined(DEBUG)
+  std::cout << "Send BinaryMsg [" << (int)this->networkId << "/" << (int)this->thingId
+            << "] " << (int)this->dataLength << std::endl;
+#endif
+  unsigned char ix = 0;
+  buffer[ix++] = this->id;
+  buffer[ix++] = this->networkId;
+  buffer[ix++] = this->thingId;
+  buffer[ix++] = this->dataLength;
+  return this->length + this->dataLength;
 }
 
-
 }  // namespace RoboidControl

Messages/BinaryMsg.h

@@ -10,7 +10,7 @@ class BinaryMsg : public IMessage {
   /// @brief The message ID
   static const unsigned char id = 0xB1;
   /// @brief The length of the message without the binary data itslef
-  static const unsigned length = 3;
+  static const unsigned length = 4;
 
   /// @brief The network ID of the thing
   unsigned char networkId;
@@ -19,8 +19,9 @@ class BinaryMsg : public IMessage {
   /// @brief The thing for which the binary data is communicated
   Thing* thing;
 
+  unsigned char dataLength;
   /// @brief The binary data which is communicated
-  char* bytes = nullptr;
+  char* data = nullptr;
 
   /// @brief Create a new message for sending
   /// @param networkId The network ID of the thing

Messages/DestroyMsg.cpp

@@ -12,6 +12,10 @@ DestroyMsg::DestroyMsg(char* buffer) {}
 DestroyMsg::~DestroyMsg() {}
 
 unsigned char DestroyMsg::Serialize(char *buffer) {
+  #if defined(DEBUG)
+  std::cout << "Send DestroyMsg [" << (int)this->networkId << "/" << (int)this->thingId
+            << "] " << std::endl;
+#endif
   unsigned char ix = 0;
   buffer[ix++] = this->id;
   buffer[ix++] = this->networkId;

Messages/InvestigateMsg.cpp

@@ -14,6 +14,10 @@ InvestigateMsg::InvestigateMsg(unsigned char networkId, unsigned char thingId) {
 
 InvestigateMsg::~InvestigateMsg() {}
 unsigned char InvestigateMsg::Serialize(char* buffer) {
+  #if defined(DEBUG)
+  std::cout << "Send InvestigateMsg [" << (int)this->networkId << "/" << (int)this->thingId
+            << "] " << std::endl;
+#endif
   unsigned char ix = 0;
   buffer[ix++] = this->id;
   buffer[ix++] = this->networkId;

Messages/Messages.cpp

@@ -1,7 +1,6 @@
 #include "Messages.h"
 
 #include "LowLevelMessages.h"
-//#include "Participant.h"
 #include "string.h"
 
 namespace RoboidControl {
@@ -10,26 +9,10 @@ namespace RoboidControl {
 
 IMessage::IMessage() {}
 
-// IMessage::IMessage(unsigned char *buffer) { Deserialize(buffer); }
-
-// IMessage::IMessage(char* buffer) {}
-
 unsigned char IMessage::Serialize(char* buffer) {
   return 0;
 }
 
-// bool IMessage::SendMsg(LocalParticipant *client, IMessage msg) {
-//   // return SendMsg(client, client.buffer, );nameLength
-//   return client->SendBuffer(msg.Serialize(client->buffer));
-// }
-
-// bool IMessage::Publish(LocalParticipant *participant) {
-//   return participant->PublishBuffer(Serialize(participant->buffer));
-// }
-// bool IMessage::SendTo(LocalParticipant *participant) {
-//   return participant->SendBuffer(Serialize(participant->buffer));
-// }
-
 // IMessage
 #pragma endregion
 

Messages/Messages.h

@@ -6,7 +6,7 @@
 
 namespace RoboidControl {
 
-class LocalParticipant;
+class ParticipantUDP;
 
 class IMessage {
  public:
@@ -15,8 +15,8 @@ class IMessage {
 
   static unsigned char* ReceiveMsg(unsigned char packetSize);
 
-  // bool Publish(LocalParticipant *participant);
-  // bool SendTo(LocalParticipant *participant);
+  // bool Publish(ParticipantUDP *participant);
+  // bool SendTo(ParticipantUDP *participant);
 };
 
 }  // namespace RoboidControl

Messages/ModelUrlMsg.cpp

@@ -4,16 +4,6 @@
 
 namespace RoboidControl {
 
-// ModelUrlMsg::ModelUrlMsg(unsigned char networkId, unsigned char thingId,
-//                          unsigned char urlLength, const char *url,
-//                          float scale) {
-//   this->networkId = networkId;
-//   this->thingId = thingId;
-//   this->urlLength = urlLength;
-//   this->url = url;
-//   this->scale = scale;
-// }
-
 ModelUrlMsg::ModelUrlMsg(unsigned char networkId, Thing* thing) {
   this->networkId = networkId;
   this->thingId = thing->id;
@@ -22,21 +12,21 @@ ModelUrlMsg::ModelUrlMsg(unsigned char networkId, Thing* thing) {
   else
     this->urlLength = (unsigned char)strlen(thing->modelUrl);
 
-  //this->url = thing->modelUrl;  // dangerous!
-
   // the url string in the buffer is not \0 terminated!
   char* url = new char[this->urlLength + 1];
   for (int i = 0; i < this->urlLength; i++)
     url[i] = thing->modelUrl[i];
   url[this->urlLength] = '\0';
-  this->url = url;}
+  this->url = url;
+}
 
 ModelUrlMsg::ModelUrlMsg(const char* buffer) {
   unsigned char ix = 1;  // first byte is msg id
   this->networkId = buffer[ix++];
   this->thingId = buffer[ix++];
   this->urlLength = buffer[ix++];
-  // this->url = &buffer[ix];  // dangerous! name should not be used anymore after
+  // this->url = &buffer[ix];  // dangerous! name should not be used anymore
+  // after
   //                           // buffer has been re-used...
 
   // the url string in the buffer is not \0 terminated!
@@ -54,6 +44,11 @@ ModelUrlMsg::~ModelUrlMsg() {
 unsigned char ModelUrlMsg::Serialize(char* buffer) {
   if (this->urlLength == 0 || this->url == nullptr)
     return 0;
+#if defined(DEBUG)
+  std::cout << "Send ModelUrlMsg [" << (int)this->networkId << "/"
+            << (int)this->thingId << "] " << (int)this->urlLength << " "
+            << this->url << std::endl;
+#endif
   unsigned char ix = 0;
   buffer[ix++] = this->id;
   buffer[ix++] = this->networkId;

Messages/ModelUrlMsg.h

@@ -8,14 +8,14 @@ class ModelUrlMsg : public IMessage {
   /// @brief The message ID
   static const unsigned char id = 0x90;
   /// @brief The length of the message without the URL string itself
-  static const unsigned char length = 3;
+  static const unsigned char length = 4;
 
   /// @brief The network ID of the thing
   unsigned char networkId;
   /// @brief The ID of the thing
   unsigned char thingId;
 
-  /// @brief The length of the url st5ring, excluding the null terminator
+  /// @brief The length of the url string, excluding the null terminator
   unsigned char urlLength;
   /// @brief The url of the model, not terminated by a null character
   const char* url;

Messages/NameMsg.cpp

@@ -25,6 +25,7 @@ NameMsg::NameMsg(const char* buffer) {
   this->networkId = buffer[ix++];
   this->thingId = buffer[ix++];
   this->nameLength = buffer[ix++];
+
   // the name string in the buffer is not \0 terminated!
   char* name = new char[this->nameLength + 1];
   for (int i = 0; i < this->nameLength; i++)
@@ -41,6 +42,11 @@ unsigned char NameMsg::Serialize(char* buffer) {
   if (this->nameLength == 0 || this->name == nullptr)
     return 0;
 
+#if defined(DEBUG)
+  std::cout << "Send NameMsg [" << (int)this->networkId << "/"
+            << (int)this->thingId << "] " << (int)this->nameLength << " "
+            << this->name << std::endl;
+#endif
   unsigned char ix = 0;
   buffer[ix++] = this->id;
   buffer[ix++] = this->networkId;

Messages/ParticipantMsg.cpp

@@ -13,13 +13,19 @@ ParticipantMsg::ParticipantMsg(const char* buffer) {
 ParticipantMsg::~ParticipantMsg() {}
 
 unsigned char ParticipantMsg::Serialize(char* buffer) {
+#if defined(DEBUG)
+  std::cout << "Send ParticipantMsg [" << (int)this->networkId << "] "
+            << std::endl;
+#endif
+
   unsigned char ix = 0;
   buffer[ix++] = this->id;
   buffer[ix++] = this->networkId;
   return ParticipantMsg::length;
 }
 
-// bool ParticipantMsg::Send(LocalParticipant *participant, unsigned char networkId) {
+// bool ParticipantMsg::Send(ParticipantUDP *participant, unsigned char
+// networkId) {
 //   ParticipantMsg msg = ParticipantMsg()
 // }
 // Client Msg

Messages/PoseMsg.cpp

@@ -11,12 +11,10 @@ PoseMsg::PoseMsg(unsigned char networkId, Thing* thing, bool force) {
   if (thing->positionUpdated || force) {
     this->position = thing->GetPosition();
     this->poseType |= Pose_Position;
-    thing->positionUpdated = false;
   }
   if (thing->orientationUpdated || force) {
     this->orientation = thing->GetOrientation();
     this->poseType |= Pose_Orientation;
-    thing->orientationUpdated = false;
   }
   if (thing->linearVelocityUpdated) {
     this->linearVelocity = thing->GetLinearVelocity();
@@ -47,6 +45,10 @@ unsigned char PoseMsg::Serialize(char* buffer) {
   if (this->poseType == 0)
     return 0;
 
+#if defined(DEBUG)
+  std::cout << "Send PoseMsg [" << (int)this->networkId << "/"
+            << (int)this->thingId << "] " << (int)this->poseType << std::endl;
+#endif
   unsigned char ix = 0;
   buffer[ix++] = PoseMsg::id;
   buffer[ix++] = this->networkId;

Messages/SiteMsg.cpp

@@ -13,6 +13,9 @@ SiteMsg::SiteMsg(unsigned char networkId) {
 SiteMsg::~SiteMsg() {}
 
 unsigned char SiteMsg::Serialize(char* buffer) {
+#if defined(DEBUG)
+  std::cout << "Send SiteMsg [" << (int)this->networkId << "] " << std::endl;
+#endif
   unsigned char ix = 0;
   buffer[ix++] = this->id;
   buffer[ix++] = this->networkId;

Messages/TextMsg.cpp

@@ -24,6 +24,9 @@ unsigned char TextMsg::Serialize(char* buffer) {
   if (this->textLength == 0 || this->text == nullptr)
     return 0;
 
+#if defined(DEBUG)
+  std::cout << "Send TextMsg " << (int)this->textLength << " " << this->text << std::endl;
+#endif
   unsigned char ix = 0;
   buffer[ix++] = this->id;
   buffer[ix++] = this->textLength;

Messages/ThingMsg.cpp

@@ -32,6 +32,10 @@ ThingMsg::ThingMsg(unsigned char networkId, Thing* thing) {
 ThingMsg::~ThingMsg() {}
 
 unsigned char ThingMsg::Serialize(char* buffer) {
+#if defined(DEBUG)
+  std::cout << "Send ThingMsg [" << (int)this->networkId << "/" << (int)this->thingId
+            << "] " << (int)this->thingType << " " << (int)this->parentId << std::endl;
+#endif
   unsigned char ix = 0;
   buffer[ix++] = this->id;
   buffer[ix++] = this->networkId;

Participant.cpp

@@ -27,15 +27,20 @@ Participant::~Participant() {
   delete[] this->ipAddress;
 }
 
-Thing* Participant::Get(unsigned char networkId, unsigned char thingId) {
+void Participant::Update(unsigned long currentTimeMs) {
+  for (Thing* thing : this->things) {
+    if (thing != nullptr)
+      thing->Update(currentTimeMs, true);
+  }
+}
+
+Thing* Participant::Get(unsigned char thingId) {
   for (Thing* thing : this->things) {
-    // if (thing->networkId == networkId && thing->id == thingId)
     if (thing->id == thingId)
       return thing;
   }
-  //   std::cout << "Could not find thing " << this->ipAddress << ":" <<
-  //   this->port
-  //             << "[" << (int)networkId << "/" << (int)thingId << "]\n";
+  // std::cout << "Could not find thing " << this->ipAddress << ":" << this->port
+  //           << "[" << (int)thingId << "]\n";
   return nullptr;
 }
 
@@ -49,25 +54,22 @@ void Participant::Add(Thing* thing, bool checkId) {
     thing->id = (unsigned char)this->things.size() + 1;
     this->things.push_back(thing);
 #endif
-    // std::cout << "Add thing with generated ID " << this->ipAddress << ":" <<
-    // this->port << "[" << (int)thing->networkId << "/"
-    //           << (int)thing->id << "]\n";
+    // std::cout << "Add thing with generated ID " << this->ipAddress << ":"
+    //           << this->port << "[" << (int)thing->id << "]\n";
   } else {
-    Thing* foundThing = Get(thing->networkId, thing->id);
+    Thing* foundThing = Get(thing->id);
     if (foundThing == nullptr) {
 #if defined(NO_STD)
       this->things[this->thingCount++] = thing;
 #else
       this->things.push_back(thing);
 #endif
-      // std::cout << "Add thing " << this->ipAddress << ":" << this->port <<
-      // "[" << (int)thing->networkId << "/"
+      // std::cout << "Add thing " << this->ipAddress << ":" << this->port << "["
       //           << (int)thing->id << "]\n";
-    }
-    // else
+    } else {
       // std::cout << "Did not add, existing thing " << this->ipAddress << ":"
-    //   << this->port << "["
-    //             << (int)thing->networkId << "/" << (int)thing->id << "]\n";
+      //           << this->port << "[" << (int)thing->id << "]\n";
+    }
   }
 }
 

Participant.h

@@ -33,6 +33,8 @@ class Participant {
   /// @brief Destructor for the participant
   ~Participant();
 
+  virtual void Update(unsigned long currentTimeMs = 0);
+
  protected:
 #if defined(NO_STD)
   unsigned char thingCount = 0;
@@ -48,7 +50,7 @@ class Participant {
   /// @param thingId The ID of the thing
   /// @return The thing if found or nullptr when no thing has been found
   /// @note The use of the network ID is likely to disappear in future versions.
-  Thing* Get(unsigned char networkId, unsigned char thingId);
+  Thing* Get(unsigned char thingId);
   /// @brief Add a new thing for this participant.
   /// @param thing The thing to add
   /// @param checkId Checks the thing ID of the thing. If it is 0, a new thing

Participants/IsolatedParticipant.cpp

@@ -0,0 +1,14 @@
+#include "IsolatedParticipant.h"
+#include "ParticipantUDP.h"
+
+namespace RoboidControl {
+
+static ParticipantUDP* isolatedParticipant = nullptr;
+
+Participant* IsolatedParticipant::Isolated() {
+  if (isolatedParticipant == nullptr)
+    isolatedParticipant = new ParticipantUDP(0);
+  return isolatedParticipant;
+}
+
+}  // namespace RoboidControl

Participants/IsolatedParticipant.h

@@ -0,0 +1,13 @@
+#include "Participant.h"
+
+namespace RoboidControl {
+
+class IsolatedParticipant {
+ public:
+  /// @brief Isolated participant is used when the application is run without
+  /// networking
+  /// @return A participant without networking support
+  static Participant* Isolated();
+};
+
+}

Participants/ParticipantUDP.cpp

@@ -0,0 +1,421 @@
+#include "ParticipantUDP.h"
+
+#include "Thing.h"
+
+#include "Arduino/ArduinoParticipant.h"
+#include "EspIdf/EspIdfParticipant.h"
+
+#if defined(_WIN32) || defined(_WIN64)
+#include <winsock2.h>
+#include <ws2tcpip.h>
+#include "Windows/WindowsParticipant.h"
+#pragma comment(lib, "ws2_32.lib")
+
+#elif defined(__unix__) || defined(__APPLE__)
+#include <arpa/inet.h>
+#include <fcntl.h>  // For fcntl
+#include <netinet/in.h>
+#include <sys/socket.h>
+#include <unistd.h>
+#include <chrono>
+#include "Posix/PosixParticipant.h"
+#endif
+
+#include <string.h>
+
+namespace RoboidControl {
+
+ParticipantUDP::ParticipantUDP(int port) {
+  this->ipAddress = "0.0.0.0";
+  this->port = port;
+  this->remoteSite = nullptr;
+  if (this->port == 0)
+    this->isIsolated = true;
+}
+
+ParticipantUDP::ParticipantUDP(const char* ipAddress, int port, int localPort)
+    : Participant("127.0.0.1", localPort) {
+  if (this->port == 0)
+    this->isIsolated = true;
+  else
+    this->remoteSite = new Participant(ipAddress, port);
+}
+
+static ParticipantUDP* isolatedParticipant = nullptr;
+
+ParticipantUDP* ParticipantUDP::Isolated() {
+  if (isolatedParticipant == nullptr)
+    isolatedParticipant = new ParticipantUDP(0);
+  return isolatedParticipant;
+}
+
+void ParticipantUDP::begin() {
+  if (this->isIsolated)
+    return;
+
+  SetupUDP(this->port, this->remoteSite->ipAddress, this->remoteSite->port);
+}
+
+void ParticipantUDP::SetupUDP(int localPort,
+                              const char* remoteIpAddress,
+                              int remotePort) {
+#if defined(_WIN32) || defined(_WIN64)
+  Windows::ParticipantUDP* thisWindows =
+      static_cast<Windows::ParticipantUDP*>(this);
+  thisWindows->Setup(localPort, remoteIpAddress, remotePort);
+#elif defined(__unix__) || defined(__APPLE__)
+  Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
+  thisPosix->Setup(localPort, remoteIpAddress, remotePort);
+#elif defined(ARDUINO)
+  Arduino::ParticipantUDP* thisArduino =
+      static_cast<Arduino::ParticipantUDP*>(this);
+  thisArduino->Setup(localPort, remoteIpAddress, remotePort);
+#elif defined(IDF_VER)
+  EspIdf::ParticipantUDP* thisEspIdf =
+      static_cast<EspIdf::ParticipantUDP*>(this);
+  thisEspIdf->Setup(localPort, remoteIpAddress, remotePort);
+#endif
+  this->connected = true;
+}
+
+void ParticipantUDP::Update(unsigned long currentTimeMs) {
+  if (currentTimeMs == 0)
+    currentTimeMs = Thing::GetTimeMs();
+
+  if (this->isIsolated == false) {
+    if (this->connected == false)
+      begin();
+
+    if (this->publishInterval > 0 && currentTimeMs > this->nextPublishMe) {
+      ParticipantMsg* msg = new ParticipantMsg(this->networkId);
+      if (this->remoteSite == nullptr)
+        this->Publish(msg);
+      else
+        this->Send(this->remoteSite, msg);
+      delete msg;
+
+      this->nextPublishMe = currentTimeMs + this->publishInterval;
+    }
+
+    this->ReceiveUDP();
+  }
+
+  for (Thing* thing : this->things) {
+    if (thing == nullptr)
+      continue;
+
+    if (this->isIsolated == false) {
+      PoseMsg* poseMsg = new PoseMsg(this->networkId, thing);
+      this->Send(thing->owner, poseMsg);
+      BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing);
+      this->Send(thing->owner, binaryMsg);
+      delete poseMsg;
+    }
+    thing->Update(currentTimeMs, true);
+  }
+}
+
+void ParticipantUDP::ReceiveUDP() {
+#if defined(_WIN32) || defined(_WIN64)
+  Windows::ParticipantUDP* thisWindows =
+      static_cast<Windows::ParticipantUDP*>(this);
+  thisWindows->Receive();
+#elif defined(__unix__) || defined(__APPLE__)
+  Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
+  thisPosix->Receive();
+#elif defined(ARDUINO)
+  Arduino::ParticipantUDP* thisArduino =
+      static_cast<Arduino::ParticipantUDP*>(this);
+  thisArduino->Receive();
+#elif defined(IDF_VER)
+  EspIdf::ParticipantUDP* thisEspIdf =
+      static_cast<EspIdf::ParticipantUDP*>(this);
+  thisEspIdf->Receive();
+#endif
+}
+
+Participant* ParticipantUDP::GetParticipant(const char* ipAddress, int port) {
+  for (Participant* sender : this->senders) {
+    if (strcmp(sender->ipAddress, ipAddress) == 0 && sender->port == port)
+      return sender;
+  }
+  return nullptr;
+}
+
+Participant* ParticipantUDP::AddParticipant(const char* ipAddress, int port) {
+  // std::cout << "New Participant " << ipAddress << ":" << port << "\n";
+  Participant* participant = new Participant(ipAddress, port);
+#if defined(NO_STD)
+  participant->networkId = this->senderCount;
+  this->senders[this->senderCount++] = participant;
+#else
+  participant->networkId = (unsigned char)this->senders.size();
+  this->senders.push_back(participant);
+#endif
+  return participant;
+}
+
+#pragma region Send
+
+void ParticipantUDP::SendThingInfo(Participant* remoteParticipant,
+                                   Thing* thing) {
+  // std::cout << "Send thing info [" << (int)thing->id << "] \n";
+  ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
+  this->Send(remoteParticipant, thingMsg);
+  delete thingMsg;
+  NameMsg* nameMsg = new NameMsg(this->networkId, thing);
+  this->Send(remoteParticipant, nameMsg);
+  delete nameMsg;
+  ModelUrlMsg* modelMsg = new ModelUrlMsg(this->networkId, thing);
+  this->Send(remoteParticipant, modelMsg);
+  delete modelMsg;
+  PoseMsg* poseMsg = new PoseMsg(this->networkId, thing, true);
+  this->Send(remoteParticipant, poseMsg);
+  delete poseMsg;
+  BinaryMsg* customMsg = new BinaryMsg(this->networkId, thing);
+  this->Send(remoteParticipant, customMsg);
+  delete customMsg;
+}
+
+bool ParticipantUDP::Send(Participant* remoteParticipant, IMessage* msg) {
+  int bufferSize = msg->Serialize(this->buffer);
+  if (bufferSize <= 0)
+    return true;
+
+#if defined(_WIN32) || defined(_WIN64)
+  Windows::ParticipantUDP* thisWindows =
+      static_cast<Windows::ParticipantUDP*>(this);
+  return thisWindows->Send(remoteParticipant, bufferSize);
+#elif defined(__unix__) || defined(__APPLE__)
+  Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
+  return thisPosix->Send(remoteParticipant, bufferSize);
+#elif defined(ARDUINO)
+  Arduino::ParticipantUDP* thisArduino =
+      static_cast<Arduino::ParticipantUDP*>(this);
+  return thisArduino->Send(remoteParticipant, bufferSize);
+#elif defined(IDF_VER)
+  EspIdf::ParticipantUDP* thisEspIdf =
+      static_cast<EspIdf::ParticipantUDP*>(this);
+  return thisEspIdf->Send(remoteParticipant, bufferSize);
+#else
+  return false;
+#endif
+}
+
+void ParticipantUDP::PublishThingInfo(Thing* thing) {
+  // std::cout << "Publish thing info" << thing->networkId << "\n";
+  //  Strange, when publishing, the network id is irrelevant, because it is
+  //  connected to a specific site...
+  ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
+  this->Publish(thingMsg);
+  delete thingMsg;
+  NameMsg* nameMsg = new NameMsg(this->networkId, thing);
+  this->Publish(nameMsg);
+  delete nameMsg;
+  ModelUrlMsg* modelMsg = new ModelUrlMsg(this->networkId, thing);
+  this->Publish(modelMsg);
+  delete modelMsg;
+  PoseMsg* poseMsg = new PoseMsg(this->networkId, thing, true);
+  this->Publish(poseMsg);
+  delete poseMsg;
+  BinaryMsg* customMsg = new BinaryMsg(this->networkId, thing);
+  this->Publish(customMsg);
+  delete customMsg;
+}
+
+bool ParticipantUDP::Publish(IMessage* msg) {
+#if defined(_WIN32) || defined(_WIN64)
+  Windows::ParticipantUDP* thisWindows =
+      static_cast<Windows::ParticipantUDP*>(this);
+  return thisWindows->Publish(msg);
+#elif defined(__unix__) || defined(__APPLE__)
+  Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
+  return thisPosix->Publish(msg);
+#elif defined(ARDUINO)
+  Arduino::ParticipantUDP* thisArduino =
+      static_cast<Arduino::ParticipantUDP*>(this);
+  return thisArduino->Publish(msg);
+#elif defined(IDF_VER)
+  EspIdf::ParticipantUDP* thisEspIdf =
+      static_cast<EspIdf::ParticipantUDP*>(this);
+  return thisEspIdf->Publish(msg);
+#else
+  return false;
+#endif
+}
+
+// Send
+#pragma endregion
+
+#pragma region Receive
+
+void ParticipantUDP::ReceiveData(unsigned char packetSize,
+                                 char* senderIpAddress,
+                                 unsigned int senderPort) {
+  Participant* sender = this->GetParticipant(senderIpAddress, senderPort);
+  if (sender == nullptr) {
+    sender = this->AddParticipant(senderIpAddress, senderPort);
+    std::cout << "New remote participant " << sender->ipAddress << ":"
+              << sender->port << std::endl;
+  }
+
+  ReceiveData(packetSize, sender);
+}
+
+void ParticipantUDP::ReceiveData(unsigned char bufferSize,
+                                 Participant* sender) {
+  unsigned char msgId = this->buffer[0];
+  // std::cout << "receive msg " << (int)msgId << "\n";
+  // std::cout << "  buffer size = " <<(int) bufferSize << "\n";
+  switch (msgId) {
+    case ParticipantMsg::id: {
+      ParticipantMsg* msg = new ParticipantMsg(this->buffer);
+      bufferSize -= msg->length;
+      Process(sender, msg);
+      delete msg;
+    } break;
+    case SiteMsg::id: {
+      SiteMsg* msg = new SiteMsg(this->buffer);
+      bufferSize -= msg->length;
+      Process(sender, msg);
+      delete msg;
+    } break;
+    case InvestigateMsg::id: {
+      InvestigateMsg* msg = new InvestigateMsg(this->buffer);
+      Process(sender, msg);
+      delete msg;
+    } break;
+    case ThingMsg::id: {
+      ThingMsg* msg = new ThingMsg(this->buffer);
+      bufferSize -= msg->length;
+      Process(sender, msg);
+      delete msg;
+    } break;
+    case NameMsg::id: {
+      NameMsg* msg = new NameMsg(this->buffer);
+      bufferSize -= msg->length + msg->nameLength;
+      Process(sender, msg);
+      delete msg;
+    } break;
+    case ModelUrlMsg::id: {
+      ModelUrlMsg* msg = new ModelUrlMsg(this->buffer);
+      bufferSize -= msg->length + msg->urlLength;
+      Process(sender, msg);
+      delete msg;
+    } break;
+    case PoseMsg::id: {
+      PoseMsg* msg = new PoseMsg(this->buffer);
+      bufferSize -= msg->length;
+      Process(sender, msg);
+      delete msg;
+    } break;
+    case BinaryMsg::id: {
+      BinaryMsg* msg = new BinaryMsg(this->buffer);
+      bufferSize -= msg->length + msg->dataLength;
+      Process(sender, msg);
+      delete msg;
+    } break;
+  };
+
+  // Check if the buffer has been read completely
+  if (bufferSize > 0)
+    std::cout << "Buffer not fully read, remaining " << (int)bufferSize << "\n";
+}
+
+void ParticipantUDP::Process(Participant* sender, ParticipantMsg* msg) {
+#if defined(DEBUG)
+  std::cout << this->name << ": Process ParticipantMsg " << (int)msg->networkId
+            << "\n";
+#endif
+}
+
+void ParticipantUDP::Process(Participant* sender, SiteMsg* msg) {
+#if defined(DEBUG)
+  std::cout << this->name << ": process SiteMsg " << (int)this->networkId
+            << " -> " << (int)msg->networkId << "\n";
+#endif
+
+  if (this->networkId != msg->networkId) {
+    this->networkId = msg->networkId;
+    // std::cout << this->things.size() << " things\n";
+    for (Thing* thing : this->things)
+      this->SendThingInfo(sender, thing);
+  }
+}
+
+void ParticipantUDP::Process(Participant* sender, InvestigateMsg* msg) {
+#if defined(DEBUG)
+  std::cout << this->name << ": Process InvestigateMsg [" << (int)msg->networkId
+            << "/" << (int)msg->thingId << "]\n";
+#endif
+}
+
+void ParticipantUDP::Process(Participant* sender, 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
+}
+
+void ParticipantUDP::Process(Participant* sender, NameMsg* msg) {
+#if defined(DEBUG)
+  std::cout << this->name << ": process NameMsg [" << (int)msg->networkId << "/"
+            << (int)msg->thingId << "] ";
+#endif
+
+  Thing* thing = sender->Get(msg->thingId);
+  if (thing != nullptr) {
+    int nameLength = msg->nameLength;
+    int stringLen = nameLength + 1;
+    char* thingName = new char[stringLen];
+#if defined(_WIN32) || defined(_WIN64)
+    strncpy_s(thingName, stringLen, msg->name,
+              stringLen - 1);  // Leave space for null terminator
+#else
+    // Use strncpy with bounds checking for other platforms (Arduino, POSIX,
+    // ESP-IDF)
+    strncpy(thingName, msg->name,
+            nameLength);  // Leave space for null terminator
+#endif
+    thingName[nameLength] = '\0';
+    thing->name = thingName;
+
+    std::cout << thing->name;
+  }
+  std::cout << std::endl;
+}
+
+void ParticipantUDP::Process(Participant* sender, ModelUrlMsg* msg) {
+#if defined(DEBUG)
+  std::cout << this->name << ": process ModelUrlMsg [" << (int)msg->networkId
+            << "/" << (int)msg->thingId << "]\n";
+#endif
+}
+
+void ParticipantUDP::Process(Participant* sender, PoseMsg* msg) {
+#if defined(DEBUG)
+  std::cout << this->name << ": process PoseMsg [" << (int)this->networkId
+            << "/" << (int)msg->networkId << "]\n";
+#endif
+}
+
+void ParticipantUDP::Process(Participant* sender, BinaryMsg* msg) {
+#if defined(DEBUG)
+  std::cout << this->name << ": process BinaryMsg [" << (int)msg->networkId
+            << "/" << (int)msg->thingId << "] ";
+#endif
+
+  Thing* thing = sender->Get(msg->thingId);
+  if (thing != nullptr)
+    thing->ProcessBinary(msg->data);
+  else {
+    std::cout << "  unknown thing [" << (int)msg->networkId << "/"
+              << (int)msg->thingId << "]";
+  }
+  std::cout << std::endl;
+}
+
+// Receive
+#pragma endregion
+
+}  // namespace RoboidControl

Participants/ParticipantUDP.h

@@ -0,0 +1,173 @@
+#pragma once
+
+#include "Messages/BinaryMsg.h"
+#include "Messages/InvestigateMsg.h"
+#include "Messages/ModelUrlMsg.h"
+#include "Messages/NameMsg.h"
+#include "Messages/ParticipantMsg.h"
+#include "Messages/PoseMsg.h"
+#include "Messages/SiteMsg.h"
+#include "Messages/ThingMsg.h"
+#include "Participant.h"
+
+#if !defined(NO_STD)
+#include <functional>
+#include <list>
+// #include <unordered_map>
+#endif
+
+#if defined(_WIN32) || defined(_WIN64)
+#include <winsock2.h>
+#elif defined(__unix__) || defined(__APPLE__)
+#include <arpa/inet.h>
+#include <netinet/in.h>
+#include <sys/socket.h>
+#include <unistd.h>
+#endif
+
+namespace RoboidControl {
+
+constexpr int MAX_SENDER_COUNT = 256;
+
+/// @brief A local participant is the local device which can communicate with
+/// other participants It manages all local things and communcation with other
+/// participants. Each application has a local participant which is usually
+/// explicit in the code. An participant can be isolated. In that case it is
+/// standalong and does not communicate with other participants.
+///
+/// It is possible to work with an hidden participant by creating things without
+/// specifying a participant in the constructor. In that case an hidden isolated
+/// participant is created which can be obtained using
+/// RoboidControl::IsolatedParticipant::Isolated().
+/// @sa RoboidControl::Thing::Thing()
+class ParticipantUDP : public Participant {
+ public:
+  /// @brief Create a participant without connecting to a site
+  /// @param port The port on which the participant communicates
+  /// These participant typically broadcast Participant messages to let site
+  /// servers on the local network know their presence. Alternatively they can
+  /// broadcast information which can be used directly by other participants.
+  ParticipantUDP(int port = 7681);
+  /// @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
+  ParticipantUDP(const char* ipAddress,
+                   int port = 7681,
+                   int localPort = 7681);
+  // Note to self: one cannot specify the port used by the local participant
+  // now!!
+
+  /// @brief Isolated participant is used when the application is run without
+  /// networking
+  /// @return A participant without networking support
+  static ParticipantUDP* Isolated();
+
+  /// @brief True if the participant is running isolated.
+  /// Isolated participants do not communicate with other participants
+  bool isIsolated = false;
+
+  /// The interval in milliseconds for publishing (broadcasting) data on the
+  /// local network
+  long publishInterval = 3000;  // 3 seconds
+
+  /// @brief The name of the participant
+  const char* name = "ParticipantUDP";
+
+  // int localPort = 0;
+
+  /// @brief The remote site when this participant is connected to a site
+  Participant* remoteSite = nullptr;
+
+#if defined(ARDUINO)
+  // const char* remoteIpAddress = nullptr;
+  // unsigned short remotePort = 0;
+  // char* broadcastIpAddress = nullptr;
+
+  // WiFiUDP udp;
+#else
+
+#if defined(__unix__) || defined(__APPLE__)
+  int sock;
+#elif defined(_WIN32) || defined(_WIN64)
+  sockaddr_in remote_addr;
+  sockaddr_in server_addr;
+  sockaddr_in broadcast_addr;
+#endif
+
+#endif
+
+  void begin();
+  bool connected = false;
+
+  virtual void Update(unsigned long currentTimeMs = 0) override;
+
+  void SendThingInfo(Participant* remoteParticipant, Thing* thing);
+  void PublishThingInfo(Thing* thing);
+
+  bool Send(Participant* remoteParticipant, IMessage* msg);
+  bool Publish(IMessage* msg);
+
+  void ReceiveData(unsigned char bufferSize,
+                   char* senderIpAddress,
+                   unsigned int senderPort);
+  void ReceiveData(unsigned char bufferSize, Participant* remoteParticipant);
+
+#if defined(NO_STD)
+  unsigned char senderCount = 0;
+  Participant* senders[MAX_SENDER_COUNT];
+#else
+  std::list<Participant*> senders;
+#endif
+
+ protected:
+  unsigned long nextPublishMe = 0;
+
+  char buffer[1024];
+
+  void SetupUDP(int localPort, const char* remoteIpAddress, int remotePort);
+
+  Participant* GetParticipant(const char* ipAddress, int port);
+  Participant* AddParticipant(const char* ipAddress, int port);
+
+  void ReceiveUDP();
+
+  virtual void Process(Participant* sender, ParticipantMsg* msg);
+  virtual void Process(Participant* sender, SiteMsg* msg);
+  virtual void Process(Participant* sender, InvestigateMsg* msg);
+  virtual void Process(Participant* sender, ThingMsg* msg);
+  virtual void Process(Participant* sender, NameMsg* msg);
+  virtual void Process(Participant* sender, ModelUrlMsg* 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
 }
 
@@ -43,21 +43,23 @@ void SiteServer::Process(Participant* sender, ParticipantMsg* msg) {
 void SiteServer::Process(Participant* sender, SiteMsg* msg) {}
 
 void SiteServer::Process(Participant* sender, ThingMsg* msg) {
-  Thing* thing = sender->Get(msg->networkId, msg->thingId);
+  Thing* thing = sender->Get(msg->thingId);
   if (thing == nullptr) {
-#if defined(NO_STD)
-    new Thing(sender, msg->networkId, msg->thingId,
-              (Thing::Type)msg->thingType);
-#else
-    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
+    // #if defined(NO_STD)
+    new Thing(sender, (Thing::Type)msg->thingType,
+              msg->thingId);
+    // #else
+    //     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
   }
 }
 

Participants/SiteServer.h

@@ -0,0 +1,46 @@
+#pragma once
+
+#include "ParticipantUDP.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 ParticipantUDP {
+ 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

Posix/PosixParticipant.cpp

@@ -11,7 +11,7 @@
 namespace RoboidControl {
 namespace Posix {
 
-void LocalParticipant::Setup(int localPort, const char* remoteIpAddress, int remotePort) {
+void Setup(int localPort, const char* remoteIpAddress, int remotePort) {
 #if defined(__unix__) || defined(__APPLE__)
 
   // Create a UDP socket
@@ -63,7 +63,7 @@ void LocalParticipant::Setup(int localPort, const char* remoteIpAddress, int rem
 #endif
 }
 
-void LocalParticipant::Receive() {
+void Receive() {
 #if defined(__unix__) || defined(__APPLE__)
   sockaddr_in client_addr;
   socklen_t len = sizeof(client_addr);
@@ -90,7 +90,7 @@ void LocalParticipant::Receive() {
 #endif
 }
 
-bool LocalParticipant::Send(Participant* remoteParticipant, int bufferSize) {
+bool Send(Participant* remoteParticipant, int bufferSize) {
 #if defined(__unix__) || defined(__APPLE__)
   // std::cout << "Send to " << remoteParticipant->ipAddress << ":" << ntohs(remoteParticipant->port)
   //           << "\n";
@@ -113,7 +113,7 @@ bool LocalParticipant::Send(Participant* remoteParticipant, int bufferSize) {
   return true;
 }
 
-bool LocalParticipant::Publish(IMessage* msg) {
+bool Publish(IMessage* msg) {
 #if defined(__unix__) || defined(__APPLE__)
   int bufferSize = msg->Serialize(this->buffer);
   if (bufferSize <= 0)

Posix/PosixParticipant.h

@@ -1,11 +1,11 @@
 #pragma once
 
-#include "../LocalParticipant.h"
+#include "Participants/ParticipantUDP.h"
 
 namespace RoboidControl {
 namespace Posix {
 
-class LocalParticipant : public RoboidControl::LocalParticipant {
+class ParticipantUDP : public RoboidControl::ParticipantUDP {
  public:
   void Setup(int localPort, const char* remoteIpAddress, int remotePort);
   void Receive();

README.md

@@ -9,9 +9,7 @@ Supporting:
     - ESP8266
     - ESP32
     - UNO R4 WiFi
-    - ESP8266
-    - ESP32
-    - UNO R4 WiFi
+    - UNO (without networking support)
 
 # Basic components
 

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

Thing.cpp

@@ -1,15 +1,18 @@
 #include "Thing.h"
 
-#include "LocalParticipant.h"
+#include "Messages/PoseMsg.h"
+#include "Participant.h"
+#include "Participants/IsolatedParticipant.h"
 
 #include <string.h>
-// #include <algorithm>
-// #include <iostream>
-// #include <list>
-// #include <chrono>
 
 #if defined(ARDUINO)
 #include "Arduino.h"
+#else
+#include <algorithm>
+#include <chrono>
+#include <iostream>
+#include <list>
 #endif
 
 namespace RoboidControl {
@@ -20,14 +23,15 @@ namespace RoboidControl {
 //   return isolatedParticipant;
 // }
 
-Thing::Thing(int thingType) : Thing(LocalParticipant::Isolated(), thingType) {}
+Thing::Thing(int thingType)
+    : Thing(IsolatedParticipant::Isolated(), thingType) {}
 
-Thing::Thing(Participant* owner, Type thingType)
-    : Thing(owner, (unsigned char)thingType) {}
+Thing::Thing(Participant* owner, Type thingType, unsigned char thingId)
+    : Thing(owner, (unsigned char)thingType, thingId) {}
 
-Thing::Thing(Participant* owner, int thingType) {
+Thing::Thing(Participant* owner, int thingType, unsigned char thingId) {
   this->owner = owner;
-  this->id = 0;
+  this->id = thingId;
   this->type = thingType;
   this->networkId = 0;
 
@@ -37,27 +41,27 @@ Thing::Thing(Participant* owner, int thingType) {
   this->linearVelocity = Spherical::zero;
   this->angularVelocity = Spherical::zero;
 
-  // std::cout << "add thing to participant\n";
-  owner->Add(this);
+  // std::cout << "add thing [" << (int)this->id << "] to owner "
+  //           << this->owner->ipAddress << ":" << this->owner->port << std::endl;
+  this->owner->Add(this, false);
 }
 
-Thing::Thing(Participant* owner,
-             unsigned char networkId,
-             unsigned char thingId,
-             Type thingType) {
-  // no participant reference yet..
-  this->owner = owner;
-  this->networkId = networkId;
-  this->id = thingId;
-  this->type = (unsigned char)thingType;
+// Thing::Thing(Participant* owner,
+//             Type thingType,
+//              int thingId) {
+//   // no participant reference yet..
+//   this->owner = owner;
+//   this->networkId = networkId;
+//   this->id = thingId;
+//   this->type = (unsigned char)thingType;
 
-  this->linearVelocity = Spherical::zero;
-  this->angularVelocity = Spherical::zero;
-  // std::cout << "Created thing " << (int)this->networkId << "/" <<
-  // (int)this->id
-  //           << "\n";
-  owner->Add(this, false);
-}
+//   this->linearVelocity = Spherical::zero;
+//   this->angularVelocity = Spherical::zero;
+//   // std::cout << "Created thing " << (int)this->networkId << "/" <<
+//   // (int)this->id
+//   //           << "\n";
+//   owner->Add(this, false);
+// }
 
 void Thing::Terminate() {
   // Thing::Remove(this);
@@ -188,24 +192,29 @@ void Thing::Update(bool recursive) {
 }
 
 void Thing::Update(unsigned long currentTimeMs, bool recursive) {
-  (void)currentTimeMs;
+  // if (this->positionUpdated || this->orientationUpdated)
+  //   OnPoseChanged callback
+  this->positionUpdated = false;
+  this->orientationUpdated = false;
+
   if (recursive) {
     for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
       Thing* child = this->children[childIx];
       if (child == nullptr)
         continue;
       child->Update(currentTimeMs, recursive);
-}
+    }
   }
 }
 
 void Thing::UpdateThings(unsigned long currentTimeMs) {
-  LocalParticipant::Isolated()->Update(currentTimeMs);
+  IsolatedParticipant::Isolated()->Update(currentTimeMs);
 }
 
-void Thing::GenerateBinary(char* buffer, unsigned char* ix) {
+int Thing::GenerateBinary(char* buffer, unsigned char* ix) {
   (void)buffer;
   (void)ix;
+  return 0;
 }
 void Thing::ProcessBinary(char* bytes) {
   (void)bytes;

Thing.h

@@ -10,7 +10,7 @@
 namespace RoboidControl {
 
 class Participant;
-class LocalParticipant;
+class ParticipantUDP;
 
 #define THING_STORE_SIZE 256
 // IMPORTANT: values higher than 256 will need to change the Thing::id type
@@ -43,19 +43,21 @@ class Thing {
   Thing(int thingType = Type::Undetermined);
   /// @brief Create a new thing of the given type
   /// @param thingType The predefined type of thing
-  Thing(Participant* participant, Type thingType = Type::Undetermined);
+  Thing(Participant* participant,
+        Type thingType = Type::Undetermined,
+        unsigned char thingId = 0);
   /// @brief Create a new thing of the give type
   /// @param thingType The custom type of the thing
-  Thing(Participant* participant, int thingType);
+  Thing(Participant* participant, int thingType, unsigned char thingId = 0);
   /// @brief Create a new thing for the given participant
   /// @param participant The participant for which this thing is created
   /// @param networkId The network ID of the thing
   /// @param thingId The ID of the thing
   /// @param thingType The type of thing
-  Thing(Participant* participant,
-        unsigned char networkId,
-        unsigned char thingId,
-        Type thingType = Type::Undetermined);
+  // Thing(Participant* participant,
+  //       unsigned char networkId,
+  //       unsigned char thingId,
+  //       Type thingType = Type::Undetermined);
 
   /// @brief The participant managing this thing
   Participant* owner;
@@ -192,7 +194,8 @@ class Thing {
   /// @brief Function used to generate binary data for this thing
   /// @param buffer The byte array for thw binary data
   /// @param ix The starting position for writing the binary data
-  virtual void GenerateBinary(char* buffer, unsigned char* ix);
+  /// @returns The size of the binary data
+  virtual int GenerateBinary(char* buffer, unsigned char* ix);
   // /// @brief FUnction used to process binary data received for this thing
   /// @param bytes The binary data
   virtual void ProcessBinary(char* bytes);

Things/TemperatureSensor.cpp

@@ -4,22 +4,19 @@
 
 namespace RoboidControl {
 
-// TemperatureSensor::TemperatureSensor() : Thing(Type::TemperatureSensor) {}
-
-// TemperatureSensor::TemperatureSensor() : Thing(Type::TemperatureSensor) {}
-
 TemperatureSensor::TemperatureSensor(Participant* participant,
-                                     unsigned char networkId,
                                      unsigned char thingId)
-    : Thing(participant, networkId, thingId, Type::TemperatureSensor) {}
+    : Thing(participant, Type::TemperatureSensor, thingId) {}
 
 void TemperatureSensor::SetTemperature(float temp) {
   this->temperature = temp;
 }
 
-void TemperatureSensor::GenerateBinary(char* buffer, unsigned char* ix) {
+int TemperatureSensor::GenerateBinary(char* buffer, unsigned char* ix) {
+  unsigned char startIx = *ix;
   // std::cout << "Send temperature: " << this->temperature << "\n";
   LowLevelMessages::SendFloat16(buffer, ix, this->temperature);
+  return *ix - startIx;
 }
 
 void TemperatureSensor::ProcessBinary(char* bytes) {

Things/TemperatureSensor.h

@@ -15,7 +15,7 @@ class TemperatureSensor : public Thing {
   /// @brief Create a temperature sensor with the given ID
   /// @param networkId The network ID of the sensor
   /// @param thingId The ID of the thing
-  TemperatureSensor(Participant* participant, unsigned char networkId, unsigned char thingId);
+  TemperatureSensor(Participant* participant, unsigned char thingId);
 
   /// @brief Manually override the measured temperature
   /// @param temperature The new temperature
@@ -24,7 +24,7 @@ class TemperatureSensor : public Thing {
   /// @brief Function to create a binary message with the temperature
   /// @param buffer The byte array for thw binary data
   /// @param ix The starting position for writing the binary data
-  void GenerateBinary(char* bytes, unsigned char* ix) override;
+  int GenerateBinary(char* bytes, unsigned char* ix) override;
   /// @brief Function to extract the temperature received in the binary message
   /// @param bytes The binary data
   virtual void ProcessBinary(char* bytes) override;

Things/TouchSensor.cpp

@@ -10,7 +10,9 @@ TouchSensor::TouchSensor(Thing* parent) : Thing(parent->owner) {
   this->SetParent(parent);
 }
 
-void TouchSensor::GenerateBinary(char* bytes, unsigned char* ix) {}
+int TouchSensor::GenerateBinary(char* bytes, unsigned char* ix) {
+  return 0;
+}
 
 void TouchSensor::ProcessBinary(char* bytes) {
   // if (bytes[0] == 1)

Things/TouchSensor.h

@@ -25,7 +25,7 @@ class TouchSensor : public Thing {
   /// @brief Function to create a binary message with the temperature
   /// @param buffer The byte array for thw binary data
   /// @param ix The starting position for writing the binary data
-  void GenerateBinary(char* bytes, unsigned char* ix) override;
+  int GenerateBinary(char* bytes, unsigned char* ix) override;
   /// @brief Function to extract the temperature received in the binary message
   /// @param bytes The binary data
   virtual void ProcessBinary(char* bytes) override;

Windows/WindowsParticipant.cpp

@@ -4,36 +4,22 @@
 #include <winsock2.h>
 #include <ws2tcpip.h>
 #pragma comment(lib, "ws2_32.lib")
-
-#elif defined(__unix__) || defined(__APPLE__)
-#include <arpa/inet.h>
-#include <fcntl.h>  // For fcntl
-#include <netinet/in.h>
-#include <sys/socket.h>
-#include <unistd.h>
 #endif
 
 namespace RoboidControl {
 namespace Windows {
 
-void LocalParticipant::Setup(int localPort, const char* remoteIpAddress, int remotePort) {
+void ParticipantUDP::Setup(int localPort, const char* remoteIpAddress, int remotePort) {
 #if defined(_WIN32) || defined(_WIN64)
 
   // Create a UDP socket
-#if defined(_WIN32) || defined(_WIN64)
   // Windows-specific Winsock initialization
   WSADATA wsaData;
   if (WSAStartup(MAKEWORD(2, 2), &wsaData) != 0) {
     std::cerr << "WSAStartup failed" << std::endl;
     return;
   }
-#endif
-
-#if defined(_WIN32) || defined(_WIN64)
   this->sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
-#elif defined(__unix__) || defined(__APPLE__)
-  this->sock = socket(AF_INET, SOCK_DGRAM, 0);
-#endif
 
   if (this->sock < 0) {
     std::cerr << "Error creating socket" << std::endl;
@@ -41,13 +27,8 @@ void LocalParticipant::Setup(int localPort, const char* remoteIpAddress, int rem
   }
 
 // Set the socket to non-blocking mode
-#if defined(_WIN32) || defined(_WIN64)
   u_long mode = 1;  // 1 to enable non-blocking socket
   ioctlsocket(this->sock, FIONBIO, &mode);
-#elif defined(__unix__) || defined(__APPLE__)
-  int flags = fcntl(this->sock, F_GETFL, 0);
-  fcntl(this->sock, F_SETFL, flags | O_NONBLOCK);
-#endif
 
   if (remotePort != 0) {
     // Set up the address to send to
@@ -56,12 +37,8 @@ void LocalParticipant::Setup(int localPort, const char* remoteIpAddress, int rem
     remote_addr.sin_port = htons((u_short)remotePort);
     if (inet_pton(AF_INET, remoteIpAddress, &remote_addr.sin_addr) <= 0) {
       std::cerr << "Invalid address" << std::endl;
-#if defined(_WIN32) || defined(_WIN64)
       closesocket(sock);
       WSACleanup();
-#elif defined(__unix__) || defined(__APPLE__)
-      close(sock);
-#endif
       return;
     }
   }
@@ -72,30 +49,22 @@ void LocalParticipant::Setup(int localPort, const char* remoteIpAddress, int rem
   server_addr.sin_port = htons((u_short)localPort);
   if (inet_pton(AF_INET, "0.0.0.0", &server_addr.sin_addr) <= 0) {
     std::cerr << "Invalid address" << std::endl;
-#if defined(_WIN32) || defined(_WIN64)
     closesocket(sock);
     WSACleanup();
-#elif defined(__unix__) || defined(__APPLE__)
-    close(sock);
-#endif
     return;
   }
 
   // Bind the socket to the specified port
   if (bind(this->sock, (const struct sockaddr*)&server_addr, sizeof(server_addr)) < 0) {
     std::cerr << "Bind failed" << std::endl;
-#if defined(_WIN32) || defined(_WIN64)
     closesocket(sock);
     WSACleanup();
-#elif defined(__unix__) || defined(__APPLE__)
-    close(sock);
-#endif
   }
 
-#endif
+#endif // _WIN32 || _WIN64
 }
 
-void LocalParticipant::Receive() {
+void ParticipantUDP::Receive() {
 #if defined(_WIN32) || defined(_WIN64)
   // char ip_str[INET_ADDRSTRLEN];
   // inet_ntop(AF_INET, &(server_addr.sin_addr), ip_str, INET_ADDRSTRLEN);
@@ -103,28 +72,20 @@ void LocalParticipant::Receive() {
   //           << ntohs(server_addr.sin_port) << "\n";
 
   sockaddr_in client_addr;
-#if defined(_WIN32) || defined(_WIN64)
   int len = sizeof(client_addr);
-#elif defined(__unix__) || defined(__APPLE__)
-  socklen_t len = sizeof(client_addr);
-#endif
   int packetSize = recvfrom(this->sock, buffer, sizeof(buffer), 0, (struct sockaddr*)&client_addr, &len);
   // std::cout << "received data " << packetSize << "\n";
   if (packetSize < 0) {
-#if defined(_WIN32) || defined(_WIN64)
     int error_code = WSAGetLastError();  // Get the error code on Windows
     if (error_code != WSAEWOULDBLOCK)
       std::cerr << "recvfrom failed with error: " << error_code << std::endl;
-#else
-    // std::cerr << "recvfrom failed with error: " << packetSize << std::endl;
-#endif
   } else if (packetSize > 0) {
     char sender_ipAddress[INET_ADDRSTRLEN];
     inet_ntop(AF_INET, &(client_addr.sin_addr), sender_ipAddress, INET_ADDRSTRLEN);
     unsigned int sender_port = ntohs(client_addr.sin_port);
 
     ReceiveData(packetSize, sender_ipAddress, sender_port);
-    // RoboidControl::LocalParticipant* remoteParticipant = this->GetParticipant(sender_ipAddress, sender_port);
+    // RoboidControl::ParticipantUDP* remoteParticipant = this->GetParticipant(sender_ipAddress, sender_port);
     // if (remoteParticipant == nullptr) {
     //   remoteParticipant = this->AddParticipant(sender_ipAddress, sender_port);
     //   // std::cout << "New sender " << sender_ipAddress << ":"
@@ -138,16 +99,16 @@ void LocalParticipant::Receive() {
 
     // ReceiveData(packetSize, remoteParticipant);
   }
-#endif
+#endif // _WIN32 || _WIN64
 }
 
-bool LocalParticipant::Send(Participant* remoteParticipant, int bufferSize) {
+bool ParticipantUDP::Send(Participant* 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);
   std::cout << "Send to " << ip_str << ":" << ntohs(remote_addr.sin_port) << "\n";
   int sent_bytes = sendto(sock, this->buffer, bufferSize, 0, (struct sockaddr*)&remote_addr, sizeof(remote_addr));
-#if defined(_WIN32) || defined(_WIN64)
+
   if (sent_bytes <= SOCKET_ERROR) {
     int error_code = WSAGetLastError();  // Get the error code on Windows
     std::cerr << "sendto failed with error: " << error_code << std::endl;
@@ -155,18 +116,11 @@ bool LocalParticipant::Send(Participant* remoteParticipant, int bufferSize) {
     WSACleanup();
     return false;
   }
-#elif defined(__unix__) || defined(__APPLE__)
-  if (sent_bytes < 0) {
-    std::cerr << "sendto failed with error: " << sent_bytes << std::endl;
-    close(sock);
-    return false;
-  }
-#endif
-#endif
+#endif // _WIN32 || _WIN64
   return true;
 }
 
-bool LocalParticipant::Publish(IMessage* msg) {
+bool ParticipantUDP::Publish(IMessage* msg) {
 #if defined(_WIN32) || defined(_WIN64)
   int bufferSize = msg->Serialize(this->buffer);
   if (bufferSize <= 0)
@@ -176,7 +130,7 @@ bool LocalParticipant::Publish(IMessage* msg) {
   inet_ntop(AF_INET, &(broadcast_addr.sin_addr), ip_str, INET_ADDRSTRLEN);
   std::cout << "Publish to " << ip_str << ":" << ntohs(broadcast_addr.sin_port) << "\n";
   int sent_bytes = sendto(sock, this->buffer, bufferSize, 0, (struct sockaddr*)&broadcast_addr, sizeof(broadcast_addr));
-#if defined(_WIN32) || defined(_WIN64)
+
   if (sent_bytes <= SOCKET_ERROR) {
     int error_code = WSAGetLastError();  // Get the error code on Windows
     std::cerr << "sendto failed with error: " << error_code << std::endl;
@@ -184,14 +138,7 @@ bool LocalParticipant::Publish(IMessage* msg) {
     WSACleanup();
     return false;
   }
-#elif defined(__unix__) || defined(__APPLE__)
-  if (sent_bytes < 0) {
-    std::cerr << "sendto failed with error: " << sent_bytes << std::endl;
-    close(sock);
-    return false;
-  }
-#endif
-#endif
+#endif // _WIN32 || _WIN64
   return true;
 }
 

Windows/WindowsParticipant.h

@@ -1,11 +1,11 @@
 #pragma once
 
-#include "../LocalParticipant.h"
+#include "Participants/ParticipantUDP.h"
 
 namespace RoboidControl {
 namespace Windows {
 
-class LocalParticipant : public RoboidControl::LocalParticipant {
+class ParticipantUDP : public RoboidControl::ParticipantUDP {
  public:
   void Setup(int localPort, const char* remoteIpAddress, int remotePort);
   void Receive();

test/participant_test.cc

@@ -9,7 +9,7 @@
 // #include <ws2tcpip.h>
 
 #include "Participant.h"
-#include "SiteServer.h"
+#include "Participants/SiteServer.h"
 #include "Thing.h"
 
 using namespace RoboidControl;
@@ -52,8 +52,8 @@ protected:
 
 
 
-// TEST_F(ParticipantSuite, LocalParticipant) {
-//   LocalParticipant* participant = new LocalParticipant("127.0.0.1", 7681);
+// TEST_F(ParticipantSuite, ParticipantUDP) {
+//   ParticipantUDP* participant = new ParticipantUDP("127.0.0.1", 7681);
 
 //   unsigned long milliseconds = get_time_ms();
 //   unsigned long startTime = milliseconds;
@@ -80,7 +80,7 @@ protected:
 
 // TEST_F(ParticipantSuite, SiteParticipant) {
 //   SiteServer site = SiteServer(7681);
-//   LocalParticipant participant = LocalParticipant("127.0.0.1", 7681);
+//   ParticipantUDP participant = ParticipantUDP("127.0.0.1", 7681);
 
 //   unsigned long milliseconds = get_time_ms();
 //   unsigned long startTime = milliseconds;
@@ -96,7 +96,7 @@ protected:
 
 // TEST_F(ParticipantSuite, Thing) {
 //   SiteServer site = SiteServer(7681);
-//   LocalParticipant participant = LocalParticipant("127.0.0.1", 7681);
+//   ParticipantUDP participant = ParticipantUDP("127.0.0.1", 7681);
 //   Thing thing = Thing(&participant);
 
 //   unsigned long milliseconds = get_time_ms();

test/thing_test.cc

@@ -4,7 +4,7 @@
 // not supported using Visual Studio 2022 compiler...
 #include <gtest/gtest.h>
 
-#include "LocalParticipant.h"
+#include "Participants/ParticipantUDP.h"
 #include "Thing.h"
 
 using namespace RoboidControl;
@@ -23,7 +23,7 @@ TEST(RoboidControlSuite, HiddenParticipant) {
 }
 
 TEST(RoboidControlSuite, IsolatedParticipant) {
-  LocalParticipant* participant = LocalParticipant::Isolated();
+  ParticipantUDP* participant = ParticipantUDP::Isolated();
   Thing* thing = new Thing(participant);
 
   unsigned long milliseconds = Thing::GetTimeMs();