server still rejects this

Merge branch '0.4_dev' into MQTT and fixed compatibility
Improvements
2025-06-29 13:01:31 +02:00 · 2025-06-29 11:42:46 +02:00 · 2025-06-28 09:15:16 +02:00 · 2025-06-27 17:45:52 +02:00 · 2025-06-27 17:12:42 +02:00 · 2025-06-27 15:00:54 +02:00
55 changed files with 2000 additions and 746 deletions
--- a/Arduino/ArduinoParticipant.cpp
+++ b/Arduino/ArduinoParticipant.cpp
@ -1,4 +1,5 @@
 #include "ArduinoParticipant.h"
+#if defined(ARDUINO)

 #if !defined(NO_STD)
 #include <iostream>
@ -21,7 +22,6 @@
 #endif

 namespace RoboidControl {
-namespace Arduino {

 #if defined(ARDUINO) && defined(HAS_WIFI)
 WiFiUDP* udp;
@ -88,7 +88,7 @@ void ParticipantUDP::Receive() {
 #endif
 }

-bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
+bool ParticipantUDP::SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize) {
 #if defined(ARDUINO) && defined(HAS_WIFI)
  // std::cout << "Sending to:\n " << remoteParticipant->ipAddress << ":"
  // << remoteParticipant->port << "\n";
@ -131,5 +131,5 @@ bool ParticipantUDP::Publish(IMessage* msg) {
  return true;
 };

-}  // namespace Arduino
 }  // namespace RoboidControl
+#endif
--- a/Arduino/ArduinoParticipant.h
+++ b/Arduino/ArduinoParticipant.h
@ -1,15 +1,15 @@
 #pragma once
+#if defined(ARDUINO)

 #include "Participants/ParticipantUDP.h"

 namespace RoboidControl {
-namespace Arduino {

-class ParticipantUDP : public RoboidControl::ParticipantUDP {
+class ParticipantUDP : public ParticipantUDPGeneric {
 public:
  void Setup();
  void Receive();
-  bool Send(Participant* remoteParticipant, int bufferSize);
+  bool SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize);
  bool Publish(IMessage* msg);

 protected:
@ -18,5 +18,5 @@ class ParticipantUDP : public RoboidControl::ParticipantUDP {
  void GetBroadcastAddress();
 };

-}  // namespace Arduino
 }  // namespace RoboidControl
+#endif
--- a/Arduino/Examples/BB2A/BB2A.cpp
+++ b/Arduino/Examples/BB2A/BB2A.cpp
@ -0,0 +1,95 @@
+#include "Arduino.h"
+
+#include "Things/ControlledMotor.h"
+#include "Things/DifferentialDrive.h"
+#include "Things/RelativeEncoder.h"
+#include "Things/TouchSensor.h"
+
+#include "Arduino/Things/DRV8833.h"
+#include "Arduino/Things/DigitalInput.h"
+#include "Arduino/Things/UltrasonicSensor.h"
+#include "Arduino/ArduinoUtils.h"
+
+#include "Participants/ParticipantUDP.h"
+
+#include "configuration.h"
+
+#include <iostream>
+
+using namespace RoboidControl;
+using namespace RoboidControl::Arduino;
+
+ParticipantUDPGeneric* localParticipant;
+
+DifferentialDrive* bb2b;
+TouchSensor* touchLeft;
+TouchSensor* touchRight;
+
+// RelativeEncoder* encoderLeft;
+// RelativeEncoder* encoderRight;
+
+void setup() {
+  Serial.begin(115200);
+  delay(3000);
+  Serial.println("started");
+
+  StartWifi("serrarens", "192.168.76.44");
+  localParticipant = new ParticipantUDPGeneric("192.168.77.76");
+
+  bb2b = new DifferentialDrive();
+  touchLeft = new TouchSensor(bb2b);
+  touchRight = new TouchSensor(bb2b);
+  
+  // bb2b = new DRV8833::DifferentialDrive(driveConfig);
+  // touchLeft = new UltrasonicSensor::TouchSensor(leftTouchConfig, bb2b);
+  // touchRight = new UltrasonicSensor::TouchSensor(rightTouchConfig, bb2b);
+
+  touchLeft->name = "Left Touch Sensor";
+  touchLeft->SetPosition(Spherical::Degrees(0.15, -30, 0));
+  touchRight->name = "Right Touch Sensor";
+  touchRight->SetPosition(Spherical::Degrees(0.15, 30, 0));
+
+  // encoderLeft = new DigitalInput::RelativeEncoder(leftEncoderConfig);
+  // encoderRight = new DigitalInput::RelativeEncoder(rightEncoderConfig);
+
+  // bb2b->leftWheel = new ControlledMotor(bb2b->leftWheel, encoderLeft, bb2b);
+  // bb2b->rightWheel = new ControlledMotor(bb2b->rightWheel, encoderRight, bb2b);
+}
+
+void loop() {
+  
+  // std::cout << touchLeft->touchedSomething << " | "
+  //           << touchRight->touchedSomething << std::endl;
+  // std::cout << encoderLeft->rotationSpeed << " : "
+  //           << encoderRight->rotationSpeed << std::endl;
+  // std::cout << bb2b->leftWheel->encoder->rotationSpeed
+  //           << " :: " << bb2b->rightWheel->encoder->rotationSpeed << std::endl;
+
+  
+  // The left wheel turns forward when nothing is touched on the right side
+  // and turn backward when the roboid hits something on the right
+  float leftMotorVelocity = (touchRight->IsTouching()) ? -1.0f : 1.0f;
+  // The right wheel does the same, but instead is controlled by
+  // touches on the left side
+  float rightMotorVelocity = (touchLeft->IsTouching()) ? -1.0f : 1.0f;
+  // When both sides are touching something, both wheels will turn backward
+  // and the roboid will move backwards
+
+  bb2b->leftWheel->SetTargetVelocity(leftMotorVelocity);
+  bb2b->rightWheel->SetTargetVelocity(rightMotorVelocity);
+  // std::cout << "  " << bb2b->leftWheel->GetTargetVelocity() << " : " << bb2b->rightWheel->GetTargetVelocity() << std::endl;
+
+  // float leftWheelVelocity = (touchRight->touchedSomething) ? -1.0f : 1.0f;
+  // float rightWheelVelocity = (touchLeft->touchedSomething) ? -1.0f : 1.0f;
+  // bb2b->SetWheelVelocity(leftWheelVelocity, rightWheelVelocity);
+  // std::cout << "  " << leftWheelVelocity << " # " << rightWheelVelocity << std::endl;
+
+  // std::cout << leftMotor->actualVelocity << std::endl;
+  
+  //Serial.println(".");
+  // Update the roboid state
+  localParticipant->Update();
+
+  // and sleep for 100ms
+  delay(10);
+}
--- a/Arduino/Examples/BB2A/configuration.h
+++ b/Arduino/Examples/BB2A/configuration.h
@ -0,0 +1,52 @@
+#pragma once
+
+#include "Arduino/Things/UltrasonicSensor.h"
+#include "Arduino/Things/DRV8833.h"
+#include "Arduino/Things/DigitalInput.h"
+
+using namespace RoboidControl::Arduino;
+
+#if defined(ESP32)
+static constexpr DRV8833::Configuration driveConfig = {
+    17,  // AIn1
+    16,  // AIn2
+    14,  // BIn1
+    27   // BIn2
+};
+static constexpr UltrasonicSensor::Configuration leftTouchConfig = {
+    25,  // Trigger
+    33   // Echo
+};
+static constexpr UltrasonicSensor::Configuration rightTouchConfig = {
+    15,  // Trigger
+    5    // Echo
+};
+
+#elif defined(UNO) || defined(UNO_R4)
+static constexpr DRV8833::Configuration driveConfig = {
+    5,  // AIn1
+    6,  // AIn2
+    7,  // BIn1
+    10  // BIn2
+};
+
+static constexpr UltrasonicSensor::Configuration leftTouchConfig = {
+    A0,  // Trigger
+    12   // Echo
+};
+static constexpr UltrasonicSensor::Configuration rightTouchConfig = {
+    A1,  // Trigger
+    11   // Echo
+};
+
+static constexpr DigitalInput::RelativeEncoder::Configuration
+    leftEncoderConfig = {
+        2,  // Input pin
+        80  // Pulses per revolution
+};
+static constexpr DigitalInput::RelativeEncoder::Configuration
+    rightEncoderConfig = {
+        3,  // Input pin
+        80  // Pulses per revolution
+};
+#endif
--- a/Arduino/Things/DRV8833.cpp
+++ b/Arduino/Things/DRV8833.cpp
@ -7,7 +7,9 @@ namespace Arduino {

 #pragma region DRV8833

-DRV8833::DRV8833(Configuration config, Thing* parent) : Thing(Type::Undetermined, parent) {
+DRV8833::DRV8833(Configuration config, Thing* parent) : Thing(parent) {
+  this->type = Type::Undetermined;
+  this->name = "DRV8833";
  this->pinStandby = config.standby;
  if (pinStandby != 255)
    pinMode(pinStandby, OUTPUT);
@ -24,14 +26,15 @@ DRV8833::DRV8833(Configuration config, Thing* parent) : Thing(Type::Undetermined

 DRV8833::DifferentialDrive::DifferentialDrive(DRV8833::Configuration config,
                                              Thing* parent)
-    : RoboidControl::DifferentialDrive(this->drv8833.motorA,
-                                       this->drv8833.motorB,
-                                       parent),
-      drv8833(config, this) {}
+    : RoboidControl::DifferentialDrive(nullptr, nullptr, parent) {
+  this->drv8833 = new DRV8833(config, this);
+  this->leftWheel = this->drv8833->motorA;
+  this->rightWheel = this->drv8833->motorB;
+}

 void DRV8833::DifferentialDrive::Update(bool recurse) {
  RoboidControl::DifferentialDrive::Update(recurse);
-  this->drv8833.Update(false);
+  this->drv8833->Update(false);
 }

 #pragma endregion Differential drive
@ -46,8 +49,7 @@ DRV8833Motor::DRV8833Motor(DRV8833* driver,
                           unsigned char pinIn1,
                           unsigned char pinIn2,
                           bool reverse)
-    : Motor() {
-  this->SetParent(driver);
+    : Motor(driver) {

  this->pinIn1 = pinIn1;
  this->pinIn2 = pinIn2;
--- a/Arduino/Things/DRV8833.h
+++ b/Arduino/Things/DRV8833.h
@ -19,6 +19,8 @@ class DRV8833 : public Thing {
    int BIn1;
    int BIn2;
    int standby = 255;
+
+    constexpr Configuration(int a1, int a2, int b1, int b2, int standby = 255) : AIn1(a1), AIn2(a2), BIn1(b1), BIn2(b2), standby(standby) {}
  };

  /// @brief Setup a DRV8833 motor controller
@ -43,7 +45,7 @@ class DRV8833::DifferentialDrive : public RoboidControl::DifferentialDrive {
  virtual void Update(bool recurse = false) override;

 protected:
-  DRV8833 drv8833;
+  DRV8833* drv8833 = nullptr;
 };

 #pragma endregion Differential drive
--- a/Arduino/Things/DigitalInput.cpp
+++ b/Arduino/Things/DigitalInput.cpp
@ -7,8 +7,9 @@ namespace Arduino {

 #pragma region Digital input

-DigitalInput::DigitalInput(unsigned char pin, Thing* parent)
-    : Thing(Type::Undetermined, parent) {
+DigitalInput::DigitalInput(unsigned char pin, Thing* parent) : Thing(parent) {
+  this->type = Type::Switch;
+  this->name = "Digital Input";
  this->pin = pin;
  pinMode(this->pin, INPUT);
  std::cout << "digital input start\n";
@ -28,7 +29,7 @@ DigitalInput::TouchSensor::TouchSensor(unsigned char pin, Thing* parent)
    : RoboidControl::TouchSensor(parent), digitalInput(pin, parent) {}

 void DigitalInput::TouchSensor::Update(bool recursive) {
-  this->touchedSomething = digitalInput.isLow;
+  this->internalTouch = digitalInput.isLow;
 }

 #pragma endregion Touch sensor
@ -97,8 +98,8 @@ void DigitalInput::RelativeEncoder::Update(bool recursive) {
  this->pulseFrequency = pulseCount / timeStep;
  this->rotationSpeed = pulseFrequency / pulsesPerRevolution;

-  std::cout << "pulses: " << pulseCount << " per second " << pulseFrequency
-            << " timestep " << timeStep << std::endl;
+  // std::cout << "pulses: " << pulseCount << " per second " << pulseFrequency
+  //           << " timestep " << timeStep << std::endl;

  this->lastUpdateTime = currentTimeMs;
 }
--- a/Arduino/Things/UltrasonicSensor.cpp
+++ b/Arduino/Things/UltrasonicSensor.cpp
@ -7,7 +7,8 @@ namespace RoboidControl {
 namespace Arduino {

 UltrasonicSensor::UltrasonicSensor(Configuration config, Thing* parent)
-    : Thing(Type::Undetermined, parent) {
+    : Thing(parent) {
+  this->type = Type::DistanceSensor;
  this->name = "Ultrasonic sensor";
  this->pinTrigger = config.trigger;
  this->pinEcho = config.echo;
@ -56,6 +57,28 @@ void UltrasonicSensor::Update(bool recursive) {
  Thing::Update(recursive);
 }

+#pragma region Distance sensor
+
+UltrasonicSensor::DistanceSensor::DistanceSensor(
+    UltrasonicSensor::Configuration config,
+    Thing* parent)
+    : RoboidControl::DistanceSensor(parent), ultrasonic(config, this) {}
+
+void UltrasonicSensor::DistanceSensor::Update(bool recursive) {
+  RoboidControl::DistanceSensor::Update(recursive);
+  this->ultrasonic.Update(false);
+  if (this->ultrasonic.distance > 0)
+    this->internalDistance = this->ultrasonic.distance;
+  else
+#if ARDUNIO
+    this->internalDistance = INFINITY;
+#else
+    this->internalDistance = std::numeric_limits<float>::infinity();
+#endif
+}
+
+#pragma endregion Distance sensor
+
 #pragma region Touch sensor

 UltrasonicSensor::TouchSensor::TouchSensor(Configuration config, Thing* parent)
@ -64,11 +87,11 @@ UltrasonicSensor::TouchSensor::TouchSensor(Configuration config, Thing* parent)
 void UltrasonicSensor::TouchSensor::Update(bool recursive) {
  RoboidControl::TouchSensor::Update(recursive);
  this->ultrasonic.Update(false);
-  this->touchedSomething |= (this->ultrasonic.distance > 0 &&
-                            this->ultrasonic.distance <= this->touchDistance);
+  this->internalTouch = (this->ultrasonic.distance > 0 &&
+                         this->ultrasonic.distance <= this->touchDistance);
 }

-#pragma region Touch sensor
+#pragma endregion Touch sensor

 }  // namespace Arduino
 }  // namespace RoboidControl
--- a/Arduino/Things/UltrasonicSensor.h
+++ b/Arduino/Things/UltrasonicSensor.h
@ -1,5 +1,6 @@
 #pragma once

+#include "Things/DistanceSensor.h"
 #include "Things/TouchSensor.h"

 namespace RoboidControl {
@ -39,9 +40,26 @@ class UltrasonicSensor : Thing {
  unsigned char pinEcho = 0;

 public:
+  class DistanceSensor;
  class TouchSensor;
 };

+#pragma region Distance sensor
+
+class UltrasonicSensor::DistanceSensor : public RoboidControl::DistanceSensor {
+ public:
+  DistanceSensor(UltrasonicSensor::Configuration config,
+                 Thing* parent = Thing::LocalRoot());
+
+  /// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs)
+  virtual void Update(bool recursive = false) override;
+
+ protected:
+  UltrasonicSensor ultrasonic;
+};
+
+#pragma endregion Distance sensor
+
 #pragma region Touch sensor

 class UltrasonicSensor::TouchSensor : public RoboidControl::TouchSensor {
@ -58,7 +76,7 @@ class UltrasonicSensor::TouchSensor : public RoboidControl::TouchSensor {
  UltrasonicSensor ultrasonic;
 };

-#pragma region Touch sensor
+#pragma endregion Touch sensor

 }  // namespace Arduino
 }  // namespace RoboidControl
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@ -22,43 +22,23 @@ else()
    set(CMAKE_CXX_STANDARD 17)      # Enable c++11 standard
    set(CMAKE_POSITION_INDEPENDENT_CODE ON)

-    project(RoboidControl)
-    add_subdirectory(LinearAlgebra)
-    add_subdirectory(Examples)
-
-    add_compile_definitions(GTEST)
-    include(FetchContent)
-    FetchContent_Declare(
-        googletest
-        DOWNLOAD_EXTRACT_TIMESTAMP ON
-        URL https://github.com/google/googletest/archive/refs/heads/main.zip
-    )
-
-    # For Windows: Prevent overriding the parent project's compiler/linker settings
-    set(gtest_force_shared_crt ON CACHE BOOL "" FORCE)
-    FetchContent_MakeAvailable(googletest)
-
    include_directories(
        .
        LinearAlgebra
    )
    add_library(RoboidControl STATIC ${srcs})
-    
+    target_include_directories(RoboidControl PUBLIC
+        ${CMAKE_CURRENT_SOURCE_DIR}
+    )
+
+    project(RoboidControl)
+    add_subdirectory(LinearAlgebra)
+
+    # Examples
+    option(BUILD_EXAMPLE_BB2A "Build BB2A Example" OFF)
+    add_subdirectory(examples)
+
    enable_testing()
+    add_subdirectory(test)

-    file(GLOB_RECURSE test_srcs test/*_test.cc)
-    add_executable(
-        RoboidControlTest
-        ${test_srcs}
-    )
-    target_link_libraries(
-        RoboidControlTest
-        gtest_main
-        RoboidControl
-        LinearAlgebra
-    )
-
-    include(GoogleTest)
-    gtest_discover_tests(RoboidControlTest)
 endif()
-
--- a/DoxyGen/Doxyfile
+++ b/DoxyGen/Doxyfile
@ -48,7 +48,7 @@ PROJECT_NAME           = "Roboid Control for C++"
 # could be handy for archiving the generated documentation or if some version
 # control system is used.

-PROJECT_NUMBER         =
+PROJECT_NUMBER         = 0.4

 # Using the PROJECT_BRIEF tag one can provide an optional one line description
 # for a project that appears at the top of each page and should give viewer a
--- a/EspIdf/EspIdfParticipant.cpp
+++ b/EspIdf/EspIdfParticipant.cpp
@ -2,15 +2,13 @@

 #if defined(IDF_VER)
 #include "esp_wifi.h"
-#endif

+#include <arpa/inet.h>
 namespace RoboidControl {
-namespace EspIdf {

-void ParticipantUDP::Setup(int localPort,
+void ParticipantUDP::SetupUDP(int localPort,
                           const char* remoteIpAddress,
                           int remotePort) {
-#if defined(IDF_VER)
  std::cout << "Set up UDP\n";
  GetBroadcastAddress();

@ -22,42 +20,57 @@ void ParticipantUDP::Setup(int localPort,
  }

  // Create a UDP socket
-  this->sockfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
-  if (this->sockfd < 0) {
+  this->sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
+  if (this->sock < 0) {
    std::cout << "Unable to create UDP socket: errno " << errno << "\n";
    vTaskDelete(NULL);
    return;
  }

-  // Set up the server address structure
+  /*
+  // Set up the receiving(local) address
  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_port = htons(localPort);
  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) {
+  // Bind the socket to the receiving address
+  if (bind(this->sock, (struct sockaddr*)&local_addr, sizeof(local_addr)) < 0) {
    std::cout << "Unable to bind UDP socket: errno " << errno << "\n";
-    close(sockfd);
+    close(sock);
    vTaskDelete(NULL);
    return;
  }
+    */

-  // Initialize the dest_addr structure
+  // Initialize the destination(remote) address
  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);
+  this->dest_addr.sin_addr.s_addr = inet_addr(this->remoteSite->ipAddress);
+  // inet_pton(AF_INET, this->remoteSite->ipAddress,
+  //           &this->dest_addr.sin_addr.s_addr);

-  std::cout << "Wifi sync started local " << this->port << ", remote "
+    this->connected = true;
+
+  std::cout << "Wifi sync started local " << localPort << ", remote "
            << this->remoteSite->ipAddress << ":" << this->remoteSite->port
            << "\n";
-#endif  // IDF_VER
+
+  // std::cout << "socket: " << (int)this->sock << std::endl;
+  // ParticipantMsg* msg = new ParticipantMsg(this->networkId);
+  // int bufferSize = msg->Serialize(this->buffer);
+  // int err = sendto(this->sock, buffer, bufferSize, 0,
+  //                  (struct sockaddr*)&dest_addr, sizeof(dest_addr));
+  // if (errno != 0)
+  //   std::cout << "AASend error " << err << " or " << errno << "\n";
+  // else
+  //   std::cout << "AASend SUCCESS\n";
+
+  //SendTest();
 }

 void ParticipantUDP::GetBroadcastAddress() {
@ -83,10 +96,11 @@ void ParticipantUDP::GetBroadcastAddress() {
 #endif  // IDF_VER
 }

-void ParticipantUDP::Receive() {
+void ParticipantUDP::ReceiveUDP() {
 #if defined(IDF_VER)
+/*
  struct pollfd fds[1];
-  fds[0].fd = sockfd;
+  fds[0].fd = sock;
  fds[0].events = POLLIN;  // We're looking for data available to read

  // Use poll() with a timeout of 0 to return immediately
@ -103,7 +117,7 @@ void ParticipantUDP::Receive() {
  char sender_ipAddress[INET_ADDRSTRLEN];

  while (ret > 0 && fds[0].revents & POLLIN) {
-    int packetSize = recvfrom(this->sockfd, buffer, sizeof(buffer) - 1, 0,
+    int packetSize = recvfrom(this->sock, buffer, sizeof(buffer) - 1, 0,
                              (struct sockaddr*)&source_addr, &addr_len);
    if (packetSize < 0) {
      std::cout << "recvfrom() error\n";
@ -126,18 +140,66 @@ void ParticipantUDP::Receive() {
    }
  }
  // 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";
+ParticipantUDP::ParticipantUDP(int port) : ParticipantUDPGeneric(port) {}

-  int err = sendto(this->sockfd, buffer, bufferSize, 0,
-                   (struct sockaddr*)&dest_addr, sizeof(dest_addr));
-  if (errno != 0)
+ParticipantUDP::ParticipantUDP(const char* ipAddress, int port, int localPort)
+    : ParticipantUDPGeneric(ipAddress, port, localPort) {}
+
+// bool ParticipantUDP::SendTest() {
+// #if defined(IDF_VER)
+//   // std::cout << "socket: " << (int)this->sock << std::endl;
+//   // UBaseType_t stack_size = uxTaskGetStackHighWaterMark(NULL);  // NULL to check the main task
+//   // size_t free_heap = xPortGetFreeHeapSize();
+//   // std::cout <<  "Stack High Water Mark: " << stack_size << " heap " << free_heap << std::endl;
+
+//   ParticipantMsg* msg = new ParticipantMsg(this->networkId);
+//   int bSize = msg->Serialize(this->buffer);
+//   // std::cout << "buffer size " << bSize << std::endl;
+//   int err = sendto(this->sock, buffer, bSize, 0, (struct sockaddr*)&dest_addr,
+//                    sizeof(dest_addr));
+//   if (errno != 0)
+//     std::cout << "BBSend error " << err << " or " << errno << "\n";
+//   else
+//     std::cout << "BBSend SUCCESS\n";
+
+// #endif
+//   return true;
+// }
+
+bool ParticipantUDP::Send(IMessage* msg) {
+  int bufferSize = msg->Serialize(this->buffer);
+  if (bufferSize <= 0)
+    return true;
+
+  std::cout << "send msg " << (static_cast<int>(this->buffer[0]) & 0xff)
+            << " to " << this->remoteSite->ipAddress << std::endl;
+
+  return this->SendTo(this->remoteSite, bufferSize);
+}
+
+bool ParticipantUDP::SendTo(RemoteParticipantUDP* remoteParticipant,
+                            int bufferSize) {
+#if defined(IDF_VER)
+  uint16_t port = ntohs(dest_addr.sin_port);
+
+  char ip_str[INET_ADDRSTRLEN];
+  inet_ntop(AF_INET, &dest_addr.sin_addr, ip_str, sizeof(ip_str));
+  std::cout << "Sending " << bufferSize << " bytes to " << ip_str << ":" << port << "\n";
+
+  // Print the IP address and port
+  // printf("IP Address: %s\n", ip_str);
+  // printf("Port: %d\n", port);
+
+  this->dest_addr.sin_port = htons(remoteParticipant->port);
+  this->dest_addr.sin_addr.s_addr = inet_addr(remoteParticipant->ipAddress);
+
+  int err = sendto(this->sock, buffer, bufferSize, 0,
+                   (struct sockaddr*)&this->dest_addr, sizeof(this->dest_addr));
+  if (errno < 0)
    std::cout << "Send error " << err << " or " << errno << "\n";

 #endif
@ -154,7 +216,7 @@ bool ParticipantUDP::Publish(IMessage* msg) {
  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,
+  int err = sendto(sock, buffer, bufferSize, 0, (struct sockaddr*)&dest_addr,
                   sizeof(dest_addr));
  if (err != 0)
    std::cout << "Publish error\n";
@ -162,5 +224,6 @@ bool ParticipantUDP::Publish(IMessage* msg) {
  return true;
 };

-}  // namespace EspIdf
 }  // namespace RoboidControl
+
+#endif
--- a/EspIdf/EspIdfParticipant.h
+++ b/EspIdf/EspIdfParticipant.h
@ -1,26 +1,47 @@
 #pragma once
+#if defined(IDF_VER)

 #include "Participants/ParticipantUDP.h"

-#if defined(IDF_VER)
 #include "lwip/sockets.h"
-#endif

 namespace RoboidControl {
-namespace EspIdf {

-class ParticipantUDP : public RoboidControl::ParticipantUDP {
+class ParticipantUDP : public ParticipantUDPGeneric {
 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
+  /// @param localPort The port used by the local participant
+  ParticipantUDP(const char* ipAddress, int port = 7681, int localPort = 7681);
+
  void Setup(int localPort, const char* remoteIpAddress, int remotePort);
  void Receive();
-  bool Send(Participant* remoteParticipant, int bufferSize);
+  bool SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize);
  bool Publish(IMessage* msg);

+  // bool SendTest();
+
+  /// @brief Sens a message to the remote site (if set)
+  /// @param msg The message to send
+  /// @return True if a message could be sent.
+  bool Send(IMessage* msg) override;
+  void SetupUDP(int localPort,
+                const char* remoteIpAddress,
+                int remotePort) override;
+  void ReceiveUDP() override;
+
 protected:
 #if defined(IDF_VER)
  char broadcastIpAddress[INET_ADDRSTRLEN];

-  int sockfd;
+  int sock;
  struct sockaddr_in dest_addr;
  // struct sockaddr_in src_addr;
 #endif
@ -28,5 +49,6 @@ class ParticipantUDP : public RoboidControl::ParticipantUDP {
  void GetBroadcastAddress();
 };

-}  // namespace EspIdf
 }  // namespace RoboidControl
+
+#endif
--- a/Messages/LowLevelMessages.cpp
+++ b/Messages/LowLevelMessages.cpp
@ -5,6 +5,83 @@

 namespace RoboidControl {

+void LowLevelMessages::SendSpherical(char* buffer,
+                                     unsigned char* ix,
+                                     Spherical s) {
+  SendFloat16(buffer, ix, s.distance);
+  SendAngle8(buffer, ix, s.direction.horizontal.InDegrees());
+  SendAngle8(buffer, ix, s.direction.vertical.InDegrees());
+}
+Spherical LowLevelMessages::ReceiveSpherical(const char* buffer,
+                                             unsigned char* startIndex) {
+  float distance = ReceiveFloat16(buffer, startIndex);
+
+  Angle8 horizontal8 = ReceiveAngle8(buffer, startIndex);
+  Angle horizontal = Angle::Radians(horizontal8.InRadians());
+
+  Angle8 vertical8 = ReceiveAngle8(buffer, startIndex);
+  Angle vertical = Angle::Radians(vertical8.InRadians());
+
+  Spherical s = Spherical(distance, horizontal, vertical);
+  return s;
+}
+
+void LowLevelMessages::SendSwingTwist(char* buffer,
+                                      unsigned char* ix,
+                                      SwingTwist s) {
+  SendAngle8(buffer, ix, s.swing.horizontal.InDegrees());
+  SendAngle8(buffer, ix, s.swing.vertical.InDegrees());
+  SendAngle8(buffer, ix, s.twist.InDegrees());
+}
+
+SwingTwist LowLevelMessages::ReceiveSwingTwist(const char* buffer,
+                                               unsigned char* startIndex) {
+  Angle8 horizontal8 = ReceiveAngle8(buffer, startIndex);
+  Angle horizontal = Angle::Radians(horizontal8.InRadians());
+
+  Angle8 vertical8 = ReceiveAngle8(buffer, startIndex);
+  Angle vertical = Angle::Radians(vertical8.InRadians());
+
+  Angle8 twist8 = ReceiveAngle8(buffer, startIndex);
+  Angle twist = Angle::Radians(twist8.InRadians());
+
+  SwingTwist s = SwingTwist(horizontal, vertical, twist);
+  return s;
+}
+
+void LowLevelMessages::SendQuat32(char* buffer,
+                                  unsigned char* ix,
+                                  SwingTwist rotation) {
+  Quaternion q = rotation.ToQuaternion();
+  unsigned char qx = (char)(q.x * 127 + 128);
+  unsigned char qy = (char)(q.y * 127 + 128);
+  unsigned char qz = (char)(q.z * 127 + 128);
+  unsigned char qw = (char)(q.w * 255);
+  if (q.w < 0) {
+    qx = -qx;
+    qy = -qy;
+    qz = -qz;
+    qw = -qw;
+  }
+  // std::cout << (int)qx << "," << (int)qy << "," << (int)qz << "," << (int)qw
+  // << "\n";
+  buffer[(*ix)++] = qx;
+  buffer[(*ix)++] = qy;
+  buffer[(*ix)++] = qz;
+  buffer[(*ix)++] = qw;
+}
+
+SwingTwist LowLevelMessages::ReceiveQuat32(const char* buffer,
+                                           unsigned char* ix) {
+  float qx = (buffer[(*ix)++] - 128.0F) / 127.0F;
+  float qy = (buffer[(*ix)++] - 128.0F) / 127.0F;
+  float qz = (buffer[(*ix)++] - 128.0F) / 127.0F;
+  float qw = buffer[(*ix)++] / 255.0F;
+  Quaternion q = Quaternion(qx, qy, qz, qw);
+  SwingTwist s = SwingTwist::FromQuaternion(q);
+  return s;
+}
+
 void LowLevelMessages::SendAngle8(char* buffer,
                                  unsigned char* ix,
                                  const float angle) {
@ -42,58 +119,5 @@ float LowLevelMessages::ReceiveFloat16(const char* buffer,
  return (float)f.toFloat();
 }

-void LowLevelMessages::SendSpherical(char* buffer,
-                                     unsigned char* ix,
-                                     Spherical s) {
-  SendFloat16(buffer, ix, s.distance);
-  SendAngle8(buffer, ix, s.direction.horizontal.InDegrees());
-  SendAngle8(buffer, ix, s.direction.vertical.InDegrees());
-}
-Spherical LowLevelMessages::ReceiveSpherical(const char* buffer,
-                                             unsigned char* startIndex) {
-  float distance = ReceiveFloat16(buffer, startIndex);
-
-  Angle8 horizontal8 = ReceiveAngle8(buffer, startIndex);
-  Angle horizontal = Angle::Radians(horizontal8.InRadians());
-
-  Angle8 vertical8 = ReceiveAngle8(buffer, startIndex);
-  Angle vertical = Angle::Radians(vertical8.InRadians());
-
-  Spherical s = Spherical(distance, horizontal, vertical);
-  return s;
-}
-
-void LowLevelMessages::SendQuat32(char* buffer,
-                                  unsigned char* ix,
-                                  SwingTwist rotation) {
-  Quaternion q = rotation.ToQuaternion();
-  unsigned char qx = (char)(q.x * 127 + 128);
-  unsigned char qy = (char)(q.y * 127 + 128);
-  unsigned char qz = (char)(q.z * 127 + 128);
-  unsigned char qw = (char)(q.w * 255);
-  if (q.w < 0) {
-    qx = -qx;
-    qy = -qy;
-    qz = -qz;
-    qw = -qw;
-  }
-  // std::cout << (int)qx << "," << (int)qy << "," << (int)qz << "," << (int)qw
-  // << "\n";
-  buffer[(*ix)++] = qx;
-  buffer[(*ix)++] = qy;
-  buffer[(*ix)++] = qz;
-  buffer[(*ix)++] = qw;
-}
-
-SwingTwist LowLevelMessages::ReceiveQuat32(const char* buffer,
-                                           unsigned char* ix) {
-  float qx = (buffer[(*ix)++] - 128.0F) / 127.0F;
-  float qy = (buffer[(*ix)++] - 128.0F) / 127.0F;
-  float qz = (buffer[(*ix)++] - 128.0F) / 127.0F;
-  float qw = buffer[(*ix)++] / 255.0F;
-  Quaternion q = Quaternion(qx, qy, qz, qw);
-  SwingTwist s = SwingTwist::FromQuaternion(q);
-  return s;
-}

 }  // namespace RoboidControl
--- a/Messages/LowLevelMessages.h
+++ b/Messages/LowLevelMessages.h
@ -11,6 +11,10 @@ class LowLevelMessages {
  static Spherical ReceiveSpherical(const char* buffer,
                                    unsigned char* startIndex);

+  static void SendSwingTwist(char* buffer, unsigned char* ix, SwingTwist s);
+  static SwingTwist ReceiveSwingTwist(const char* buffer,
+                                      unsigned char* startIndex);
+
  static void SendQuat32(char* buffer, unsigned char* ix, SwingTwist q);
  static SwingTwist ReceiveQuat32(const char* buffer, unsigned char* ix);

--- a/Messages/PoseMsg.cpp
+++ b/Messages/PoseMsg.cpp
@ -8,11 +8,11 @@ PoseMsg::PoseMsg(unsigned char networkId, Thing* thing, bool force) {
  this->thingId = thing->id;

  this->poseType = 0;
-  if (thing->positionUpdated || (force && thing->IsRoot())) {
+  if (thing->positionUpdated || (force || thing->IsRoot())) {
    this->position = thing->GetPosition();
    this->poseType |= Pose_Position;
  }
-  if (thing->orientationUpdated || (force && thing->IsRoot())) {
+  if (thing->orientationUpdated || (force || thing->IsRoot())) {
    this->orientation = thing->GetOrientation();
    this->poseType |= Pose_Orientation;
  }
@ -34,7 +34,7 @@ PoseMsg::PoseMsg(const char* buffer) {
  this->thingId = buffer[ix++];
  this->poseType = buffer[ix++];
  this->position = LowLevelMessages::ReceiveSpherical(buffer, &ix);
-  this->orientation = LowLevelMessages::ReceiveQuat32(buffer, &ix);
+  this->orientation = LowLevelMessages::ReceiveSwingTwist(buffer, &ix);
  // linearVelocity
  // angularVelocity
 }
@ -57,7 +57,7 @@ unsigned char PoseMsg::Serialize(char* buffer) {
  if ((this->poseType & Pose_Position) != 0)
    LowLevelMessages::SendSpherical(buffer, &ix, this->position);
  if ((this->poseType & Pose_Orientation) != 0)
-    LowLevelMessages::SendQuat32(buffer, &ix, this->orientation);
+    LowLevelMessages::SendSwingTwist(buffer, &ix, this->orientation);
  if ((this->poseType & Pose_LinearVelocity) != 0)
    LowLevelMessages::SendSpherical(buffer, &ix, this->linearVelocity);
  if ((this->poseType & Pose_AngularVelocity) != 0)
--- a/Messages/TextMsg.h
+++ b/Messages/TextMsg.h
@ -1,3 +1,5 @@
+#pragma once
+
 #include "IMessage.h"

 namespace RoboidControl {
--- a/Messages/ThingMsg.h
+++ b/Messages/ThingMsg.h
@ -1,3 +1,5 @@
+#pragma once
+
 #include "IMessage.h"
 #include "Thing.h"

--- a/Participant.cpp
+++ b/Participant.cpp
@ -1,13 +1,15 @@
 #include "Participant.h"

 #include <string.h>
+#include "Arduino/ArduinoParticipant.h"
+#include "EspIdf/EspIdfParticipant.h"
+#include "Posix/PosixParticipant.h"
+#include "Windows/WindowsParticipant.h"

 namespace RoboidControl {

 #pragma region Participant

-ParticipantRegistry Participant::registry;
-
 Participant* Participant::LocalParticipant = new Participant();

 void Participant::ReplaceLocalParticipant(Participant& newParticipant) {
@ -16,17 +18,14 @@ void Participant::ReplaceLocalParticipant(Participant& newParticipant) {
 }

 Participant::Participant() {
-  std::cout << "P\n";
-  //this->root.name = "Isolated";
-  this->root = new Thing(this);
-  this->root->name = "Root";
-  this->Add(this->root);
+  Thing::CreateRoot(this);
+  //this->Add(this->root);
 }

+/*
 Participant::Participant(const char* ipAddress, int port) {
-  // Add the root thing to the list of things, because we could not do that
-  // earlier (I think)
-  this->Add(this->root);
+  Thing::CreateRoot(this);
+  //this->Add(this->root);

  // make a copy of the ip address string
  int addressLength = (int)strlen(ipAddress);
@ -43,27 +42,62 @@ Participant::Participant(const char* ipAddress, int port) {
  this->ipAddress = addressString;
  this->port = port;
 }
-
+*/
 Participant::~Participant() {
  // registry.Remove(this);
-  delete[] this->ipAddress;
+  // delete[] this->ipAddress;
 }

-void Participant::Update() {
+void Participant::Update(bool recurse) {
  for (Thing* thing : this->things) {
    if (thing != nullptr)
-      thing->Update(true);
+      thing->Update();
  }
 }

-Thing* Participant::Get(unsigned char thingId) {
+bool Participant::Send(IMessage* msg) {
+    std::cout << "sending message " << (static_cast<int>(this->buffer[0]) & 0xff)
+            << " to base Participant without communcation support " << std::endl;
+
+  return true;
+}
+/*
+bool Participant::Send(IMessage* msg) {
+  int bufferSize = msg->Serialize(this->buffer);
+  if (bufferSize <= 0)
+    return true;
+
+  // std::cout << "send msg " << (static_cast<int>(this->buffer[0]) & 0xff)
+  //           << " to " << this->ipAddress << std::endl;
+
+#if defined(_WIN32) || defined(_WIN64)
+  Windows::ParticipantUDP* thisWindows =
+      static_cast<Windows::ParticipantUDP*>(this);
+  return thisWindows->SendTo(this, bufferSize);
+#elif defined(__unix__) || defined(__APPLE__)
+  Posix::ParticipantUDP* thisPosix = static_cast<Posix::ParticipantUDP*>(this);
+  return thisPosix->SendTo(this, bufferSize);
+#elif defined(ARDUINO)
+  Arduino::ParticipantUDP* thisArduino =
+      static_cast<Arduino::ParticipantUDP*>(this);
+  return thisArduino->SendTo(this, bufferSize);
+#elif defined(IDF_VER)
+  EspIdf::ParticipantUDP* thisEspIdf =
+      static_cast<EspIdf::ParticipantUDP*>(this);
+  return thisEspIdf->SendTo(this, bufferSize);
+#else
+  return false;
+#endif
+}
+*/
+
+Thing* Participant::Get(unsigned char networkId, unsigned char thingId) {
  for (Thing* thing : this->things) {
-    if (thing->id == thingId)
+    if (thing->owner->networkId == networkId && thing->id == thingId)
      return thing;
  }
-  // std::cout << "Could not find thing " << this->ipAddress << ":" <<
-  // this->port
-  //           << "[" << (int)thingId << "]\n";
+  std::cout << "Could not find thing " //<< this->ipAddress << ":" << this->port
+            << "[" << (int)thingId << "]\n";
  return nullptr;
 }

@ -85,22 +119,24 @@ void Participant::Add(Thing* thing, bool checkId) {
    thing->id = highestIx + 1;
    this->things.push_back(thing);
 #endif
-    // std::cout << "Add thing with generated ID " << this->ipAddress << ":"
-    //           << this->port << "[" << (int)thing->id << "]\n";
+    std::cout << "Add thing with generated ID " 
+    //<< this->ipAddress << ":" << this->port
+    << "[" << (int)thing->id << "]\n";
  } else {
-    Thing* foundThing = Get(thing->id);
+    Thing* foundThing = Get(thing->owner->networkId, 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->id << "]\n";
+      std::cout << "Add thing " << //this->ipAddress << ":" << this->port <<
+      "["
+                << (int)thing->id << "]\n";
    } else {
-      // std::cout << "Did not add, existing thing " << this->ipAddress << ":"
-      //           << this->port << "[" << (int)thing->id << "]\n";
+      std::cout << "Did not add, existing thing " 
+      //<< this->ipAddress << ":" << this->port
+       << "[" << (int)thing->id << "]\n";
    }
  }
 }
@ -128,86 +164,88 @@ void Participant::Remove(Thing* thing) {

 #pragma endregion

-#pragma region ParticipantRegistry
+// #pragma region ParticipantRegistry

-Participant* ParticipantRegistry::Get(const char* ipAddress,
-                                      unsigned int port) {
-#if !defined(NO_STD)
-  for (Participant* participant : ParticipantRegistry::participants) {
-    if (participant == nullptr)
-      continue;
-    if (strcmp(participant->ipAddress, ipAddress) == 0 &&
-        participant->port == port) {
-      // std::cout << "found participant " << participant->ipAddress << ":"
-      //           << (int)participant->port << std::endl;
-      return participant;
-    }
-  }
-  std::cout << "Could not find participant " << ipAddress << ":" << (int)port
-            << std::endl;
-#endif
-  return nullptr;
-}
+// /*
+// Participant* ParticipantRegistry::Get(const char* ipAddress,
+//                                       unsigned int port) {
+// #if !defined(NO_STD)
+//   for (Participant* participant : ParticipantRegistry::participants) {
+//     if (participant == nullptr)
+//       continue;
+//     if (strcmp(participant->ipAddress, ipAddress) == 0 &&
+//         participant->port == port) {
+//       // std::cout << "found participant " << participant->ipAddress << ":"
+//       //           << (int)participant->port << std::endl;
+//       return participant;
+//     }
+//   }
+//   std::cout << "Could not find participant " << ipAddress << ":" << (int)port
+//             << std::endl;
+// #endif
+//   return nullptr;
+// }
+// */
+// Participant* ParticipantRegistry::Get(unsigned char participantId) {
+// #if !defined(NO_STD)
+//   for (Participant* participant : ParticipantRegistry::participants) {
+//     if (participant == nullptr)
+//       continue;
+//     if (participant->networkId == participantId)
+//       return participant;
+//   }
+//   std::cout << "Could not find participant " << (int)participantId << std::endl;
+// #endif
+//   return nullptr;
+// }

-Participant* ParticipantRegistry::Get(unsigned char participantId) {
-#if !defined(NO_STD)
-  for (Participant* participant : ParticipantRegistry::participants) {
-    if (participant == nullptr)
-      continue;
-    if (participant->networkId == participantId)
-      return participant;
-  }
-  std::cout << "Could not find participant " << (int)participantId << std::endl;
-#endif
-  return nullptr;
-}
+// // Participant* ParticipantRegistry::Add(const char* ipAddress,
+// //                                       unsigned int port) {
+// //   Participant* participant = new Participant(ipAddress, port);
+// //   Add(participant);
+// //   return participant;
+// // }

-Participant* ParticipantRegistry::Add(const char* ipAddress,
-                                      unsigned int port) {
-  Participant* participant = new Participant(ipAddress, port);
-  Add(participant);
-  return participant;
-}
+// void ParticipantRegistry::Add(Participant* participant) {
+//   Participant* foundParticipant =
+//     Get(participant->networkId);
+//       //Get(participant->ipAddress, participant->port);

-void ParticipantRegistry::Add(Participant* participant) {
-  Participant* foundParticipant =
-      Get(participant->ipAddress, participant->port);
+//   if (foundParticipant == nullptr) {
+// #if defined(NO_STD)
+//     // this->things[this->thingCount++] = thing;
+// #else
+//     ParticipantRegistry::participants.push_back(participant);
+// #endif
+//     //   std::cout << "Add participant " << participant->ipAddress << ":"
+//     //             << participant->port << "[" << (int)participant->networkId
+//     //             << "]\n";
+//     //   std::cout << "participants " <<
+//     //   ParticipantRegistry::participants.size()
+//     //             << "\n";
+//     // } else {
+//     //   std::cout << "Did not add, existing participant " <<
+//     //   participant->ipAddress
+//     //             << ":" << participant->port << "[" <<
+//     //             (int)participant->networkId
+//     //             << "]\n";
+//   }
+// }

-  if (foundParticipant == nullptr) {
-#if defined(NO_STD)
-    // this->things[this->thingCount++] = thing;
-#else
-    ParticipantRegistry::participants.push_back(participant);
-#endif
-    //   std::cout << "Add participant " << participant->ipAddress << ":"
-    //             << participant->port << "[" << (int)participant->networkId
-    //             << "]\n";
-    //   std::cout << "participants " <<
-    //   ParticipantRegistry::participants.size()
-    //             << "\n";
-    // } else {
-    //   std::cout << "Did not add, existing participant " <<
-    //   participant->ipAddress
-    //             << ":" << participant->port << "[" <<
-    //             (int)participant->networkId
-    //             << "]\n";
-  }
-}
+// void ParticipantRegistry::Remove(Participant* participant) {
+//   // participants.remove(participant);
+// }

-void ParticipantRegistry::Remove(Participant* participant) {
-  // participants.remove(participant);
-}
+// #if defined(NO_STD)
+// Participant** ParticipantRegistry::GetAll() const {
+//   return ParticipantRegistry::participants;
+// }
+// #else
+// const std::list<Participant*>& ParticipantRegistry::GetAll() const {
+//   return ParticipantRegistry::participants;
+// }
+// #endif

-#if defined(NO_STD)
-Participant** ParticipantRegistry::GetAll() const {
-  return ParticipantRegistry::participants;
-}
-#else
-const std::list<Participant*>& ParticipantRegistry::GetAll() const {
-  return ParticipantRegistry::participants;
-}
-#endif
-
-#pragma endregion ParticipantRegistry
+// #pragma endregion ParticipantRegistry

 }  // namespace RoboidControl
--- a/Participant.h
+++ b/Participant.h
@ -1,11 +1,13 @@
 #pragma once

+#include "Messages/IMessage.h"
 #include "Thing.h"

 namespace RoboidControl {

 constexpr int MAX_THING_COUNT = 256;

+/*
 /// @brief class which manages all known participants
 class ParticipantRegistry {
 public:
@ -13,7 +15,7 @@ class ParticipantRegistry {
  /// @param ipAddress The IP address of the participant
  /// @param port The port number of the participant
  /// @return The participant or a nullptr when it could not be found
-  Participant* Get(const char* ipAddress, unsigned int port);
+  //Participant* Get(const char* ipAddress, unsigned int port);
  /// @brief Retrieve a participant by its network ID
  /// @param networkID The network ID of the participant
  /// @return The participant or a nullptr when it could not be found
@ -23,7 +25,8 @@ class ParticipantRegistry {
  /// @param ipAddress The IP address of the participant
  /// @param port The port number of the participant
  /// @return The added participant
-  Participant* Add(const char* ipAddress, unsigned int port);
+  //Participant* Add(const char* ipAddress, unsigned int port);
+
  /// @brief Add a participant
  /// @param participant The participant to add
  void Add(Participant* participant);
@ -51,6 +54,7 @@ class ParticipantRegistry {
  std::list<Participant*> participants;
 #endif
 };
+*/

 /// @brief A participant is a device which manages things.
 /// It can communicate with other participant to synchronise the state of
@ -59,30 +63,39 @@ class ParticipantRegistry {
 /// participant. It is used as a basis for the local participant, but also as a
 /// reference to remote participants.
 class Participant {
+#pragma region Init
+
 public:
-  /// @brief The name of the participant
-  const char* name = "Participant";
-
-  /// @brief The Ip Address of a participant.
-  const char* ipAddress = "0.0.0.0";
-  /// @brief The port number for UDP communication with the participant.
-  unsigned int port = 0;
-
-  /// @brief The network Id to identify the participant
-  unsigned char networkId = 0;
-
+  /// @brief Create a generic participant
  Participant();
  /// @brief Create a new participant with the given communcation info
  /// @param ipAddress The IP address of the participant
  /// @param port The UDP port of the participant
+  /// @remarks This does not belong here, it should move to ParticipantUDP or
+  /// something like that in the future
  Participant(const char* ipAddress, int port);
  /// @brief Destructor for the participant
  ~Participant();

+  /// @brief The local participant for this application
  static Participant* LocalParticipant;
+  /// @brief Replace the local participant
+  /// @param newParticipant The new local Participant
  static void ReplaceLocalParticipant(Participant& newParticipant);

-  Thing* root = new Thing(this);
+#pragma endregion Init
+
+#pragma region Properties
+
+ public:
+  /// @brief The name of the participant
+  const char* name = "Participant";
+
+  /// @brief The network Id to identify the participant
+  unsigned char networkId = 0;
+
+  /// @brief The root thing for this participant
+  Thing* root = nullptr;

 public:
 #if defined(NO_STD)
@ -93,9 +106,10 @@ class Participant {
  std::list<Thing*> things;
 #endif
  /// @brief Find a thing managed by this participant
+  /// @param networkId The network ID of the thing
  /// @param thingId The ID of the thing
  /// @return The thing if found, nullptr when no thing has been found
-  Thing* Get(unsigned char thingId);
+  Thing* Get(unsigned char networkId, unsigned char thingId);
  /// @brief Add a new thing for this participant.
  /// @param thing The thing to add
  /// @param checkId If true, the thing.id is regenerated if it is zero
@ -104,12 +118,31 @@ class Participant {
  /// @param thing The thing to remove
  void Remove(Thing* thing);

-  /// @brief Update all things for this participant
-  /// @param currentTimeMs The current time in milliseconds (optional)
-  virtual void Update();
+#pragma endregion Properties
+
+#pragma region Update

 public:
-  static ParticipantRegistry registry;
+  /// @brief Update all things for this participant
+  virtual void Update(bool recurse = true);
+
+#pragma endregion Update
+
+#pragma region Send
+
+ public:
+  char buffer[1024];
+
+  virtual bool Send(IMessage* msg);
+
+#pragma endregion Send
+
+// #pragma region Participant Registry
+
+//  public:
+//   static ParticipantRegistry registry;
+
+// #pragma endregion Participant Registry
 };

 }  // namespace RoboidControl
--- a/Participants/IsolatedParticipant.cpp
+++ b/Participants/IsolatedParticipant.cpp
@ -1,14 +1,16 @@
+/*
 #include "IsolatedParticipant.h"
 #include "ParticipantUDP.h"

 namespace RoboidControl {

-static ParticipantUDP* isolatedParticipant = nullptr;
+static ParticipantUDPGeneric* isolatedParticipant = nullptr;

 Participant* IsolatedParticipant::Isolated() {
  if (isolatedParticipant == nullptr)
-    isolatedParticipant = new ParticipantUDP(0);
+    isolatedParticipant = new ParticipantUDPGeneric(0);
  return isolatedParticipant;
 }

-}  // namespace RoboidControl
+}  // namespace RoboidControl
+  */
--- a/Participants/IsolatedParticipant.h
+++ b/Participants/IsolatedParticipant.h
@ -1,3 +1,4 @@
+/*
 #include "Participant.h"

 namespace RoboidControl {
@ -10,4 +11,5 @@ class IsolatedParticipant {
  static Participant* Isolated();
 };

-}
+}
+*/
--- a/Participants/ParticipantMQTT.cpp
+++ b/Participants/ParticipantMQTT.cpp
@ -0,0 +1,154 @@
+#include "ParticipantMQTT.h"
+
+#if 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
+
+#include "Participants/ParticipantUDP.h"
+
+namespace RoboidControl {
+
+MQTTParticipantBase::MQTTParticipantBase(const char* ipAddress, int port)
+    : RemoteParticipantUDP("127.0.0.1", port) {
+  this->name = "ParticipantUDP";
+  this->remoteSite = new RemoteParticipantUDP(ipAddress, port);
+  // Participant::registry.Add(this);
+
+  this->root = Thing::LocalRoot();  // Participant::LocalParticipant->root;
+  this->root->owner = this;
+  this->root->name = "UDP Root";
+  this->Add(this->root);
+
+  Participant::ReplaceLocalParticipant(*this);
+}
+
+void MQTTParticipantBase::SetupTCP() {
+  this->connected = false;
+}
+
+#define MQTT_CONNECT 0x10
+#define MQTT_CONNECT_ACK 0x20
+
+void MQTTParticipantBase::send_mqtt_connect(const char* client_id) {
+  MQTTConnectPacket packet;
+  packet.fixed_header = MQTT_CONNECT;
+  packet.remaining_length =  // 14;
+      2 + 4 + 2 + strlen(client_id) +
+      3;                            // Protocol name + protocol level + connect
+                                    // flags + keep alive + client ID
+  packet.protocol_name_length = 4;  // "MQTT"
+  packet.protocol_name = "MQTT";
+  packet.protocol_level = 4;      // MQTT version 3.1.1
+  packet.connect_flags = 2;       // Clean session
+  packet.keep_alive = htons(15);  // Keep alive time in seconds
+
+  // Create the MQTT connect packet
+  int index = 0;
+
+  this->buffer[index++] = packet.fixed_header;
+  this->buffer[index++] = packet.remaining_length;
+  this->buffer[index++] = 0;  // MSB protocol_name_length
+  this->buffer[index++] = packet.protocol_name_length;
+  memcpy(&this->buffer[index], packet.protocol_name,
+         packet.protocol_name_length);
+  index += packet.protocol_name_length;
+  this->buffer[index++] = packet.protocol_level;
+  this->buffer[index++] = packet.connect_flags;
+  this->buffer[index++] = packet.keep_alive & 0xFF;         // LSB
+  this->buffer[index++] = (packet.keep_alive >> 8) & 0xFF;  // MSB
+  size_t client_id_length = strlen(client_id);
+  this->buffer[index++] = (client_id_length >> 8) & 0xFF;  // MSB
+  this->buffer[index++] = client_id_length & 0xFF;         // LSB
+  memcpy(&this->buffer[index], client_id, client_id_length);
+  index += client_id_length;
+
+  for (int ix = 0; ix < index; ix++)
+    std::cout << std::hex << "0x" << (int)this->buffer[ix] << " ";
+  std::cout << std::dec << std::endl;
+
+  // Send the MQTT connect packet
+  SendTCP(index);
+
+  std::cout << "Send connect, client ID = " << client_id << std::endl;
+}
+
+void MQTTParticipantBase::sendSubscribe(const char* topic) {
+  // Packet Identifier (2 bytes)
+  static unsigned short packetId =
+      1;  // Increment this for each new subscription
+
+  // Calculate the length of the topic name
+  size_t topicLength = strlen(topic);
+
+  // Remaining length = Packet Identifier (2 bytes) + Topic Length (2 bytes) +
+  // Topic Name + QoS (1 byte)
+  size_t remainingLength = 1 + 2 + 2 + topicLength + 1;
+
+  // Construct the SUBSCRIBE packet
+  // unsigned char subscribePacket[3 + topicLength];  // 3 bytes for fixed
+  // header
+  //                                                  // and packet ID
+  this->buffer[0] = (char)0x82;                 // Packet type and flags
+  this->buffer[1] = (char)remainingLength;      // Remaining length
+  this->buffer[2] = (packetId >> 8) & 0xFF;     // Packet Identifier MSB
+  this->buffer[3] = packetId & 0xFF;            // Packet Identifier LSB
+  this->buffer[4] = (topicLength >> 8) & 0xFF;  // Topic Length MSB
+  this->buffer[5] = topicLength & 0xFF;         // Topic Length LSB
+
+  // Copy the topic name into the packet
+  memcpy(&this->buffer[6], topic, topicLength);
+  this->buffer[6 + topicLength] = 0x00;  // QoS level (0)
+
+  int index = 7 + topicLength;
+  //   for (int ix = 0; ix < index; ix++)
+  //     std::cout << std::hex << (int)this->buffer[ix] << std::dec << "\n";
+
+  // Send the SUBSCRIBE packet
+  SendTCP(7 + topicLength);
+
+  std::cout << "Send subscribe to topic: " << topic << std::endl;
+}
+
+void MQTTParticipantBase::Update(bool recurse) {
+  int packetSize = ReceiveTCP();
+  if (packetSize > 0) {
+    ReceiveData(packetSize);
+  }
+}
+void MQTTParticipantBase::SendTCP(int bufferSize) {}
+
+int MQTTParticipantBase::ReceiveTCP() {
+  return 0;
+}
+
+void MQTTParticipantBase::ReceiveData(unsigned char bufferSize) {
+  std::cout << " receivemsg\n";
+  // std::cout << "receive msg " << (int)msgId << "\n";
+  // std::cout << "  buffer size = " <<(int) bufferSize << "\n";
+};
+
+// void ParticipantMQTT::receiveMessages(int sock) {
+//   unsigned char buffer[1024];
+//   // Check for incoming messages without blocking
+//   int bytesRead = recv(sock, buffer, sizeof(buffer), MSG_DONTWAIT);
+//   if (bytesRead > 0) {
+//     // Process the incoming MQTT message
+//     std::cout << "Received message: ";
+//     for (int i = 0; i < bytesRead; ++i) {
+//       std::cout << std::hex << (int)buffer[i] << " ";
+//     }
+//     std::cout << std::dec << std::endl;  // Reset to decimal output
+//   } else if (bytesRead < 0) {
+//     // If no messages are available, check for errors
+//     if (errno != EAGAIN && errno != EWOULDBLOCK) {
+//       std::cerr << "Error receiving message: " << strerror(errno) <<
+//       std::endl;
+//     }
+//   }
+// }
+
+}  // namespace RoboidControl
--- a/Participants/ParticipantMQTT.h
+++ b/Participants/ParticipantMQTT.h
@ -0,0 +1,130 @@
+#pragma once
+
+#include "Messages/BinaryMsg.h"
+#include "Messages/DestroyMsg.h"
+#include "Messages/InvestigateMsg.h"
+#include "Messages/ModelUrlMsg.h"
+#include "Messages/NameMsg.h"
+#include "Messages/NetworkIdMsg.h"
+#include "Messages/ParticipantMsg.h"
+#include "Messages/PoseMsg.h"
+#include "Messages/TextMsg.h"
+#include "Messages/ThingMsg.h"
+#include "Participant.h"
+#include "Participants/ParticipantUDP.h"
+
+#if !defined(NO_STD)
+#include <functional>
+#include <list>
+// #include <unordered_map>
+#endif
+
+#if defined(_WIN32) || defined(_WIN64)
+#include "Windows/WindowsParticipant.h"
+#elif defined(__unix__) || defined(__APPLE__)
+#include <arpa/inet.h>
+#include <netinet/in.h>
+#include <sys/socket.h>
+#include <unistd.h>
+#include "Posix/PosixParticipant.h"
+#endif
+
+namespace RoboidControl {
+
+/// @brief A participant using UDP communication
+/// A local participant is the local device which can communicate with
+/// other participants It manages all local things and communcation with other
+/// 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 MQTTParticipantBase : public RemoteParticipantUDP {
+#pragma region Init
+
+ public:
+  /// @brief Create a participant which will try to connect to a MQTT server.
+  /// @param ipAddress The IP address of the MQTT server
+  /// @param port The port used by the MQTT server
+  MQTTParticipantBase(const char* ipAddress, int port = 1883);
+protected:
+  virtual void SetupTCP();
+
+#pragma endregion Init
+
+#pragma region Properties
+
+ public:
+  /// @brief The remote site when this participant is connected to a site
+  RemoteParticipantUDP* remoteSite = nullptr;
+
+ protected:
+
+ public:
+  // void Begin();
+  bool connected = false;
+
+#pragma endregion Properties
+
+#pragma region Update
+
+ public:
+  virtual void Update(bool recurse = true) override;
+
+#pragma endregion Update
+
+#pragma region Send
+
+ protected:
+  // MQTT Connect Packet Structure
+  struct MQTTConnectPacket {
+    uint8_t fixed_header;
+    uint8_t remaining_length;
+    uint8_t protocol_name_length;
+    const char* protocol_name;
+    uint8_t protocol_level;
+    uint8_t connect_flags;
+    uint16_t keep_alive;
+    const char* client_id;
+  };
+
+  void send_mqtt_connect(const char* client_id);
+  void sendSubscribe(const char* topic);
+  virtual void SendTCP(int bufferSize);
+  
+#pragma endregion Send
+
+#pragma region Receive
+
+ protected:
+  virtual int ReceiveTCP();
+  void ReceiveData(unsigned char bufferSize);
+  //   void ReceiveData(unsigned char bufferSize, Participant*
+  //   remoteParticipant);
+
+  //   void SetupUDP(int localPort, const char* remoteIpAddress, int
+  //   remotePort);
+
+  //   void ReceiveUDP();
+
+  //   virtual void Process(Participant* sender, ParticipantMsg* msg);
+  //   virtual void Process(Participant* sender, NetworkIdMsg* 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);
+  //   virtual void Process(Participant* sender, TextMsg* msg);
+  //   virtual void Process(Participant* sender, DestroyMsg* msg);
+
+#pragma endregion Receive
+};
+
+}  // namespace RoboidControl
+
+#include "Posix/PosixMQTT.h"
--- a/Participants/ParticipantUDP.cpp
+++ b/Participants/ParticipantUDP.cpp
@ -8,81 +8,167 @@
 #include "Posix/PosixParticipant.h"
 #include "Windows/WindowsParticipant.h"

+#include "Things/DifferentialDrive.h"
+#include "Things/DistanceSensor.h"
+#include "Things/TouchSensor.h"
+
 #include <string.h>

 namespace RoboidControl {

+#pragma region ParticipantRegistry
+
+ParticipantRegistry ParticipantUDPGeneric::registry;
+
+RemoteParticipantUDP* ParticipantRegistry::Get(const char* ipAddress,
+                                               unsigned int port) {
+#if !defined(NO_STD)
+  for (RemoteParticipantUDP* participant : ParticipantRegistry::participants) {
+    if (participant == nullptr)
+      continue;
+    if (strcmp(participant->ipAddress, ipAddress) == 0 &&
+        participant->port == port) {
+      // std::cout << "found participant " << participant->ipAddress << ":"
+      //           << (int)participant->port << std::endl;
+      return participant;
+    }
+  }
+  std::cout << "Could not find participant " << ipAddress << ":" << (int)port
+            << std::endl;
+#endif
+  return nullptr;
+}
+
+RemoteParticipantUDP* ParticipantRegistry::Get(unsigned char participantId) {
+#if !defined(NO_STD)
+  for (RemoteParticipantUDP* participant : ParticipantRegistry::participants) {
+    if (participant == nullptr)
+      continue;
+    if (participant->networkId == participantId)
+      return participant;
+  }
+  std::cout << "Could not find participant " << (int)participantId << std::endl;
+#endif
+  return nullptr;
+}
+
+RemoteParticipantUDP* ParticipantRegistry::Add(const char* ipAddress,
+                                               unsigned int port) {
+  RemoteParticipantUDP* participant = new RemoteParticipantUDP(ipAddress, port);
+  Add(participant);
+  return participant;
+}
+
+void ParticipantRegistry::Add(RemoteParticipantUDP* participant) {
+  Participant* foundParticipant = Get(participant->networkId);
+  // Get(participant->ipAddress, participant->port);
+
+  if (foundParticipant == nullptr) {
+#if defined(NO_STD)
+    // this->things[this->thingCount++] = thing;
+#else
+    ParticipantRegistry::participants.push_back(participant);
+#endif
+    //   std::cout << "Add participant " << participant->ipAddress << ":"
+    //             << participant->port << "[" << (int)participant->networkId
+    //             << "]\n";
+    //   std::cout << "participants " <<
+    //   ParticipantRegistry::participants.size()
+    //             << "\n";
+    // } else {
+    //   std::cout << "Did not add, existing participant " <<
+    //   participant->ipAddress
+    //             << ":" << participant->port << "[" <<
+    //             (int)participant->networkId
+    //             << "]\n";
+  }
+}
+
+void ParticipantRegistry::Remove(RemoteParticipantUDP* participant) {
+  // participants.remove(participant);
+}
+
+#if defined(NO_STD)
+RemoteParticipantUDP** ParticipantRegistry::GetAll() const {
+  return ParticipantRegistry::participants;
+}
+#else
+const std::list<RemoteParticipantUDP*>& ParticipantRegistry::GetAll() const {
+  return ParticipantRegistry::participants;
+}
+#endif
+
+#pragma endregion ParticipantRegistry
+
+RemoteParticipantUDP::RemoteParticipantUDP(const char* ipAddress, int port) {
+  // make a copy of the ip address string
+  int addressLength = (int)strlen(ipAddress);
+  int stringLength = addressLength + 1;
+  char* addressString = new char[stringLength];
+#if defined(_WIN32) || defined(_WIN64)
+  strncpy_s(addressString, stringLength, ipAddress,
+            addressLength);  // Leave space for null terminator
+#else
+  strncpy(addressString, ipAddress, addressLength);
+#endif
+  addressString[addressLength] = '\0';
+
+  this->ipAddress = addressString;
+  this->port = port;
+}
+
+bool RemoteParticipantUDP::Send(IMessage* msg) {
+  // No message is actually sent, because this class has no networking
+  // implementation
+  return false;
+}
+
 #pragma region Init

-ParticipantUDP::ParticipantUDP(int port) : Participant("127.0.0.1", port) {
+ParticipantUDPGeneric::ParticipantUDPGeneric(int port)
+    : RemoteParticipantUDP("127.0.0.1", port) {
  this->name = "ParticipantUDP";
  this->remoteSite = nullptr;
  if (this->port == 0)
    this->isIsolated = true;
-  Participant::registry.Add(this);
+  registry.Add(this);

  this->root = Thing::LocalRoot();  //::LocalParticipant->root;
  this->root->owner = this;
  this->root->name = "UDP Root";
+  std::cout << "P2 " << (long)this->root << std::endl;
  this->Add(this->root);

  Participant::ReplaceLocalParticipant(*this);
 }

-ParticipantUDP::ParticipantUDP(const char* ipAddress, int port, int localPort)
-    : Participant("127.0.0.1", localPort) {
+ParticipantUDPGeneric::ParticipantUDPGeneric(const char* ipAddress,
+                                             int port,
+                                             int localPort)
+    : RemoteParticipantUDP("127.0.0.1", localPort) {
  this->name = "ParticipantUDP";
  if (this->port == 0)
    this->isIsolated = true;
  else
-    this->remoteSite = new Participant(ipAddress, port);
-  Participant::registry.Add(this);
+    this->remoteSite = new RemoteParticipantUDP(ipAddress, port);
+  registry.Add(this);

  this->root = Thing::LocalRoot();  // Participant::LocalParticipant->root;
  this->root->owner = this;
  this->root->name = "UDP Root";
+  std::cout << "P1 " << (long)this->root << std::endl;
  this->Add(this->root);

  Participant::ReplaceLocalParticipant(*this);
 }

-static ParticipantUDP* isolatedParticipant = nullptr;
-
-ParticipantUDP* ParticipantUDP::Isolated() {
-  if (isolatedParticipant == nullptr)
-    isolatedParticipant = new ParticipantUDP(0);
-  return isolatedParticipant;
-}
-
-void ParticipantUDP::begin() {
+void ParticipantUDPGeneric::begin() {
  if (this->isIsolated || this->remoteSite == nullptr)
    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();
-#elif defined(IDF_VER)
-  EspIdf::ParticipantUDP* thisEspIdf =
-      static_cast<EspIdf::ParticipantUDP*>(this);
-  thisEspIdf->Setup(localPort, remoteIpAddress, remotePort);
-#endif
-  this->connected = true;
-}
-
 #pragma endregion Init

 #pragma region Update
@ -91,105 +177,55 @@ void ParticipantUDP::SetupUDP(int localPort,
 // 1. receive external messages
 // 2. update the state
 // 3. send out the updated messages
-void ParticipantUDP::Update() {
+void ParticipantUDPGeneric::Update(bool recurse) {
  unsigned long currentTimeMs = Thing::GetTimeMs();

-  PrepMyThings();
-
  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->Send(msg);

+      delete msg;
      this->nextPublishMe = currentTimeMs + this->publishInterval;
    }

-    this->ReceiveUDP();
+    //this->ReceiveUDP();
  }

  UpdateMyThings();
  UpdateOtherThings();
 }

-void ParticipantUDP::PrepMyThings() {
-  for (Thing* thing : this->things) {
-    if (thing == nullptr)
-      continue;
-      
-    thing->PrepareForUpdate();
-  }
-}
-
-void ParticipantUDP::UpdateMyThings() {
-  // std::cout << this->things.size() << std::endl;
+void ParticipantUDPGeneric::UpdateMyThings() {
  for (Thing* thing : this->things) {
    if (thing == nullptr)  // || thing->GetParent() != nullptr)
      continue;

-    // std::cout << thing->name << "\n";
-    if (thing->hierarchyChanged) {
-      if (!(this->isIsolated || this->networkId == 0)) {
-        ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
-        this->Send(this->remoteSite, thingMsg);
-        delete thingMsg;
-
-        if (thing->nameChanged) {
-          NameMsg* nameMsg = new NameMsg(this->networkId, thing);
-          this->Send(this->remoteSite, nameMsg);
-          delete nameMsg;
-        }
-      }
-    }
-
-    // std::cout << "B\n";
-    // Why don't we do recursive?
+      // Why don't we do recursive?
    // Because when a thing creates a thing in the update,
    // that new thing is not sent out (because of hierarchyChanged)
    // before it is updated itself: it is immediatedly updated!
    thing->Update(false);

-    // std::cout << "C\n";
-    if (!(this->isIsolated || this->networkId == 0)) {
-      if (thing->terminate) {
-        DestroyMsg* destroyMsg = new DestroyMsg(this->networkId, thing);
-        this->Send(this->remoteSite, destroyMsg);
-        delete destroyMsg;
-      } else {
-        //  Send to remote site
-        if (thing->nameChanged) {
-          NameMsg* nameMsg = new NameMsg(this->networkId, thing);
-          this->Send(this->remoteSite, nameMsg);
-          delete nameMsg;
-        }
-        PoseMsg* poseMsg = new PoseMsg(this->networkId, thing);
-        this->Send(this->remoteSite, poseMsg);
-        delete poseMsg;
-        BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing);
-        this->Send(this->remoteSite, binaryMsg);
-        delete binaryMsg;
-      }
-    }
-    // std::cout << "D\n";
    if (thing->terminate)
      this->Remove(thing);
-    // std::cout << "E\n";
  }
 }

-void ParticipantUDP::UpdateOtherThings() {
+void ParticipantUDPGeneric::UpdateOtherThings() {
 #if defined(NO_STD)
  Participant** participants = Participant::registry.GetAll();
  for (int ix = 0; ix < Participant::registry.count; ix++) {
    Participant* participant = participants[ix];
 #else
-  for (Participant* participant : Participant::registry.GetAll()) {
+  for (Participant* participant : registry.GetAll()) {
 #endif
    if (participant == nullptr || participant == this)
      continue;
@ -203,10 +239,10 @@ void ParticipantUDP::UpdateOtherThings() {

    for (Thing* thing : participant->things) {
      PoseMsg* poseMsg = new PoseMsg(participant->networkId, thing);
-      this->Send(participant, poseMsg);
+      participant->Send(poseMsg);
      delete poseMsg;
      BinaryMsg* binaryMsg = new BinaryMsg(participant->networkId, thing);
-      this->Send(participant, binaryMsg);
+      participant->Send(binaryMsg);
      delete binaryMsg;
    }
  }
@ -217,55 +253,34 @@ void ParticipantUDP::UpdateOtherThings() {

 #pragma region Send

-void ParticipantUDP::SendThingInfo(Participant* remoteParticipant,
-                                   Thing* thing) {
+void ParticipantUDPGeneric::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);
+  remoteParticipant->Send(thingMsg);
  delete thingMsg;
  NameMsg* nameMsg = new NameMsg(this->networkId, thing);
-  this->Send(remoteParticipant, nameMsg);
+  remoteParticipant->Send(nameMsg);
  delete nameMsg;
  ModelUrlMsg* modelMsg = new ModelUrlMsg(this->networkId, thing);
-  this->Send(remoteParticipant, modelMsg);
+  remoteParticipant->Send(modelMsg);
  delete modelMsg;
  PoseMsg* poseMsg = new PoseMsg(this->networkId, thing, true);
-  this->Send(remoteParticipant, poseMsg);
+  remoteParticipant->Send(poseMsg);
  delete poseMsg;
-  BinaryMsg* customMsg = new BinaryMsg(this->networkId, thing);
-  this->Send(remoteParticipant, customMsg);
-  delete customMsg;
+  BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing);
+  remoteParticipant->Send(binaryMsg);
+  delete binaryMsg;
 }

-bool ParticipantUDP::Send(Participant* remoteParticipant, IMessage* msg) {
-  int bufferSize = msg->Serialize(this->buffer);
-  if (bufferSize <= 0)
-    return true;
+bool ParticipantUDPGeneric::Send(IMessage* msg) {
+  if (this->remoteSite != nullptr)
+    return this->remoteSite->Send(msg);

-  // std::cout << "send msg " << (static_cast<int>(this->buffer[0]) & 0xff)
-  //           << " to " << remoteParticipant->ipAddress << std::endl;
-
-#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
+  return true;
 }

-void ParticipantUDP::PublishThingInfo(Thing* thing) {
+void ParticipantUDPGeneric::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...
@ -286,71 +301,31 @@ void ParticipantUDP::PublishThingInfo(Thing* thing) {
  delete customMsg;
 }

-bool ParticipantUDP::Publish(IMessage* msg) {
-  // std::cout << "publish msg\n";
-#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::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
-}
-
-void ParticipantUDP::ReceiveData(unsigned char packetSize,
-                                 char* senderIpAddress,
-                                 unsigned int senderPort) {
+void ParticipantUDPGeneric::ReceiveData(unsigned char packetSize,
+                                        char* senderIpAddress,
+                                        unsigned int senderPort) {
  // std::cout << "Receive data from " << senderIpAddress << ":" << senderPort
  //           << std::endl;
-  Participant* sender = this->registry.Get(senderIpAddress, senderPort);
+  RemoteParticipantUDP* sender =
+      this->registry.Get(senderIpAddress, senderPort);
  if (sender == nullptr) {
    sender = this->registry.Add(senderIpAddress, senderPort);
 #if !defined(NO_STD)
-    std::cout << "New remote participant " << sender->ipAddress << ":"
-              << sender->port << std::endl;
+    // std::cout << "New remote participant " << sender->ipAddress << ":"
+    //           << sender->port << std::endl;
 #endif
  }

  ReceiveData(packetSize, sender);
 }

-void ParticipantUDP::ReceiveData(unsigned char bufferSize,
-                                 Participant* sender) {
+void ParticipantUDPGeneric::ReceiveData(unsigned char bufferSize,
+                                        RemoteParticipantUDP* sender) {
  unsigned char msgId = this->buffer[0];
  // std::cout << "receive msg " << (int)msgId << "\n";
  // std::cout << "  buffer size = " <<(int) bufferSize << "\n";
@ -402,6 +377,18 @@ void ParticipantUDP::ReceiveData(unsigned char bufferSize,
      Process(sender, msg);
      delete msg;
    } break;
+    case TextMsg::id: {
+      TextMsg* msg = new TextMsg(this->buffer);
+      bufferSize -= msg->length + msg->textLength;
+      Process(sender, msg);
+      delete msg;
+    } break;
+    case DestroyMsg::id: {
+      DestroyMsg* msg = new DestroyMsg(this->buffer);
+      bufferSize -= msg->length;
+      Process(sender, msg);
+      delete msg;
+    } break;
  };

 // Check if the buffer has been read completely
@ -411,14 +398,16 @@ void ParticipantUDP::ReceiveData(unsigned char bufferSize,
 #endif
 }

-void ParticipantUDP::Process(Participant* sender, ParticipantMsg* msg) {
+void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
+                                    ParticipantMsg* msg) {
 #if defined(DEBUG)
  std::cout << this->name << ": Process ParticipantMsg " << (int)msg->networkId
            << "\n";
 #endif
 }

-void ParticipantUDP::Process(Participant* sender, NetworkIdMsg* msg) {
+void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
+                                    NetworkIdMsg* msg) {
 #if defined(DEBUG)
  std::cout << this->name << ": process NetworkIdMsg " << (int)this->networkId
            << " -> " << (int)msg->networkId << "\n";
@ -433,28 +422,68 @@ void ParticipantUDP::Process(Participant* sender, NetworkIdMsg* msg) {
  }
 }

-void ParticipantUDP::Process(Participant* sender, InvestigateMsg* msg) {
+void ParticipantUDPGeneric::Process(RemoteParticipantUDP* 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) {
+void ParticipantUDPGeneric::Process(RemoteParticipantUDP* 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
+  RemoteParticipantUDP* owner = registry.Get(msg->networkId);
+  if (owner == nullptr) {
+    owner = new RemoteParticipantUDP(sender->ipAddress, sender->port);
+    owner->networkId = msg->networkId;
+    registry.Add(owner);
+  }
+
+  Thing* thing = owner->Get(msg->networkId, msg->thingId);
+  if (thing == nullptr) {
+    bool isRemote = (sender->networkId != owner->networkId);
+    thing = ProcessNewThing(owner, msg, isRemote);
+    thing->id = msg->thingId;
+    thing->type = msg->thingType;
+    thing->isRemote = isRemote;
+  }
+
+  if (msg->parentId != 0) {
+    thing->SetParent(owner->Get(msg->networkId, msg->parentId));
+    if (thing->GetParent() == nullptr)
+      std::cout << "Could not find parent" << std::endl;
+  } else
+    thing->SetParent(nullptr);
 }

-void ParticipantUDP::Process(Participant* sender, NameMsg* msg) {
+Thing* ParticipantUDPGeneric::ProcessNewThing(RemoteParticipantUDP* owner,
+                                              ThingMsg* msg,
+                                              bool isRemote) {
+  switch (msg->thingType) {
+    case Thing::Type::DistanceSensor:
+      return new DistanceSensor(owner->root);
+    case Thing::Type::TouchSensor:
+      return new TouchSensor(owner->root);
+    case Thing::Type::DifferentialDrive:
+      return new DifferentialDrive(owner->root);
+    default:
+      return new Thing(owner->root);
+  }
+}
+
+void ParticipantUDPGeneric::Process(RemoteParticipantUDP* 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);
+  Thing* thing = sender->Get(msg->networkId, msg->thingId);
  if (thing != nullptr) {
    int nameLength = msg->nameLength;
    int stringLen = nameLength + 1;
@ -480,23 +509,25 @@ void ParticipantUDP::Process(Participant* sender, NameMsg* msg) {
 #endif
 }

-void ParticipantUDP::Process(Participant* sender, ModelUrlMsg* msg) {
+void ParticipantUDPGeneric::Process(RemoteParticipantUDP* 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) {
+void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
+                                    PoseMsg* msg) {
 #if !defined(DEBUG) && !defined(NO_STD)
  std::cout << this->name << ": process PoseMsg [" << (int)this->networkId
            << "/" << (int)msg->networkId << "] " << (int)msg->poseType << "\n";
 #endif
-  Participant* owner = Participant::registry.Get(msg->networkId);
+  Participant* owner = registry.Get(msg->networkId);
  if (owner == nullptr)
    return;

-  Thing* thing = owner->Get(msg->thingId);
+  Thing* thing = owner->Get(msg->networkId, msg->thingId);
  if (thing == nullptr)
    return;

@ -510,15 +541,16 @@ void ParticipantUDP::Process(Participant* sender, PoseMsg* msg) {
    thing->SetAngularVelocity(msg->angularVelocity);
 }

-void ParticipantUDP::Process(Participant* sender, BinaryMsg* msg) {
+void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
+                                    BinaryMsg* msg) {
 #if defined(DEBUG)
  std::cout << this->name << ": process BinaryMsg [" << (int)msg->networkId
            << "/" << (int)msg->thingId << "]\n";
 #endif

-  Participant* owner = Participant::registry.Get(msg->networkId);
+  Participant* owner = registry.Get(msg->networkId);
  if (owner != nullptr) {
-    Thing* thing = owner->Get(msg->thingId);
+    Thing* thing = owner->Get(msg->networkId, msg->thingId);
    if (thing != nullptr)
      thing->ProcessBinary(msg->data);
 #if !defined(NO_STD)
@ -532,6 +564,26 @@ void ParticipantUDP::Process(Participant* sender, BinaryMsg* msg) {
  }
 }

+void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
+                                    TextMsg* msg) {
+#if defined(DEBUG)
+  std::cout << this->name << ": process TextMsg " << (int)msg->textLength << " "
+            << (int)msg->text << "\n";
+#endif
+}
+
+void ParticipantUDPGeneric::Process(RemoteParticipantUDP* sender,
+                                    DestroyMsg* msg) {
+#if defined(DEBUG)
+  std::cout << this->name << ": process Destroy [" << (int)msg->networkId << "/"
+            << (int)msg->thingId << "]\n";
+#endif
+
+  Thing* thing = sender->Get(msg->networkId, msg->thingId);
+  if (thing != nullptr)
+    this->Remove(thing);
+}
+
 // Receive
 #pragma endregion

--- a/Participants/ParticipantUDP.h
+++ b/Participants/ParticipantUDP.h
@ -5,9 +5,10 @@
 #include "Messages/InvestigateMsg.h"
 #include "Messages/ModelUrlMsg.h"
 #include "Messages/NameMsg.h"
+#include "Messages/NetworkIdMsg.h"
 #include "Messages/ParticipantMsg.h"
 #include "Messages/PoseMsg.h"
-#include "Messages/NetworkIdMsg.h"
+#include "Messages/TextMsg.h"
 #include "Messages/ThingMsg.h"
 #include "Participant.h"

@ -30,6 +31,74 @@ namespace RoboidControl {

 constexpr int MAX_SENDER_COUNT = 256;

+class RemoteParticipantUDP : public Participant {
+ public:
+ /// @brief Create a new participant with the given communcation info
+  /// @param ipAddress The IP address of the participant
+  /// @param port The UDP port of the participant
+  /// @remarks This does not belong here, it should move to ParticipantUDP or
+  /// something like that in the future
+  RemoteParticipantUDP(const char* ipAddress, int port);
+
+  /// @brief The Ip Address of a participant.
+  /// @remarks This does not belong here, it should move to ParticipantUDP or
+  /// something like that in the future
+  const char* ipAddress = "0.0.0.0";
+  /// @brief The port number for UDP communication with the participant.
+  /// @remarks This does not belong here, it should move to ParticipantUDP or
+  /// something like that in the future
+  unsigned int port = 0;
+
+  bool Send(IMessage* msg) override;
+};
+
+/// @brief class which manages all known participants
+class ParticipantRegistry {
+ public:
+  /// @brief Retrieve a participant by its address
+  /// @param ipAddress The IP address of the participant
+  /// @param port The port number of the participant
+  /// @return The participant or a nullptr when it could not be found
+  RemoteParticipantUDP* Get(const char* ipAddress, unsigned int port);
+  /// @brief Retrieve a participant by its network ID
+  /// @param networkID The network ID of the participant
+  /// @return The participant or a nullptr when it could not be found
+  RemoteParticipantUDP* Get(unsigned char networkID);
+
+  /// @brief Add a participant with the given details
+  /// @param ipAddress The IP address of the participant
+  /// @param port The port number of the participant
+  /// @return The added participant
+  RemoteParticipantUDP* Add(const char* ipAddress, unsigned int port);
+
+  /// @brief Add a participant
+  /// @param participant The participant to add
+  void Add(RemoteParticipantUDP* participant);
+
+  /// @brief Remove a participant
+  /// @param participant The participant to remove
+  void Remove(RemoteParticipantUDP* participant);
+
+ private:
+#if defined(NO_STD)
+ public:
+  RemoteParticipantUDP** GetAll() const;
+  int count = 0;
+
+ private:
+  RemoteParticipantUDP** participants;
+#else
+ public:
+  /// @brief Get all participants
+  /// @return All participants
+  const std::list<RemoteParticipantUDP*>& GetAll() const;
+
+ private:
+  /// @brief The list of known participants
+  std::list<RemoteParticipantUDP*> participants;
+#endif
+};
+
 /// @brief A participant using UDP communication
 /// A local participant is the local device which can communicate with
 /// other participants It manages all local things and communcation with other
@ -42,7 +111,7 @@ constexpr int MAX_SENDER_COUNT = 256;
 /// participant is created which can be obtained using
 /// RoboidControl::IsolatedParticipant::Isolated().
 /// @sa RoboidControl::Thing::Thing()
-class ParticipantUDP : public Participant {
+class ParticipantUDPGeneric : public RemoteParticipantUDP {
 #pragma region Init

 public:
@ -51,36 +120,32 @@ class ParticipantUDP : public Participant {
  /// 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);
+  ParticipantUDPGeneric(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
  /// @param localPort The port used by the local participant
-  ParticipantUDP(const char* ipAddress, int port = 7681, int localPort = 7681);
-
-  /// @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
+  ParticipantUDPGeneric(const char* ipAddress,
+                        int port = 7681,
+                        int localPort = 7681);

 #pragma endregion Init

+#pragma region Properties
+
+ public:
  /// @brief True if the participant is running isolated.
  /// Isolated participants do not communicate with other participants
  bool isIsolated = false;
+
  /// @brief The remote site when this participant is connected to a site
-  Participant* remoteSite = nullptr;
+  RemoteParticipantUDP* remoteSite = nullptr;

  /// The interval in milliseconds for publishing (broadcasting) data on the
  /// local network
  long publishInterval = 3000;  // 3 seconds

 protected:
-  char buffer[1024];
-
 #if !defined(ARDUINO)
 #if defined(__unix__) || defined(__APPLE__)
  int sock;
@ -94,16 +159,18 @@ class ParticipantUDP : public Participant {
  void begin();
  bool connected = false;

+#pragma endregion Properties
+
 #pragma region Update

 public:
-  virtual void Update() override;
+  virtual void Update(bool recurse = true) override;

 protected:
  unsigned long nextPublishMe = 0;

  /// @brief Prepare the local things for the next update
-  virtual void PrepMyThings();
+  // virtual void PrepMyThings();
  virtual void UpdateMyThings();
  virtual void UpdateOtherThings();

@ -114,34 +181,47 @@ class ParticipantUDP : public Participant {
  void SendThingInfo(Participant* remoteParticipant, Thing* thing);
  void PublishThingInfo(Thing* thing);

-  bool Send(Participant* remoteParticipant, IMessage* msg);
-  bool Publish(IMessage* msg);
+  virtual bool Send(IMessage* msg) override;
+  virtual bool Publish(IMessage* msg) = 0;

 #pragma endregion Send

 #pragma region Receive

-protected:
+ protected:
  void ReceiveData(unsigned char bufferSize,
                   char* senderIpAddress,
                   unsigned int senderPort);
-  void ReceiveData(unsigned char bufferSize, Participant* remoteParticipant);
+  void ReceiveData(unsigned char bufferSize, RemoteParticipantUDP* remoteParticipant);

-  void SetupUDP(int localPort, const char* remoteIpAddress, int remotePort);
+  virtual void SetupUDP(int localPort, const char* remoteIpAddress, int remotePort) = 0;

-  void ReceiveUDP();
+  virtual void ReceiveUDP() = 0;

-  virtual void Process(Participant* sender, ParticipantMsg* msg);
-  virtual void Process(Participant* sender, NetworkIdMsg* 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);
+  virtual void Process(RemoteParticipantUDP* sender, ParticipantMsg* msg);
+  virtual void Process(RemoteParticipantUDP* sender, NetworkIdMsg* msg);
+  virtual void Process(RemoteParticipantUDP* sender, InvestigateMsg* msg);
+
+  virtual void Process(RemoteParticipantUDP* sender, ThingMsg* msg);
+  virtual Thing* ProcessNewThing(RemoteParticipantUDP* sender,
+                                 ThingMsg* msg,
+                                 bool isRemote);
+
+  virtual void Process(RemoteParticipantUDP* sender, NameMsg* msg);
+  virtual void Process(RemoteParticipantUDP* sender, ModelUrlMsg* msg);
+  virtual void Process(RemoteParticipantUDP* sender, PoseMsg* msg);
+  virtual void Process(RemoteParticipantUDP* sender, BinaryMsg* msg);
+  virtual void Process(RemoteParticipantUDP* sender, TextMsg* msg);
+  virtual void Process(RemoteParticipantUDP* sender, DestroyMsg* msg);

 #pragma endregion Receive

+public:
+  static ParticipantRegistry registry;
+
 };

 }  // namespace RoboidControl
+
+#include "EspIdf/EspIdfParticipant.h"
+#include "Posix/PosixParticipant.h"
--- a/Participants/SiteServer.cpp
+++ b/Participants/SiteServer.cpp
@ -15,7 +15,7 @@ SiteServer::SiteServer(int port) : ParticipantUDP(port) {
  this->name = "Site Server";
  this->publishInterval = 0;

-  SetupUDP(port, ipAddress, 0);
+  //SetupUDP(port, ipAddress, 0);
 }

 #pragma endregion Init
@ -36,16 +36,16 @@ void SiteServer::UpdateMyThings() {
      for (int ix = 0; ix < Participant::registry.count; ix++) {
        Participant* participant = participants[ix];
 #else
-      for (Participant* participant : Participant::registry.GetAll()) {
+      for (Participant* participant : registry.GetAll()) {
 #endif
        if (participant == nullptr || participant == this)
          continue;

        PoseMsg* poseMsg = new PoseMsg(this->networkId, thing);
-        this->Send(participant, poseMsg);
+        participant->Send(poseMsg);
        delete poseMsg;
        BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing);
-        this->Send(participant, binaryMsg);
+        participant->Send(binaryMsg);
        delete binaryMsg;
      }
    }
@ -56,30 +56,30 @@ void SiteServer::UpdateMyThings() {

 #pragma region Receive

-void SiteServer::Process(Participant* sender, ParticipantMsg* msg) {
+void SiteServer::Process(RemoteParticipantUDP* sender, ParticipantMsg* msg) {
  if (msg->networkId != sender->networkId) {
    // std::cout << this->name << " received New Client -> " <<
    // sender->ipAddress
    //           << ":" << (int)sender->port << "\n";
    NetworkIdMsg* msg = new NetworkIdMsg(sender->networkId);
-    this->Send(sender, msg);
+    sender->Send(msg);
    delete msg;
  }
 }

-void SiteServer::Process(Participant* sender, NetworkIdMsg* msg) {}
+void SiteServer::Process(RemoteParticipantUDP* sender, NetworkIdMsg* msg) {}

-void SiteServer::Process(Participant* sender, ThingMsg* msg) {
-  Thing* thing = sender->Get(msg->thingId);
-  if (thing == nullptr)
+void SiteServer::Process(RemoteParticipantUDP* sender, ThingMsg* msg) {
+  Thing* thing = sender->Get(msg->networkId, msg->thingId);
+  if (thing == nullptr) {
    // new Thing(sender, (Thing::Type)msg->thingType, msg->thingId);
    // Thing::Reconstruct(sender, msg->thingType, msg->thingId);
    //thing = new Thing(msg->thingType, sender->root);
-    ;
+  }
  thing->id = msg->thingId;

  if (msg->parentId != 0) {
-    thing->SetParent(Get(msg->parentId));
+    thing->SetParent(Get(msg->networkId, msg->parentId));
    if (thing->IsRoot())
    // if (thing->GetParent() != nullptr)
 #if defined(NO_STD)
--- a/Participants/SiteServer.h
+++ b/Participants/SiteServer.h
@ -2,11 +2,11 @@

 #include "ParticipantUDP.h"

-#if !defined(NO_STD)
-#include <functional>
-#include <memory>
-#include <unordered_map>
-#endif
+// #if !defined(NO_STD)
+// #include <functional>
+// #include <memory>
+// #include <unordered_map>
+// #endif

 namespace RoboidControl {

@ -33,9 +33,9 @@ class SiteServer : public ParticipantUDP {
 protected:
  unsigned long nextPublishMe = 0;

-  virtual void Process(Participant* sender, ParticipantMsg* msg) override;
-  virtual void Process(Participant* sender, NetworkIdMsg* msg) override;
-  virtual void Process(Participant* sender, ThingMsg* msg) override;
+  virtual void Process(RemoteParticipantUDP* sender, ParticipantMsg* msg) override;
+  virtual void Process(RemoteParticipantUDP* sender, NetworkIdMsg* msg) override;
+  virtual void Process(RemoteParticipantUDP* sender, ThingMsg* msg) override;

 #pragma endregion Receive

--- a/Posix/PosixMQTT.cpp
+++ b/Posix/PosixMQTT.cpp
@ -0,0 +1,95 @@
+#include "PosixMQTT.h"
+
+#if 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 <thread>
+
+
+namespace RoboidControl {
+
+MQTTParticipant::MQTTParticipant(const char* remoteIpAddress, int port)
+    : MQTTParticipantBase(remoteIpAddress, port) {
+  SetupTCP();
+
+  std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+  send_mqtt_connect("RoboidControl1");
+  // sendSubscribe("domoticz/out");
+}
+
+void MQTTParticipant::SetupTCP() {
+  // Create a TCP socket
+  this->sock = socket(AF_INET, SOCK_STREAM, 0);
+  if (this->sock < 0) {
+    std::cerr << "Error creating socket" << std::endl;
+    return;
+  }
+
+  // Set the socket to non-blocking mode
+  int flags = fcntl(this->sock, F_GETFL, 0);
+  fcntl(this->sock, F_SETFL, flags | O_NONBLOCK);
+
+  this->remote_addr.sin_family = AF_INET;
+  this->remote_addr.sin_port = htons(this->remoteSite->port);
+  int result =
+      inet_pton(AF_INET, this->remoteSite->ipAddress, &remote_addr.sin_addr);
+  if (result <= 0) {
+    std::cerr << "Invalid address/ Address not supported" << std::endl;
+    close(this->sock);
+    return;
+  }
+
+  int connect_result =
+      connect(this->sock, (struct sockaddr*)&remote_addr, sizeof(remote_addr));
+  if (connect_result < 0 && errno != EINPROGRESS) {
+    std::cerr << "Error connecting to server:" << (int)errno << std::endl;
+    close(this->sock);
+    return;
+  }
+
+  std::cout << "TCP connection to " << this->remoteSite->ipAddress << ":"
+            << this->remoteSite->port << "\n";
+
+  this->connected = true;
+}
+
+void MQTTParticipant::SendTCP(int bufferSize) {
+  send(this->sock, this->buffer, bufferSize, 0);
+  std::cout << "Posix: sent TCP\n";
+}
+
+int MQTTParticipant::ReceiveTCP() {
+  if (this->connected == false)
+    return 0;
+
+  int bytesReceived = recv(this->sock, this->buffer, sizeof(this->buffer) - 1, 0);
+  if (bytesReceived > 0) {
+    std::cout << " !\n";
+    buffer[bytesReceived] = '\0';  // Null-terminate the received data
+    std::cout << "Received: " << this->buffer << std::endl;
+    return bytesReceived;
+  } else if (bytesReceived == 0) {
+    this->connected = false;
+    // Connection has been gracefully closed
+    std::cout << "Connection closed by the server." << std::endl;
+    return 0;
+  } else {
+    if (errno == EAGAIN || errno == EWOULDBLOCK) {
+      // No data available to read, continue the loop
+      // std::cout << "No data available" << std::endl;
+    } else {
+      std::cerr << "Error receiving data: " << strerror(errno) << std::endl;
+    }
+    return 0;
+  }
+  return 0;
+}
+
+}  // namespace RoboidControl
+#endif
--- a/Posix/PosixMQTT.h
+++ b/Posix/PosixMQTT.h
@ -0,0 +1,25 @@
+#pragma once
+#if defined(__unix__) || defined(__APPLE__)
+
+#include "Participants/ParticipantMQTT.h"
+
+namespace RoboidControl {
+
+class MQTTParticipant : public MQTTParticipantBase {
+ public:
+  MQTTParticipant(const char* ipAddress, int port = 1883);
+
+ protected:
+  void SetupTCP() override;
+
+  void SendTCP(int bufferSize) override;
+
+  int ReceiveTCP() override;
+
+  sockaddr_in remote_addr;
+  int sock;
+
+};
+
+}  // namespace RoboidControl
+#endif
--- a/Posix/PosixParticipant.cpp
+++ b/Posix/PosixParticipant.cpp
@ -1,4 +1,5 @@
 #include "PosixParticipant.h"
+#if defined(__unix__) || defined(__APPLE__)

 #if defined(__unix__) || defined(__APPLE__)
 #include <arpa/inet.h>
@ -9,28 +10,28 @@
 #endif

 namespace RoboidControl {
-namespace Posix {

-void ParticipantUDP::Setup(int localPort, const char* remoteIpAddress, int remotePort) {
+ParticipantUDP::ParticipantUDP(int port) : ParticipantUDPGeneric(port) {}
+
+ParticipantUDP::ParticipantUDP(const char* ipAddress, int port, int localPort)
+    : ParticipantUDPGeneric(ipAddress, port, localPort) {}
+
+void ParticipantUDP::SetupUDP(int localPort,
+                              const char* remoteIpAddress,
+                              int remotePort) {
 #if defined(__unix__) || defined(__APPLE__)

  // Create a UDP socket

  this->sock = socket(AF_INET, SOCK_DGRAM, 0);
-
  if (this->sock < 0) {
    std::cerr << "Error creating socket" << std::endl;
    return;
  }

-// 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__)
+  // Set the socket to non-blocking mode
  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
@ -55,7 +56,8 @@ void ParticipantUDP::Setup(int localPort, const char* remoteIpAddress, int remot
  }

  // Bind the socket to the specified port
-  if (bind(this->sock, (const struct sockaddr*)&server_addr, sizeof(server_addr)) < 0) {
+  if (bind(this->sock, (const struct sockaddr*)&server_addr,
+           sizeof(server_addr)) < 0) {
    std::cerr << "Bind failed" << std::endl;
    close(sock);
  }
@ -63,23 +65,59 @@ void ParticipantUDP::Setup(int localPort, const char* remoteIpAddress, int remot
 #endif
 }

-void ParticipantUDP::Receive() {
+void ParticipantUDP::SetupTCP(const char* remoteIpAddress, int remotePort) {
+#if defined(__unix__) || defined(__APPLE__)
+
+  // Create a UDP socket
+
+  this->sock = socket(AF_INET, SOCK_STREAM, 0);
+  if (this->sock < 0) {
+    std::cerr << "Error creating socket" << std::endl;
+    return;
+  }
+
+  remote_addr.sin_family = AF_INET;
+  remote_addr.sin_port = htons(remotePort);
+  inet_pton(AF_INET, remoteIpAddress, &remote_addr.sin_addr);
+
+  int connect_result =
+      connect(this->sock, (struct sockaddr*)&remote_addr, sizeof(remote_addr));
+  if (connect_result < 0) {  //} && errno != EINPROGRESS) {
+    std::cerr << "Error connecting to server" << std::endl;
+    close(this->sock);
+  }
+
+  std::cout << "TCP connection to " << remoteIpAddress << ":" << remotePort
+            << "\n";
+
+  // Set the socket to non-blocking mode
+  int flags = fcntl(this->sock, F_GETFL, 0);
+  fcntl(this->sock, F_SETFL, flags | O_NONBLOCK);
+
+#endif
+}
+
+void ParticipantUDP::ReceiveUDP() {
 #if defined(__unix__) || defined(__APPLE__)
  sockaddr_in client_addr;
  socklen_t len = sizeof(client_addr);
-  int packetSize = recvfrom(this->sock, buffer, sizeof(buffer), 0, (struct sockaddr*)&client_addr, &len);
+  int packetSize = recvfrom(this->sock, buffer, sizeof(buffer), 0,
+                            (struct sockaddr*)&client_addr, &len);
  if (packetSize > 0) {
    char sender_ipAddress[INET_ADDRSTRLEN];
-    inet_ntop(AF_INET, &(client_addr.sin_addr), 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::Participant* remoteParticipant = this->Get(sender_ipAddress, sender_port);
-    // if (remoteParticipant == nullptr) {
+    // RoboidControl::Participant* remoteParticipant =
+    // this->Get(sender_ipAddress, sender_port); if (remoteParticipant ==
+    // nullptr) {
    //   remoteParticipant = this->Add(sender_ipAddress, sender_port);
    //   // std::cout << "New sender " << sender_ipAddress << ":" << sender_port
    //   //           << "\n";
-    //   // std::cout << "New remote participant " << remoteParticipant->ipAddress
+    //   // std::cout << "New remote participant " <<
+    //   remoteParticipant->ipAddress
    //   //           << ":" << remoteParticipant->port << " "
    //   //           << (int)remoteParticipant->networkId << "\n";
    // }
@ -90,12 +128,38 @@ void ParticipantUDP::Receive() {
 #endif
 }

-bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
+// int ParticipantUDP::ReceiveTCP() {
+// #if defined(__unix__) || defined(__APPLE__)
+//   int bytesReceived = recv(sock, buffer, sizeof(buffer) - 1, 0);
+//   if (bytesReceived > 0) {
+//     buffer[bytesReceived] = '\0';  // Null-terminate the received data
+//     std::cout << "Received: " << buffer << std::endl;
+//     return bytesReceived;
+//   } else if (bytesReceived == 0) {
+//     // Connection has been gracefully closed
+//     std::cout << "Connection closed by the server." << std::endl;
+//     return 0;
+//   } else {
+//     if (errno == EAGAIN || errno == EWOULDBLOCK) {
+//       // No data available to read, continue the loop
+//       // std::cout << "No data available" << std::endl;
+//     } else {
+//       std::cerr << "Error receiving data: " << strerror(errno) << std::endl;
+//     }
+//     return 0;
+//   }
+// #endif  // _WIN32 || _WIN64
+//   return 0;
+// }
+
+bool ParticipantUDP::SendTo(RemoteParticipantUDP* remoteParticipant,
+                            int bufferSize) {
 #if defined(__unix__) || defined(__APPLE__)
-  // std::cout << "Send to " << remoteParticipant->ipAddress << ":" << ntohs(remoteParticipant->port)
+  // std::cout << "Send to " << remoteParticipant->ipAddress << ":" <<
+  // ntohs(remoteParticipant->port)
  //           << "\n";

-            // Set up the destination address
+  // Set up the destination address
  struct sockaddr_in dest_addr;
  memset(&dest_addr, 0, sizeof(dest_addr));
  dest_addr.sin_family = AF_INET;
@ -103,9 +167,11 @@ bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
  dest_addr.sin_addr.s_addr = inet_addr(remoteParticipant->ipAddress);

  // Send the message
-  int sent_bytes = sendto(sock, this->buffer, bufferSize, 0, (struct sockaddr*)&dest_addr, sizeof(dest_addr));
+  int sent_bytes = sendto(sock, this->buffer, bufferSize, 0,
+                          (struct sockaddr*)&dest_addr, sizeof(dest_addr));
  if (sent_bytes < 0) {
-    std::cerr << "sendto failed with error: " << sent_bytes << "  " << strerror(errno) << std::endl;
+    std::cerr << "sendto failed with error: " << sent_bytes << "  "
+              << strerror(errno) << std::endl;
    close(sock);
    return false;
  }
@ -113,6 +179,19 @@ bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
  return true;
 }

+// bool ParticipantUDP::SendTCP(int bufferSize) {
+// #if defined(__unix__) || defined(__APPLE__)
+//   // send(sock, this->buffer, bufferSize, 0);
+//   ssize_t sent_bytes = send(this->sock, this->buffer, bufferSize, 0);
+//   if (sent_bytes < 0) {
+//     std::cerr << "Failed to send packet" << strerror(errno) << std::endl;
+//     close(sock);
+//     return -1;
+//   }
+// #endif
+//   return false;
+// }
+
 bool ParticipantUDP::Publish(IMessage* msg) {
 #if defined(__unix__) || defined(__APPLE__)
  int bufferSize = msg->Serialize(this->buffer);
@ -121,8 +200,11 @@ bool ParticipantUDP::Publish(IMessage* msg) {

  char ip_str[INET_ADDRSTRLEN];
  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));
+  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 (sent_bytes < 0) {
    std::cerr << "sendto failed with error: " << sent_bytes << std::endl;
    close(sock);
@ -132,5 +214,6 @@ bool ParticipantUDP::Publish(IMessage* msg) {
  return true;
 }

-}  // namespace Posix
 }  // namespace RoboidControl
+
+#endif
--- a/Posix/PosixParticipant.h
+++ b/Posix/PosixParticipant.h
@ -1,16 +1,36 @@
 #pragma once
+#if defined(__unix__) || defined(__APPLE__)

 #include "Participants/ParticipantUDP.h"

 namespace RoboidControl {
-namespace Posix {

-class ParticipantUDP : public RoboidControl::ParticipantUDP {
+class ParticipantUDP : public ParticipantUDPGeneric {
 public:
-  void Setup(int localPort, const char* remoteIpAddress, int remotePort);
-  void Receive();
-  bool Send(Participant* remoteParticipant, int bufferSize);
-  bool Publish(IMessage* msg);
+  /// @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
+  /// @param localPort The port used by the local participant
+  ParticipantUDP(const char* ipAddress, int port = 7681, int localPort = 7681);
+
+  void SetupUDP(int localPort,
+                const char* remoteIpAddress,
+                int remotePort) override;
+  void ReceiveUDP() override;
+
+  //void Setup(int localPort, const char* remoteIpAddress, int remotePort);
+  void SetupTCP(const char* remoteIpAddress, int remotePort);
+
+  // void Receive();
+  int ReceiveTCP();
+  bool SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize);
+  bool Publish(IMessage* msg) override;

 protected:
 #if defined(__unix__) || defined(__APPLE__)
@ -20,5 +40,5 @@ class ParticipantUDP : public RoboidControl::ParticipantUDP {
 #endif
 };

-}  // namespace Posix
 }  // namespace RoboidControl
+#endif
--- a/README.md
+++ b/README.md
@ -14,4 +14,60 @@ Supporting:
 # Basic components

 - RoboidControl::Thing
- RoboidControl::Participant
+- RoboidControl::Participant
+
+# Installation
+
+## Core code
+
+The repository uses cmake for building. You can place it in a subfolder of your project and include it in you `CMakeLists.txt`.
+For example if the library is placed in the subfolder `roboidcontrol`:
+```
+# Add the path to Roboid Control
+add_subdirectory(roboidcontrol)
+
+# Your source files/executable
+add_executable(my_executable main.cpp) 
+
+# Link against RoboidControl
+target_link_libraries(my_executable RoboidControl)
+```
+
+## Arduino (PlatformIO)
+
+Arduino is only supported in combination with PlatformIO. The Arduino IDE is not (yet?) supported.
+
+The best way to include support for Roboid Control in PlatformIO is
+to clone the Roboid Control for C++ repository into a subfolder of the /lib folder.
+Alternatively you can download the zip file and unpack it as a subfolder of the /lib folder.
+
+## ESP-IDF
+
+The best way to include support for Roboid Control in PlatformIO is
+to clone the Roboid Control for C++ repository into a subfolder of the /components folder.
+Alternatively you can download the zip file and unpack it as a subfolder of the /components folder.
+
+Make sure you have included RoboidControl as a component in your top-level CMakeLists.txt, for example:
+```
+list(APPEND EXTRA_COMPONENT_DIRS
+    components/RoboidControl    
+    )
+```
+
+# Get Started
+
+## Core C++ Examples
+
+This repository contains examples in the `examples` folder. You can build these using cmake.
+
+For example, to build the BB2A example:
+``` 
+cmake -B build -D BUILD_EXAMPLE_BB2A=ON
+cmake --build build
+```
+The resulting executable is then `build/examples/Debug/BB2A.exe`
+
+## Arduino (PlatformIO) Examples
+
+Specific examples for the Arduino platform are found in the `Arduino\examples` folder.
+To use them you should create a new project in PlatformIO and then copy the example code to your project.
--- a/Testing/Temporary/LastTest.log
+++ b/Testing/Temporary/LastTest.log
@ -0,0 +1,3 @@
+Start testing: Jun 17 17:17 W. Europe Summer Time
+----------------------------------------------------------
+End testing: Jun 17 17:17 W. Europe Summer Time
--- a/Thing.cpp
+++ b/Thing.cpp
@ -28,7 +28,8 @@ Thing* Thing::LocalRoot() {

 // Only use this for root things
 Thing::Thing(Participant* owner) {
-  this->type = Type::Roboid;  // should become root
+  this->type = Type::Root;
+  this->name = "Root";

  this->position = Spherical::zero;
  this->positionUpdated = true;
@ -40,12 +41,16 @@ Thing::Thing(Participant* owner) {
  this->angularVelocity = Spherical::zero;

  this->owner = owner;
-  //this->owner->Add(this, true);
+  this->owner->Add(this);
  std::cout << this->owner->name << ": New root thing " << std::endl;
 }

-Thing::Thing(unsigned char thingType, Thing* parent) {
-  this->type = thingType;
+void Thing::CreateRoot(Participant* owner) {
+  owner->root = new Thing(owner);
+}
+
+Thing::Thing(Thing* parent) {
+  this->type = Type::Undetermined;

  this->position = Spherical::zero;
  this->positionUpdated = true;
@ -68,13 +73,6 @@ Thing::~Thing() {
  std::cout << "Destroy thing " << this->name << std::endl;
 }

-// Thing Thing::Reconstruct(Participant* owner, unsigned char thingType,
-// unsigned char thingId) {
-//   Thing thing = Thing(owner, thingType);
-//   thing.id = thingId;
-//   return thing;
-// }
-
 #pragma endregion Init

 void Thing::SetName(const char* name) {
@ -103,25 +101,10 @@ void Thing::SetParent(Thing* parent) {
  this->hierarchyChanged = true;
 }

-// void Thing::SetParent(Thing* parent) {
-//   parent->AddChild(this);
-//   this->hierarchyChanged = true;
-// }
-
-// const Thing& Thing::GetParent() {
-//   return *this->parent;
-// }
-
 bool Thing::IsRoot() const {
-  return this == LocalRoot() || this->parent == nullptr;  //&Thing::Root;
+  return this == LocalRoot() || this->parent == nullptr;
 }

-// void Thing::SetParent(Thing* root, const char* name) {
-//   Thing* thing = root->FindChild(name);
-//   if (thing != nullptr)
-//     this->SetParent(thing);
-// }
-
 Thing* Thing::GetParent() {
  return this->parent;
 }
@ -270,24 +253,16 @@ unsigned long Thing::GetTimeMs() {
 #endif
 }

-// void Thing::Update(bool recursive) {
-//   Update(GetTimeMs(), recursive);
-// }
-
-void Thing::PrepareForUpdate() {}
-
 void Thing::Update(bool recursive) {
-  // if (this->positionUpdated || this->orientationUpdated)
-  //   OnPoseChanged callback
  this->positionUpdated = false;
  this->orientationUpdated = false;
-  // this->linearVelocityUpdated = false;
-  // this->angularVelocityUpdated = false;
+  this->linearVelocityUpdated = false;
+  this->angularVelocityUpdated = false;
  this->hierarchyChanged = false;
  this->nameChanged = false;

  if (recursive) {
-    // std::cout << "# children: " << (int)this->childCount << std::endl;
+     std::cout << "# children: " << (int)this->childCount << std::endl;
    for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
      Thing* child = this->children[childIx];
      if (child == nullptr)
@ -297,10 +272,6 @@ void Thing::Update(bool recursive) {
  }
 }

-void Thing::UpdateThings() {
-  IsolatedParticipant::Isolated()->Update();
-}
-
 #pragma endregion Update

 int Thing::GenerateBinary(char* buffer, unsigned char* ix) {
--- a/Thing.h
+++ b/Thing.h
@ -10,7 +10,7 @@
 namespace RoboidControl {

 class Participant;
-class ParticipantUDP;
+class ParticipantUDPGeneric;

 #define THING_STORE_SIZE 256
 // IMPORTANT: values higher than 256 will need to change the Thing::id type
@ -20,57 +20,57 @@ class ParticipantUDP;
 class Thing {
 public:
  /// @brief Predefined thing types
-  enum Type : unsigned char {
-    Undetermined,
-    // Sensor,
-    Switch,
-    DistanceSensor,
-    DirectionalSensor,
-    TemperatureSensor,
-    TouchSensor,
-    // Motor,
-    ControlledMotor,
-    UncontrolledMotor,
-    Servo,
-    IncrementalEncoder,
+  struct Type {
+    static const unsigned char Undetermined = 0x00;
+    // Sensor
+    static const unsigned char Switch = 0x01;
+    static const unsigned char DistanceSensor = 0x02;
+    static const unsigned char DirectionalSensor = 0x03;
+    static const unsigned char TemperatureSensor = 0x04;
+    static const unsigned char TouchSensor = 0x05;
+    // Motor
+    static const unsigned char ControlledMotor = 0x06;
+    static const unsigned char UncontrolledMotor = 0x07;
+    static const unsigned char Servo = 0x08;
+    static const unsigned char RelativeEncoder = 0x19;
    // Other
-    Roboid,
-    Humanoid,
-    ExternalSensor,
-    DifferentialDrive
+    static const unsigned char Root = 0x10;
+    static const unsigned char Roboid = 0x09;
+    static const unsigned char Humanoid = 0x0A;
+    static const unsigned char ExternalSensor = 0x08;
+    static const unsigned char Animator = 0x0C;
+    static const unsigned char DifferentialDrive = 0x0D;
  };

 #pragma region Init
-  static Thing* LocalRoot();

 private:
-  // Special constructor to create a root thing
-  Thing(Participant* parent);
-  // Which can only be used by the Participant
-  friend class Participant;
-
 public:
-  /// @brief Create a new thing
-  /// @param thingType The type of thing (can use Thing::Type)
+  /// @brief Create a new Thing
  /// @param parent (optional) The parent thing
  /// The owner will be the same as the owner of the parent thing, it will
  /// be Participant::LocalParticipant if the parent is not specified. A thing
-  /// without a parent will be a root thing.
-  Thing(unsigned char thingType = Thing::Type::Undetermined,
-        Thing* parent = LocalRoot());
+  /// without a parent will be connected to the root thing.
+  Thing(Thing* parent = LocalRoot());

-  /// @brief Create a new child thing
-  /// @param parent The parent thing
-  /// @param thingType The type of thing (can use Thing::Type)
-  /// @param thingId The ID of the thing, leave out or set to zero to generate
-  /// an ID
-  /// @note The owner will be the same as the owner of the parent thing
+ private:
+  /// @brief Constructor to create a root thing
+  /// @param owner The participant who will own this root thing
+  /// @remarks This function is private because CreateRoot() should be used
+  /// instead
+  Thing(Participant* owener);

+ public:
+  /// @brief Destructor for a Thing
  ~Thing();

-  static Thing Reconstruct(Participant* owner,
-                           unsigned char thingType,
-                           unsigned char thingId);
+  /// @brief Create a root thing for a participant
+  /// @param owner The participant who will own this root thing
+  static void CreateRoot(Participant* owner);
+
+  /// @brief The root thing for the local participant
+  /// @return The root thing for the local participant
+  static Thing* LocalRoot();

 #pragma endregion Init

@ -80,9 +80,7 @@ class Thing {

 #pragma region Properties

-  /// @brief The participant managing this thing
-  Participant* owner = nullptr;
-
+ public:
  /// @brief The ID of the thing
  unsigned char id = 0;

@ -90,10 +88,17 @@ class Thing {
  /// This can be either a Thing::Type of a byte value for custom types
  unsigned char type = Type::Undetermined;

+  /// @brief Is this a remote thing?
+  /// A remote thing is owned by other participant
+  /// and is not simulated by the local participant
+  bool isRemote = false;
+
+  /// @brief The participant owning this thing
+  Participant* owner = nullptr;
+
  /// @brief The name of the thing
  const char* name = nullptr;

- public:
  void SetName(const char* name);
  const char* GetName() const;
  bool nameChanged = false;
@ -102,14 +107,12 @@ class Thing {
  /// loaded from
  /// @param url The url of the model
  /// @remark Although the roboid implementation is not dependent on the model,
-  /// the only official supported model format is .obj
+  /// the only official supported model formats are .png (sprite), .gltf and .glb
  void SetModel(const char* url);

  /// @brief An URL pointing to the location where a model of the thing can be
  /// found
  const char* modelUrl = nullptr;
-  /// @brief The scale of the model (deprecated I think)
-  float modelScale = 1;

 #pragma endregion Properties

@ -117,13 +120,13 @@ class Thing {

  /// @brief Sets the parent of this Thing
  /// @param parent The Thing which should become the parent
-  // virtual void SetParent(Thing* parent);
  void SetParent(Thing* parent);
  /// @brief Gets the parent of this Thing
  /// @return The parent Thing
-  // Thing* GetParent();
  Thing* GetParent();

+  /// @brief Check if this is a root thing
+  /// @return True is this thing is a root
  bool IsRoot() const;

  /// @brief The number of children
@ -225,16 +228,12 @@ class Thing {
 #pragma region Update

 public:
-  virtual void PrepareForUpdate();
+  //virtual void PrepareForUpdate();

  /// @brief Updates the state of the thing
-  /// @param currentTimeMs The current clock time in milliseconds; if this is
-  /// zero, the current time is retrieved automatically
  /// @param recurse When true, this will Update the descendants recursively
  virtual void Update(bool recurse = false);

-  static void UpdateThings();
-
  /// @brief Get the current time in milliseconds
  /// @return The current time in milliseconds
  static unsigned long GetTimeMs();
--- a/Things/ControlledMotor.cpp
+++ b/Things/ControlledMotor.cpp
@ -39,8 +39,8 @@ void ControlledMotor::Update(bool recurse) {
  this->lastError = error;

  float output = p_term + i_term + d_term;
-  std::cout << "target " << this->targetVelocity << " actual "
-            << this->actualVelocity << " output = " << output << std::endl;
+  // std::cout << "target " << this->targetVelocity << " actual "
+  //           << this->actualVelocity << " output = " << output << std::endl;
  // float acceleration =
  //     error * timeStep * pidP;  // Just P is used at this moment
  // std::cout << "motor acc. " << acceleration << std::endl;
--- a/Things/DifferentialDrive.cpp
+++ b/Things/DifferentialDrive.cpp
@ -4,8 +4,8 @@

 namespace RoboidControl {

-DifferentialDrive::DifferentialDrive(Thing* parent)
-    : Thing(Type::DifferentialDrive, parent) {
+DifferentialDrive::DifferentialDrive(Thing* parent) : Thing(parent) {
+  this->type = Type::DifferentialDrive;
  this->name = "Differential drive";

  this->leftWheel = new Motor(this);
@ -18,7 +18,8 @@ DifferentialDrive::DifferentialDrive(Thing* parent)
 DifferentialDrive::DifferentialDrive(Motor* leftMotor,
                                     Motor* rightMotor,
                                     Thing* parent)
-    : Thing(Type::DifferentialDrive, parent) {
+    : Thing(parent) {
+  this->type = Type::DifferentialDrive;
  this->name = "Differential drive";
  this->leftWheel = leftMotor;
  this->rightWheel = rightMotor;
--- a/Things/DigitalSensor.cpp
+++ b/Things/DigitalSensor.cpp
@ -2,18 +2,9 @@

 namespace RoboidControl {

-//DigitalSensor::DigitalSensor() : Thing(Type::Switch) {}
-
-// DigitalSensor::DigitalSensor(Participant* owner, unsigned char thingId)
-//     : Thing(owner, Type::Switch, thingId) {}
-
-// DigitalSensor::DigitalSensor(Thing* parent, unsigned char thingId)
-//     : Thing(parent, Type::Switch) {}
-
-// DigitalSensor::DigitalSensor(Participant* owner) : Thing(owner, Type::Switch) {}
-
-// DigitalSensor::DigitalSensor(Thing* parent) : Thing(parent, Type::Switch) {}
-DigitalSensor::DigitalSensor(Thing* parent) : Thing(Type::Switch, parent) {}
+DigitalSensor::DigitalSensor(Thing* parent) : Thing(parent) {
+  this->type = Type::Switch;
+}

 int DigitalSensor::GenerateBinary(char* bytes, unsigned char* ix) {
  bytes[(*ix)++] = state ? 1 : 0;
--- a/Things/DistanceSensor.cpp
+++ b/Things/DistanceSensor.cpp
@ -0,0 +1,29 @@
+#include "DistanceSensor.h"
+
+#include "Messages/LowLevelMessages.h"
+
+namespace RoboidControl {
+
+DistanceSensor::DistanceSensor(Thing* parent) : Thing(parent) {
+  this->type = Type::DistanceSensor;
+  this->name = "Distance sensor";
+}
+
+float DistanceSensor::GetDistance() {
+  if (this->externalDistance < this->internalDistance)
+    return this->externalDistance;
+  else
+    return this->internalDistance;
+}
+
+int DistanceSensor::GenerateBinary(char* bytes, unsigned char* ix) {
+  LowLevelMessages::SendFloat16(bytes, ix, this->internalDistance);
+  return *ix;
+}
+
+void DistanceSensor::ProcessBinary(char* bytes) {
+  unsigned char ix = 0;
+  this->externalDistance = LowLevelMessages::ReceiveFloat16(bytes, &ix);
+}
+
+}  // namespace RoboidControl
--- a/Things/DistanceSensor.h
+++ b/Things/DistanceSensor.h
@ -0,0 +1,41 @@
+#pragma once
+
+#if !NO_STD
+#include <limits>
+#endif
+
+#include "Thing.h"
+
+namespace RoboidControl {
+
+/// @brief A sensor measuring distance
+class DistanceSensor : public Thing {
+ public:
+  /// @brief Create a new child touch sensor
+  /// @param parent The parent thing
+  /// @param thingId The ID of the thing, leave out or set to zero to generate
+  /// an ID
+  DistanceSensor(Thing* parent = Thing::LocalRoot());
+
+  /// @brief Get the current distance
+  float GetDistance();
+
+  /// @brief Function used to generate binary data for this sensor
+  /// @param buffer The byte array for thw binary data
+  /// @param ix The starting position for writing the binary data
+  int GenerateBinary(char* bytes, unsigned char* ix) override;
+  /// @brief Function used to process binary data received for this sensor
+  /// @param bytes The binary data to process
+  virtual void ProcessBinary(char* bytes) override;
+
+ protected:
+#if ARDUNIO
+  float internalDistance = INFINITY;
+  float externalDistance = INFINITY;
+#else
+  float internalDistance = std::numeric_limits<double>::infinity();
+  float externalDistance = std::numeric_limits<double>::infinity();
+#endif
+};
+
+}  // namespace RoboidControl
--- a/Things/Motor.cpp
+++ b/Things/Motor.cpp
@ -1,11 +1,29 @@
 #include "Motor.h"

+#include "Messages/BinaryMsg.h"
+#include "Participant.h"
+
 namespace RoboidControl {

-Motor::Motor(Thing* parent) : Thing(Type::UncontrolledMotor, parent) {}
+Motor::Motor(Thing* parent) : Thing(parent) {
+  this->type = Type::UncontrolledMotor;
+}

 void Motor::SetTargetVelocity(float targetSpeed) {
+  if (targetSpeed != this->targetVelocity) {
    this->targetVelocity = targetSpeed;
+
+    if (this->owner->networkId != 0) {
+      // in other word: if we are connected...
+      BinaryMsg* binaryMsg = new BinaryMsg(this->owner->networkId, this);
+      this->owner->Send(binaryMsg);
+      delete binaryMsg;
+    }
+  }
+}
+
+float Motor::GetTargetVelocity() {
+  return this->targetVelocity;
 }

 int Motor::GenerateBinary(char* data, unsigned char* ix) {
--- a/Things/Motor.h
+++ b/Things/Motor.h
@ -14,11 +14,11 @@ class Motor : public Thing {
  Direction direction;

  virtual void SetTargetVelocity(float velocity);  // -1..0..1
+  virtual float GetTargetVelocity();

  int GenerateBinary(char* bytes, unsigned char* ix) override;
-  // virtual void ProcessBinary(char* bytes) override;

- //protected:
+ protected:
  float targetVelocity = 0;
 };

--- a/Things/RelativeEncoder.cpp
+++ b/Things/RelativeEncoder.cpp
@ -2,8 +2,9 @@

 namespace RoboidControl {

-RelativeEncoder::RelativeEncoder(Thing* parent)
-    : Thing(Type::IncrementalEncoder, parent) {}
+RelativeEncoder::RelativeEncoder(Thing* parent) : Thing(parent) {
+  this->type = Type::RelativeEncoder;
+}

 float RelativeEncoder::GetRotationSpeed() {
  return rotationSpeed;
--- a/Things/TemperatureSensor.cpp
+++ b/Things/TemperatureSensor.cpp
@ -4,15 +4,9 @@

 namespace RoboidControl {

-// TemperatureSensor::TemperatureSensor(Participant* participant,
-//                                      unsigned char thingId)
-//     : Thing(participant, Type::TemperatureSensor, thingId) {}
-
-// TemperatureSensor::TemperatureSensor(Participant* owner) : Thing(owner, Type::TemperatureSensor) {}
-
-TemperatureSensor::TemperatureSensor(Thing* parent) : Thing(Type::TemperatureSensor, parent) {}
-
-// TemperatureSensor::TemperatureSensor(Thing* parent) : Thing(parent, Type::TemperatureSensor) {}
+TemperatureSensor::TemperatureSensor(Thing* parent) : Thing(parent) {
+  this->type = Type::TemperatureSensor;
+}

 void TemperatureSensor::SetTemperature(float temp) {
  this->temperature = temp;
--- a/Things/TouchSensor.cpp
+++ b/Things/TouchSensor.cpp
@ -2,20 +2,26 @@

 namespace RoboidControl {

-TouchSensor::TouchSensor(Thing* parent) : Thing(Type::TouchSensor, parent) {
+TouchSensor::TouchSensor(Thing* parent) : Thing(parent) {
+  this->type = Type::TouchSensor;
  this->name = "Touch sensor";
 }

-void TouchSensor::PrepareForUpdate() {
-  this->touchedSomething = this->externalTouch;
+bool TouchSensor::IsTouching() {
+  return this->internalTouch || this->externalTouch;
 }

+// void TouchSensor::PrepareForUpdate() {
+//   //this->internalTouch = this->externalTouch;
+// }
+
 void TouchSensor::Update(bool recursive) {
  Thing::Update(recursive);
 }

 int TouchSensor::GenerateBinary(char* bytes, unsigned char* ix) {
-  bytes[(*ix)++] = this->touchedSomething ? 1 : 0;
+  std::cout << "BinaryMsg Touch " << this->internalTouch << std::endl;
+  bytes[(*ix)++] = this->internalTouch ? 1 : 0;
  return 1;
 }

--- a/Things/TouchSensor.h
+++ b/Things/TouchSensor.h
@ -6,7 +6,7 @@ namespace RoboidControl {

 /// @brief A sensor which can detect touches
 class TouchSensor : public Thing {
-// Why finishing this release (0.3), I notice that this is equivalent to a digital sensor
+// When finishing this release (0.3), I notice that this is equivalent to a digital sensor
 public:
  /// @brief Create a new child touch sensor
  /// @param parent The parent thing
@ -16,9 +16,9 @@ class TouchSensor : public Thing {

  /// @brief Value which is true when the sensor is touching something, false
  /// otherwise
-  bool touchedSomething = false;
+  bool IsTouching();

-  virtual void PrepareForUpdate() override;
+  //virtual void PrepareForUpdate() override;
  virtual void Update(bool recursive) override;

  /// @brief Function used to generate binary data for this touch sensor
@ -30,6 +30,7 @@ class TouchSensor : public Thing {
  virtual void ProcessBinary(char* bytes) override;
 protected:
  bool externalTouch = false;
+  bool internalTouch = false;
 };

 }  // namespace RoboidControl
--- a/Windows/WindowsParticipant.cpp
+++ b/Windows/WindowsParticipant.cpp
@ -1,4 +1,5 @@
 #include "WindowsParticipant.h"
+#if defined(_WIN32) || defined(_WIN64)

 #if defined(_WIN32) || defined(_WIN64)
 #include <winsock2.h>
@ -7,9 +8,12 @@
 #endif

 namespace RoboidControl {
-namespace Windows {

-void ParticipantUDP::Setup(int localPort, const char* remoteIpAddress, int remotePort) {
+ParticipantUDP::ParticipantUDP() {}
+
+void ParticipantUDP::Setup(int localPort,
+                           const char* remoteIpAddress,
+                           int remotePort) {
 #if defined(_WIN32) || defined(_WIN64)

  // Create a UDP socket
@ -19,19 +23,28 @@ void ParticipantUDP::Setup(int localPort, const char* remoteIpAddress, int remot
    std::cerr << "WSAStartup failed" << std::endl;
    return;
  }
-  this->sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);

-  if (this->sock < 0) {
-    std::cerr << "Error creating socket" << std::endl;
+  // Create an UDP socket
+  this->sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
+  if (this->sock == INVALID_SOCKET) {
+    std::cerr << "UDP Socket creation failed: " << WSAGetLastError()
+              << std::endl;
+    WSACleanup();
    return;
  }

-// Set the socket to non-blocking mode
+  // Set the socket to non-blocking mode
  u_long mode = 1;  // 1 to enable non-blocking socket
-  ioctlsocket(this->sock, FIONBIO, &mode);
+  if (ioctlsocket(sock, FIONBIO, &mode) != NO_ERROR) {
+    std::cerr << "Failed to set non-blocking mode: " << WSAGetLastError()
+              << std::endl;
+    closesocket(sock);
+    WSACleanup();
+    return;
+  }

  if (remotePort != 0) {
-    // Set up the address to send to
+    // Define the server address
    memset(&remote_addr, 0, sizeof(remote_addr));
    remote_addr.sin_family = AF_INET;
    remote_addr.sin_port = htons((u_short)remotePort);
@ -55,13 +68,64 @@ void ParticipantUDP::Setup(int localPort, const char* remoteIpAddress, int remot
  }

  // Bind the socket to the specified port
-  if (bind(this->sock, (const struct sockaddr*)&server_addr, sizeof(server_addr)) < 0) {
+  if (bind(this->sock, (const struct sockaddr*)&server_addr,
+           sizeof(server_addr)) < 0) {
    std::cerr << "Bind failed" << std::endl;
    closesocket(sock);
    WSACleanup();
  }

-#endif // _WIN32 || _WIN64
+#endif  // _WIN32 || _WIN64
+}
+
+void ParticipantUDP::SetupTCP(const char* remoteIpAddress, int remotePort) {
+#if defined(_WIN32) || defined(_WIN64)
+  // Initialize Winsock
+  WSADATA wsaData;
+  if (WSAStartup(MAKEWORD(2, 2), &wsaData) != 0) {
+    std::cerr << "WSAStartup failed: " << std::endl;
+    return;
+  }
+
+  // Create a TCP socket
+  this->sock = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
+  if (sock == INVALID_SOCKET) {
+    std::cerr << "TCP Socket creation failed: " << WSAGetLastError()
+              << std::endl;
+    WSACleanup();
+    return;
+  }
+
+  // Set the socket to non-blocking mode
+  u_long mode = 1;  // 1 to enable non-blocking socket
+  if (ioctlsocket(sock, FIONBIO, &mode) != NO_ERROR) {
+    std::cerr << "Failed to set non-blocking mode: " << WSAGetLastError()
+              << std::endl;
+    closesocket(sock);
+    WSACleanup();
+    return;
+  }
+
+  // Define the server address
+  memset(&this->remote_addr, 0, sizeof(server_addr));
+  this->remote_addr.sin_family = AF_INET;
+  this->remote_addr.sin_port = htons((u_short)remotePort);
+  if (inet_pton(AF_INET, remoteIpAddress, &this->remote_addr.sin_addr) <= 0) {
+    std::cerr << "Invalid address" << std::endl;
+    closesocket(sock);
+    WSACleanup();
+    return;
+  }
+
+  // Connect to the server
+  if (connect(sock, (sockaddr*)&this->remote_addr, sizeof(this->remote_addr)) ==
+      SOCKET_ERROR) {
+    std::cerr << "Connection failed: " << WSAGetLastError() << std::endl;
+    closesocket(sock);
+    WSACleanup();
+    return;
+  }
+#endif  // _WIN32 || _WIN64
 }

 void ParticipantUDP::Receive() {
@ -73,7 +137,8 @@ void ParticipantUDP::Receive() {

  sockaddr_in client_addr;
  int len = sizeof(client_addr);
-  int packetSize = recvfrom(this->sock, buffer, sizeof(buffer), 0, (struct sockaddr*)&client_addr, &len);
+  int packetSize = recvfrom(this->sock, buffer, sizeof(buffer), 0,
+                            (struct sockaddr*)&client_addr, &len);
  // std::cout << "received data " << packetSize << "\n";
  if (packetSize < 0) {
    int error_code = WSAGetLastError();  // Get the error code on Windows
@ -81,12 +146,14 @@ void ParticipantUDP::Receive() {
      std::cerr << "recvfrom failed with error: " << error_code << std::endl;
  } else if (packetSize > 0) {
    char sender_ipAddress[INET_ADDRSTRLEN];
-    inet_ntop(AF_INET, &(client_addr.sin_addr), 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::ParticipantUDP* remoteParticipant = this->Get(sender_ipAddress, sender_port);
-    // if (remoteParticipant == nullptr) {
+    // RoboidControl::ParticipantUDP* remoteParticipant =
+    // this->Get(sender_ipAddress, sender_port); if (remoteParticipant ==
+    // nullptr) {
    //   remoteParticipant = this->Add(sender_ipAddress, sender_port);
    //   // std::cout << "New sender " << sender_ipAddress << ":"
    //   //           << sender_port << "\n";
@ -99,15 +166,43 @@ void ParticipantUDP::Receive() {

    // ReceiveData(packetSize, remoteParticipant);
  }
-#endif // _WIN32 || _WIN64
+#endif  // _WIN32 || _WIN64
 }

-bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
+int ParticipantUDP::ReceiveTCP() {
+#if defined(_WIN32) || defined(_WIN64)
+  int bytesReceived = recv(sock, buffer, sizeof(buffer) - 1, 0);
+  if (bytesReceived > 0) {
+    buffer[bytesReceived] = '\0';  // Null-terminate the received data
+    std::cout << "Received: " << buffer << std::endl;
+    return bytesReceived;
+  } else if (bytesReceived == 0) {
+    // Connection has been gracefully closed
+    std::cout << "Connection closed by the server." << std::endl;
+    return 0;
+  } else {
+    int error = WSAGetLastError();
+    if (error == WSAEWOULDBLOCK) {
+      // No data available, continue with other tasks
+      std::cout << "No data available, continuing..." << std::endl;
+      // You can add a sleep or other logic here to avoid busy waiting
+    } else {
+      std::cerr << "Receive failed: " << error << std::endl;
+    }
+    return 0;
+  }
+#endif  // _WIN32 || _WIN64
+  return 0;
+}
+
+bool ParticipantUDP::SendTo(RemoteParticipantUDP* 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));
+  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 (sent_bytes <= SOCKET_ERROR) {
    int error_code = WSAGetLastError();  // Get the error code on Windows
@ -116,10 +211,17 @@ bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
    WSACleanup();
    return false;
  }
-#endif // _WIN32 || _WIN64
+#endif  // _WIN32 || _WIN64
  return true;
 }

+bool ParticipantUDP::SendTCP(int bufferSize) {
+#if defined(_WIN32) || defined(_WIN64)
+  send(sock, this->buffer, bufferSize, 0);
+#endif
+  return false;
+}
+
 bool ParticipantUDP::Publish(IMessage* msg) {
 #if defined(_WIN32) || defined(_WIN64)
  int bufferSize = msg->Serialize(this->buffer);
@ -128,8 +230,11 @@ bool ParticipantUDP::Publish(IMessage* msg) {

  char ip_str[INET_ADDRSTRLEN];
  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));
+  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 (sent_bytes <= SOCKET_ERROR) {
    int error_code = WSAGetLastError();  // Get the error code on Windows
@ -138,9 +243,10 @@ bool ParticipantUDP::Publish(IMessage* msg) {
    WSACleanup();
    return false;
  }
-#endif // _WIN32 || _WIN64
+#endif  // _WIN32 || _WIN64
  return true;
 }

-}  // namespace Windows
 }  // namespace RoboidControl
+
+#endif
--- a/Windows/WindowsParticipant.h
+++ b/Windows/WindowsParticipant.h
@ -1,22 +1,27 @@
 #pragma once
+#if defined(_WIN32) || defined(_WIN64)

 #include "Participants/ParticipantUDP.h"

 namespace RoboidControl {
-namespace Windows {

-class ParticipantUDP : public RoboidControl::ParticipantUDP {
+class ParticipantUDP : public ParticipantUDPGeneric {
 public:
+  ParticipantUDP();
  void Setup(int localPort, const char* remoteIpAddress, int remotePort);
-  void Receive();
-  bool Send(Participant* remoteParticipant, int bufferSize);
+  void SetupTCP(const char* remoteIpAddres, int remotePort);
+  bool SendTo(RemoteParticipantUDP* remoteParticipant, int bufferSize);
+  bool SendTCP(int bufferSize);
  bool Publish(IMessage* msg);

+  void Receive();
+  int ReceiveTCP();
+
 protected:
- #if defined(_WIN32) || defined(_WIN64)
+#if defined(_WIN32) || defined(_WIN64)
  SOCKET sock;
-  #endif
+#endif
 };

-}  // namespace Windows
 }  // namespace RoboidControl
+#endif
--- a/examples/BB2A/main.cpp
+++ b/examples/BB2A/main.cpp
@ -27,10 +27,10 @@ int main() {
  while (true) {
    // The left wheel turns forward when nothing is touched on the right side
    // and turn backward when the roboid hits something on the right
-    float leftWheelSpeed = (touchRight.touchedSomething) ? -600.0f : 600.0f;
+    float leftWheelSpeed = (touchRight.IsTouching()) ? -600.0f : 600.0f;
    // The right wheel does the same, but instead is controlled by
    // touches on the left side
-    float rightWheelSpeed = (touchLeft.touchedSomething) ? -600.0f : 600.0f;
+    float rightWheelSpeed = (touchLeft.IsTouching()) ? -600.0f : 600.0f;
    // When both sides are touching something, both wheels will turn backward
    // and the roboid will move backwards
    bb2b.SetWheelVelocity(leftWheelSpeed, rightWheelSpeed);
@ -40,9 +40,9 @@ int main() {

    // and sleep for 100ms
 #if defined(ARDUINO)
-    delay(100);
+    delay(10);
 #else
-    sleep_for(milliseconds(100));
+    sleep_for(milliseconds(10));
 #endif
  }

--- a/examples/CMakeLists.txt
+++ b/examples/CMakeLists.txt
@ -1,25 +1,8 @@
 # examples/CMakeLists.txt

-# Specify the minimum CMake version
-cmake_minimum_required(VERSION 3.10)
+# Check if the options are enabled and add the corresponding examples
+if(BUILD_EXAMPLE_BB2A)
+    add_executable(BB2A BB2A/main.cpp)  # Adjust the path as necessary

-# Specify the path to the main project directory
-set(MAIN_PROJECT_DIR "${CMAKE_SOURCE_DIR}/..")
-
-# Set the project name
-project(Examples)
-
-include_directories(..)
-
-# Add the executable for the main project
-#add_executable(MainExecutable ${SOURCES})
-# Find the main project library (assuming it's defined in the root CMakeLists.txt)
-#find_package(RoboidControl REQUIRED)  # Replace MyLibrary with your actual library name
-
-# Add example executables
-add_executable(BB2B BB2B.cpp)
-target_link_libraries(
-    BB2B
-    RoboidControl
-    LinearAlgebra
-)
+    target_link_libraries(BB2A RoboidControl)
+endif()
--- a/examples/README.md
+++ b/examples/README.md
@ -1 +1 @@
-Important: this folder has to be names 'examples' exactly to maintain compatibility with Arduino
+Important: this folder has to be named 'examples' exactly to maintain compatibility with Arduino
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@ -0,0 +1,30 @@
+# Unit test configuration
+
+add_compile_definitions(GTEST)
+include(FetchContent)
+FetchContent_Declare(
+    googletest
+    DOWNLOAD_EXTRACT_TIMESTAMP ON
+    URL https://github.com/google/googletest/archive/refs/heads/main.zip
+)
+
+# For Windows: Prevent overriding the parent project's compiler/linker settings
+set(gtest_force_shared_crt ON CACHE BOOL "" FORCE)
+FetchContent_MakeAvailable(googletest)
+
+file(GLOB_RECURSE test_srcs *.cc)
+message(STATUS "Test sources: ${test_srcs}")
+add_executable(
+    RoboidControlTest
+    ${test_srcs}
+)
+message(STATUS "RoboidControlTest target created")
+target_link_libraries(
+    RoboidControlTest
+    gtest_main
+    RoboidControl
+    LinearAlgebra
+)
+
+include(GoogleTest)
+gtest_discover_tests(RoboidControlTest)
--- a/test/participant_test.cc
+++ b/test/participant_test.cc
@ -5,6 +5,7 @@
 #include <gtest/gtest.h>

 #include "Participants/SiteServer.h"
+#include "Participants/ParticipantMQTT.h"
 #include "Thing.h"

 #include <chrono>
@ -75,4 +76,18 @@ TEST(Participant, ThingMsg) {
  SUCCEED();
 }

+TEST(Participant, MQTT) {
+  MQTTParticipant* participant = new MQTTParticipant("192.168.77.11");
+
+  unsigned long milliseconds = Thing::GetTimeMs();
+  unsigned long startTime = milliseconds;
+  while (milliseconds < startTime + 10000) {
+    participant->Update();
+
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    milliseconds = Thing::GetTimeMs();
+  }
+  SUCCEED();
+}
+
 #endif
--- a/test/thing_test.cc
+++ b/test/thing_test.cc
@ -15,7 +15,7 @@ TEST(RoboidControlSuite, HiddenParticipant) {
  unsigned long milliseconds = Thing::GetTimeMs();
  unsigned long startTime = milliseconds;
  while (milliseconds < startTime + 1000) {
-    Thing::UpdateThings();
+    thing->Update();

    milliseconds = Thing::GetTimeMs();
  }
@ -23,7 +23,7 @@ TEST(RoboidControlSuite, HiddenParticipant) {
 }

 TEST(RoboidControlSuite, IsolatedParticipant) {
-  ParticipantUDP* participant = ParticipantUDP::Isolated();
+  ParticipantUDP* participant = new ParticipantUDP(0);
  Thing* thing = new Thing();

  unsigned long milliseconds = Thing::GetTimeMs();
Author	SHA1	Message	Date
Pascal Serrarens	5b582fe9b8	server still rejects this	2025-06-29 13:01:31 +02:00
Pascal Serrarens	ff8570e2aa	Merge branch '0.4_dev' into MQTT and fixed compatibility	2025-06-29 11:42:46 +02:00
Pascal Serrarens	26a80cd94a	Improvements	2025-06-28 09:15:16 +02:00
Pascal Serrarens	19a5c5d4d5	Cleanup, but still crashes after about 20-30 secs.	2025-06-27 17:45:52 +02:00
Pascal Serrarens	f20ebd7ad2	Switch to MPU6050, finally getting good data	2025-06-27 17:12:42 +02:00
Pascal Serrarens	8901e6933a	ESP-IDF sending now works	2025-06-27 15:00:54 +02:00
Pascal Serrarens	b5f07f77c2	Major refactoring participants	2025-06-27 12:38:44 +02:00
Pascal Serrarens	9a26e98b0f	Improved wifi code (but still not working)	2025-06-26 17:25:32 +02:00
Pascal Serrarens	1add0647e1	Improved new thing processing	2025-06-26 16:33:33 +02:00
Pascal Serrarens	0ad9d3e9ec	Merge commit 'b6fbfd92d26c91b680fb6c43da2db19adec82376'	2025-06-26 13:58:01 +02:00
Pascal Serrarens	230a41f143	Add Distance Sensor	2025-06-19 11:58:25 +02:00
Pascal Serrarens	17916ae3db	Cleanup	2025-06-18 17:16:42 +02:00
Pascal Serrarens	634d560ee1	Fix crash when updating DRV8833 Diff Drive	2025-06-18 15:38:46 +02:00
Pascal Serrarens	6eafbe538f	Fix intialization issues	2025-06-18 15:27:08 +02:00
Pascal Serrarens	e0b8f3eac3	Fix touchsensor not resetting	2025-06-18 14:37:06 +02:00
Pascal Serrarens	e89c246753	Increased update rate	2025-06-18 14:04:01 +02:00
Pascal Serrarens	e5916466a6	Fixed force sending posmsg	2025-06-18 14:00:39 +02:00
Pascal Serrarens	6532d656de	Updated core code installation doc.	2025-06-17 17:58:07 +02:00
Pascal Serrarens	f4d36ad960	Add target_include_directories	2025-06-17 17:55:01 +02:00
Pascal Serrarens	3bb3c68e95	Moved unit test cmake commands to test	2025-06-17 17:18:17 +02:00
Pascal Serrarens	70c4afca89	Add setup documentation	2025-06-17 15:50:56 +02:00
Pascal Serrarens	0066042e07	Added initial Windows code	2025-06-09 15:35:09 +02:00
Pascal Serrarens	19e4e70e97	Initial Posix MQTT implementation	2025-06-09 13:09:20 +02:00
Pascal Serrarens	315c37d3bf	Fix MacOS compatibility issues	2025-06-09 12:07:50 +02:00
Pascal Serrarens	c2ac149df6	Optimise sending of messages	2025-06-06 09:37:30 +02:00
Pascal Serrarens	9a2ce4dd5e	use SwingTwist for orientation	2025-06-06 09:36:42 +02:00
Pascal Serrarens	b8eb9bb712	Improved touch sensor	2025-06-06 09:36:24 +02:00
Pascal Serrarens	2a83dbe7ca	Alignment with C#	2025-06-04 12:17:30 +02:00
Pascal Serrarens	e36f3bb45d	Alignment with C#	2025-06-04 11:49:47 +02:00
Pascal Serrarens	c24925a4da	Compatibility fixes	2025-06-04 10:58:05 +02:00
Pascal Serrarens	be95dbeedc	Renamed thing type incremental to relative encoder	2025-06-02 12:39:47 +02:00
Pascal Serrarens	57cf14b487	Removed reconstruct thing and thingtype in constructor	2025-06-02 12:35:53 +02:00
Pascal Serrarens	2883f5e0fa	thing tytpe enum->const	2025-06-02 12:30:21 +02:00
Pascal Serrarens	df19ecf953	Attempts to get I2C communcation working	2025-04-15 12:38:02 +02:00