Merge branch 'main' of https://git.passer.life/RoboidControl/RoboidControl-cpp

Merge commit '90e89b2ecd49870e84af4b66fe41610e4855673c'
Merge commit 'e50c8eb9c89c7fc801ed2c543a1d3970011efb2c'
2025-06-02 12:26:25 +02:00 · 2025-06-02 12:26:07 +02:00 · 2025-05-26 15:29:28 +02:00 · 2025-05-26 15:26:44 +02:00 · 2025-05-26 14:04:37 +02:00 · 2025-05-26 12:27:09 +02:00
90 changed files with 3009 additions and 1456 deletions
--- a/Arduino/ArduinoParticipant.cpp
+++ b/Arduino/ArduinoParticipant.cpp
@ -1,5 +1,9 @@
 #include "ArduinoParticipant.h"

+#if !defined(NO_STD)
+#include <iostream>
+#endif
+
 #if defined(ARDUINO)
 #if defined(ARDUINO_ARCH_ESP8266)
 #include <ESP8266WiFi.h>
@ -19,17 +23,19 @@
 namespace RoboidControl {
 namespace Arduino {

-void LocalParticipant::Setup(int localPort,
-                             const char* remoteIpAddress,
-                             int remotePort) {
 #if defined(ARDUINO) && defined(HAS_WIFI)
-  this->remoteIpAddress = remoteIpAddress;
-  this->remotePort = remotePort;
+WiFiUDP* udp;
+#endif
+
+void ParticipantUDP::Setup() {
+#if defined(ARDUINO) && defined(HAS_WIFI)
  GetBroadcastAddress();

 #if defined(UNO_R4)
  if (WiFi.status() == WL_NO_MODULE) {
+#if !defined(NO_STD)
    std::cout << "No network available!\n";
+#endif
    return;
  }
 #else
@ -38,13 +44,20 @@ void LocalParticipant::Setup(int localPort,
    return;
  }
 #endif
-  udp.begin(localPort);

-  std::cout << "Wifi sync started to port " << this->remotePort << "\n";
+  udp = new WiFiUDP();
+  udp->begin(this->port);
+
+#if !defined(NO_STD)
+  std::cout << "Wifi sync started local " << this->port;
+  if (this->remoteSite != nullptr)
+    std::cout << ", remote " << this->remoteSite->ipAddress << ":"
+              << this->remoteSite->port << "\n";
+#endif
 #endif
 }

-void LocalParticipant::GetBroadcastAddress() {
+void ParticipantUDP::GetBroadcastAddress() {
 #if defined(ARDUINO) && defined(HAS_WIFI)
  IPAddress broadcastAddress = WiFi.localIP();
  broadcastAddress[3] = 255;
@ -52,69 +65,65 @@ void LocalParticipant::GetBroadcastAddress() {
  this->broadcastIpAddress = new char[broadcastIpString.length() + 1];
  broadcastIpString.toCharArray(this->broadcastIpAddress,
                                broadcastIpString.length() + 1);
+#if !defined(NO_STD)
  std::cout << "Broadcast address: " << broadcastIpAddress << "\n";
 #endif
+#endif
 }

-void LocalParticipant::Receive() {
+void ParticipantUDP::Receive() {
 #if defined(ARDUINO) && defined(HAS_WIFI)
-  int packetSize = udp.parsePacket();
+  int packetSize = udp->parsePacket();
  while (packetSize > 0) {
-    udp.read(buffer, packetSize);
+    udp->read(buffer, packetSize);

-    String senderAddress = udp.remoteIP().toString();
+    String senderAddress = udp->remoteIP().toString();
    char sender_ipAddress[16];
    senderAddress.toCharArray(sender_ipAddress, 16);
-    unsigned int sender_port = udp.remotePort();
+    unsigned int sender_port = udp->remotePort();

-    // Participant* remoteParticipant = this->GetParticipant(sender_ipAddress,
-    // sender_port); if (remoteParticipant == nullptr) {
-    //   remoteParticipant = this->AddParticipant(sender_ipAddress,
-    //   sender_port);
-    //   // std::cout << "New sender " << sender_ipAddress << ":" << sender_port
-    //   //           << "\n";
-    //   // std::cout << "New remote participant " <<
-    //   remoteParticipant->ipAddress
-    //   //           << ":" << remoteParticipant->port << " "
-    //   //           << (int)remoteParticipant->networkId << "\n";
-    // }
-
-    // ReceiveData(packetSize, remoteParticipant);
    ReceiveData(packetSize, sender_ipAddress, sender_port);
-    packetSize = udp.parsePacket();
+    packetSize = udp->parsePacket();
  }
 #endif
 }

-bool LocalParticipant::Send(Participant* remoteParticipant, int bufferSize) {
+bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
 #if defined(ARDUINO) && defined(HAS_WIFI)
  // std::cout << "Sending to:\n " << remoteParticipant->ipAddress << ":"
  // << remoteParticipant->port << "\n";

  int n = 0;
+  int r = 0;
  do {
    if (n > 0) {
+#if !defined(NO_STD)
      std::cout << "Retry sending\n";
+#endif
      delay(10);
    }
    n++;
-    udp.beginPacket(remoteParticipant->ipAddress, remoteParticipant->port);
-    udp.write((unsigned char*)buffer, bufferSize);
-  } while (udp.endPacket() == 0 && n < 10);
+
+    udp->beginPacket(remoteParticipant->ipAddress, remoteParticipant->port);
+    udp->write((unsigned char*)buffer, bufferSize);
+    r = udp->endPacket();
+    // On an Uno R4 WiFi, endPacket blocks for 10 seconds the first time
+    // It is not cleary yet why
+  } while (r == 0 && n < 10);

 #endif
  return true;
 }

-bool LocalParticipant::Publish(IMessage* msg) {
+bool ParticipantUDP::Publish(IMessage* msg) {
 #if defined(ARDUINO) && defined(HAS_WIFI)
  int bufferSize = msg->Serialize((char*)this->buffer);
  if (bufferSize <= 0)
    return true;

-  udp.beginPacket(this->broadcastIpAddress, this->remotePort);
-  udp.write((unsigned char*)buffer, bufferSize);
-  udp.endPacket();
+  udp->beginPacket(this->broadcastIpAddress, this->port);
+  udp->write((unsigned char*)buffer, bufferSize);
+  udp->endPacket();

  //   std::cout << "Publish to " << this->broadcastIpAddress << ":"
  //             << this->remotePort << "\n";
--- a/Arduino/ArduinoParticipant.h
+++ b/Arduino/ArduinoParticipant.h
@ -1,30 +1,20 @@
 #pragma once

-#include "../LocalParticipant.h"
-
-#if defined(HAS_WIFI)
-#include <WiFiUdp.h>
-#endif
+#include "Participants/ParticipantUDP.h"

 namespace RoboidControl {
 namespace Arduino {

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

 protected:
-#if defined(HAS_WIFI)
-  const char* remoteIpAddress = nullptr;
-  unsigned short remotePort = 0;
  char* broadcastIpAddress = nullptr;

-  WiFiUDP udp;
-#endif
-
  void GetBroadcastAddress();
 };

--- a/Arduino/Things/DRV8833.cpp
+++ b/Arduino/Things/DRV8833.cpp
@ -5,49 +5,81 @@
 namespace RoboidControl {
 namespace Arduino {

-DRV8833Motor::DRV8833Motor(Participant* participant, unsigned char pinIn1, unsigned char pinIn2, bool reverse)
-    : Thing(participant) {
+#pragma region DRV8833
+
+DRV8833::DRV8833(Configuration config, Thing* parent) : Thing(Type::Undetermined, parent) {
+  this->pinStandby = config.standby;
+  if (pinStandby != 255)
+    pinMode(pinStandby, OUTPUT);
+
+  this->motorA = new DRV8833Motor(this, config.AIn1, config.AIn2);
+  this->motorA->SetName("Motor A");
+  this->motorB = new DRV8833Motor(this, config.BIn1, config.BIn2);
+  this->motorB->SetName("Motor B");
+}
+
+#pragma endregion DRV8833
+
+#pragma region Differential drive
+
+DRV8833::DifferentialDrive::DifferentialDrive(DRV8833::Configuration config,
+                                              Thing* parent)
+    : RoboidControl::DifferentialDrive(this->drv8833.motorA,
+                                       this->drv8833.motorB,
+                                       parent),
+      drv8833(config, this) {}
+
+void DRV8833::DifferentialDrive::Update(bool recurse) {
+  RoboidControl::DifferentialDrive::Update(recurse);
+  this->drv8833.Update(false);
+}
+
+#pragma endregion Differential drive
+
+#pragma region Motor
+
+#if (ESP32)
+uint8_t DRV8833Motor::nextAvailablePwmChannel = 0;
+#endif
+
+DRV8833Motor::DRV8833Motor(DRV8833* driver,
+                           unsigned char pinIn1,
+                           unsigned char pinIn2,
+                           bool reverse)
+    : Motor() {
+  this->SetParent(driver);
+
  this->pinIn1 = pinIn1;
  this->pinIn2 = pinIn2;

 #if (ESP32)
-  in1Ch = nextAvailablePwmChannel++;
+  in1Ch = DRV8833Motor::nextAvailablePwmChannel++;
  ledcSetup(in1Ch, 500, 8);
  ledcAttachPin(pinIn1, in1Ch);
-  in2Ch = nextAvailablePwmChannel++;
+
+  in2Ch = DRV8833Motor::nextAvailablePwmChannel++;
  ledcSetup(in2Ch, 500, 8);
  ledcAttachPin(pinIn2, in2Ch);
+
 #else
  pinMode(pinIn1, OUTPUT);  // configure the in1 pin to output mode
  pinMode(pinIn2, OUTPUT);  // configure the in1 pin to output mode
 #endif

-  this->reverse = reverse;
+  // this->reverse = reverse;
 }

-void DRV8833Motor::SetMaxRPM(unsigned int rpm) {
-  this->maxRpm = rpm;
-}
+// void DRV8833Motor::SetMaxRPM(unsigned int rpm) {
+//   this->maxRpm = rpm;
+// }

-void DRV8833Motor::SetAngularVelocity(Spherical velocity) {
-  Thing::SetAngularVelocity(velocity);
-  // ignoring rotation axis for now.
-  // Spherical angularVelocity = this->GetAngularVelocity();
-  float angularSpeed = velocity.distance;  // in degrees/sec
+void DRV8833Motor::SetTargetVelocity(float motorSpeed) {
+  Motor::SetTargetVelocity(motorSpeed);

-  float rpm = angularSpeed / 360 * 60;
-  float motorSpeed = rpm / this->maxRpm;
+  uint8_t motorSignal =
+      (uint8_t)(motorSpeed > 0 ? motorSpeed * 255 : -motorSpeed * 255);

-  uint8_t motorSignal = (uint8_t)(motorSpeed * 255);
-  // if (direction == RotationDirection::CounterClockwise)
-  //   speed = -speed;
-  // Determine the rotation direction
-  if (velocity.direction.horizontal.InDegrees() < 0)
-    motorSpeed = -motorSpeed;
-  if (this->reverse)
-    motorSpeed = -motorSpeed;
-
-  // std::cout << "ang speed " << this->name << " = " << angularSpeed << "   rpm " << rpm
+  // std::cout << "moto speed " << this->name << " = " << motorSpeed
  //           << ", motor signal = " << (int)motorSignal << "\n";

 #if (ESP32)
@ -97,24 +129,7 @@ void DRV8833Motor::SetAngularVelocity(Spherical velocity) {
 #endif
 }

-DRV8833::DRV8833(Participant* participant,
-                 unsigned char pinAIn1,
-                 unsigned char pinAIn2,
-                 unsigned char pinBIn1,
-                 unsigned char pinBIn2,
-                 unsigned char pinStandby,
-                 bool reverseA,
-                 bool reverseB)
-    : Thing(participant) {
-  this->pinStandby = pinStandby;
-  if (pinStandby != 255)
-    pinMode(pinStandby, OUTPUT);
-
-  this->motorA = new DRV8833Motor(participant, pinAIn1, pinAIn2, reverseA);
-  this->motorA->name = "Motor A";
-  this->motorB = new DRV8833Motor(participant, pinBIn1, pinBIn2, reverseB);
-  this->motorB->name = "Motor B";
-}
+#pragma endregion Motor

 }  // namespace Arduino
 }  // namespace RoboidControl
--- a/Arduino/Things/DRV8833.h
+++ b/Arduino/Things/DRV8833.h
@ -1,60 +1,85 @@
 #pragma once

+#include <Arduino.h>
+#include "Participants/IsolatedParticipant.h"
 #include "Thing.h"
 #include "Things/DifferentialDrive.h"
+#include "Things/Motor.h"

 namespace RoboidControl {
 namespace Arduino {

-/// @brief Support for a DRV8833 motor controller
-class DRV8833Motor : public Thing {
- public:
-  /// @brief Motor turning direction
-  enum class RotationDirection { Clockwise = 1, CounterClockwise = -1 };
-
-  /// @brief Setup the DC motor
-  /// @param pinIn1 the pin number for the in1 signal
-  /// @param pinIn2 the pin number for the in2 signal
-  /// @param direction the forward turning direction of the motor
-  DRV8833Motor(Participant* participant, unsigned char pinIn1, unsigned char pinIn2, bool reverse = false);
-  void SetMaxRPM(unsigned int rpm);
-
-  virtual void SetAngularVelocity(Spherical velocity) override;
-
-  bool reverse = false;
-
- protected:
-  unsigned char pinIn1 = 255;
-  unsigned char pinIn2 = 255;
-  unsigned int maxRpm = 200;
-};
+class DRV8833Motor;

 class DRV8833 : public Thing {
 public:
+  struct Configuration {
+    int AIn1;
+    int AIn2;
+    int BIn1;
+    int BIn2;
+    int standby = 255;
+  };
+
  /// @brief Setup a DRV8833 motor controller
-  /// @param pinAIn1 The pin number connected to the AIn1 port
-  /// @param pinAIn2 The pin number connected to the AIn2 port
-  /// @param pinBIn1 The pin number connected to the BIn1 port
-  /// @param pinBIn2 The pin number connceted to the BIn2 port
-  /// @param pinStandby The pin number connected to the standby port, 255
-  /// indicated that the port is not connected
-  /// @param reverseA The forward turning direction of motor A
-  /// @param reverseB The forward turning direction of motor B
-  DRV8833(Participant* participant,
-          unsigned char pinAIn1,
-          unsigned char pinAIn2,
-          unsigned char pinBIn1,
-          unsigned char pinBIn2,
-          unsigned char pinStandby = 255,
-          bool reverseA = false,
-          bool reverseB = false);
+  DRV8833(Configuration config, Thing* parent = Thing::LocalRoot());

  DRV8833Motor* motorA = nullptr;
  DRV8833Motor* motorB = nullptr;

 protected:
  unsigned char pinStandby = 255;
+
+ public:
+  class DifferentialDrive;
 };

+#pragma region Differential drive
+
+class DRV8833::DifferentialDrive : public RoboidControl::DifferentialDrive {
+ public:
+  DifferentialDrive(DRV8833::Configuration config, Thing* parent = Thing::LocalRoot());
+
+  virtual void Update(bool recurse = false) override;
+
+ protected:
+  DRV8833 drv8833;
+};
+
+#pragma endregion Differential drive
+
+#pragma region Motor
+
+/// @brief Support for a DRV8833 motor controller
+class DRV8833Motor : public Motor {
+ public:
+  /// @brief Setup the DC motor
+  /// @param pinIn1 the pin number for the in1 signal
+  /// @param pinIn2 the pin number for the in2 signal
+  /// @param direction the forward turning direction of the motor
+  DRV8833Motor(DRV8833* driver,
+               unsigned char pinIn1,
+               unsigned char pinIn2,
+               bool reverse = false);
+  // void SetMaxRPM(unsigned int rpm);
+
+  // virtual void SetAngularVelocity(Spherical velocity) override;
+  virtual void SetTargetVelocity(float targetSpeed) override;
+  // bool reverse = false;
+
+ protected:
+  unsigned char pinIn1 = 255;
+  unsigned char pinIn2 = 255;
+  // unsigned int maxRpm = 200;
+
+#if (ESP32)
+  uint8_t in1Ch;
+  uint8_t in2Ch;
+  static uint8_t nextAvailablePwmChannel;
+#endif
+};
+
+#pragma endregion Motor
+
 }  // namespace Arduino
 }  // namespace RoboidControl
--- a/Arduino/Things/DigitalInput.cpp
+++ b/Arduino/Things/DigitalInput.cpp
@ -5,19 +5,125 @@
 namespace RoboidControl {
 namespace Arduino {

-DigitalInput::DigitalInput(Participant* participant, unsigned char pin)
-    : TouchSensor(participant) {
+#pragma region Digital input
+
+DigitalInput::DigitalInput(unsigned char pin, Thing* parent)
+    : Thing(Type::Undetermined, parent) {
  this->pin = pin;
-
-  pinMode(pin, INPUT);
+  pinMode(this->pin, INPUT);
+  std::cout << "digital input start\n";
 }

-void DigitalInput::Update(unsigned long currentTimeMs, bool recursive) {
-  this->touchedSomething = digitalRead(pin) == LOW;
-  // std::cout << "DigitalINput pin " << (int)this->pin << ": " <<
-  // this->touchedSomething << "\n";
-  Thing::Update(currentTimeMs, recursive);
+void DigitalInput::Update(bool recursive) {
+  this->isHigh = digitalRead(this->pin);
+  this->isLow = !this->isHigh;
+  Thing::Update(recursive);
 }

+#pragma endregion Digital input
+
+#pragma region Touch sensor
+
+DigitalInput::TouchSensor::TouchSensor(unsigned char pin, Thing* parent)
+    : RoboidControl::TouchSensor(parent), digitalInput(pin, parent) {}
+
+void DigitalInput::TouchSensor::Update(bool recursive) {
+  this->touchedSomething = digitalInput.isLow;
+}
+
+#pragma endregion Touch sensor
+
+#pragma region Relative encoder
+
+int DigitalInput::RelativeEncoder::interruptCount = 0;
+volatile int DigitalInput::RelativeEncoder::pulseCount0 = 0;
+volatile int DigitalInput::RelativeEncoder::pulseCount1 = 0;
+
+DigitalInput::RelativeEncoder::RelativeEncoder(Configuration config,
+                                               Thing* parent)
+    : RoboidControl::RelativeEncoder(parent),
+      digitalInput(config.pin, parent),
+      pulsesPerRevolution(config.pulsesPerRevolution) {}
+
+void DigitalInput::RelativeEncoder::Start() {
+  // We support up to 2 pulse counters
+  if (interruptCount == 0) {
+    std::cout << "pin interrupt 1 activated" << std::endl;
+    attachInterrupt(digitalPinToInterrupt(digitalInput.pin), PulseInterrupt0,
+                    RISING);
+  } else if (interruptCount == 1) {
+    std::cout << "pin interrupt 2 activated" << std::endl;
+    attachInterrupt(digitalPinToInterrupt(digitalInput.pin), PulseInterrupt1,
+                    RISING);
+  } else {
+    // maximum interrupt count reached
+    std::cout << "DigitalInput::RelativeEncoder: max. # counters of 2 reached"
+              << std::endl;
+    return;
+  }
+  interruptIx = interruptCount;
+  interruptCount++;
+  std::cout << "pin ints. " << interruptCount << std::endl;
+}
+
+int DigitalInput::RelativeEncoder::GetPulseCount() {
+  if (interruptIx == 0) {
+    int pulseCount = pulseCount0;
+    pulseCount0 = 0;
+    return pulseCount;
+  } else if (interruptIx == 1) {
+    int pulseCount = pulseCount1;
+    pulseCount1 = 0;
+    return pulseCount;
+  }
+  return 0;
+}
+
+void DigitalInput::RelativeEncoder::Update(bool recursive) {
+  unsigned long currentTimeMs = GetTimeMs();
+  if (this->lastUpdateTime == 0) {
+    this->Start();
+    this->lastUpdateTime = currentTimeMs;
+    return;
+  }
+
+  float timeStep = (float)(currentTimeMs - this->lastUpdateTime) / 1000.0f;
+  if (timeStep <= 0)
+    return;
+
+  int pulseCount = GetPulseCount();
+  netPulseCount += pulseCount;
+
+  this->pulseFrequency = pulseCount / timeStep;
+  this->rotationSpeed = pulseFrequency / pulsesPerRevolution;
+
+  std::cout << "pulses: " << pulseCount << " per second " << pulseFrequency
+            << " timestep " << timeStep << std::endl;
+
+  this->lastUpdateTime = currentTimeMs;
+}
+
+#if defined(ESP8266) || defined(ESP32)
+void ICACHE_RAM_ATTR DigitalInput::RelativeEncoder::PulseInterrupt0() {
+  pulseCount0++;
+}
+#else
+void DigitalInput::RelativeEncoder::PulseInterrupt0() {
+  pulseCount0++;
+}
+#endif
+
+#if defined(ESP8266) || defined(ESP32)
+void IRAM_ATTR DigitalInput::RelativeEncoder::PulseInterrupt1() {
+  pulseCount1++;
+}
+#else
+void DigitalInput::RelativeEncoder::PulseInterrupt1() {
+  pulseCount1++;
+}
+#endif
+
+#pragma endregion Relative encoder
+
 }  // namespace Arduino
 }  // namespace RoboidControl
--- a/Arduino/Things/DigitalInput.h
+++ b/Arduino/Things/DigitalInput.h
@ -1,26 +1,92 @@
 #pragma once

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

 namespace RoboidControl {
 namespace Arduino {

 /// @brief A digital input represents the stat of a digital GPIO pin
-class DigitalInput : public TouchSensor {
+class DigitalInput : public Thing {
 public:
  /// @brief Create a new digital input
  /// @param participant The participant to use
  /// @param pin The digital pin
-  DigitalInput(Participant* participant, unsigned char pin);
+  //DigitalInput(Participant* participant, unsigned char pin);
+  // DigitalInput(Thing* parent, unsigned char pin);
+  DigitalInput(unsigned char pin, Thing* parent);
+
+  bool isHigh = false;
+  bool isLow = false;

  /// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs)
-  virtual void Update(unsigned long currentTimeMs,
-                      bool recursive = false) override;
+  virtual void Update(bool recursive = false) override;

 protected:
  /// @brief The pin used for digital input
  unsigned char pin = 0;
+
+ public:
+  class TouchSensor;
+  class RelativeEncoder;
 };

+#pragma region Touch sensor
+
+class DigitalInput::TouchSensor : public RoboidControl::TouchSensor {
+ public:
+  TouchSensor(unsigned char pin, Thing* parent);
+
+  /// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs)
+  virtual void Update(bool recurse = false) override;
+
+ protected:
+  DigitalInput digitalInput;
+};
+
+#pragma endregion Touch sensor
+
+#pragma region Incremental encoder
+
+class DigitalInput::RelativeEncoder : public RoboidControl::RelativeEncoder {
+ public:
+  struct Configuration {
+    unsigned char pin;
+    unsigned char pulsesPerRevolution;
+  };
+
+  RelativeEncoder(Configuration config, Thing* parent = Thing::LocalRoot());
+
+  unsigned char pulsesPerRevolution;
+
+  /// @brief The current pulse frequency in Hz
+  float pulseFrequency = 0;
+
+  /// @copydoc RoboidControl::Thing::Update()
+  virtual void Update(bool recurse = false) override;
+
+ protected:
+  DigitalInput digitalInput;
+
+  int interruptIx = 0;
+
+  static int interruptCount;
+
+  volatile static int pulseCount0;
+  static void PulseInterrupt0();
+
+  volatile static int pulseCount1;
+  static void PulseInterrupt1();
+
+  int GetPulseCount();
+  long netPulseCount = 0;
+  unsigned long lastUpdateTime = 0;
+
+ private:
+  void Start();
+};
+
+#pragma endregion Incremental encoder
+
 }  // namespace Arduino
 }  // namespace RoboidControl
--- a/Arduino/Things/UltrasonicSensor.cpp
+++ b/Arduino/Things/UltrasonicSensor.cpp
@ -1,16 +1,16 @@
 #include "UltrasonicSensor.h"

 #include <Arduino.h>
+#include <iostream>

 namespace RoboidControl {
 namespace Arduino {

-UltrasonicSensor::UltrasonicSensor(Participant* participant,
-                                   unsigned char pinTrigger,
-                                   unsigned char pinEcho)
-    : TouchSensor(participant) {
-  this->pinTrigger = pinTrigger;
-  this->pinEcho = pinEcho;
+UltrasonicSensor::UltrasonicSensor(Configuration config, Thing* parent)
+    : Thing(Type::Undetermined, parent) {
+  this->name = "Ultrasonic sensor";
+  this->pinTrigger = config.trigger;
+  this->pinEcho = config.echo;

  pinMode(pinTrigger, OUTPUT);  // configure the trigger pin to output mode
  pinMode(pinEcho, INPUT);      // configure the echo pin to input mode
@ -19,14 +19,14 @@ UltrasonicSensor::UltrasonicSensor(Participant* participant,
 float UltrasonicSensor::GetDistance() {
  // Start the ultrasonic 'ping'
  digitalWrite(pinTrigger, LOW);
-  delayMicroseconds(5);
+  delayMicroseconds(2);
  digitalWrite(pinTrigger, HIGH);
  delayMicroseconds(10);
  digitalWrite(pinTrigger, LOW);

  // Measure the duration of the pulse on the echo pin
-  float duration_us =
-      pulseIn(pinEcho, HIGH, 100000);  // the result is in microseconds
+  unsigned long duration_us =
+      pulseIn(pinEcho, HIGH, 10000);  // the result is in microseconds

  // Calculate the distance:
  // * Duration should be divided by 2, because the ping goes to the object
@ -37,32 +37,38 @@ float UltrasonicSensor::GetDistance() {
  // * Now we calculate the distance based on the speed of sound (340 m/s):
  // distance = duration_sec * 340;
  // * The result calculation is therefore:
-  this->distance = duration_us / 2 / 1000000 * 340;
+  this->distance = (float)duration_us / 2 / 1000000 * 340;
+
+  // Serial.println(this->distance);
+
+  // std::cout << "US distance " << this->distance << std::endl;

  // Filter faulty measurements. The sensor can often give values > 30 m which
  // are not correct
  // if (distance > 30)
  //   distance = 0;

-  this->touchedSomething |= (this->distance <= this->touchDistance);
-
-  //   std::cout << "Ultrasonic " << this->distance << " " <<
-  //   this->touchedSomething << "\n";
-
-  return distance;
+  return this->distance;
 }

-void UltrasonicSensor::Update(unsigned long currentTimeMs, bool recursive) {
-  this->touchedSomething = false;
+void UltrasonicSensor::Update(bool recursive) {
  GetDistance();
-  Thing::Update(currentTimeMs, recursive);
+  Thing::Update(recursive);
 }

-// void UltrasonicSensor::ProcessBinary(char* bytes) {
-//   this->touchedSomething = (bytes[0] == 1);
-//   if (this->touchedSomething)
-//     std::cout << "Touching something!\n";
-// }
+#pragma region Touch sensor
+
+UltrasonicSensor::TouchSensor::TouchSensor(Configuration config, Thing* parent)
+    : RoboidControl::TouchSensor(parent), ultrasonic(config, this) {}
+
+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);
+}
+
+#pragma region Touch sensor

 }  // namespace Arduino
 }  // namespace RoboidControl
--- a/Arduino/Things/UltrasonicSensor.h
+++ b/Arduino/Things/UltrasonicSensor.h
@ -6,20 +6,19 @@ namespace RoboidControl {
 namespace Arduino {

 /// @brief An HC-SR04 ultrasonic distance sensor
-class UltrasonicSensor : public TouchSensor {
+class UltrasonicSensor : Thing {
 public:
-  /// @brief  Setup an ultrasonic sensor
-  /// @param participant The participant to use
-  /// @param pinTrigger The pin number of the trigger signal
-  /// @param pinEcho The pin number of the echo signal
-  UltrasonicSensor(Participant* participant,
-                   unsigned char pinTrigger,
-                   unsigned char pinEcho);
+  struct Configuration {
+    int trigger;
+    int echo;
+  };
+
+  UltrasonicSensor(Configuration config, Thing* parent = Thing::LocalRoot());

  // parameters

  /// @brief The distance at which the object is considered to be touched
-  float touchDistance = 0.2f;
+  // float touchDistance = 0.2f;

  // state

@ -31,15 +30,35 @@ class UltrasonicSensor : public TouchSensor {
  float GetDistance();

  /// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs)
-  virtual void Update(unsigned long currentTimeMs,
-                      bool recursive = false) override;
+  virtual void Update(bool recursive = false) override;

 protected:
  /// @brief The pin number of the trigger signal
  unsigned char pinTrigger = 0;
  /// @brief The pin number of the echo signal
  unsigned char pinEcho = 0;
+
+ public:
+  class TouchSensor;
 };

+#pragma region Touch sensor
+
+class UltrasonicSensor::TouchSensor : public RoboidControl::TouchSensor {
+ public:
+  TouchSensor(UltrasonicSensor::Configuration config,
+              Thing* parent = Thing::LocalRoot());
+
+  float touchDistance = 0.2f;
+
+  /// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs)
+  virtual void Update(bool recursive = false) override;
+
+ protected:
+  UltrasonicSensor ultrasonic;
+};
+
+#pragma region Touch sensor
+
 }  // namespace Arduino
 }  // namespace RoboidControl
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@ -1,17 +1,31 @@
 cmake_minimum_required(VERSION 3.13)  # CMake version check
+
+file(GLOB srcs 
+    *.cpp
+    Things/*.cpp
+    Messages/*.cpp
+    Arduino/*.cpp
+    Posix/*.cpp
+    Windows/*.cpp
+    EspIdf/*.cpp
+    LinearAlgebra/*.cpp
+    Participants/*.cpp
+)
+
 if(ESP_PLATFORM)
    idf_component_register(
-        SRC_DIRS "." "LinearAlgebra"
+        SRCS ${srcs}
        INCLUDE_DIRS "." "LinearAlgebra"
+        REQUIRES esp_netif esp_wifi
    )
 else() 
+    set(CMAKE_CXX_STANDARD 17)      # Enable c++11 standard
+    set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+
    project(RoboidControl)
    add_subdirectory(LinearAlgebra)
    add_subdirectory(Examples)

-    set(CMAKE_CXX_STANDARD 17)      # Enable c++11 standard
-    set(CMAKE_POSITION_INDEPENDENT_CODE ON)
-
    add_compile_definitions(GTEST)
    include(FetchContent)
    FetchContent_Declare(
@ -28,14 +42,6 @@ else()
        .
        LinearAlgebra
    )
-    file(GLOB srcs 
-        *.cpp
-        Things/*.cpp
-        Messages/*.cpp
-        Arduino/*.cpp
-        Posix/*.cpp
-        Windows/*.cpp
-    )
    add_library(RoboidControl STATIC ${srcs})
    
    enable_testing()
--- a/EspIdf/EspIdfParticipant.cpp
+++ b/EspIdf/EspIdfParticipant.cpp
@ -0,0 +1,166 @@
+#include "EspIdfParticipant.h"
+
+#if defined(IDF_VER)
+#include "esp_wifi.h"
+#endif
+
+namespace RoboidControl {
+namespace EspIdf {
+
+void ParticipantUDP::Setup(int localPort,
+                           const char* remoteIpAddress,
+                           int remotePort) {
+#if defined(IDF_VER)
+  std::cout << "Set up UDP\n";
+  GetBroadcastAddress();
+
+  wifi_ap_record_t ap_info;
+  esp_err_t result = esp_wifi_sta_get_ap_info(&ap_info);
+  if (result != ESP_OK) {
+    std::cout << "No network available!\n";
+    return;
+  }
+
+  // Create a UDP socket
+  this->sockfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
+  if (this->sockfd < 0) {
+    std::cout << "Unable to create UDP socket: errno " << errno << "\n";
+    vTaskDelete(NULL);
+    return;
+  }
+
+  // Set up the server address structure
+  struct sockaddr_in local_addr;
+  memset(&local_addr, 0, sizeof(local_addr));
+  local_addr.sin_family = AF_INET;
+  local_addr.sin_port = htons(this->port);
+  local_addr.sin_addr.s_addr =
+      htonl(INADDR_ANY);  // Listen on all available network interfaces
+
+  // Bind the socket to the address and port
+  if (bind(this->sockfd, (struct sockaddr*)&local_addr, sizeof(local_addr)) <
+      0) {
+    std::cout << "Unable to bind UDP socket: errno " << errno << "\n";
+    close(sockfd);
+    vTaskDelete(NULL);
+    return;
+  }
+
+  // Initialize the dest_addr structure
+  memset(&this->dest_addr, 0,
+         sizeof(this->dest_addr));  // Clear the entire structure
+  this->dest_addr.sin_family = AF_INET;
+  this->dest_addr.sin_port = htons(this->remoteSite->port);
+  inet_pton(AF_INET, this->remoteSite->ipAddress,
+            &this->dest_addr.sin_addr.s_addr);
+
+  std::cout << "Wifi sync started local " << this->port << ", remote "
+            << this->remoteSite->ipAddress << ":" << this->remoteSite->port
+            << "\n";
+#endif  // IDF_VER
+}
+
+void ParticipantUDP::GetBroadcastAddress() {
+#if defined(IDF_VER)
+  // SOMEHOW, THIS FUNCTION RESULTS IN MEMORY CORRUPION...
+
+  esp_netif_ip_info_t ip_info;
+  esp_netif_t* esp_netif = esp_netif_get_handle_from_ifkey("WIFI_STA_DEF");
+
+  // Get IP information (IP address, netmask, gateway)
+  if (esp_netif_get_ip_info(esp_netif, &ip_info) != ESP_OK) {
+    std::cout << "Failed to get IP info\n";
+    return;
+  }
+
+  ip_addr_t broadcast_addr = {};
+  broadcast_addr.u_addr.ip4.addr =
+      (ip_info.ip.addr & ip_info.netmask.addr) | ~ip_info.netmask.addr;
+
+  snprintf(this->broadcastIpAddress, INET_ADDRSTRLEN, IPSTR,
+           IP2STR(&broadcast_addr.u_addr.ip4));
+  std::cout << "Broadcast address: " << this->broadcastIpAddress << "\n";
+#endif  // IDF_VER
+}
+
+void ParticipantUDP::Receive() {
+#if defined(IDF_VER)
+  struct pollfd fds[1];
+  fds[0].fd = sockfd;
+  fds[0].events = POLLIN;  // We're looking for data available to read
+
+  // Use poll() with a timeout of 0 to return immediately
+  int ret = poll(fds, 1, 0);
+  if (ret == -1) {
+    std::cout << "poll() error\n";
+    return;
+  }
+
+  // char buffer[1024];
+  struct sockaddr_in source_addr;
+
+  socklen_t addr_len = sizeof(source_addr);
+  char sender_ipAddress[INET_ADDRSTRLEN];
+
+  while (ret > 0 && fds[0].revents & POLLIN) {
+    int packetSize = recvfrom(this->sockfd, buffer, sizeof(buffer) - 1, 0,
+                              (struct sockaddr*)&source_addr, &addr_len);
+    if (packetSize < 0) {
+      std::cout << "recvfrom() error\n";
+      return;
+    } else if (packetSize == 0) {
+      break;
+    }
+
+    // std::cout << "receiving " << packetSize << " bytes, msgId " <<
+    // (int)this->buffer[0] << "\n";
+    inet_ntoa_r(source_addr.sin_addr, sender_ipAddress, INET_ADDRSTRLEN);
+    unsigned int sender_port = ntohs(source_addr.sin_port);
+
+    ReceiveData(packetSize, sender_ipAddress, sender_port);
+
+    ret = poll(fds, 1, 0);
+    if (ret == -1) {
+      std::cout << "poll() error\n";
+      return;
+    }
+  }
+  // std::cout << "no more messages\n";
+
+#endif  // IDF_VER
+}
+
+bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
+#if defined(IDF_VER)
+  // std::cout << "Sending to " << remoteParticipant->ipAddress << ":"
+  //           << remoteParticipant->port << "\n";
+
+  int err = sendto(this->sockfd, buffer, bufferSize, 0,
+                   (struct sockaddr*)&dest_addr, sizeof(dest_addr));
+  if (errno != 0)
+    std::cout << "Send error " << err << " or " << errno << "\n";
+
+#endif
+  return true;
+}
+
+bool ParticipantUDP::Publish(IMessage* msg) {
+#if defined(IDF_VER)
+  int bufferSize = msg->Serialize((char*)this->buffer);
+  if (bufferSize <= 0)
+    return true;
+
+  struct sockaddr_in dest_addr;
+  dest_addr.sin_family = AF_INET;
+  dest_addr.sin_port = htons(this->port);
+  inet_pton(AF_INET, this->broadcastIpAddress, &dest_addr.sin_addr.s_addr);
+  int err = sendto(sockfd, buffer, bufferSize, 0, (struct sockaddr*)&dest_addr,
+                   sizeof(dest_addr));
+  if (err != 0)
+    std::cout << "Publish error\n";
+#endif
+  return true;
+};
+
+}  // namespace EspIdf
+}  // namespace RoboidControl
--- a/EspIdf/EspIdfParticipant.h
+++ b/EspIdf/EspIdfParticipant.h
@ -0,0 +1,32 @@
+#pragma once
+
+#include "Participants/ParticipantUDP.h"
+
+#if defined(IDF_VER)
+#include "lwip/sockets.h"
+#endif
+
+namespace RoboidControl {
+namespace EspIdf {
+
+class ParticipantUDP : public RoboidControl::ParticipantUDP {
+ public:
+  void Setup(int localPort, const char* remoteIpAddress, int remotePort);
+  void Receive();
+  bool Send(Participant* remoteParticipant, int bufferSize);
+  bool Publish(IMessage* msg);
+
+ protected:
+#if defined(IDF_VER)
+  char broadcastIpAddress[INET_ADDRSTRLEN];
+
+  int sockfd;
+  struct sockaddr_in dest_addr;
+  // struct sockaddr_in src_addr;
+#endif
+
+  void GetBroadcastAddress();
+};
+
+}  // namespace EspIdf
+}  // namespace RoboidControl
--- a/EspIdf/EspIdfUtils.cpp
+++ b/EspIdf/EspIdfUtils.cpp
@ -0,0 +1,100 @@
+#include "EspIdfUtils.h"
+
+#if defined(IDF_VER)
+#include <iostream>
+// #include "esp_event.h"
+// #include "esp_log.h"
+#include "esp_netif.h"
+#include "esp_wifi.h"
+// #include "lwip/inet.h"
+// #include "lwip/ip_addr.h"
+#include "string.h"
+
+const char* hotspotSSID = "Roboid";
+const char* hotspotPassword = "alchemy7000";
+
+esp_netif_t* wifi_netif = nullptr;
+// Semaphore to signal Wi-Fi connection status
+// SemaphoreHandle_t wifi_semaphore;
+static bool wifi_connected = false;
+
+static void wifi_event_handler(void* arg,
+                               esp_event_base_t event_base,
+                               int32_t event_id,
+                               void* event_data) {
+  if (event_base == WIFI_EVENT) {
+    if (event_id == WIFI_EVENT_STA_START)
+      esp_wifi_connect();
+    else if (event_id == WIFI_EVENT_STA_DISCONNECTED)
+      esp_wifi_connect();
+  } else if (event_base == IP_EVENT) {
+    if (event_id == IP_EVENT_STA_GOT_IP) {
+      // ip_event_got_ip_t* event = (ip_event_got_ip_t*)event_data;
+      // const char* ipaddr = IP2STR(&event->ip_info.ip);
+      wifi_connected = true;
+      // xSemaphoreGive(wifi_semaphore);  // Signal that connection is
+      // established
+    }
+  }
+}
+
+bool StartWifi(const char* wifiSsid, const char* wifiPassword) {
+  std::cout << "Connecting to WiFi " << wifiSsid << "\n";
+
+  esp_netif_init();
+  esp_event_loop_create_default();
+
+  wifi_netif = esp_netif_create_default_wifi_sta();
+  wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT();
+  esp_wifi_init(&cfg);
+
+  esp_event_handler_register(WIFI_EVENT, ESP_EVENT_ANY_ID, &wifi_event_handler,
+                             NULL);
+  esp_event_handler_register(IP_EVENT, IP_EVENT_STA_GOT_IP, &wifi_event_handler,
+                             NULL);
+
+  wifi_config_t wifi_config = {};
+  strncpy((char*)wifi_config.sta.ssid, wifiSsid, strlen(wifiSsid) + 1);
+  strncpy((char*)wifi_config.sta.password, wifiPassword,
+          strlen(wifiPassword) + 1);
+
+  esp_wifi_set_mode(WIFI_MODE_STA);
+  esp_wifi_set_config(WIFI_IF_STA, &wifi_config);
+  esp_wifi_start();
+
+  // Wait for connection with a timeout of 10 seconds
+  TickType_t xLastWakeTime = xTaskGetTickCount();
+  bool success = false;
+  for (int i = 0; i < 20; i++) {  // 20 iterations, each 500ms
+    if (wifi_connected) {
+      success = true;
+      std::cout << " Connected.\n";
+      break;
+    }
+    std::cout << ".";
+    fflush(stdout);  // Ensure output is printed immediately
+    vTaskDelayUntil(&xLastWakeTime, pdMS_TO_TICKS(500));  // Wait 500ms
+  }
+
+  if (wifi_connected) {
+    esp_netif_ip_info_t ip_info = {};
+    esp_netif_t* esp_netif = esp_netif_get_handle_from_ifkey("WIFI_STA_DEF");
+
+    // Get IP information (IP address, netmask, gateway)
+    if (esp_netif_get_ip_info(esp_netif, &ip_info) != ESP_OK) {
+      std::cout << "Failed to get IP info\n";
+      return false;
+    }
+
+    // Convert the IP address to string format using inet_ntoa
+    char ip_str[16];  // IPv4 address can have a max of 15 characters + null
+                      // terminator
+    snprintf(ip_str, sizeof(ip_str), IPSTR, IP2STR(&ip_info.ip));
+    std::cout << "IP address = " << ip_str << "\n";
+  } else
+    std::cout << "\nCould not connect to home network.\n";
+
+  return success;
+}
+
+#endif
--- a/EspIdf/EspIdfUtils.h
+++ b/EspIdf/EspIdfUtils.h
@ -0,0 +1,6 @@
+#pragma once
+#if defined(IDF_VER)
+
+bool StartWifi(const char *wifiSsid, const char *wifiPassword);
+
+#endif
--- a/LinearAlgebra/Angle.h
+++ b/LinearAlgebra/Angle.h
@ -21,7 +21,7 @@ template <typename T>
 class AngleOf {
 public:
  /// @brief Create a new angle with a zero value
-  AngleOf<T>();
+  AngleOf();

  /// @brief An zero value angle
  const static AngleOf<T> zero;
@ -209,7 +209,7 @@ class AngleOf {
 private:
  T value;

-  AngleOf<T>(T rawValue);
+  AngleOf(T rawValue);
 };

 using AngleSingle = AngleOf<float>;
--- a/LinearAlgebra/Direction.cpp
+++ b/LinearAlgebra/Direction.cpp
@ -9,6 +9,7 @@

 #include <math.h>

+namespace LinearAlgebra {
 template <typename T>
 DirectionOf<T>::DirectionOf() {
  this->horizontal = AngleOf<T>();
@ -98,5 +99,6 @@ void DirectionOf<T>::Normalize() {
  }
 }

-template class DirectionOf<float>;
-template class DirectionOf<signed short>;
+template class LinearAlgebra::DirectionOf<float>;
+template class LinearAlgebra::DirectionOf<signed short>;
+}
--- a/LinearAlgebra/Direction.h
+++ b/LinearAlgebra/Direction.h
@ -30,11 +30,11 @@ class DirectionOf {
  AngleOf<T> vertical;

  /// @brief Create a new direction with zero angles
-  DirectionOf<T>();
+  DirectionOf();
  /// @brief Create a new direction
  /// @param horizontal The horizontal angle
  /// @param vertical The vertical angle.
-  DirectionOf<T>(AngleOf<T> horizontal, AngleOf<T> vertical);
+  DirectionOf(AngleOf<T> horizontal, AngleOf<T> vertical);

  /// @brief Convert the direction into a carthesian vector
  /// @return The carthesian vector corresponding to this direction.
@ -99,6 +99,4 @@ using Direction = DirectionSingle;

 }  // namespace LinearAlgebra

-using namespace LinearAlgebra;
-
 #endif
--- a/LinearAlgebra/Matrix.cpp
+++ b/LinearAlgebra/Matrix.cpp
@ -1,46 +1,125 @@
 #include "Matrix.h"
+#if !defined(NO_STD) 
 #include <iostream>
+#endif
+
+namespace LinearAlgebra {
+
+#pragma region Matrix1
+
+Matrix1::Matrix1(int size) : size(size) {
+  if (this->size == 0)
+    data = nullptr;
+  else {
+    this->data = new float[size]();
+    this->externalData = false;
+  }
+}
+
+Matrix1::Matrix1(float* data, int size) : data(data), size(size) {
+  this->externalData = true;
+}
+
+Matrix1 LinearAlgebra::Matrix1::FromQuaternion(Quaternion q) {
+  Matrix1 r = Matrix1(4);
+  float* data = r.data;
+  data[0] = q.x;
+  data[1] = q.y;
+  data[2] = q.z;
+  data[3] = q.w;
+  return r;
+}
+
+Quaternion LinearAlgebra::Matrix1::ToQuaternion() {
+  return Quaternion(this->data[0], this->data[1], this->data[2], this->data[3]);
+}
+
+// Matrix1
+#pragma endregion

 #pragma region Matrix2

+Matrix2::Matrix2() {}
+
 Matrix2::Matrix2(int nRows, int nCols) : nRows(nRows), nCols(nCols) {
  this->nValues = nRows * nCols;
  if (this->nValues == 0)
-    data = nullptr;
+    this->data = nullptr;
  else {
-    this->data = new float[nValues]();
+    this->data = new float[this->nValues];
    this->externalData = false;
  }
 }

 Matrix2::Matrix2(float* data, int nRows, int nCols)
-    : nRows(nRows), nCols(nCols), data(data){
+    : nRows(nRows), nCols(nCols), data(data) {
  this->nValues = nRows * nCols;
  this->externalData = true;
 }

+Matrix2::Matrix2(const Matrix2& m)
+    : nRows(m.nRows), nCols(m.nCols), nValues(m.nValues) {
+  if (this->nValues == 0)
+    this->data = nullptr;
+  else {
+    this->data = new float[this->nValues];
+
+    for (int ix = 0; ix < this->nValues; ++ix)
+      this->data[ix] = m.data[ix];
+  }
+}
+
+Matrix2& Matrix2::operator=(const Matrix2& m) {
+  if (this != &m) {
+    delete[] this->data;  // Free the current memory
+
+    this->nRows = m.nRows;
+    this->nCols = m.nCols;
+    this->nValues = m.nValues;
+    if (this->nValues == 0)
+      this->data = nullptr;
+    else {
+      this->data = new float[this->nValues];
+      for (int ix = 0; ix < this->nValues; ++ix)
+        this->data[ix] = m.data[ix];
+    }
+  }
+  return *this;
+}
+
 Matrix2::~Matrix2() {
-  if (data != nullptr && !this->externalData)
+  if (!this->externalData)
    delete[] data;
 }

+Matrix2 Matrix2::Clone() const {
+  Matrix2 r = Matrix2(this->nRows, this->nCols);
+  for (int ix = 0; ix < this->nValues; ++ix)
+    r.data[ix] = this->data[ix];
+  return r;
+}
+
 // Move constructor
-Matrix2::Matrix2(Matrix2&& other) noexcept 
-    : nRows(other.nRows), nCols(other.nCols), nValues(other.nValues), data(other.data) {
-    other.data = nullptr;  // Set the other object's pointer to nullptr to avoid double deletion
+Matrix2::Matrix2(Matrix2&& other) noexcept
+    : nRows(other.nRows),
+      nCols(other.nCols),
+      nValues(other.nValues),
+      data(other.data) {
+  other.data = nullptr;  // Set the other object's pointer to nullptr to avoid
+                         // double deletion
 }

 // Move assignment operator
 Matrix2& Matrix2::operator=(Matrix2&& other) noexcept {
-    if (this != &other) {
-        delete[] data;  // Clean up current data
-        nRows = other.nRows;
-        nCols = other.nCols;
-        nValues = other.nValues;
-        data = other.data;
-        other.data = nullptr;  // Avoid double deletion
-    }
-    return *this;
+  if (this != &other) {
+    delete[] data;  // Clean up current data
+    nRows = other.nRows;
+    nCols = other.nCols;
+    nValues = other.nValues;
+    data = other.data;
+    other.data = nullptr;  // Avoid double deletion
+  }
+  return *this;
 }

 Matrix2 Matrix2::Zero(int nRows, int nCols) {
@ -50,12 +129,17 @@ Matrix2 Matrix2::Zero(int nRows, int nCols) {
  return r;
 }

+void Matrix2::Clear() {
+  for (int ix = 0; ix < this->nValues; ix++)
+    this->data[ix] = 0;
+}
+
 Matrix2 Matrix2::Identity(int size) {
  return Diagonal(1, size);
 }

 Matrix2 Matrix2::Diagonal(float f, int size) {
-  Matrix2 r = Matrix2(size, size);
+  Matrix2 r = Matrix2::Zero(size, size);
  float* data = r.data;
  int valueIx = 0;
  for (int ix = 0; ix < size; ix++) {
@ -77,6 +161,17 @@ Matrix2 Matrix2::SkewMatrix(const Vector3& v) {
  return r;
 }

+Matrix2 Matrix2::Transpose() const {
+  Matrix2 r = Matrix2(this->nCols, this->nRows);
+
+  for (int rowIx = 0; rowIx < this->nRows; rowIx++) {
+    for (int colIx = 0; colIx < this->nCols; colIx++)
+      r.data[colIx * this->nCols + rowIx] =
+          this->data[rowIx * this->nCols + colIx];
+  }
+  return r;
+}
+
 Matrix2 LinearAlgebra::Matrix2::operator-() const {
  Matrix2 r = Matrix2(this->nRows, this->nCols);
  for (int ix = 0; ix < r.nValues; ix++)
@ -84,13 +179,26 @@ Matrix2 LinearAlgebra::Matrix2::operator-() const {
  return r;
 }

+Matrix2 LinearAlgebra::Matrix2::operator+(const Matrix2& v) const {
+  Matrix2 r = Matrix2(this->nRows, this->nCols);
+  for (int ix = 0; ix < r.nValues; ix++)
+    r.data[ix] = this->data[ix] + v.data[ix];
+  return r;
+}
+
+Matrix2 Matrix2::operator+=(const Matrix2& v) {
+  for (int ix = 0; ix < this->nValues; ix++)
+    this->data[ix] += v.data[ix];
+  return *this;
+}
+
 Matrix2 LinearAlgebra::Matrix2::operator*(const Matrix2& B) const {
  Matrix2 r = Matrix2(this->nRows, B.nCols);

  int ACols = this->nCols;
  int BCols = B.nCols;
  int ARows = this->nRows;
-  //int BRows = B.nRows;
+  // int BRows = B.nRows;

  for (int i = 0; i < ARows; ++i) {
    // Pre-compute row offsets
@ -101,7 +209,8 @@ Matrix2 LinearAlgebra::Matrix2::operator*(const Matrix2& B) const {
      std::cout << " 0";
      int BIndex = j;
      for (int k = 0; k < ACols; ++k) {
-        std::cout << " + " << this->data[ARowOffset + k] << " * " << B.data[BIndex];
+        std::cout << " + " << this->data[ARowOffset + k] << " * "
+                  << B.data[BIndex];
        sum += this->data[ARowOffset + k] * B.data[BIndex];
        BIndex += BCols;
      }
@ -112,22 +221,68 @@ Matrix2 LinearAlgebra::Matrix2::operator*(const Matrix2& B) const {
  return r;
 }

-void LinearAlgebra::Matrix2::SetSlice(int rowStart,
-                                      int rowStop,
-                                      int colStart,
-                                      int colStop,
-                                      const Matrix2& m) const {
+Matrix2 Matrix2::Slice(int rowStart, int rowStop, int colStart, int colStop) {
+  Matrix2 r = Matrix2(rowStop - rowStart, colStop - colStart);
+
+  int resultRowIx = 0;
+  int resultColIx = 0;
  for (int i = rowStart; i < rowStop; i++) {
    for (int j = colStart; j < colStop; j++)
-      this->data[i * this->nCols + j] =
-          m.data[(i - rowStart) * m.nCols + (j - colStart)];
-    // this->data[i, j] = m.data[i - rowStart, j - colStart];
+      r.data[resultRowIx * r.nCols + resultColIx] =
+          this->data[i * this->nCols + j];
  }
+  return r;
+}
+
+void Matrix2::UpdateSlice(int rowStart,
+                          int rowStop,
+                          int colStart,
+                          int colStop,
+                          const Matrix2& m) const {
+  // for (int i = rowStart; i < rowStop; i++) {
+  //   for (int j = colStart; j < colStop; j++)
+  //     this->data[i * this->nCols + j] =
+  //         m.data[(i - rowStart) * m.nCols + (j - colStart)];
+  // }
+
+  int rRowDataIx = rowStart * this->nCols;
+  int mRowDataIx = 0;
+  for (int rowIx = rowStart; rowIx < rowStop; rowIx++) {
+    rRowDataIx = rowIx * this->nCols;
+    // rRowDataIx += this->nCols;
+    mRowDataIx += m.nCols;
+    for (int colIx = colStart; colIx < colStop; colIx++) {
+      this->data[rRowDataIx + colIx] = m.data[mRowDataIx + (colIx - colStart)];
+    }
+  }
+}
+
+/// @brief Compute the Omega matrix of a 3D vector
+/// @param v The vector
+/// @return 4x4 Omega matrix
+Matrix2 LinearAlgebra::Matrix2::Omega(const Vector3& v) {
+  Matrix2 r = Matrix2::Zero(4, 4);
+  r.UpdateSlice(0, 3, 0, 3, -Matrix2::SkewMatrix(v));
+
+  // set last row to -v
+  int ix = 3 * 4;
+  r.data[ix++] = -v.x;
+  r.data[ix++] = -v.y;
+  r.data[ix] = -v.z;
+
+  // Set last column to v
+  ix = 3;
+  r.data[ix += 4] = v.x;
+  r.data[ix += 4] = v.y;
+  r.data[ix] = v.z;
+  return r;
 }

 // Matrix2
 #pragma endregion

+}  // namespace LinearAlgebra
+
 template <>
 MatrixOf<float>::MatrixOf(unsigned int rows, unsigned int cols) {
  if (rows <= 0 || cols <= 0) {
--- a/LinearAlgebra/Matrix.h
+++ b/LinearAlgebra/Matrix.h
@ -1,24 +1,47 @@
 #ifndef MATRIX_H
 #define MATRIX_H

+#include "Quaternion.h"
 #include "Vector3.h"

 namespace LinearAlgebra {

+/// @brief A 1-dimensional matrix or vector of arbitrary size
+class Matrix1 {
+ public:
+  float* data = nullptr;
+  int size = 0;
+
+  Matrix1(int size);
+  Matrix1(float* data, int size);
+
+  static Matrix1 FromQuaternion(Quaternion q);
+  Quaternion ToQuaternion();
+
+ private:
+  bool externalData = true;
+};
+
+/// @brief A 2-dimensional matrix of arbitrary size
 class Matrix2 {
 public:
  int nRows = 0;
  int nCols = 0;
  int nValues = 0;
  float* data = nullptr;
-  bool externalData = true;

+  Matrix2();
  Matrix2(int nRows, int nCols);
  Matrix2(float* data, int nRows, int nCols);
+  Matrix2(const Matrix2& m);
+  Matrix2& operator=(const Matrix2& other);

- ~Matrix2();
+  ~Matrix2();
+
+  Matrix2 Clone() const;

  static Matrix2 Zero(int nRows, int nCols);
+  void Clear();

  static Matrix2 Identity(int size);

@ -26,15 +49,69 @@ class Matrix2 {

  static Matrix2 SkewMatrix(const Vector3& v);

+  Matrix2 Transpose() const;
+
  Matrix2 operator-() const;

-  Matrix2 operator*(const Matrix2& m) const;
+  /// @brief Add a matrix to this matrix
+  /// @param m The matrix to add to this matrix
+  /// @return The result of the addition
+  Matrix2 operator+(const Matrix2& v) const;
+  Matrix2 operator+=(const Matrix2& v);

-  void SetSlice(int rowStart, int rowStop, int colStart, int colStop, const Matrix2& m) const;
-//private:
-  // move constructor and move assignment operator
+  Matrix2 operator*(const Matrix2& m) const;
+  friend Matrix2 operator*(const Matrix2& m, float f) {
+    Matrix2 r = Matrix2(m.nRows, m.nCols);
+    for (int ix = 0; ix < r.nValues; ix++)
+      r.data[ix] = m.data[ix] * f;
+    return r;
+  }
+  friend Matrix2 operator*(float f, const Matrix2& m) {
+    Matrix2 r = Matrix2(m.nRows, m.nCols);
+    for (int ix = 0; ix < r.nValues; ix++)
+      r.data[ix] = f * m.data[ix];
+    return r;
+  }
+
+  friend Matrix1 operator*(const Matrix2& m, const Matrix1& v) {
+    Matrix1 r = Matrix1(m.nRows);
+    for (int rowIx = 0; rowIx < m.nRows; rowIx++) {
+      int mRowIx = rowIx * m.nCols;
+      for (int colIx = 0; colIx < m.nCols; colIx++)
+        r.data[rowIx] += m.data[mRowIx + colIx] * v.data[rowIx];
+    }
+    return r;
+  }
+
+  friend Matrix2 operator/(const Matrix2& m, float f) {
+    Matrix2 r = Matrix2(m.nRows, m.nCols);
+    for (int ix = 0; ix < r.nValues; ix++)
+      r.data[ix] = m.data[ix] / f;
+    return r;
+  }
+  friend Matrix2 operator/(float f, const Matrix2& m) {
+    Matrix2 r = Matrix2(m.nRows, m.nCols);
+    for (int ix = 0; ix < r.nValues; ix++)
+      r.data[ix] = f / m.data[ix];
+    return r;
+  }
+
+  Matrix2 Slice(int rawStart, int rowStop, int colStart, int colStop);
+
+  void UpdateSlice(int rowStart,
+                int rowStop,
+                int colStart,
+                int colStop,
+                const Matrix2& m) const;
+  // private:
+  //  move constructor and move assignment operator
  Matrix2(Matrix2&& other) noexcept;
  Matrix2& operator=(Matrix2&& other) noexcept;
+
+  static Matrix2 Omega(const Vector3& v);
+
+ private:
+  bool externalData = true;
 };

 /// @brief Single precision float matrix
@ -148,6 +225,6 @@ class MatrixOf {
 };

 }  // namespace LinearAlgebra
-using namespace LinearAlgebra;
+// using namespace LinearAlgebra;

 #endif
--- a/LinearAlgebra/Polar.cpp
+++ b/LinearAlgebra/Polar.cpp
@ -175,5 +175,5 @@ PolarOf<T> PolarOf<T>::Rotate(const PolarOf& v, AngleOf<T> angle) {
  return r;
 }

-template class PolarOf<float>;
-template class PolarOf<signed short>;
+template class LinearAlgebra::PolarOf<float>;
+template class LinearAlgebra::PolarOf<signed short>;
--- a/LinearAlgebra/Spherical.cpp
+++ b/LinearAlgebra/Spherical.cpp
@ -5,6 +5,8 @@

 #include <math.h>

+namespace LinearAlgebra {
+
 template <typename T>
 SphericalOf<T>::SphericalOf() {
  this->distance = 0.0f;
@ -301,3 +303,5 @@ SphericalOf<T> SphericalOf<T>::RotateVertical(const SphericalOf<T>& v,

 template class SphericalOf<float>;
 template class SphericalOf<signed short>;
+
+}  // namespace LinearAlgebra
--- a/LinearAlgebra/Spherical.h
+++ b/LinearAlgebra/Spherical.h
@ -24,9 +24,9 @@ class SphericalOf {
  /// @brief The direction of the vector
  DirectionOf<T> direction;

-  SphericalOf<T>();
-  SphericalOf<T>(float distance, AngleOf<T> horizontal, AngleOf<T> vertical);
-  SphericalOf<T>(float distance, DirectionOf<T> direction);
+  SphericalOf();
+  SphericalOf(float distance, AngleOf<T> horizontal, AngleOf<T> vertical);
+  SphericalOf(float distance, DirectionOf<T> direction);

  /// @brief Create spherical vector without using AngleOf type. All given
  /// angles are in degrees
@ -186,7 +186,6 @@ using Spherical = SphericalSingle;
 #endif

 }  // namespace LinearAlgebra
-using namespace LinearAlgebra;

 #include "Polar.h"
 #include "Vector3.h"
--- a/LinearAlgebra/SwingTwist.cpp
+++ b/LinearAlgebra/SwingTwist.cpp
@ -4,6 +4,8 @@

 #include "SwingTwist.h"

+namespace LinearAlgebra {
+
 template <typename T>
 SwingTwistOf<T>::SwingTwistOf() {
  this->swing = DirectionOf<T>(AngleOf<T>(), AngleOf<T>());
@ -165,4 +167,6 @@ void SwingTwistOf<T>::Normalize() {
 }

 template class SwingTwistOf<float>;
-template class SwingTwistOf<signed short>;
+template class SwingTwistOf<signed short>;
+
+}
--- a/LinearAlgebra/SwingTwist.h
+++ b/LinearAlgebra/SwingTwist.h
@ -21,9 +21,9 @@ class SwingTwistOf {
  DirectionOf<T> swing;
  AngleOf<T> twist;

-  SwingTwistOf<T>();
-  SwingTwistOf<T>(DirectionOf<T> swing, AngleOf<T> twist);
-  SwingTwistOf<T>(AngleOf<T> horizontal, AngleOf<T> vertical, AngleOf<T> twist);
+  SwingTwistOf();
+  SwingTwistOf(DirectionOf<T> swing, AngleOf<T> twist);
+  SwingTwistOf(AngleOf<T> horizontal, AngleOf<T> vertical, AngleOf<T> twist);

  static SwingTwistOf<T> Degrees(float horizontal,
                                 float vertical = 0,
--- a/LinearAlgebra/test/Direction_test.cc
+++ b/LinearAlgebra/test/Direction_test.cc
@ -6,6 +6,8 @@

 #include "Direction.h"

+using namespace LinearAlgebra;
+
 #define FLOAT_INFINITY std::numeric_limits<float>::infinity()

 TEST(Direction16, Compare) {
--- a/LinearAlgebra/test/Polar_test.cc
+++ b/LinearAlgebra/test/Polar_test.cc
@ -2,6 +2,7 @@
 #include <gtest/gtest.h>
 #include <limits>
 #include <math.h>
+#include <chrono>

 #include "Polar.h"
 #include "Spherical.h"
--- a/LinearAlgebra/test/Spherical16_test.cc
+++ b/LinearAlgebra/test/Spherical16_test.cc
@ -2,6 +2,7 @@
 #include <gtest/gtest.h>
 #include <limits>
 #include <math.h>
+#include <chrono>

 #include "Spherical.h"
 #include "Vector3.h"
--- a/LinearAlgebra/test/SphericalSingle_test.cc
+++ b/LinearAlgebra/test/SphericalSingle_test.cc
@ -2,6 +2,7 @@
 #include <gtest/gtest.h>
 #include <limits>
 #include <math.h>
+#include <chrono>

 #include "Spherical.h"

--- a/LocalParticipant.cpp
+++ b/LocalParticipant.cpp
@ -1,370 +0,0 @@
-#include "LocalParticipant.h"
-
-#include "Thing.h"
-
-#include "Arduino/ArduinoParticipant.h"
-
-#if defined(_WIN32) || defined(_WIN64)
-#include <winsock2.h>
-#include <ws2tcpip.h>
-#include "Windows/WindowsParticipant.h"
-#pragma comment(lib, "ws2_32.lib")
-
-#elif defined(__unix__) || defined(__APPLE__)
-#include <arpa/inet.h>
-#include <fcntl.h>  // For fcntl
-#include <netinet/in.h>
-#include <sys/socket.h>
-#include <unistd.h>
-#include <chrono>
-#include "Posix/PosixParticipant.h"
-#endif
-
-#include <string.h>
-
-namespace RoboidControl {
-
-// LocalParticipant::LocalParticipant() {}
-
-LocalParticipant::LocalParticipant(int port) {
-  this->ipAddress = "0.0.0.0";
-  this->port = port;
-  if (this->port == 0)
-    this->isIsolated = true;
-}
-
-LocalParticipant::LocalParticipant(const char* ipAddress, int port) {
-  this->ipAddress = "0.0.0.0";  // ipAddress;  // maybe this is not needed
-                                // anymore, keeping it to "0.0.0.0"
-  this->port = port;
-  if (this->port == 0)
-    this->isIsolated = true;
-  else
-    this->remoteSite = new Participant(ipAddress, port);
-}
-
-static LocalParticipant* isolatedParticipant = nullptr;
-
-LocalParticipant* LocalParticipant::Isolated() {
-  if (isolatedParticipant == nullptr)
-    isolatedParticipant = new LocalParticipant(0);
-  return isolatedParticipant;
-}
-
-void LocalParticipant::begin() {
-  if (this->isIsolated)
-    return;
-
-  SetupUDP(this->port, this->ipAddress, this->port);
-}
-
-void LocalParticipant::SetupUDP(int localPort,
-                                const char* remoteIpAddress,
-                                int remotePort) {
-#if defined(_WIN32) || defined(_WIN64)
-  Windows::LocalParticipant* thisWindows =
-      static_cast<Windows::LocalParticipant*>(this);
-  thisWindows->Setup(localPort, remoteIpAddress, remotePort);
-#elif defined(__unix__) || defined(__APPLE__)
-  Posix::LocalParticipant* thisPosix =
-      static_cast<Posix::LocalParticipant*>(this);
-  thisPosix->Setup(localPort, remoteIpAddress, remotePort);
-#elif defined(ARDUINO)
-  Arduino::LocalParticipant* thisArduino =
-      static_cast<Arduino::LocalParticipant*>(this);
-  thisArduino->Setup(localPort, remoteIpAddress, remotePort);
-#endif
-  this->connected = true;
-}
-
-void LocalParticipant::Update(unsigned long currentTimeMs) {
-  if (currentTimeMs == 0) {
-    currentTimeMs = Thing::GetTimeMs();
-    // #if defined(ARDUINO)
-    //     currentTimeMs = millis();
-    // #elif defined(__unix__) || defined(__APPLE__)
-    //     auto now = std::chrono::steady_clock::now();
-    //     auto ms =
-    //     std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch());
-    //     currentTimeMs = static_cast<unsigned long>(ms.count());
-    // #endif
-  }
-
-  if (this->isIsolated == false) {
-    if (this->connected == false)
-      begin();
-
-    if (this->publishInterval > 0 && currentTimeMs > this->nextPublishMe) {
-      ParticipantMsg* msg = new ParticipantMsg(this->networkId);
-      if (this->remoteSite == nullptr)
-        this->Publish(msg);
-      else
-        this->Send(this->remoteSite, msg);
-      delete msg;
-
-      this->nextPublishMe = currentTimeMs + this->publishInterval;
-    }
-    this->ReceiveUDP();
-  }
-
-  for (Thing* thing : this->things) {
-    if (thing != nullptr) {
-      thing->Update(currentTimeMs);
-      if (this->isIsolated == false) {
-        PoseMsg* poseMsg = new PoseMsg(this->networkId, thing);
-        for (Participant* sender : this->senders)
-          this->Send(sender, poseMsg);
-        delete poseMsg;
-      }
-    }
-  }
-}
-
-void LocalParticipant::ReceiveUDP() {
-#if defined(_WIN32) || defined(_WIN64)
-  Windows::LocalParticipant* thisWindows =
-      static_cast<Windows::LocalParticipant*>(this);
-  thisWindows->Receive();
-#elif defined(__unix__) || defined(__APPLE__)
-  Posix::LocalParticipant* thisPosix =
-      static_cast<Posix::LocalParticipant*>(this);
-  thisPosix->Receive();
-#elif defined(ARDUINO)
-  Arduino::LocalParticipant* thisArduino =
-      static_cast<Arduino::LocalParticipant*>(this);
-  thisArduino->Receive();
-#endif
-}
-
-Participant* LocalParticipant::GetParticipant(const char* ipAddress, int port) {
-  for (Participant* sender : this->senders) {
-    if (strcmp(sender->ipAddress, ipAddress) == 0 && sender->port == port)
-      return sender;
-  }
-  return nullptr;
-}
-
-Participant* LocalParticipant::AddParticipant(const char* ipAddress, int port) {
-  // std::cout << "New Participant " << ipAddress << ":" << port << "\n";
-  Participant* participant = new Participant(ipAddress, port);
-#if defined(NO_STD)
-  participant->networkId = this->senderCount;
-  this->senders[this->senderCount++] = participant;
-#else
-  participant->networkId = (unsigned char)this->senders.size();
-  this->senders.push_back(participant);
-#endif
-  return participant;
-}
-
-#pragma region Send
-
-void LocalParticipant::SendThingInfo(Participant* remoteParticipant,
-                                     Thing* thing) {
-  // std::cout << "Send thing info " << (int)thing->id << " \n";
-  ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
-  this->Send(remoteParticipant, thingMsg);
-  delete thingMsg;
-  NameMsg* nameMsg = new NameMsg(this->networkId, thing);
-  this->Send(remoteParticipant, nameMsg);
-  delete nameMsg;
-  ModelUrlMsg* modelMsg = new ModelUrlMsg(this->networkId, thing);
-  this->Send(remoteParticipant, modelMsg);
-  delete modelMsg;
-  PoseMsg* poseMsg = new PoseMsg(this->networkId, thing, true);
-  this->Send(remoteParticipant, poseMsg);
-  delete poseMsg;
-  BinaryMsg* customMsg = new BinaryMsg(this->networkId, thing);
-  this->Send(remoteParticipant, customMsg);
-  delete customMsg;
-}
-
-bool LocalParticipant::Send(Participant* remoteParticipant, IMessage* msg) {
-  int bufferSize = msg->Serialize(this->buffer);
-  if (bufferSize <= 0)
-    return true;
-
-#if defined(_WIN32) || defined(_WIN64)
-  Windows::LocalParticipant* thisWindows =
-      static_cast<Windows::LocalParticipant*>(this);
-  return thisWindows->Send(remoteParticipant, bufferSize);
-#elif defined(__unix__) || defined(__APPLE__)
-  Posix::LocalParticipant* thisPosix =
-      static_cast<Posix::LocalParticipant*>(this);
-  return thisPosix->Send(remoteParticipant, bufferSize);
-#elif defined(ARDUINO)
-  Arduino::LocalParticipant* thisArduino =
-      static_cast<Arduino::LocalParticipant*>(this);
-  return thisArduino->Send(remoteParticipant, bufferSize);
-#else
-  return false;  
-#endif
-}
-
-void LocalParticipant::PublishThingInfo(Thing* thing) {
-  // std::cout << "Publish thing info" << thing->networkId << "\n";
-  //  Strange, when publishing, the network id is irrelevant, because it is
-  //  connected to a specific site...
-  ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
-  this->Publish(thingMsg);
-  delete thingMsg;
-  NameMsg* nameMsg = new NameMsg(this->networkId, thing);
-  this->Publish(nameMsg);
-  delete nameMsg;
-  ModelUrlMsg* modelMsg = new ModelUrlMsg(this->networkId, thing);
-  this->Publish(modelMsg);
-  delete modelMsg;
-  PoseMsg* poseMsg = new PoseMsg(this->networkId, thing, true);
-  this->Publish(poseMsg);
-  delete poseMsg;
-  BinaryMsg* customMsg = new BinaryMsg(this->networkId, thing);
-  this->Publish(customMsg);
-  delete customMsg;
-}
-
-bool LocalParticipant::Publish(IMessage* msg) {
-#if defined(_WIN32) || defined(_WIN64)
-  Windows::LocalParticipant* thisWindows =
-      static_cast<Windows::LocalParticipant*>(this);
-  return thisWindows->Publish(msg);
-#elif defined(__unix__) || defined(__APPLE__)
-  Posix::LocalParticipant* thisPosix =
-      static_cast<Posix::LocalParticipant*>(this);
-  return thisPosix->Publish(msg);
-#elif defined(ARDUINO)
-  Arduino::LocalParticipant* thisArduino =
-      static_cast<Arduino::LocalParticipant*>(this);
-  return thisArduino->Publish(msg);
-#else
-  return false;
-#endif
-}
-
-// Send
-#pragma endregion
-
-#pragma region Receive
-
-void LocalParticipant::ReceiveData(unsigned char packetSize,
-                                   char* senderIpAddress,
-                                   unsigned int senderPort) {
-  Participant* remoteParticipant =
-      this->GetParticipant(senderIpAddress, senderPort);
-  if (remoteParticipant == nullptr) {
-    remoteParticipant = this->AddParticipant(senderIpAddress, senderPort);
-    // std::cout << "New sender " << sender_ipAddress << ":" << sender_port
-    //           << "\n";
-    // std::cout << "New remote participant " << remoteParticipant->ipAddress
-    //           << ":" << remoteParticipant->port << " "
-    //           << (int)remoteParticipant->networkId << "\n";
-  }
-
-  ReceiveData(packetSize, remoteParticipant);
-}
-
-void LocalParticipant::ReceiveData(unsigned char bufferSize,
-                                   Participant* remoteParticipant) {
-  unsigned char msgId = this->buffer[0];
-  // std::cout << "receive msg " << (int)msgId << "\n";
-  switch (msgId) {
-    case ParticipantMsg::id: {
-      ParticipantMsg* msg = new ParticipantMsg(this->buffer);
-      Process(remoteParticipant, msg);
-      delete msg;
-    } break;
-    case SiteMsg::id: {
-      SiteMsg* msg = new SiteMsg(this->buffer);
-      Process(remoteParticipant, msg);
-      delete msg;
-    } break;
-    case InvestigateMsg::id: {
-      InvestigateMsg* msg = new InvestigateMsg(this->buffer);
-      Process(remoteParticipant, msg);
-      delete msg;
-    } break;
-    case ThingMsg::id: {
-      ThingMsg* msg = new ThingMsg(this->buffer);
-      Process(remoteParticipant, msg);
-      delete msg;
-    } break;
-    case NameMsg::id: {
-      NameMsg* msg = new NameMsg(this->buffer);
-      Process(remoteParticipant, msg);
-      delete msg;
-    } break;
-    case PoseMsg::id: {
-      PoseMsg* msg = new PoseMsg(this->buffer);
-      Process(remoteParticipant, msg);
-      delete msg;
-    } break;
-    case BinaryMsg::id: {
-      BinaryMsg* msg = new BinaryMsg(this->buffer);
-      Process(remoteParticipant, msg);
-      delete msg;
-    } break;
-  };
-}
-
-void LocalParticipant::Process(Participant* sender, ParticipantMsg* msg) {}
-
-void LocalParticipant::Process(Participant* sender, SiteMsg* msg) {
-  // std::cout << this->name << ": process NetworkId [" << (int)this->networkId
-  //           << "/" << (int)msg->networkId << "]\n";
-  if (this->networkId != msg->networkId) {
-    this->networkId = msg->networkId;
-    // std::cout << this->things.size() << " things\n";
-    for (Thing* thing : this->things)
-      this->SendThingInfo(sender, thing);
-  }
-}
-
-void LocalParticipant::Process(Participant* sender, InvestigateMsg* msg) {}
-
-void LocalParticipant::Process(Participant* sender, ThingMsg* msg) {}
-
-void LocalParticipant::Process(Participant* sender, NameMsg* msg) {
-  Thing* thing = sender->Get(msg->networkId, msg->thingId);
-  if (thing != nullptr) {
-    int nameLength = msg->nameLength;
-    int stringLen = nameLength + 1;
-    char* thingName = new char[stringLen];
-#if defined(_WIN32) || defined(_WIN64)
-    strncpy_s(thingName, stringLen, msg->name,
-              stringLen - 1);  // Leave space for null terminator
-#else
-    // Use strncpy with bounds checking for other platforms (Arduino, POSIX,
-    // ESP-IDF)
-    strncpy(thingName, msg->name,
-            stringLen - 1);           // Leave space for null terminator
-    thingName[stringLen - 1] = '\0';  // Ensure null termination
-#endif
-    thingName[nameLength] = '\0';
-    thing->name = thingName;
-    // std::cout << "thing name = " << thing->name << " length = " << nameLength
-    //           << "\n";
-  }
-}
-
-void LocalParticipant::Process(Participant* sender, PoseMsg* msg) {}
-
-void LocalParticipant::Process(Participant* sender, BinaryMsg* msg) {
-  // std::cout << this->name << ": process Binary [" << (int)this->networkId <<
-  // "/"
-  //           << (int)msg->networkId << "]\n";
-  Thing* thing = sender->Get(msg->networkId, msg->thingId);
-  if (thing != nullptr)
-    thing->ProcessBinary(msg->bytes);
-  else {
-    thing = this->Get(msg->networkId, msg->thingId);
-    if (thing != nullptr)
-      thing->ProcessBinary(msg->bytes);
-    // else
-    //   std::cout << "custom msg for unknown thing [" << (int)msg->networkId
-    //             << "/" << (int)msg->thingId << "]\n";
-  }
-}
-
-// Receive
-#pragma endregion
-
-}  // namespace RoboidControl
--- a/Messages/BinaryMsg.cpp
+++ b/Messages/BinaryMsg.cpp
@ -2,33 +2,48 @@

 namespace RoboidControl {

-BinaryMsg::BinaryMsg(char* buffer) {
-  unsigned char ix = 1;
-  this->networkId = buffer[ix++];
-  this->thingId = buffer[ix++];
-  this->bytes = buffer + ix;  // This is only valid because the code ensures the the msg
-                              // lifetime is shorter than the buffer lifetime...
-}
-
 BinaryMsg::BinaryMsg(unsigned char networkId, Thing* thing) {
  this->networkId = networkId;
  this->thingId = thing->id;
  this->thing = thing;
+  unsigned char ix = 0; //BinaryMsg::length;
+  this->data = new char[255];
+  this->dataLength = this->thing->GenerateBinary(this->data, &ix);
 }

-BinaryMsg::~BinaryMsg() {}
+BinaryMsg::BinaryMsg(char* buffer) {
+  unsigned char ix = 1;
+  this->networkId = buffer[ix++];
+  this->thingId = buffer[ix++];
+  this->dataLength = buffer[ix++];
+  char* data = new char[this->dataLength];
+  for (int i = 0; i < this->dataLength; i++)
+    data[i] = buffer[ix++];
+  this->data = data;
+}
+
+BinaryMsg::~BinaryMsg() {
+  delete[] this->data;
+}

 unsigned char BinaryMsg::Serialize(char* buffer) {
-  unsigned char ix = this->length;
-  this->thing->GenerateBinary(buffer, &ix);
-  if (ix <= this->length)  // in this case, no data is actually sent
+  // unsigned char ix = this->length;
+  //  this->dataLength = this->thing->GenerateBinary(buffer, &ix);
+  if (this->dataLength <= 0)  // in this case, no data is actually sent
    return 0;

-  buffer[0] = this->id;
-  buffer[1] = this->networkId;
-  buffer[2] = this->thingId;
-  return ix;
+#if defined(DEBUG)
+  std::cout << "Send BinaryMsg [" << (int)this->networkId << "/" << (int)this->thingId
+            << "] " << (int)this->dataLength << std::endl;
+#endif
+  unsigned char ix = 0;
+  buffer[ix++] = this->id;
+  buffer[ix++] = this->networkId;
+  buffer[ix++] = this->thingId;
+  buffer[ix++] = this->dataLength;
+  for (int dataIx = 0; dataIx < this->dataLength; dataIx++)
+    buffer[ix++] = this->data[dataIx];
+  return this->length + this->dataLength;
 }

-
 }  // namespace RoboidControl
--- a/Messages/BinaryMsg.h
+++ b/Messages/BinaryMsg.h
@ -1,16 +1,18 @@
 #pragma once

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

 namespace RoboidControl {

-/// @brief Message to send thing-specific data
+/// @brief A message containing binary data for custom communication
 class BinaryMsg : public IMessage {
 public:
  /// @brief The message ID
  static const unsigned char id = 0xB1;
-  /// @brief The length of the message without the binary data itslef
-  static const unsigned length = 3;
+  /// @brief The length of the message in bytes, excluding the binary data
+  /// For the total size of the message this.bytes.Length should be added to this value.
+  static const unsigned length = 4;

  /// @brief The network ID of the thing
  unsigned char networkId;
@ -19,10 +21,11 @@ class BinaryMsg : public IMessage {
  /// @brief The thing for which the binary data is communicated
  Thing* thing;

+  unsigned char dataLength;
  /// @brief The binary data which is communicated
-  char* bytes = nullptr;
+  char* data = nullptr;

-  /// @brief Create a new message for sending
+  /// @brief Create a BinaryMsg
  /// @param networkId The network ID of the thing
  /// @param thing The thing for which binary data is sent
  BinaryMsg(unsigned char networkId, Thing* thing);
--- a/Messages/DestroyMsg.cpp
+++ b/Messages/DestroyMsg.cpp
@ -2,16 +2,23 @@

 namespace RoboidControl {

-DestroyMsg::DestroyMsg(unsigned char networkId, Thing *thing) {
+DestroyMsg::DestroyMsg(unsigned char networkId, Thing* thing) {
  this->networkId = networkId;
  this->thingId = thing->id;
 }

-DestroyMsg::DestroyMsg(char* buffer) {}
+DestroyMsg::DestroyMsg(char* buffer) {
+  this->networkId = buffer[1];
+  this->thingId = buffer[2];
+}

 DestroyMsg::~DestroyMsg() {}

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

-} // namespace RoboidControl
+}  // namespace RoboidControl
--- a/Messages/DestroyMsg.h
+++ b/Messages/DestroyMsg.h
@ -1,13 +1,16 @@
-#include "Messages.h"
+#pragma once
+
+#include "IMessage.h"
+#include "Thing.h"

 namespace RoboidControl {

-/// @brief Message notifiying that a Thing no longer exists
+/// @brief A Message notifiying that a Thing no longer exists
 class DestroyMsg : public IMessage {
 public:
  /// @brief The message ID
  static const unsigned char id = 0x20;
-  /// @brief The length of the message
+  /// @brief The length of the message in bytes
  static const unsigned length = 3;
  /// @brief The network ID of the thing
  unsigned char networkId;
--- a/Messages/IMessage.cpp
+++ b/Messages/IMessage.cpp
@ -0,0 +1,16 @@
+#include "IMessage.h"
+
+namespace RoboidControl {
+
+#pragma region IMessage
+
+IMessage::IMessage() {}
+
+unsigned char IMessage::Serialize(char* buffer) {
+  return 0;
+}
+
+// IMessage
+#pragma endregion
+
+}  // namespace RoboidControl
--- a/Messages/IMessage.h
+++ b/Messages/IMessage.h
@ -0,0 +1,16 @@
+#pragma once
+
+namespace RoboidControl {
+
+/// @brief Root structure for all communcation messages
+class IMessage {
+ public:
+  IMessage();
+  /// @brief Serialize the message into a byte array for sending
+  /// @param buffer The buffer to serilize into
+  /// @return The length of the message in the buffer
+  virtual unsigned char Serialize(char* buffer);
+
+};
+
+}  // namespace RoboidControl
--- a/Messages/InvestigateMsg.cpp
+++ b/Messages/InvestigateMsg.cpp
@ -7,13 +7,17 @@ InvestigateMsg::InvestigateMsg(char* buffer) {
  this->networkId = buffer[ix++];
  this->thingId = buffer[ix++];
 }
-InvestigateMsg::InvestigateMsg(unsigned char networkId, unsigned char thingId) {
+InvestigateMsg::InvestigateMsg(unsigned char networkId, Thing* thing) {
  this->networkId = networkId;
-  this->thingId = thingId;
+  this->thingId = thing->id;
 }

 InvestigateMsg::~InvestigateMsg() {}
 unsigned char InvestigateMsg::Serialize(char* buffer) {
+  #if defined(DEBUG)
+  std::cout << "Send InvestigateMsg [" << (int)this->networkId << "/" << (int)this->thingId
+            << "] " << std::endl;
+#endif
  unsigned char ix = 0;
  buffer[ix++] = this->id;
  buffer[ix++] = this->networkId;
--- a/Messages/InvestigateMsg.h
+++ b/Messages/InvestigateMsg.h
@ -1,4 +1,7 @@
-#include "Messages.h"
+#pragma once
+
+#include "IMessage.h"
+#include "Thing.h"

 namespace RoboidControl {

@ -14,10 +17,10 @@ class InvestigateMsg : public IMessage {
  /// @brief the ID of the thing
  unsigned char thingId;

-  /// @brief Create a new message for sending
+  /// @brief Create an investigate message
  /// @param networkId The network ID for the thing
-  /// @param thingId The ID of the thing
-  InvestigateMsg(unsigned char networkId, unsigned char thingId);
+  /// @param thing The thing for which the details are requested
+  InvestigateMsg(unsigned char networkId, Thing* thing);
  /// @copydoc RoboidControl::IMessage::IMessage(char*)
  InvestigateMsg(char* buffer);
  /// @brief Destructor for the message
--- a/Messages/LowLevelMessages.h
+++ b/Messages/LowLevelMessages.h
@ -1,3 +1,5 @@
+#pragma once
+
 #include "LinearAlgebra/Spherical.h"
 #include "LinearAlgebra/SwingTwist.h"

@ -5,18 +7,18 @@ namespace RoboidControl {

 class LowLevelMessages {
 public:
-  static void SendAngle8(char* buffer, unsigned char* ix, const float angle);
-  static Angle8 ReceiveAngle8(const char* buffer, unsigned char* startIndex);
-
-  static void SendFloat16(char* buffer, unsigned char* ix, float value);
-  static float ReceiveFloat16(const char* buffer, unsigned char* startIndex);
-
  static void SendSpherical(char* buffer, unsigned char* ix, Spherical s);
  static Spherical ReceiveSpherical(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);
+
+  static void SendAngle8(char* buffer, unsigned char* ix, const float angle);
+  static Angle8 ReceiveAngle8(const char* buffer, unsigned char* startIndex);
+
+  static void SendFloat16(char* buffer, unsigned char* ix, float value);
+  static float ReceiveFloat16(const char* buffer, unsigned char* startIndex);
 };

 }  // namespace RoboidControl
--- a/Messages/Messages.cpp
+++ b/Messages/Messages.cpp
@ -1,36 +0,0 @@
-#include "Messages.h"
-
-#include "LowLevelMessages.h"
-//#include "Participant.h"
-#include "string.h"
-
-namespace RoboidControl {
-
-#pragma region IMessage
-
-IMessage::IMessage() {}
-
-// IMessage::IMessage(unsigned char *buffer) { Deserialize(buffer); }
-
-// IMessage::IMessage(char* buffer) {}
-
-unsigned char IMessage::Serialize(char* buffer) {
-  return 0;
-}
-
-// bool IMessage::SendMsg(LocalParticipant *client, IMessage msg) {
-//   // return SendMsg(client, client.buffer, );nameLength
-//   return client->SendBuffer(msg.Serialize(client->buffer));
-// }
-
-// bool IMessage::Publish(LocalParticipant *participant) {
-//   return participant->PublishBuffer(Serialize(participant->buffer));
-// }
-// bool IMessage::SendTo(LocalParticipant *participant) {
-//   return participant->SendBuffer(Serialize(participant->buffer));
-// }
-
-// IMessage
-#pragma endregion
-
-}  // namespace RoboidControl
--- a/Messages/Messages.h
+++ b/Messages/Messages.h
@ -1,22 +0,0 @@
-#pragma once
-
-#include "LinearAlgebra/Spherical.h"
-#include "LinearAlgebra/SwingTwist.h"
-#include "Thing.h"
-
-namespace RoboidControl {
-
-class LocalParticipant;
-
-class IMessage {
- public:
-  IMessage();
-  virtual unsigned char Serialize(char* buffer);
-
-  static unsigned char* ReceiveMsg(unsigned char packetSize);
-
-  // bool Publish(LocalParticipant *participant);
-  // bool SendTo(LocalParticipant *participant);
-};
-
-}  // namespace RoboidControl
--- a/Messages/ModelUrlMsg.cpp
+++ b/Messages/ModelUrlMsg.cpp
@ -4,16 +4,6 @@

 namespace RoboidControl {

-// ModelUrlMsg::ModelUrlMsg(unsigned char networkId, unsigned char thingId,
-//                          unsigned char urlLength, const char *url,
-//                          float scale) {
-//   this->networkId = networkId;
-//   this->thingId = thingId;
-//   this->urlLength = urlLength;
-//   this->url = url;
-//   this->scale = scale;
-// }
-
 ModelUrlMsg::ModelUrlMsg(unsigned char networkId, Thing* thing) {
  this->networkId = networkId;
  this->thingId = thing->id;
@ -22,21 +12,21 @@ ModelUrlMsg::ModelUrlMsg(unsigned char networkId, Thing* thing) {
  else
    this->urlLength = (unsigned char)strlen(thing->modelUrl);

-  //this->url = thing->modelUrl;  // dangerous!
-
  // the url string in the buffer is not \0 terminated!
  char* url = new char[this->urlLength + 1];
  for (int i = 0; i < this->urlLength; i++)
    url[i] = thing->modelUrl[i];
  url[this->urlLength] = '\0';
-  this->url = url;}
+  this->url = url;
+}

 ModelUrlMsg::ModelUrlMsg(const char* buffer) {
  unsigned char ix = 1;  // first byte is msg id
  this->networkId = buffer[ix++];
  this->thingId = buffer[ix++];
  this->urlLength = buffer[ix++];
-  // this->url = &buffer[ix];  // dangerous! name should not be used anymore after
+  // this->url = &buffer[ix];  // dangerous! name should not be used anymore
+  // after
  //                           // buffer has been re-used...

  // the url string in the buffer is not \0 terminated!
@ -54,6 +44,11 @@ ModelUrlMsg::~ModelUrlMsg() {
 unsigned char ModelUrlMsg::Serialize(char* buffer) {
  if (this->urlLength == 0 || this->url == nullptr)
    return 0;
+#if defined(DEBUG)
+  std::cout << "Send ModelUrlMsg [" << (int)this->networkId << "/"
+            << (int)this->thingId << "] " << (int)this->urlLength << " "
+            << this->url << std::endl;
+#endif
  unsigned char ix = 0;
  buffer[ix++] = this->id;
  buffer[ix++] = this->networkId;
--- a/Messages/ModelUrlMsg.h
+++ b/Messages/ModelUrlMsg.h
@ -1,4 +1,7 @@
-#include "Messages.h"
+#pragma once
+
+#include "IMessage.h"
+#include "Thing.h"

 namespace RoboidControl {

@ -8,14 +11,14 @@ class ModelUrlMsg : public IMessage {
  /// @brief The message ID
  static const unsigned char id = 0x90;
  /// @brief The length of the message without the URL string itself
-  static const unsigned char length = 3;
+  static const unsigned char length = 4;

  /// @brief The network ID of the thing
  unsigned char networkId;
  /// @brief The ID of the thing
  unsigned char thingId;

-  /// @brief The length of the url st5ring, excluding the null terminator
+  /// @brief The length of the url string, excluding the null terminator
  unsigned char urlLength;
  /// @brief The url of the model, not terminated by a null character
  const char* url;
@ -26,8 +29,6 @@ class ModelUrlMsg : public IMessage {
  ModelUrlMsg(unsigned char networkId, Thing* thing);
  /// @copydoc RoboidControl::IMessage::IMessage(char*)
  ModelUrlMsg(const char* buffer);
-  //   ModelUrlMsg(unsigned char networkId, unsigned char thingId,
-  //               unsigned char urlLegth, const char *url, float scale = 1);

  /// @brief Destructor for the message
  virtual ~ModelUrlMsg();
--- a/Messages/NameMsg.cpp
+++ b/Messages/NameMsg.cpp
@ -7,15 +7,16 @@ namespace RoboidControl {
 NameMsg::NameMsg(unsigned char networkId, Thing* thing) {
  this->networkId = networkId;
  this->thingId = thing->id;
-  if (thing->name == nullptr)
+  const char* thingName = thing->GetName();
+  if (thingName == nullptr)
    this->nameLength = 0;
  else
-    this->nameLength = (unsigned char)strlen(thing->name);
+    this->nameLength = (unsigned char)strlen(thingName);

  // the name string in the buffer is not \0 terminated!
  char* name = new char[this->nameLength + 1];
  for (int i = 0; i < this->nameLength; i++)
-    name[i] = thing->name[i];
+    name[i] = thingName[i];
  name[this->nameLength] = '\0';
  this->name = name;
 }
@ -25,6 +26,7 @@ NameMsg::NameMsg(const char* buffer) {
  this->networkId = buffer[ix++];
  this->thingId = buffer[ix++];
  this->nameLength = buffer[ix++];
+
  // the name string in the buffer is not \0 terminated!
  char* name = new char[this->nameLength + 1];
  for (int i = 0; i < this->nameLength; i++)
@ -41,6 +43,11 @@ unsigned char NameMsg::Serialize(char* buffer) {
  if (this->nameLength == 0 || this->name == nullptr)
    return 0;

+#if defined(DEBUG)
+  std::cout << "Send NameMsg [" << (int)this->networkId << "/"
+            << (int)this->thingId << "] " << (int)this->nameLength << " "
+            << this->name << std::endl;
+#endif
  unsigned char ix = 0;
  buffer[ix++] = this->id;
  buffer[ix++] = this->networkId;
--- a/Messages/NameMsg.h
+++ b/Messages/NameMsg.h
@ -1,4 +1,7 @@
-#include "Messages.h"
+#pragma once
+
+#include "IMessage.h"
+#include "Thing.h"

 namespace RoboidControl {

@ -22,9 +25,6 @@ class NameMsg : public IMessage {
  /// @param networkId The network ID of the thing
  /// @param thing The ID of the thing
  NameMsg(unsigned char networkId, Thing* thing);
-  //   NameMsg(unsigned char networkId, unsigned char thingId, const char *name,
-  //           unsigned char nameLength);
-
  /// @copydoc RoboidControl::IMessage::IMessage(char*)
  NameMsg(const char* buffer);
  /// @brief Destructor for the message
--- a/Messages/NetworkIdMsg.cpp
+++ b/Messages/NetworkIdMsg.cpp
@ -0,0 +1,27 @@
+#include "NetworkIdMsg.h"
+
+#include <iostream>
+
+namespace RoboidControl {
+
+NetworkIdMsg::NetworkIdMsg(const char* buffer) {
+  this->networkId = buffer[1];
+}
+
+NetworkIdMsg::NetworkIdMsg(unsigned char networkId) {
+  this->networkId = networkId;
+}
+
+NetworkIdMsg::~NetworkIdMsg() {}
+
+unsigned char NetworkIdMsg::Serialize(char* buffer) {
+#if defined(DEBUG)
+  std::cout << "Send NetworkIdMsg [" << (int)this->networkId << "] " << std::endl;
+#endif
+  unsigned char ix = 0;
+  buffer[ix++] = this->id;
+  buffer[ix++] = this->networkId;
+  return NetworkIdMsg::length;
+}
+
+}  // namespace RoboidControl
--- a/Messages/NetworkIdMsg.h
+++ b/Messages/NetworkIdMsg.h
@ -1,9 +1,11 @@
-#include "Messages.h"
+#pragma once
+
+#include "IMessage.h"

 namespace RoboidControl {

 /// @brief A message communicating the network ID for that participant
-class SiteMsg : public IMessage {
+class NetworkIdMsg : public IMessage {
 public:
  /// @brief The message ID
  static const unsigned char id = 0xA1;
@ -14,11 +16,11 @@ public:

  /// @brief Create a new message for sending
  /// @param networkId The network ID for the participant
-  SiteMsg(unsigned char networkId);
+  NetworkIdMsg(unsigned char networkId);
  /// @copydoc RoboidControl::IMessage::IMessage(char*)
-  SiteMsg(const char *buffer);
+  NetworkIdMsg(const char *buffer);
  /// @brief Destructor for the message
-  virtual ~SiteMsg();
+  virtual ~NetworkIdMsg();

  /// @copydoc RoboidControl::IMessage::Serialize
  virtual unsigned char Serialize(char *buffer) override;
--- a/Messages/ParticipantMsg.cpp
+++ b/Messages/ParticipantMsg.cpp
@ -1,5 +1,9 @@
 #include "ParticipantMsg.h"

+#if !defined(NO_STD)
+#include <iostream>
+#endif
+
 namespace RoboidControl {

 ParticipantMsg::ParticipantMsg(char networkId) {
@ -13,13 +17,19 @@ ParticipantMsg::ParticipantMsg(const char* buffer) {
 ParticipantMsg::~ParticipantMsg() {}

 unsigned char ParticipantMsg::Serialize(char* buffer) {
+#if defined(DEBUG) && !defined(NO_STD)
+  std::cout << "Send ParticipantMsg [" << (int)this->networkId << "] "
+            << std::endl;
+#endif
+
  unsigned char ix = 0;
  buffer[ix++] = this->id;
  buffer[ix++] = this->networkId;
  return ParticipantMsg::length;
 }

-// bool ParticipantMsg::Send(LocalParticipant *participant, unsigned char networkId) {
+// bool ParticipantMsg::Send(ParticipantUDP *participant, unsigned char
+// networkId) {
 //   ParticipantMsg msg = ParticipantMsg()
 // }
 // Client Msg
--- a/Messages/ParticipantMsg.h
+++ b/Messages/ParticipantMsg.h
@ -1,6 +1,6 @@
 #pragma once

-#include "Messages.h"
+#include "IMessage.h"

 namespace RoboidControl {

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

  this->poseType = 0;
-  if (thing->positionUpdated || force) {
+  if (thing->positionUpdated || (force && thing->IsRoot())) {
    this->position = thing->GetPosition();
    this->poseType |= Pose_Position;
-    thing->positionUpdated = false;
  }
-  if (thing->orientationUpdated || force) {
+  if (thing->orientationUpdated || (force && thing->IsRoot())) {
    this->orientation = thing->GetOrientation();
    this->poseType |= Pose_Orientation;
-    thing->orientationUpdated = false;
  }
  if (thing->linearVelocityUpdated) {
    this->linearVelocity = thing->GetLinearVelocity();
@ -47,6 +45,10 @@ unsigned char PoseMsg::Serialize(char* buffer) {
  if (this->poseType == 0)
    return 0;

+#if defined(DEBUG) && DEBUG > 1
+  std::cout << "Send PoseMsg [" << (int)this->networkId << "/"
+            << (int)this->thingId << "] " << (int)this->poseType << std::endl;
+#endif
  unsigned char ix = 0;
  buffer[ix++] = PoseMsg::id;
  buffer[ix++] = this->networkId;
--- a/Messages/PoseMsg.h
+++ b/Messages/PoseMsg.h
@ -1,4 +1,6 @@
-#include "Messages.h"
+#pragma once
+#include "IMessage.h"
+#include "Thing.h"

 namespace RoboidControl {

@ -9,7 +11,7 @@ class PoseMsg : public IMessage {
 public:
  /// @brief The message ID
  static const unsigned char id = 0x10;
-  /// @brief The length of the message
+  /// @brief The length of the message in bytes
  unsigned char length = 4 + 4 + 4;

  /// @brief The network ID of the thing
@ -40,7 +42,8 @@ class PoseMsg : public IMessage {

  /// @brief Create a new message for sending
  /// @param networkId he network ID of the thing
-  /// @param thing The thing for which the pose shouldbe sent
+  /// @param thing The thing for which the pose should be sent
+  /// @param force If true, position and orientation are always included, even when they are not updated
  PoseMsg(unsigned char networkId, Thing* thing, bool force = false);

  /// @copydoc RoboidControl::IMessage::IMessage(char*)
--- a/Messages/SiteMsg.cpp
+++ b/Messages/SiteMsg.cpp
@ -1,22 +0,0 @@
-#include "SiteMsg.h"
-
-namespace RoboidControl {
-
-SiteMsg::SiteMsg(const char* buffer) {
-  this->networkId = buffer[1];
-}
-
-SiteMsg::SiteMsg(unsigned char networkId) {
-  this->networkId = networkId;
-}
-
-SiteMsg::~SiteMsg() {}
-
-unsigned char SiteMsg::Serialize(char* buffer) {
-  unsigned char ix = 0;
-  buffer[ix++] = this->id;
-  buffer[ix++] = this->networkId;
-  return SiteMsg::length;
-}
-
-}  // namespace RoboidControl
--- a/Messages/TextMsg.cpp
+++ b/Messages/TextMsg.cpp
@ -1,5 +1,9 @@
 #include "TextMsg.h"

+#if !defined(NO_STD)
+#include <iostream>
+#endif
+
 namespace RoboidControl {

 TextMsg::TextMsg(const char* text, unsigned char textLength) {
@ -24,6 +28,9 @@ unsigned char TextMsg::Serialize(char* buffer) {
  if (this->textLength == 0 || this->text == nullptr)
    return 0;

+#if defined(DEBUG) && !defined(NO_STD)
+  std::cout << "Send TextMsg " << (int)this->textLength << " " << this->text << std::endl;
+#endif
  unsigned char ix = 0;
  buffer[ix++] = this->id;
  buffer[ix++] = this->textLength;
--- a/Messages/TextMsg.h
+++ b/Messages/TextMsg.h
@ -1,4 +1,4 @@
-#include "Messages.h"
+#include "IMessage.h"

 namespace RoboidControl {

@ -9,10 +9,6 @@ class TextMsg : public IMessage {
  static const unsigned char id = 0xB0;
  /// @brief The length of the message without the text itself
  static const unsigned char length = 2;
-  /// @brief The network ID of the thing
-  unsigned char networkId;
-  /// @brief the ID of the thing
-  unsigned char thingId;
  /// @brief The text without the null terminator
  const char* text;
  /// @brief The length of the text
--- a/Messages/ThingMsg.cpp
+++ b/Messages/ThingMsg.cpp
@ -14,24 +14,22 @@ ThingMsg::ThingMsg(unsigned char networkId, Thing* thing) {
  this->networkId = networkId;
  this->thingId = thing->id;
  this->thingType = thing->type;
-  Thing* parent = thing->GetParent();
-  if (parent != nullptr)
-    this->parentId = parent->id;
-  else
+  if (thing->IsRoot())
    this->parentId = 0;
+  else {
+    Thing* parent = thing->GetParent();
+    this->parentId = parent->id;
+  }
 }

-// ThingMsg::ThingMsg(unsigned char networkId, unsigned char thingId,
-//                    unsigned char thingType, unsigned char parentId) {
-//   this->networkId = networkId;
-//   this->thingId = thingId;
-//   this->thingType = thingType;
-//   this->parentId = parentId;
-// }
-
 ThingMsg::~ThingMsg() {}

 unsigned char ThingMsg::Serialize(char* buffer) {
+#if defined(DEBUG)
+  std::cout << "Send ThingMsg [" << (int)this->networkId << "/"
+            << (int)this->thingId << "] " << (int)this->thingType << " "
+            << (int)this->parentId << std::endl;
+#endif
  unsigned char ix = 0;
  buffer[ix++] = this->id;
  buffer[ix++] = this->networkId;
--- a/Messages/ThingMsg.h
+++ b/Messages/ThingMsg.h
@ -1,8 +1,9 @@
-#include "Messages.h"
+#include "IMessage.h"
+#include "Thing.h"

 namespace RoboidControl {

-/// @brief  Message providing generic information about a Thing
+/// @brief Message providing generic details about a Thing
 class ThingMsg : public IMessage {
 public:
  /// @brief The message ID
@ -13,17 +14,15 @@ class ThingMsg : public IMessage {
  unsigned char networkId;
  /// @brief The ID of the thing
  unsigned char thingId;
-  /// @brief The Thing.Type of the thing
+  /// @brief The type of thing
  unsigned char thingType;
-  /// @brief The parent of the thing in the hierarachy. This is null for root Things
+  /// @brief The ID of the parent thing in the hierarchy. This is zero for root things
  unsigned char parentId;

  /// @brief Create a message for sending
  /// @param networkId The network ID of the thing</param>
  /// @param thing The thing
  ThingMsg(unsigned char networkId, Thing* thing);
-  //   ThingMsg(unsigned char networkId, unsigned char thingId,
-  //            unsigned char thingType, unsigned char parentId);

  /// @copydoc RoboidControl::IMessage::IMessage(char*)
  ThingMsg(const char* buffer);
--- a/Participant.cpp
+++ b/Participant.cpp
@ -4,9 +4,30 @@

 namespace RoboidControl {

-Participant::Participant() {}
+#pragma region Participant
+
+ParticipantRegistry Participant::registry;
+
+Participant* Participant::LocalParticipant = new Participant();
+
+void Participant::ReplaceLocalParticipant(Participant& newParticipant) {
+  LocalParticipant = &newParticipant;
+  std::cout << "Replaced local participant" << std::endl;
+}
+
+Participant::Participant() {
+  std::cout << "P\n";
+  //this->root.name = "Isolated";
+  this->root = new Thing(this);
+  this->root->name = "Root";
+  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);
+
  // make a copy of the ip address string
  int addressLength = (int)strlen(ipAddress);
  int stringLength = addressLength + 1;
@ -24,18 +45,25 @@ Participant::Participant(const char* ipAddress, int port) {
 }

 Participant::~Participant() {
+  // registry.Remove(this);
  delete[] this->ipAddress;
 }

-Thing* Participant::Get(unsigned char networkId, unsigned char thingId) {
+void Participant::Update() {
+  for (Thing* thing : this->things) {
+    if (thing != nullptr)
+      thing->Update(true);
+  }
+}
+
+Thing* Participant::Get(unsigned char thingId) {
  for (Thing* thing : this->things) {
-    // if (thing->networkId == networkId && thing->id == thingId)
    if (thing->id == thingId)
      return thing;
  }
-  //   std::cout << "Could not find thing " << this->ipAddress << ":" <<
-  //   this->port
-  //             << "[" << (int)networkId << "/" << (int)thingId << "]\n";
+  // std::cout << "Could not find thing " << this->ipAddress << ":" <<
+  // this->port
+  //           << "[" << (int)thingId << "]\n";
  return nullptr;
 }

@ -46,14 +74,21 @@ void Participant::Add(Thing* thing, bool checkId) {
    thing->id = this->thingCount + 1;
    this->things[this->thingCount++] = thing;
 #else
-    thing->id = (unsigned char)this->things.size() + 1;
+    // find highest id
+    int highestIx = 0;
+    for (Thing* thing : this->things) {
+      if (thing == nullptr)
+        continue;
+      if (thing->id > highestIx)
+        highestIx = thing->id;
+    }
+    thing->id = highestIx + 1;
    this->things.push_back(thing);
 #endif
-    // std::cout << "Add thing with generated ID " << this->ipAddress << ":" <<
-    // this->port << "[" << (int)thing->networkId << "/"
-    //           << (int)thing->id << "]\n";
+    // std::cout << "Add thing with generated ID " << this->ipAddress << ":"
+    //           << this->port << "[" << (int)thing->id << "]\n";
  } else {
-    Thing* foundThing = Get(thing->networkId, thing->id);
+    Thing* foundThing = Get(thing->id);
    if (foundThing == nullptr) {
 #if defined(NO_STD)
      this->things[this->thingCount++] = thing;
@ -61,13 +96,12 @@ void Participant::Add(Thing* thing, bool checkId) {
      this->things.push_back(thing);
 #endif
      // std::cout << "Add thing " << this->ipAddress << ":" << this->port <<
-      // "[" << (int)thing->networkId << "/"
+      // "["
      //           << (int)thing->id << "]\n";
+    } else {
+      // std::cout << "Did not add, existing thing " << this->ipAddress << ":"
+      //           << this->port << "[" << (int)thing->id << "]\n";
    }
-    // else
-    //   std::cout << "Did not add, existing thing " << this->ipAddress << ":"
-    //   << this->port << "["
-    //             << (int)thing->networkId << "/" << (int)thing->id << "]\n";
  }
 }

@ -87,22 +121,93 @@ void Participant::Remove(Thing* thing) {
  this->thingCount = lastThingIx;
 #else
  this->things.remove_if([thing](Thing* obj) { return obj == thing; });
-  std::cout << "Removing " << thing->networkId << "/" << thing->id
-            << " list size = " << this->things.size() << "\n";
+  // std::cout << "Removing [" << (int)thing->networkId << "/" << (int)thing->id
+  //           << "] list size = " << this->things.size() << "\n";
 #endif
 }

-// void Participant::UpdateAll(unsigned long currentTimeMs) {
-//   // Not very efficient, but it works for now.
+#pragma endregion

-//   for (Thing* thing : this->things) {
-//     if (thing != nullptr && thing->GetParent() == nullptr) {  // update all
-//     root things
-//       // std::cout << " update " << (int)ix << " thingid " << (int)thing->id
-//       //           << "\n";
-//       thing->Update(currentTimeMs);
-//     }
-//   }
-// }
+#pragma region ParticipantRegistry
+
+Participant* ParticipantRegistry::Get(const char* ipAddress,
+                                      unsigned int port) {
+#if !defined(NO_STD)
+  for (Participant* participant : ParticipantRegistry::participants) {
+    if (participant == nullptr)
+      continue;
+    if (strcmp(participant->ipAddress, ipAddress) == 0 &&
+        participant->port == port) {
+      // std::cout << "found participant " << participant->ipAddress << ":"
+      //           << (int)participant->port << std::endl;
+      return participant;
+    }
+  }
+  std::cout << "Could not find participant " << ipAddress << ":" << (int)port
+            << std::endl;
+#endif
+  return nullptr;
+}
+
+Participant* ParticipantRegistry::Get(unsigned char participantId) {
+#if !defined(NO_STD)
+  for (Participant* participant : ParticipantRegistry::participants) {
+    if (participant == nullptr)
+      continue;
+    if (participant->networkId == participantId)
+      return participant;
+  }
+  std::cout << "Could not find participant " << (int)participantId << std::endl;
+#endif
+  return nullptr;
+}
+
+Participant* ParticipantRegistry::Add(const char* ipAddress,
+                                      unsigned int port) {
+  Participant* participant = new Participant(ipAddress, port);
+  Add(participant);
+  return participant;
+}
+
+void ParticipantRegistry::Add(Participant* participant) {
+  Participant* foundParticipant =
+      Get(participant->ipAddress, participant->port);
+
+  if (foundParticipant == nullptr) {
+#if defined(NO_STD)
+    // this->things[this->thingCount++] = thing;
+#else
+    ParticipantRegistry::participants.push_back(participant);
+#endif
+    //   std::cout << "Add participant " << participant->ipAddress << ":"
+    //             << participant->port << "[" << (int)participant->networkId
+    //             << "]\n";
+    //   std::cout << "participants " <<
+    //   ParticipantRegistry::participants.size()
+    //             << "\n";
+    // } else {
+    //   std::cout << "Did not add, existing participant " <<
+    //   participant->ipAddress
+    //             << ":" << participant->port << "[" <<
+    //             (int)participant->networkId
+    //             << "]\n";
+  }
+}
+
+void ParticipantRegistry::Remove(Participant* participant) {
+  // participants.remove(participant);
+}
+
+#if defined(NO_STD)
+Participant** ParticipantRegistry::GetAll() const {
+  return ParticipantRegistry::participants;
+}
+#else
+const std::list<Participant*>& ParticipantRegistry::GetAll() const {
+  return ParticipantRegistry::participants;
+}
+#endif
+
+#pragma endregion ParticipantRegistry

 }  // namespace RoboidControl
--- a/Participant.h
+++ b/Participant.h
@ -1,10 +1,57 @@
 #pragma once
+
 #include "Thing.h"

 namespace RoboidControl {

 constexpr int MAX_THING_COUNT = 256;

+/// @brief class which manages all known participants
+class ParticipantRegistry {
+ public:
+  /// @brief Retrieve a participant by its address
+  /// @param ipAddress The IP address of the participant
+  /// @param port The port number of the participant
+  /// @return The participant or a nullptr when it could not be found
+  Participant* Get(const char* ipAddress, unsigned int port);
+  /// @brief Retrieve a participant by its network ID
+  /// @param networkID The network ID of the participant
+  /// @return The participant or a nullptr when it could not be found
+  Participant* Get(unsigned char networkID);
+
+  /// @brief Add a participant with the given details
+  /// @param ipAddress The IP address of the participant
+  /// @param port The port number of the participant
+  /// @return The added participant
+  Participant* Add(const char* ipAddress, unsigned int port);
+  /// @brief Add a participant
+  /// @param participant The participant to add
+  void Add(Participant* participant);
+
+  /// @brief Remove a participant
+  /// @param participant The participant to remove
+  void Remove(Participant* participant);
+
+ private:
+#if defined(NO_STD)
+ public:
+  Participant** GetAll() const;
+  int count = 0;
+
+ private:
+  Participant** participants;
+#else
+ public:
+  /// @brief Get all participants
+  /// @return All participants
+  const std::list<Participant*>& GetAll() const;
+
+ private:
+  /// @brief The list of known participants
+  std::list<Participant*> participants;
+#endif
+};
+
 /// @brief A participant is a device which manages things.
 /// It can communicate with other participant to synchronise the state of
 /// things. This class is used to register the things the participant is
@ -13,50 +60,56 @@ constexpr int MAX_THING_COUNT = 256;
 /// reference to remote participants.
 class Participant {
 public:
-  /// @brief The Ip Address of a participant. When the participant is local,
-  /// this contains 0.0.0.0
-  const char* ipAddress = "0.0.0.0";
-  /// @brief The port number for UDP communication with the participant. This is
-  /// 0 for isolated participants.
-  int port = 0;
+  /// @brief The name of the participant
+  const char* name = "Participant";

-  /// @brief The network Id to identify the participant.
-  /// @note This field is likely to disappear in future versions
+  /// @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 Default constructor
  Participant();
  /// @brief Create a new participant with the given communcation info
  /// @param ipAddress The IP address of the participant
-  /// @param port The port of the participant
+  /// @param port The UDP port of the participant
  Participant(const char* ipAddress, int port);
  /// @brief Destructor for the participant
  ~Participant();

- protected:
+  static Participant* LocalParticipant;
+  static void ReplaceLocalParticipant(Participant& newParticipant);
+
+  Thing* root = new Thing(this);
+
+ public:
 #if defined(NO_STD)
  unsigned char thingCount = 0;
  Thing* things[MAX_THING_COUNT];
 #else
-  /// @brief The list of things managed by this participant
+  /// @brief  The things managed by this participant
  std::list<Thing*> things;
 #endif
-
- public:
  /// @brief Find a thing managed by this participant
-  /// @param networkId The network ID for the thing
  /// @param thingId The ID of the thing
-  /// @return The thing if found or nullptr when no thing has been found
-  /// @note The use of the network ID is likely to disappear in future versions.
-  Thing* Get(unsigned char networkId, unsigned char thingId);
+  /// @return The thing if found, nullptr when no thing has been found
+  Thing* Get(unsigned char thingId);
  /// @brief Add a new thing for this participant.
  /// @param thing The thing to add
-  /// @param checkId Checks the thing ID of the thing. If it is 0, a new thing
-  /// Id will be assigned.
+  /// @param checkId If true, the thing.id is regenerated if it is zero
  void Add(Thing* thing, bool checkId = true);
  /// @brief Remove a thing for this 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();
+
+ public:
+  static ParticipantRegistry registry;
 };

 }  // namespace RoboidControl
--- a/Participants/IsolatedParticipant.cpp
+++ b/Participants/IsolatedParticipant.cpp
@ -0,0 +1,14 @@
+#include "IsolatedParticipant.h"
+#include "ParticipantUDP.h"
+
+namespace RoboidControl {
+
+static ParticipantUDP* isolatedParticipant = nullptr;
+
+Participant* IsolatedParticipant::Isolated() {
+  if (isolatedParticipant == nullptr)
+    isolatedParticipant = new ParticipantUDP(0);
+  return isolatedParticipant;
+}
+
+}  // namespace RoboidControl
--- a/Participants/IsolatedParticipant.h
+++ b/Participants/IsolatedParticipant.h
@ -0,0 +1,13 @@
+#include "Participant.h"
+
+namespace RoboidControl {
+
+class IsolatedParticipant {
+ public:
+  /// @brief Isolated participant is used when the application is run without
+  /// networking
+  /// @return A participant without networking support
+  static Participant* Isolated();
+};
+
+}
--- a/Participants/ParticipantUDP.cpp
+++ b/Participants/ParticipantUDP.cpp
@ -0,0 +1,538 @@
+#include "ParticipantUDP.h"
+
+#include "Participant.h"
+#include "Thing.h"
+
+#include "Arduino/ArduinoParticipant.h"
+#include "EspIdf/EspIdfParticipant.h"
+#include "Posix/PosixParticipant.h"
+#include "Windows/WindowsParticipant.h"
+
+#include <string.h>
+
+namespace RoboidControl {
+
+#pragma region Init
+
+ParticipantUDP::ParticipantUDP(int port) : Participant("127.0.0.1", port) {
+  this->name = "ParticipantUDP";
+  this->remoteSite = nullptr;
+  if (this->port == 0)
+    this->isIsolated = true;
+  Participant::registry.Add(this);
+
+  this->root = Thing::LocalRoot();  //::LocalParticipant->root;
+  this->root->owner = this;
+  this->root->name = "UDP Root";
+  this->Add(this->root);
+
+  Participant::ReplaceLocalParticipant(*this);
+}
+
+ParticipantUDP::ParticipantUDP(const char* ipAddress, int port, int localPort)
+    : Participant("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->root = Thing::LocalRoot();  // Participant::LocalParticipant->root;
+  this->root->owner = this;
+  this->root->name = "UDP Root";
+  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() {
+  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
+
+// The update order
+// 1. receive external messages
+// 2. update the state
+// 3. send out the updated messages
+void ParticipantUDP::Update() {
+  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->nextPublishMe = currentTimeMs + this->publishInterval;
+    }
+
+    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;
+  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?
+    // 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() {
+#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()) {
+#endif
+    if (participant == nullptr || participant == this)
+      continue;
+
+    // Call only the Participant version of the Update.
+    // This is to deal with the function where one of the (remote)
+    // participants is actually a local participant
+    participant->Participant::Update();
+    if (this->isIsolated)
+      continue;
+
+    for (Thing* thing : participant->things) {
+      PoseMsg* poseMsg = new PoseMsg(participant->networkId, thing);
+      this->Send(participant, poseMsg);
+      delete poseMsg;
+      BinaryMsg* binaryMsg = new BinaryMsg(participant->networkId, thing);
+      this->Send(participant, binaryMsg);
+      delete binaryMsg;
+    }
+  }
+}
+
+// Update
+#pragma endregion
+
+#pragma region Send
+
+void ParticipantUDP::SendThingInfo(Participant* remoteParticipant,
+                                   Thing* thing) {
+  // std::cout << "Send thing info [" << (int)thing->id << "] \n";
+  ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
+  this->Send(remoteParticipant, thingMsg);
+  delete thingMsg;
+  NameMsg* nameMsg = new NameMsg(this->networkId, thing);
+  this->Send(remoteParticipant, nameMsg);
+  delete nameMsg;
+  ModelUrlMsg* modelMsg = new ModelUrlMsg(this->networkId, thing);
+  this->Send(remoteParticipant, modelMsg);
+  delete modelMsg;
+  PoseMsg* poseMsg = new PoseMsg(this->networkId, thing, true);
+  this->Send(remoteParticipant, poseMsg);
+  delete poseMsg;
+  BinaryMsg* customMsg = new BinaryMsg(this->networkId, thing);
+  this->Send(remoteParticipant, customMsg);
+  delete customMsg;
+}
+
+bool ParticipantUDP::Send(Participant* remoteParticipant, IMessage* msg) {
+  int bufferSize = msg->Serialize(this->buffer);
+  if (bufferSize <= 0)
+    return true;
+
+  // 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
+}
+
+void ParticipantUDP::PublishThingInfo(Thing* thing) {
+  // std::cout << "Publish thing info" << thing->networkId << "\n";
+  //  Strange, when publishing, the network id is irrelevant, because it is
+  //  connected to a specific site...
+  ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
+  this->Publish(thingMsg);
+  delete thingMsg;
+  NameMsg* nameMsg = new NameMsg(this->networkId, thing);
+  this->Publish(nameMsg);
+  delete nameMsg;
+  ModelUrlMsg* modelMsg = new ModelUrlMsg(this->networkId, thing);
+  this->Publish(modelMsg);
+  delete modelMsg;
+  PoseMsg* poseMsg = new PoseMsg(this->networkId, thing, true);
+  this->Publish(poseMsg);
+  delete poseMsg;
+  BinaryMsg* customMsg = new BinaryMsg(this->networkId, thing);
+  this->Publish(customMsg);
+  delete customMsg;
+}
+
+bool ParticipantUDP::Publish(IMessage* msg) {
+  // 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) {
+  // std::cout << "Receive data from " << senderIpAddress << ":" << senderPort
+  //           << std::endl;
+  Participant* 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;
+#endif
+  }
+
+  ReceiveData(packetSize, sender);
+}
+
+void ParticipantUDP::ReceiveData(unsigned char bufferSize,
+                                 Participant* sender) {
+  unsigned char msgId = this->buffer[0];
+  // std::cout << "receive msg " << (int)msgId << "\n";
+  // std::cout << "  buffer size = " <<(int) bufferSize << "\n";
+  switch (msgId) {
+    case ParticipantMsg::id: {
+      ParticipantMsg* msg = new ParticipantMsg(this->buffer);
+      bufferSize -= msg->length;
+      Process(sender, msg);
+      delete msg;
+    } break;
+    case NetworkIdMsg::id: {
+      NetworkIdMsg* msg = new NetworkIdMsg(this->buffer);
+      bufferSize -= msg->length;
+      Process(sender, msg);
+      delete msg;
+    } break;
+    case InvestigateMsg::id: {
+      InvestigateMsg* msg = new InvestigateMsg(this->buffer);
+      Process(sender, msg);
+      delete msg;
+    } break;
+    case ThingMsg::id: {
+      ThingMsg* msg = new ThingMsg(this->buffer);
+      bufferSize -= msg->length;
+      Process(sender, msg);
+      delete msg;
+    } break;
+    case NameMsg::id: {
+      NameMsg* msg = new NameMsg(this->buffer);
+      bufferSize -= msg->length + msg->nameLength;
+      Process(sender, msg);
+      delete msg;
+    } break;
+    case ModelUrlMsg::id: {
+      ModelUrlMsg* msg = new ModelUrlMsg(this->buffer);
+      bufferSize -= msg->length + msg->urlLength;
+      Process(sender, msg);
+      delete msg;
+    } break;
+    case PoseMsg::id: {
+      PoseMsg* msg = new PoseMsg(this->buffer);
+      bufferSize -= msg->length;
+      Process(sender, msg);
+      delete msg;
+    } break;
+    case BinaryMsg::id: {
+      BinaryMsg* msg = new BinaryMsg(this->buffer);
+      bufferSize -= msg->length + msg->dataLength;
+      Process(sender, msg);
+      delete msg;
+    } break;
+  };
+
+// Check if the buffer has been read completely
+#if !defined(NO_STD)
+  if (bufferSize > 0)
+    std::cout << "Buffer not fully read, remaining " << (int)bufferSize << "\n";
+#endif
+}
+
+void ParticipantUDP::Process(Participant* sender, ParticipantMsg* msg) {
+#if defined(DEBUG)
+  std::cout << this->name << ": Process ParticipantMsg " << (int)msg->networkId
+            << "\n";
+#endif
+}
+
+void ParticipantUDP::Process(Participant* sender, NetworkIdMsg* msg) {
+#if defined(DEBUG)
+  std::cout << this->name << ": process NetworkIdMsg " << (int)this->networkId
+            << " -> " << (int)msg->networkId << "\n";
+#endif
+
+  if (this->networkId != msg->networkId) {
+    this->networkId = msg->networkId;
+
+    std::cout << this->things.size() << " things\n";
+    for (Thing* thing : this->things)
+      this->SendThingInfo(sender, thing);
+  }
+}
+
+void ParticipantUDP::Process(Participant* sender, InvestigateMsg* msg) {
+#if defined(DEBUG)
+  std::cout << this->name << ": Process InvestigateMsg [" << (int)msg->networkId
+            << "/" << (int)msg->thingId << "]\n";
+#endif
+}
+
+void ParticipantUDP::Process(Participant* sender, ThingMsg* msg) {
+#if defined(DEBUG)
+  std::cout << this->name << ": process ThingMsg [" << (int)msg->networkId
+            << "/" << (int)msg->thingId << "] " << (int)msg->thingType << " "
+            << (int)msg->parentId << "\n";
+#endif
+}
+
+void ParticipantUDP::Process(Participant* sender, NameMsg* msg) {
+#if defined(DEBUG)
+  std::cout << this->name << ": process NameMsg [" << (int)msg->networkId << "/"
+            << (int)msg->thingId << "] ";
+#endif
+
+  Thing* thing = sender->Get(msg->thingId);
+  if (thing != nullptr) {
+    int nameLength = msg->nameLength;
+    int stringLen = nameLength + 1;
+    char* thingName = new char[stringLen];
+#if defined(_WIN32) || defined(_WIN64)
+    strncpy_s(thingName, stringLen, msg->name,
+              stringLen - 1);  // Leave space for null terminator
+#else
+    // Use strncpy with bounds checking for other platforms (Arduino, POSIX,
+    // ESP-IDF)
+    strncpy(thingName, msg->name,
+            nameLength);  // Leave space for null terminator
+#endif
+    thingName[nameLength] = '\0';
+    thing->SetName(thingName);
+
+#if !defined(NO_STD)
+    std::cout << thing->GetName();
+#endif
+  }
+#if !defined(NO_STD)
+  std::cout << std::endl;
+#endif
+}
+
+void ParticipantUDP::Process(Participant* sender, ModelUrlMsg* msg) {
+#if defined(DEBUG)
+  std::cout << this->name << ": process ModelUrlMsg [" << (int)msg->networkId
+            << "/" << (int)msg->thingId << "]\n";
+#endif
+}
+
+void ParticipantUDP::Process(Participant* sender, PoseMsg* msg) {
+#if !defined(DEBUG) && !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);
+  if (owner == nullptr)
+    return;
+
+  Thing* thing = owner->Get(msg->thingId);
+  if (thing == nullptr)
+    return;
+
+  if ((msg->poseType & PoseMsg::Pose_Position) != 0)
+    thing->SetPosition(msg->position);
+  if ((msg->poseType & PoseMsg::Pose_Orientation) != 0)
+    thing->SetOrientation(msg->orientation);
+  if ((msg->poseType & PoseMsg::Pose_LinearVelocity) != 0)
+    thing->SetLinearVelocity(msg->linearVelocity);
+  if ((msg->poseType & PoseMsg::Pose_AngularVelocity) != 0)
+    thing->SetAngularVelocity(msg->angularVelocity);
+}
+
+void ParticipantUDP::Process(Participant* 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);
+  if (owner != nullptr) {
+    Thing* thing = owner->Get(msg->thingId);
+    if (thing != nullptr)
+      thing->ProcessBinary(msg->data);
+#if !defined(NO_STD)
+    else {
+#if defined(DEBUG)
+      std::cout << "  unknown thing [" << (int)msg->networkId << "/"
+                << (int)msg->thingId << "]";
+#endif
+    }
+#endif
+  }
+}
+
+// Receive
+#pragma endregion
+
+}  // namespace RoboidControl
--- a/Participants/ParticipantUDP.h
+++ b/Participants/ParticipantUDP.h
@ -1,17 +1,20 @@
 #pragma once

 #include "Messages/BinaryMsg.h"
+#include "Messages/DestroyMsg.h"
 #include "Messages/InvestigateMsg.h"
 #include "Messages/ModelUrlMsg.h"
 #include "Messages/NameMsg.h"
 #include "Messages/ParticipantMsg.h"
 #include "Messages/PoseMsg.h"
-#include "Messages/SiteMsg.h"
+#include "Messages/NetworkIdMsg.h"
 #include "Messages/ThingMsg.h"
 #include "Participant.h"

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

 #if defined(_WIN32) || defined(_WIN64)
@ -21,15 +24,14 @@
 #include <netinet/in.h>
 #include <sys/socket.h>
 #include <unistd.h>
-#elif defined(ARDUINO)
-// #include <WiFiUdp.h>
 #endif

 namespace RoboidControl {

 constexpr int MAX_SENDER_COUNT = 256;

-/// @brief A local participant is the local device which can communicate with
+/// @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
@ -38,52 +40,48 @@ constexpr int MAX_SENDER_COUNT = 256;
 /// 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::LocalParticipant::Isolated().
+/// RoboidControl::IsolatedParticipant::Isolated().
 /// @sa RoboidControl::Thing::Thing()
-class LocalParticipant : public Participant {
+class ParticipantUDP : public Participant {
+#pragma region Init
+
 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.
-  LocalParticipant(int port = 7681);
+  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
-  LocalParticipant(const char* ipAddress, int port = 7681);
-  // Note to self: one cannot specify the port used by the local participant
-  // now!!
+  /// @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 LocalParticipant* Isolated();
+  static ParticipantUDP* Isolated();
+
+  /// @brief True if the participant is running isolated.
+  /// Isolated participants do not communicate with other participants
+
+#pragma endregion Init

  /// @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;

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

-  /// @brief The name of the participant
-  const char* name = "LocalParticipant";
-
-  // int localPort = 0;
-
-  /// @brief The remote site when this participant is connected to a site
-  Participant* remoteSite = nullptr;
-
-#if defined(ARDUINO)
-  // const char* remoteIpAddress = nullptr;
-  // unsigned short remotePort = 0;
-  // char* broadcastIpAddress = nullptr;
-
-  // WiFiUDP udp;
-#else
+ protected:
+  char buffer[1024];

+#if !defined(ARDUINO)
 #if defined(__unix__) || defined(__APPLE__)
  int sock;
 #elif defined(_WIN32) || defined(_WIN64)
@ -91,13 +89,27 @@ class LocalParticipant : public Participant {
  sockaddr_in server_addr;
  sockaddr_in broadcast_addr;
 #endif
-
 #endif
-
+ public:
  void begin();
  bool connected = false;

-  virtual void Update(unsigned long currentTimeMs = 0);
+#pragma region Update
+
+ public:
+  virtual void Update() override;
+
+ protected:
+  unsigned long nextPublishMe = 0;
+
+  /// @brief Prepare the local things for the next update
+  virtual void PrepMyThings();
+  virtual void UpdateMyThings();
+  virtual void UpdateOtherThings();
+
+#pragma endregion Update
+
+#pragma region Send

  void SendThingInfo(Participant* remoteParticipant, Thing* thing);
  void PublishThingInfo(Thing* thing);
@ -105,37 +117,31 @@ class LocalParticipant : public Participant {
  bool Send(Participant* remoteParticipant, IMessage* msg);
  bool Publish(IMessage* msg);

+#pragma endregion Send
+
+#pragma region Receive
+
+protected:
  void ReceiveData(unsigned char bufferSize,
                   char* senderIpAddress,
                   unsigned int senderPort);
  void ReceiveData(unsigned char bufferSize, Participant* remoteParticipant);

-#if defined(NO_STD)
-  unsigned char senderCount = 0;
-  Participant* senders[MAX_SENDER_COUNT];
-#else
-  std::list<Participant*> senders;
-#endif
-
- protected:
-  unsigned long nextPublishMe = 0;
-
-  char buffer[1024];
-
  void SetupUDP(int localPort, const char* remoteIpAddress, int remotePort);

-  Participant* GetParticipant(const char* ipAddress, int port);
-  Participant* AddParticipant(const char* ipAddress, int port);
-
  void ReceiveUDP();

  virtual void Process(Participant* sender, ParticipantMsg* msg);
-  virtual void Process(Participant* sender, SiteMsg* msg);
+  virtual void Process(Participant* sender, 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);
+
+#pragma endregion Receive
+
 };

 }  // namespace RoboidControl
--- a/Participants/SiteServer.cpp
+++ b/Participants/SiteServer.cpp
@ -0,0 +1,97 @@
+#include "SiteServer.h"
+
+#include "Things/TemperatureSensor.h"
+
+#if !defined(NO_STD)
+#include <functional>
+#include <memory>
+#endif
+
+namespace RoboidControl {
+
+#pragma region Init
+
+SiteServer::SiteServer(int port) : ParticipantUDP(port) {
+  this->name = "Site Server";
+  this->publishInterval = 0;
+
+  SetupUDP(port, ipAddress, 0);
+}
+
+#pragma endregion Init
+
+#pragma region Update
+
+void SiteServer::UpdateMyThings() {
+  for (Thing* thing : this->things) {
+    if (thing == nullptr)
+      continue;
+
+    thing->Update(true);
+
+    if (this->isIsolated == false) {
+      // Send to all other participants
+#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()) {
+#endif
+        if (participant == nullptr || participant == this)
+          continue;
+
+        PoseMsg* poseMsg = new PoseMsg(this->networkId, thing);
+        this->Send(participant, poseMsg);
+        delete poseMsg;
+        BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing);
+        this->Send(participant, binaryMsg);
+        delete binaryMsg;
+      }
+    }
+  }
+}
+
+#pragma endregion Update
+
+#pragma region Receive
+
+void SiteServer::Process(Participant* 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);
+    delete msg;
+  }
+}
+
+void SiteServer::Process(Participant* sender, NetworkIdMsg* msg) {}
+
+void SiteServer::Process(Participant* sender, ThingMsg* msg) {
+  Thing* thing = sender->Get(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));
+    if (thing->IsRoot())
+    // if (thing->GetParent() != nullptr)
+#if defined(NO_STD)
+      ;
+#else
+      std::cout << "Could not find parent [" << (int)msg->networkId << "/"
+                << (int)msg->parentId << "]\n";
+#endif
+  } else
+    thing->SetParent(Thing::LocalRoot());
+}
+
+#pragma endregion Receive
+
+}  // namespace RoboidControl
--- a/Participants/SiteServer.h
+++ b/Participants/SiteServer.h
@ -0,0 +1,44 @@
+#pragma once
+
+#include "ParticipantUDP.h"
+
+#if !defined(NO_STD)
+#include <functional>
+#include <memory>
+#include <unordered_map>
+#endif
+
+namespace RoboidControl {
+
+/// @brief A participant is device which can communicate with other participants
+class SiteServer : public ParticipantUDP {
+  
+#pragma region Init
+
+ public:
+  /// @brief Create a new site server
+  /// @param port The port of which to receive the messages
+  SiteServer(int port = 7681);
+
+#pragma endregion Init
+
+#pragma region Update
+
+  virtual void UpdateMyThings() override;
+
+#pragma endregion Update
+
+#pragma region Receive
+
+ 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;
+
+#pragma endregion Receive
+
+};
+
+}  // namespace RoboidControl
--- a/Posix/PosixParticipant.cpp
+++ b/Posix/PosixParticipant.cpp
@ -11,7 +11,7 @@
 namespace RoboidControl {
 namespace Posix {

-void LocalParticipant::Setup(int localPort, const char* remoteIpAddress, int remotePort) {
+void ParticipantUDP::Setup(int localPort, const char* remoteIpAddress, int remotePort) {
 #if defined(__unix__) || defined(__APPLE__)

  // Create a UDP socket
@ -63,7 +63,7 @@ void LocalParticipant::Setup(int localPort, const char* remoteIpAddress, int rem
 #endif
 }

-void LocalParticipant::Receive() {
+void ParticipantUDP::Receive() {
 #if defined(__unix__) || defined(__APPLE__)
  sockaddr_in client_addr;
  socklen_t len = sizeof(client_addr);
@ -74,9 +74,9 @@ void LocalParticipant::Receive() {
    unsigned int sender_port = ntohs(client_addr.sin_port);

    ReceiveData(packetSize, sender_ipAddress, sender_port);
-    // RoboidControl::Participant* remoteParticipant = this->GetParticipant(sender_ipAddress, sender_port);
+    // RoboidControl::Participant* remoteParticipant = this->Get(sender_ipAddress, sender_port);
    // if (remoteParticipant == nullptr) {
-    //   remoteParticipant = this->AddParticipant(sender_ipAddress, sender_port);
+    //   remoteParticipant = this->Add(sender_ipAddress, sender_port);
    //   // std::cout << "New sender " << sender_ipAddress << ":" << sender_port
    //   //           << "\n";
    //   // std::cout << "New remote participant " << remoteParticipant->ipAddress
@ -90,7 +90,7 @@ void LocalParticipant::Receive() {
 #endif
 }

-bool LocalParticipant::Send(Participant* remoteParticipant, int bufferSize) {
+bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
 #if defined(__unix__) || defined(__APPLE__)
  // std::cout << "Send to " << remoteParticipant->ipAddress << ":" << ntohs(remoteParticipant->port)
  //           << "\n";
@ -113,7 +113,7 @@ bool LocalParticipant::Send(Participant* remoteParticipant, int bufferSize) {
  return true;
 }

-bool LocalParticipant::Publish(IMessage* msg) {
+bool ParticipantUDP::Publish(IMessage* msg) {
 #if defined(__unix__) || defined(__APPLE__)
  int bufferSize = msg->Serialize(this->buffer);
  if (bufferSize <= 0)
--- a/Posix/PosixParticipant.h
+++ b/Posix/PosixParticipant.h
@ -1,16 +1,23 @@
 #pragma once

-#include "../LocalParticipant.h"
+#include "Participants/ParticipantUDP.h"

 namespace RoboidControl {
 namespace Posix {

-class LocalParticipant : public RoboidControl::LocalParticipant {
+class ParticipantUDP : public RoboidControl::ParticipantUDP {
 public:
  void Setup(int localPort, const char* remoteIpAddress, int remotePort);
  void Receive();
  bool Send(Participant* remoteParticipant, int bufferSize);
  bool Publish(IMessage* msg);
+
+ protected:
+#if defined(__unix__) || defined(__APPLE__)
+  sockaddr_in remote_addr;
+  sockaddr_in server_addr;
+  sockaddr_in broadcast_addr;
+#endif
 };

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

 - RoboidControl::Thing
- RoboidControl::LocalParticipant
- RoboidControl::SiteServer
+- RoboidControl::Participant
--- a/SiteServer.cpp
+++ b/SiteServer.cpp
@ -1,64 +0,0 @@
-#include "SiteServer.h"
-
-#include "Things/TemperatureSensor.h"
-
-#if !defined(NO_STD)
-#include <functional>
-#include <memory>
-#endif
-
-namespace RoboidControl {
-
-SiteServer::SiteServer(int port) {
-  this->name = "Site Server";
-  this->publishInterval = 0;
-
-  this->ipAddress = "0.0.0.0";
-  this->port = port;
-
-#if defined(NO_STD)
-  this->senders[this->senderCount++] = this;
-#else
-  this->senders.push_back(this);
-#endif
-
-  SetupUDP(port, ipAddress, 0);
-
-#if !defined(NO_STD)
-  Register<TemperatureSensor>((unsigned char)Thing::Type::TemperatureSensor);
-#endif
-}
-
-void SiteServer::Process(Participant* sender, ParticipantMsg* msg) {
-  if (msg->networkId == 0) {
-    // std::cout << this->name << " received New Client -> " <<
-    // sender->ipAddress
-    //           << ":" << (int)sender->port << "\n";
-    SiteMsg* msg = new SiteMsg(sender->networkId);
-    this->Send(sender, msg);
-    delete msg;
-  }
-}
-
-void SiteServer::Process(Participant* sender, SiteMsg* msg) {}
-
-void SiteServer::Process(Participant* sender, ThingMsg* msg) {
-  Thing* thing = sender->Get(msg->networkId, msg->thingId);
-  if (thing == nullptr) {
-#if defined(NO_STD)
-    new Thing(sender, msg->networkId, msg->thingId,
-              (Thing::Type)msg->thingType);
-#else
-    auto thingMsgProcessor = thingMsgProcessors.find(msg->thingType);
-    Thing* newThing;
-    if (thingMsgProcessor != thingMsgProcessors.end())  // found item
-      newThing =
-          thingMsgProcessor->second(sender, msg->networkId, msg->thingId);
-    else
-      newThing = new Thing(sender, msg->networkId, msg->thingId,
-                           (Thing::Type)msg->thingType);
-#endif
-  }
-}
-
-}  // namespace RoboidControl
--- a/SiteServer.h
+++ b/SiteServer.h
@ -1,46 +0,0 @@
-#pragma once
-
-#include "LocalParticipant.h"
-
-#if !defined(NO_STD)
-#include <functional>
-#include <memory>
-#include <unordered_map>
-#endif
-
-namespace RoboidControl {
-
-/// @brief A participant is device which can communicate with other participants
-class SiteServer : public LocalParticipant {
- public:
-  SiteServer(int port = 7681);
-
-  // virtual void Update(unsigned long currentTimeMs = 0) override;
-
-#if !defined(NO_STD)
-  template <typename ThingClass>
-  void Register(unsigned char thingType) {
-    thingMsgProcessors[thingType] = [](Participant* participant,
-                                       unsigned char networkId,
-                                       unsigned char thingId) {
-      return new ThingClass(participant, networkId, thingId);
-    };
-  };
-#endif
-
- protected:
-  unsigned long nextPublishMe = 0;
-
-  virtual void Process(Participant* sender, ParticipantMsg* msg) override;
-  virtual void Process(Participant* sender, SiteMsg* msg) override;
-  virtual void Process(Participant* sender, ThingMsg* msg) override;
-
-#if !defined(NO_STD)
-  using ThingConstructor = std::function<Thing*(Participant* participant,
-                                                unsigned char networkId,
-                                                unsigned char thingId)>;
-  std::unordered_map<unsigned char, ThingConstructor> thingMsgProcessors;
-#endif
-};
-
-}  // namespace RoboidControl
--- a/Thing.cpp
+++ b/Thing.cpp
@ -1,8 +1,11 @@
 #include "Thing.h"

-#include "LocalParticipant.h"
+#include "Messages/PoseMsg.h"
+#include "Participant.h"
+#include "Participants/IsolatedParticipant.h"

 #include <string.h>
+// #include <iostream>

 #if defined(ARDUINO)
 #include "Arduino.h"
@ -15,71 +18,80 @@

 namespace RoboidControl {

-// LocalParticipant* Thing::CheckHiddenParticipant() {
-//   if (isolatedParticipant == nullptr)
-//     isolatedParticipant = new LocalParticipant(0);
-//   return isolatedParticipant;
-// }
+#pragma region Init

-Thing::Thing(int thingType) : Thing(LocalParticipant::Isolated(), thingType) {}
+Thing* Thing::LocalRoot() {
+  Participant* p = Participant::LocalParticipant;
+  Thing* localRoot = p->root;
+  return localRoot;
+}

-Thing::Thing(Participant* owner, Type thingType)
-    : Thing(owner, (unsigned char)thingType) {}
-
-Thing::Thing(Participant* owner, int thingType) {
-  this->owner = owner;
-  this->id = 0;
-  this->type = thingType;
-  this->networkId = 0;
+// Only use this for root things
+Thing::Thing(Participant* owner) {
+  this->type = Type::Roboid;  // should become root

  this->position = Spherical::zero;
+  this->positionUpdated = true;
  this->orientation = SwingTwist::identity;
+  this->orientationUpdated = true;
+  this->hierarchyChanged = true;

  this->linearVelocity = Spherical::zero;
  this->angularVelocity = Spherical::zero;

-  // std::cout << "add thing to participant\n";
-  owner->Add(this);
-}
-
-Thing::Thing(Participant* owner,
-             unsigned char networkId,
-             unsigned char thingId,
-             Type thingType) {
-  // no participant reference yet..
  this->owner = owner;
-  this->networkId = networkId;
-  this->id = thingId;
-  this->type = (unsigned char)thingType;
+  //this->owner->Add(this, true);
+  std::cout << this->owner->name << ": New root thing " << std::endl;
+}
+
+Thing::Thing(unsigned char thingType, Thing* parent) {
+  this->type = thingType;
+
+  this->position = Spherical::zero;
+  this->positionUpdated = true;
+  this->orientation = SwingTwist::identity;
+  this->orientationUpdated = true;
+  this->hierarchyChanged = true;

  this->linearVelocity = Spherical::zero;
  this->angularVelocity = Spherical::zero;
-  // std::cout << "Created thing " << (int)this->networkId << "/" <<
-  // (int)this->id
-  //           << "\n";
-  owner->Add(this, false);
+
+  this->owner = parent->owner;
+  this->owner->Add(this, true);
+  this->SetParent(parent);
+
+  std::cout << this->owner->name << ": New thing for " << parent->name
+            << std::endl;
 }

-void Thing::Terminate() {
-  // Thing::Remove(this);
+Thing::~Thing() {
+  std::cout << "Destroy thing " << this->name << std::endl;
 }

-Thing* Thing::FindThing(const char* name) {
-  for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
-    Thing* child = this->children[childIx];
-    if (child == nullptr || child->name == nullptr)
-      continue;
+// Thing Thing::Reconstruct(Participant* owner, unsigned char thingType,
+// unsigned char thingId) {
+//   Thing thing = Thing(owner, thingType);
+//   thing.id = thingId;
+//   return thing;
+// }

-    if (strcmp(child->name, name) == 0)
-      return child;
+#pragma endregion Init

-    Thing* foundChild = child->FindThing(name);
-    if (foundChild != nullptr)
-      return foundChild;
-  }
-  return nullptr;
+void Thing::SetName(const char* name) {
+  this->name = name;
+  this->nameChanged = true;
 }

+const char* Thing::GetName() const {
+  return this->name;
+}
+
+void Thing::SetModel(const char* url) {
+  this->modelUrl = url;
+}
+
+#pragma region Hierarchy
+
 void Thing::SetParent(Thing* parent) {
  if (parent == nullptr) {
    Thing* parentThing = this->parent;
@ -88,18 +100,36 @@ void Thing::SetParent(Thing* parent) {
    this->parent = nullptr;
  } else
    parent->AddChild(this);
+  this->hierarchyChanged = true;
 }

-void Thing::SetParent(Thing* root, const char* name) {
-  Thing* thing = root->FindThing(name);
-  if (thing != nullptr)
-    this->SetParent(thing);
+// 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;
 }

+// 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;
 }

+Thing* Thing::GetChildByIndex(unsigned char ix) {
+  return this->children[ix];
+}
+
 void Thing::AddChild(Thing* child) {
  unsigned char newChildCount = this->childCount + 1;
  Thing** newChildren = new Thing*[newChildCount];
@ -139,7 +169,7 @@ Thing* Thing::RemoveChild(Thing* child) {
    }
  }

-  child->parent = nullptr;
+  child->parent = Thing::LocalRoot();

  delete[] this->children;
  this->children = newChildren;
@ -148,7 +178,7 @@ Thing* Thing::RemoveChild(Thing* child) {
  return child;
 }

-Thing* Thing::GetChild(unsigned char id, bool recursive) {
+Thing* Thing::GetChild(unsigned char id, bool recurse) {
  for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
    Thing* child = this->children[childIx];
    if (child == nullptr)
@ -156,8 +186,8 @@ Thing* Thing::GetChild(unsigned char id, bool recursive) {
    if (child->id == id)
      return child;

-    if (recursive) {
-      Thing* foundChild = child->GetChild(id, recursive);
+    if (recurse) {
+      Thing* foundChild = child->GetChild(id, recurse);
      if (foundChild != nullptr)
        return foundChild;
    }
@ -165,52 +195,25 @@ Thing* Thing::GetChild(unsigned char id, bool recursive) {
  return nullptr;
 }

-Thing* Thing::GetChildByIndex(unsigned char ix) {
-  return this->children[ix];
-}
+Thing* Thing::FindChild(const char* name, bool recurse) {
+  for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
+    Thing* child = this->children[childIx];
+    if (child == nullptr || child->name == nullptr)
+      continue;

-void Thing::SetModel(const char* url) {
-  this->modelUrl = url;
-}
+    if (strcmp(child->name, name) == 0)
+      return child;

-unsigned long Thing::GetTimeMs() {
-#if defined(ARDUINO)
-  return millis();
-#else
-  auto now = std::chrono::steady_clock::now();
-  auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(
-      now.time_since_epoch());
-  return static_cast<unsigned long>(ms.count());
-#endif
-}
-
-void Thing::Update(bool recursive) {
-  Update(GetTimeMs(), recursive);
-}
-
-void Thing::Update(unsigned long currentTimeMs, bool recursive) {
-  (void)currentTimeMs;
-  if (recursive) {
-    for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
-      Thing* child = this->children[childIx];
-      if (child == nullptr)
-        continue;
-      child->Update(currentTimeMs, recursive);
-    }
+    Thing* foundChild = child->FindChild(name);
+    if (foundChild != nullptr)
+      return foundChild;
  }
+  return nullptr;
 }

-void Thing::UpdateThings(unsigned long currentTimeMs) {
-  LocalParticipant::Isolated()->Update(currentTimeMs);
-}
+#pragma endregion Hierarchy

-void Thing::GenerateBinary(char* buffer, unsigned char* ix) {
-  (void)buffer;
-  (void)ix;
-}
-void Thing::ProcessBinary(char* bytes) {
-  (void)bytes;
-};
+#pragma region Pose

 void Thing::SetPosition(Spherical position) {
  this->position = position;
@ -230,8 +233,10 @@ SwingTwist Thing::GetOrientation() {
 }

 void Thing::SetLinearVelocity(Spherical linearVelocity) {
-  this->linearVelocity = linearVelocity;
-  this->linearVelocityUpdated = true;
+  if (this->linearVelocity.distance != linearVelocity.distance) {
+    this->linearVelocity = linearVelocity;
+    this->linearVelocityUpdated = true;
+  }
 }

 Spherical Thing::GetLinearVelocity() {
@ -239,12 +244,72 @@ Spherical Thing::GetLinearVelocity() {
 }

 void Thing::SetAngularVelocity(Spherical angularVelocity) {
-  this->angularVelocity = angularVelocity;
-  this->angularVelocityUpdated = true;
+  if (this->angularVelocity.distance != angularVelocity.distance) {
+    this->angularVelocity = angularVelocity;
+    this->angularVelocityUpdated = true;
+  }
 }

 Spherical Thing::GetAngularVelocity() {
  return this->angularVelocity;
 }

+#pragma endregion Pose
+
+#pragma region Update
+
+unsigned long Thing::GetTimeMs() {
+#if defined(ARDUINO)
+  unsigned long ms = millis();
+  return ms;
+#else
+  auto now = std::chrono::steady_clock::now();
+  auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(
+      now.time_since_epoch());
+  return static_cast<unsigned long>(ms.count());
+#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->hierarchyChanged = false;
+  this->nameChanged = false;
+
+  if (recursive) {
+    // 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)
+        continue;
+      child->Update(recursive);
+    }
+  }
+}
+
+void Thing::UpdateThings() {
+  IsolatedParticipant::Isolated()->Update();
+}
+
+#pragma endregion Update
+
+int Thing::GenerateBinary(char* buffer, unsigned char* ix) {
+  (void)buffer;
+  (void)ix;
+  return 0;
+}
+void Thing::ProcessBinary(char* bytes) {
+  (void)bytes;
+};
+
 }  // namespace RoboidControl
--- a/Thing.h
+++ b/Thing.h
@ -10,7 +10,7 @@
 namespace RoboidControl {

 class Participant;
-class LocalParticipant;
+class ParticipantUDP;

 #define THING_STORE_SIZE 256
 // IMPORTANT: values higher than 256 will need to change the Thing::id type
@ -20,7 +20,7 @@ class LocalParticipant;
 class Thing {
 public:
  /// @brief Predefined thing types
-  enum Type {
+  enum Type : unsigned char {
    Undetermined,
    // Sensor,
    Switch,
@ -32,57 +32,107 @@ class Thing {
    ControlledMotor,
    UncontrolledMotor,
    Servo,
+    IncrementalEncoder,
    // Other
    Roboid,
    Humanoid,
    ExternalSensor,
+    DifferentialDrive
  };

-  /// @brief Create a new thing using an implicit local participant
-  /// @param thingType The type of thing
-  Thing(int thingType = Type::Undetermined);
-  /// @brief Create a new thing of the given type
-  /// @param thingType The predefined type of thing
-  Thing(Participant* participant, Type thingType = Type::Undetermined);
-  /// @brief Create a new thing of the give type
-  /// @param thingType The custom type of the thing
-  Thing(Participant* participant, int thingType);
-  /// @brief Create a new thing for the given participant
-  /// @param participant The participant for which this thing is created
-  /// @param networkId The network ID of the thing
-  /// @param thingId The ID of the thing
-  /// @param thingType The type of thing
-  Thing(Participant* participant,
-        unsigned char networkId,
-        unsigned char thingId,
-        Type thingType = Type::Undetermined);
+#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)
+  /// @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());
+
+  /// @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
+
+  ~Thing();
+
+  static Thing Reconstruct(Participant* owner,
+                           unsigned char thingType,
+                           unsigned char thingId);
+
+#pragma endregion Init
+
+ public:
+  /// @brief Terminated things are no longer updated
+  bool terminate = false;
+
+#pragma region Properties

  /// @brief The participant managing this thing
-  Participant* owner;
-  /// @brief The network ID of this thing
-  /// @note This field will likely disappear in future versions
-  unsigned char networkId = 0;
+  Participant* owner = nullptr;
+
  /// @brief The ID of the thing
  unsigned char id = 0;

  /// @brief The type of Thing
  /// This can be either a Thing::Type of a byte value for custom types
-  unsigned char type = 0;
+  unsigned char type = Type::Undetermined;

-  /// @brief Find a thing by name
-  /// @param name Rhe name of the thing
-  /// @return The found thing or nullptr when nothing is found
-  Thing* FindThing(const char* name);
+  /// @brief The name of the thing
+  const char* name = nullptr;

-  /// @brief Sets the parent Thing
-  /// @param parent The Thing which should become the parnet
-  /// @remark This is equivalent to calling parent->AddChild(this);
-  virtual void SetParent(Thing* parent);
-  void SetParent(Thing* root, const char* name);
-  /// @brief Gets the parent Thing
+ public:
+  void SetName(const char* name);
+  const char* GetName() const;
+  bool nameChanged = false;
+
+  /// @brief Sets the location from where the 3D model of this Thing can be
+  /// 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
+  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
+
+#pragma region Hierarchy
+
+  /// @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();

+  bool IsRoot() const;
+
+  /// @brief The number of children
+  unsigned char childCount = 0;
+  /// @brief Get a child by index
+  /// @param ix The child index
+  /// @return The found thing of nullptr when nothing is found
+  Thing* GetChildByIndex(unsigned char ix);
+
  /// @brief Add a child Thing to this Thing
  /// @param child The Thing which should become a child
  /// @remark When the Thing is already a child, it will not be added again
@ -92,49 +142,46 @@ class Thing {
  /// @return The removed child or nullptr if the child could not be found
  Thing* RemoveChild(Thing* child);

-  /// @brief The number of children
-  unsigned char childCount = 0;
  /// @brief Get a child by thing Id
  /// @param id The thing ID to find
-  /// @param recursive Look recursively through all descendants
+  /// @param recurse Look recursively through all descendants
  /// @return The found thing of nullptr when nothing is found
-  Thing* GetChild(unsigned char id, bool recursive = false);
-  /// @brief Get a child by index
-  /// @param ix The child index
-  /// @return The found thing of nullptr when nothing is found
-  Thing* GetChildByIndex(unsigned char ix);
+  Thing* GetChild(unsigned char id, bool recurse = false);

- protected:
+  /// @brief Find a thing by name
+  /// @param name The name of the thing
+  /// @param recurse Look recursively through all descendants
+  /// @return The found thing or nullptr when nothing is found
+  Thing* FindChild(const char* name, bool recurse = true);
+
+  /// @brief Indicator that the hierarchy of the thing has changed
+  bool hierarchyChanged = true;
+
+ private:
  Thing* parent = nullptr;
  Thing** children = nullptr;

- public:
-  /// @brief The name of the thing
-  const char* name = nullptr;
-  /// @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 Hierarchy

+#pragma region Pose
+
+ public:
  /// @brief Set the position of the thing
  /// @param position The new position in local space, in meters
  void SetPosition(Spherical position);
  /// @brief Get the position of the thing
  /// @return The position in local space, in meters
  Spherical GetPosition();
+  /// @brief Boolean indicating that the thing has an updated position
+  bool positionUpdated = false;
+
  /// @brief Set the orientation of the thing
  /// @param orientation The new orientation in local space
  void SetOrientation(SwingTwist orientation);
  /// @brief Get the orientation of the thing
  /// @return The orienation in local space
  SwingTwist GetOrientation();
-  /// @brief The scale of the thing (deprecated I think)
-  // float scale = 1;  // assuming uniform scale
-
-  /// @brief boolean indicating if the position was updated
-  bool positionUpdated = false;
-  /// @brief boolean indicating if the orientation was updated
+  /// @brief Boolean indicating the thing has an updated orientation
  bool orientationUpdated = false;

  /// @brief Set the linear velocity of the thing
@ -144,56 +191,63 @@ class Thing {
  /// @brief Get the linear velocity of the thing
  /// @return The linear velocity in local space, in meters per second
  virtual Spherical GetLinearVelocity();
+  /// @brief Boolean indicating the thing has an updated linear velocity
+  bool linearVelocityUpdated = false;
+
  /// @brief Set the angular velocity of the thing
  /// @param angularVelocity the new angular velocity in local space
  virtual void SetAngularVelocity(Spherical angularVelocity);
  /// @brief Get the angular velocity of the thing
  /// @return The angular velocity in local space
  virtual Spherical GetAngularVelocity();
-  bool linearVelocityUpdated = false;
+  /// @brief Boolean indicating the thing has an updated angular velocity
  bool angularVelocityUpdated = false;

 private:
-  /// @brief The position in local space
+  /// @brief The position of the thing in local space, in meters
  /// @remark When this Thing has a parent, the position is relative to the
  /// parent's position and orientation
  Spherical position;
-  /// @brief The orientation in local space
+  /// @brief The orientation of the thing in local space
  /// @remark When this Thing has a parent, the orientation is relative to the
  /// parent's orientation
  SwingTwist orientation;

-  /// @brief The linear velocity in local space
+  /// @brief The linear velocity of the thing in local space, in meters per
+  /// second
  Spherical linearVelocity;
-  /// @brief The angular velocity in local spze
+  /// @brief The angular velocity of the thing in local space, in degrees per
+  /// second
  Spherical angularVelocity;

+#pragma endregion Pose
+
+#pragma region Update
+
 public:
-  /// @brief Terminated things are no longer updated
-  void Terminate();
-
-  /// @brief Sets the location from where the 3D model of this Thing can be
-  /// 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
-  void SetModel(const char* url);
-
-  static unsigned long GetTimeMs();
-
-  void Update(bool recursive = false);
+  virtual void PrepareForUpdate();

  /// @brief Updates the state of the thing
-  /// @param currentTimeMs The current clock time in milliseconds
-  virtual void Update(unsigned long currentTimeMs, bool recursive = false);
+  /// @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(unsigned long currentTimeMs);
+  static void UpdateThings();

+  /// @brief Get the current time in milliseconds
+  /// @return The current time in milliseconds
+  static unsigned long GetTimeMs();
+
+#pragma endregion Update
+
+ public:
  /// @brief Function used to generate binary data for this thing
  /// @param buffer The byte array for thw binary data
  /// @param ix The starting position for writing the binary data
-  virtual void GenerateBinary(char* buffer, unsigned char* ix);
-  // /// @brief FUnction used to process binary data received for this thing
+  /// @return The size of the binary data
+  virtual int GenerateBinary(char* buffer, unsigned char* ix);
+  /// @brief Function used to process binary data received for this thing
  /// @param bytes The binary data
  virtual void ProcessBinary(char* bytes);
 };
--- a/Things/ControlledMotor.cpp
+++ b/Things/ControlledMotor.cpp
@ -0,0 +1,68 @@
+#include "ControlledMotor.h"
+#include "LinearAlgebra/FloatSingle.h"
+
+namespace RoboidControl {
+
+ControlledMotor::ControlledMotor(Motor* motor,
+                                 RelativeEncoder* encoder,
+                                 Thing* parent)
+    : Motor(parent), motor(motor), encoder(encoder) {
+  this->type = Type::ControlledMotor;
+  //encoder->SetParent(null);
+  //  Thing parent = motor.GetParent();
+  //  this->SetParent(parent);
+  this->integral = 0;
+}
+
+void ControlledMotor::SetTargetVelocity(float velocity) {
+  this->targetVelocity = velocity;
+  this->rotationDirection =
+      (targetVelocity < 0) ? Direction::Reverse : Direction::Forward;
+}
+
+void ControlledMotor::Update(bool recurse) {
+  unsigned long currentTimeMs = GetTimeMs();
+  float timeStep = (currentTimeMs - this->lastUpdateTime) / 1000.0f;
+  this->lastUpdateTime = currentTimeMs;
+  if (timeStep <= 0)
+    return;
+
+  // encoder->Update(false);
+
+  this->actualVelocity = (int)rotationDirection * encoder->rotationSpeed;
+
+  float error = this->targetVelocity - this->actualVelocity;
+  float p_term = error * pidP;
+  this->integral += error * timeStep;
+  float i_term = pidI * this->integral;
+  float d_term = pidD * (error - this->lastError) / timeStep;
+  this->lastError = error;
+
+  float output = p_term + i_term + d_term;
+  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;
+
+  // float newTargetVelocity = motor->targetVelocity + acceleration;
+  output = LinearAlgebra::Float::Clamp(output, -1, 1);
+  motor->SetTargetVelocity(output);  // or something like that
+  //motor->Update(false);
+}
+
+// float ControlledMotor::GetActualVelocity() {
+//   return (int)rotationDirection * encoder->rotationSpeed;
+// }
+
+// bool ControlledMotor::Drive(float distance) {
+//   if (!driving) {
+//     targetDistance = distance;
+//     startDistance = encoder->GetDistance();
+//     driving = true;
+//   }
+//   float totalDistance = encoder->GetDistance() - startDistance;
+//   bool completed = totalDistance > targetDistance;
+//   return completed;
+// }
+}  // namespace RoboidControl
--- a/Things/ControlledMotor.h
+++ b/Things/ControlledMotor.h
@ -0,0 +1,40 @@
+#pragma once
+
+#include "Motor.h"
+#include "RelativeEncoder.h"
+
+namespace RoboidControl {
+
+/// @brief A motor with speed control
+/// It uses a feedback loop from an encoder to regulate the speed
+/// The speed is measured in revolutions per second.
+class ControlledMotor : public Motor {
+ public:
+  ControlledMotor(Motor* motor, RelativeEncoder* encoder, Thing* parent = Thing::LocalRoot());
+
+  float pidP = 0.5;
+  float pidD = 0;
+  float pidI = 0.2;
+
+  /// @brief The actual velocity in revolutions per second
+  float actualVelocity;
+
+  enum Direction { Forward = 1, Reverse = -1 };
+  Direction rotationDirection;
+
+  virtual void Update(bool recurse = false) override;
+
+  /// @brief Set the target verlocity for the motor controller
+  /// @param speed the target velocity in revolutions per second
+  virtual void SetTargetVelocity(float velocity) override;
+
+  Motor* motor;
+  RelativeEncoder* encoder;
+
+ protected:
+  float integral = 0;
+  float lastError = 0;
+  float lastUpdateTime;
+};
+
+}  // namespace RoboidControl
--- a/Things/DifferentialDrive.cpp
+++ b/Things/DifferentialDrive.cpp
@ -1,10 +1,28 @@
 #include "DifferentialDrive.h"

-namespace RoboidControl {
-DifferentialDrive::DifferentialDrive() : Thing() {}
+#include "Messages/LowLevelMessages.h"

-RoboidControl::DifferentialDrive::DifferentialDrive(Participant* participant)
-    : Thing(participant) {}
+namespace RoboidControl {
+
+DifferentialDrive::DifferentialDrive(Thing* parent)
+    : Thing(Type::DifferentialDrive, parent) {
+  this->name = "Differential drive";
+
+  this->leftWheel = new Motor(this);
+  this->leftWheel->name = "Left motor";
+
+  this->rightWheel = new Motor(this);
+  this->rightWheel->name = "Right motor";
+}
+
+DifferentialDrive::DifferentialDrive(Motor* leftMotor,
+                                     Motor* rightMotor,
+                                     Thing* parent)
+    : Thing(Type::DifferentialDrive, parent) {
+  this->name = "Differential drive";
+  this->leftWheel = leftMotor;
+  this->rightWheel = rightMotor;
+}

 void DifferentialDrive::SetDriveDimensions(float wheelDiameter,
                                           float wheelSeparation) {
@ -21,51 +39,66 @@ void DifferentialDrive::SetDriveDimensions(float wheelDiameter,
    this->rightWheel->SetPosition(Spherical(distance, Direction::right));
 }

-void DifferentialDrive::SetMotors(Thing* leftWheel, Thing* rightWheel) {
+// Motor& DifferentialDrive::GetMotorLeft() {
+//   return *this->leftWheel;
+// }
+
+// Motor& DifferentialDrive::GetMotorRight() {
+//   return *this->rightWheel;
+// }
+
+void DifferentialDrive::SetMotors(Motor& leftMotor, Motor& rightMotor) {
  float distance = this->wheelSeparation / 2;
-  this->leftWheel = leftWheel;
-  ;
-  if (leftWheel != nullptr)
-    this->leftWheel->SetPosition(Spherical(distance, Direction::left));
+  this->leftWheel = &leftMotor;
+  this->leftWheel->SetPosition(Spherical(distance, Direction::left));

-  this->rightWheel = rightWheel;
-  if (rightWheel != nullptr)
-    this->rightWheel->SetPosition(Spherical(distance, Direction::right));
+  this->rightWheel = &rightMotor;
+  this->rightWheel->SetPosition(Spherical(distance, Direction::right));
 }

-void DifferentialDrive::SetWheelVelocity(float speedLeft, float speedRight) {
+void DifferentialDrive::SetWheelVelocity(float velocityLeft,
+                                         float velocityRight) {
+  // if (this->leftWheel != nullptr)
+  //   this->leftWheel->SetAngularVelocity(Spherical(velocityLeft,
+  //   Direction::left));
+  // if (this->rightWheel != nullptr)
+  //   this->rightWheel->SetAngularVelocity(
+  //       Spherical(velocityRight, Direction::right));
  if (this->leftWheel != nullptr)
-    this->leftWheel->SetAngularVelocity(Spherical(speedLeft, Direction::left));
+    this->leftWheel->SetTargetVelocity(velocityLeft);
  if (this->rightWheel != nullptr)
-    this->rightWheel->SetAngularVelocity(
-        Spherical(speedRight, Direction::right));
+    this->rightWheel->SetTargetVelocity(velocityRight);
 }

-void DifferentialDrive::Update(unsigned long currentMs, bool recursive) {
-  if (this->linearVelocityUpdated == false)
-    return;
-  // this assumes forward velocity only....
-  float linearVelocity = this->GetLinearVelocity().distance;
+void DifferentialDrive::Update(bool recursive) {
+  if (this->linearVelocityUpdated) {
+    // this assumes forward velocity only....
+    float linearVelocity = this->GetLinearVelocity().distance;

-  Spherical angularVelocity = this->GetAngularVelocity();
-  float angularSpeed = angularVelocity.distance * Deg2Rad;  // in degrees/sec
-  // Determine the rotation direction
-  if (angularVelocity.direction.horizontal.InDegrees() < 0)
-    angularSpeed = -angularSpeed;
+    Spherical angularVelocity = this->GetAngularVelocity();
+    float angularSpeed = angularVelocity.distance * Deg2Rad;  // in degrees/sec
+    // Determine the rotation direction
+    if (angularVelocity.direction.horizontal.InDegrees() < 0)
+      angularSpeed = -angularSpeed;

-  // wheel separation can be replaced by this->leftwheel->position->distance
-  float speedLeft =
-      (linearVelocity + angularSpeed * this->wheelSeparation / 2) /
-      this->wheelRadius * Rad2Deg;
-  float speedRight =
-      (linearVelocity - angularSpeed * this->wheelSeparation / 2) /
-      this->wheelRadius * Rad2Deg;
+    // wheel separation can be replaced by this->leftwheel->position->distance
+    float speedLeft =
+        (linearVelocity + angularSpeed * this->wheelSeparation / 2) /
+        this->wheelRadius * Rad2Deg;
+    float speedRight =
+        (linearVelocity - angularSpeed * this->wheelSeparation / 2) /
+        this->wheelRadius * Rad2Deg;

-  this->SetWheelVelocity(speedLeft, speedRight);
-  Thing::Update(currentMs, recursive);
-  // std::cout << "lin. speed " << linearVelocity << "   ang. speed " <<
-  // angularVelocity.distance << "   left wheel  "
-  //         << speedLeft << "   right wheel " << speedRight << "\n";
+    this->SetWheelVelocity(speedLeft, speedRight);
+  }
+  Thing::Update(recursive);
+}
+
+int DifferentialDrive::GenerateBinary(char* data, unsigned char* ix) {
+  data[(*ix)++] = this->leftWheel->id;
+  data[(*ix)++] = this->rightWheel->id;
+  LowLevelMessages::SendFloat16(data, ix, this->wheelRadius);
+  return 4;
 }

 }  // namespace RoboidControl
--- a/Things/DifferentialDrive.h
+++ b/Things/DifferentialDrive.h
@ -1,6 +1,7 @@
 #pragma once

 #include "Thing.h"
+#include "Motor.h"

 namespace RoboidControl {

@ -9,11 +10,13 @@ namespace RoboidControl {
 /// @sa @link https://en.wikipedia.org/wiki/Differential_wheeled_robot @endlink
 class DifferentialDrive : public Thing {
 public:
-  /// @brief Create a differential drive without networking support
-  DifferentialDrive();
-  /// @brief Create a differential drive with networking support
-  /// @param participant The local participant
-  DifferentialDrive(Participant* participant);
+  /// @brief Create a new child differential drive
+  /// @param parent The parent thing
+  /// @param thingId The ID of the thing, leave out or set to zero to generate
+  /// an ID
+  DifferentialDrive(Thing* parent = Thing::LocalRoot());
+
+  DifferentialDrive(Motor* leftMotor, Motor* rightMotor, Thing* parent = Thing::LocalRoot());

  /// @brief Configures the dimensions of the drive
  /// @param wheelDiameter The diameter of the wheels in meters
@ -23,35 +26,41 @@ class DifferentialDrive : public Thing {
  /// linear and angular velocity.
  /// @sa SetLinearVelocity SetAngularVelocity
  void SetDriveDimensions(float wheelDiameter, float wheelSeparation);
+
+  // Motor& GetMotorLeft();
+  // Motor& GetMotorRight();
  /// @brief Congures the motors for the wheels
  /// @param leftWheel The motor for the left wheel
  /// @param rightWheel The motor for the right wheel
-  void SetMotors(Thing* leftWheel, Thing* rightWheel);
+  void SetMotors(Motor& leftMotor, Motor& rightMotor);

  /// @brief Directly specify the speeds of the motors
  /// @param speedLeft The speed of the left wheel in degrees per second.
  /// Positive moves the robot in the forward direction.
  /// @param speedRight The speed of the right wheel in degrees per second.
  /// Positive moves the robot in the forward direction.
-
  void SetWheelVelocity(float speedLeft, float speedRight);

  /// @copydoc RoboidControl::Thing::Update(unsigned long)
-  virtual void Update(unsigned long currentMs, bool recursive = true) override;
+  virtual void Update(bool recursive = true) override;
+
+  int GenerateBinary(char* bytes, unsigned char* ix) override;
+  // virtual void ProcessBinary(char* bytes) override;
+
+    /// @brief The left wheel
+  Motor* leftWheel = nullptr;
+  /// @brief The right wheel
+  Motor* rightWheel = nullptr;

 protected:
  /// @brief The radius of a wheel in meters
-  float wheelRadius = 1.0f;
+  float wheelRadius = 0.0f;
  /// @brief The distance between the wheels in meters
-  float wheelSeparation = 1.0f;
+  float wheelSeparation = 0.0f;

  /// @brief Convert revolutions per second to meters per second
  float rpsToMs = 1.0f;

-  /// @brief The left wheel
-  Thing* leftWheel = nullptr;
-  /// @brief The right wheel
-  Thing* rightWheel = nullptr;
 };

 }  // namespace RoboidControl
--- a/Things/DigitalSensor.cpp
+++ b/Things/DigitalSensor.cpp
@ -2,8 +2,26 @@

 namespace RoboidControl {

-//DigitalSensor::DigitalSensor() {}
+//DigitalSensor::DigitalSensor() : Thing(Type::Switch) {}

-DigitalSensor::DigitalSensor(Participant* participant, unsigned char networkId, unsigned char thingId) : Thing(participant) {}
+// 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) {}
+
+int DigitalSensor::GenerateBinary(char* bytes, unsigned char* ix) {
+  bytes[(*ix)++] = state ? 1 : 0;
+  return 1;
+}
+
+void DigitalSensor::ProcessBinary(char* bytes) {
+  this->state |= (bytes[0] == 1);
+}

 }  // namespace RoboidControl
--- a/Things/DigitalSensor.h
+++ b/Things/DigitalSensor.h
@ -7,15 +7,31 @@ namespace RoboidControl {
 /// @brief A digital (on/off, 1/0, true/false) sensor
 class DigitalSensor : public Thing {
 public:
+  /// @brief Create a digital sensor without communication abilities
+  //DigitalSensor();
+  /// @brief Create a digital sensor for a participant
+  /// @param owner The owning participant
+  /// @param thingId The ID of the thing, leave out or set to zero to generate
+  /// an ID
+  DigitalSensor(Participant* owner = nullptr); //, unsigned char thingId = 0);
+  /// @brief Create a new child digital sensor
+  /// @param parent The parent thing
+  /// @param thingId The ID of the thing, leave out or set to zero to generate
+  /// an ID
+  // DigitalSensor(Thing* parent); //, unsigned char thingId = 0);
+  DigitalSensor(Thing* parent = Thing::LocalRoot());
+
  /// @brief The sigital state
  bool state = 0;

-  /// @brief The default constructor
-  //DigitalSensor();
-  /// @brief Create a temperature sensor with the given ID
-  /// @param networkId The network ID of the sensor
-  /// @param thingId The ID of the thing
-  DigitalSensor(Participant* participant, unsigned char networkId, unsigned char thingId);
+  /// @brief Function used to generate binary data for this digital 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 digital
+  /// sensor
+  /// @param bytes The binary data to process
+  virtual void ProcessBinary(char* bytes) override;
 };

 }  // namespace RoboidControl
--- a/Things/Motor.cpp
+++ b/Things/Motor.cpp
@ -0,0 +1,16 @@
+#include "Motor.h"
+
+namespace RoboidControl {
+
+Motor::Motor(Thing* parent) : Thing(Type::UncontrolledMotor, parent) {}
+
+void Motor::SetTargetVelocity(float targetSpeed) {
+    this->targetVelocity = targetSpeed;
+}
+
+int Motor::GenerateBinary(char* data, unsigned char* ix) {
+  data[(*ix)++] = this->targetVelocity * 127.0f;
+  return 1;
+}
+
+}  // namespace RoboidControl
--- a/Things/Motor.h
+++ b/Things/Motor.h
@ -0,0 +1,25 @@
+#pragma once
+
+#include "Thing.h"
+
+namespace RoboidControl {
+
+class Motor : public Thing {
+ public:
+  Motor(Thing* parent = Thing::LocalRoot());
+
+  /// @brief Motor turning direction
+  enum class Direction { Clockwise = 1, CounterClockwise = -1 };
+  /// @brief The forward turning direction of the motor
+  Direction direction;
+
+  virtual void SetTargetVelocity(float velocity);  // -1..0..1
+
+  int GenerateBinary(char* bytes, unsigned char* ix) override;
+  // virtual void ProcessBinary(char* bytes) override;
+
+ //protected:
+  float targetVelocity = 0;
+};
+
+}  // namespace RoboidControl
--- a/Things/RelativeEncoder.cpp
+++ b/Things/RelativeEncoder.cpp
@ -0,0 +1,21 @@
+#include "RelativeEncoder.h"
+
+namespace RoboidControl {
+
+RelativeEncoder::RelativeEncoder(Thing* parent)
+    : Thing(Type::IncrementalEncoder, parent) {}
+
+float RelativeEncoder::GetRotationSpeed() {
+  return rotationSpeed;
+}
+
+int RelativeEncoder::GenerateBinary(char* data, unsigned char* ix) {
+  data[(*ix)++] = (unsigned char)(this->rotationSpeed * 127);
+  return 1;
+}
+
+void RelativeEncoder::ProcessBinary(char* data) {
+  this->rotationSpeed = (float)data[0] / 127;
+}
+
+}  // namespace RoboidControl
--- a/Things/RelativeEncoder.h
+++ b/Things/RelativeEncoder.h
@ -0,0 +1,39 @@
+#pragma once
+
+#include "Thing.h"
+
+namespace RoboidControl {
+
+/// @brief An Incremental Encoder measures the rotations of an axle using a
+/// rotary sensor. Some encoders are able to detect direction, while others can
+/// not.
+class RelativeEncoder : public Thing {
+ public:
+  /// @brief Creates a sensor which measures distance from pulses
+  /// @param pulsesPerRevolution The number of pulse edges which make a
+  /// full rotation
+  /// @param distancePerRevolution The distance a wheel travels per full
+  /// rotation
+  //RelativeEncoder(Participant* owner);
+  // RelativeEncoder(Thing* parent);
+  RelativeEncoder(Thing* parent = Thing::LocalRoot());
+
+  /// @brief Get the rotation speed 
+  /// @return The speed in revolutions per second
+  virtual float GetRotationSpeed();
+  float rotationSpeed = 0;
+
+  /// @brief Function used to generate binary data for this touch 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 touch sensor
+  /// @param bytes The binary data to process
+  virtual void ProcessBinary(char* bytes) override;
+
+protected:
+    /// @brief rotation speed in revolutions per second
+    //float _rotationSpeed;
+};
+
+}  // namespace RoboidControl
--- a/Things/TemperatureSensor.cpp
+++ b/Things/TemperatureSensor.cpp
@ -4,22 +4,25 @@

 namespace RoboidControl {

-// TemperatureSensor::TemperatureSensor() : Thing(Type::TemperatureSensor) {}
+// TemperatureSensor::TemperatureSensor(Participant* participant,
+//                                      unsigned char thingId)
+//     : Thing(participant, Type::TemperatureSensor, thingId) {}

-// TemperatureSensor::TemperatureSensor() : Thing(Type::TemperatureSensor) {}
+// TemperatureSensor::TemperatureSensor(Participant* owner) : Thing(owner, Type::TemperatureSensor) {}

-TemperatureSensor::TemperatureSensor(Participant* participant,
-                                     unsigned char networkId,
-                                     unsigned char thingId)
-    : Thing(participant, networkId, thingId, Type::TemperatureSensor) {}
+TemperatureSensor::TemperatureSensor(Thing* parent) : Thing(Type::TemperatureSensor, parent) {}
+
+// TemperatureSensor::TemperatureSensor(Thing* parent) : Thing(parent, Type::TemperatureSensor) {}

 void TemperatureSensor::SetTemperature(float temp) {
  this->temperature = temp;
 }

-void TemperatureSensor::GenerateBinary(char* buffer, unsigned char* ix) {
+int TemperatureSensor::GenerateBinary(char* buffer, unsigned char* ix) {
+  unsigned char startIx = *ix;
  // std::cout << "Send temperature: " << this->temperature << "\n";
  LowLevelMessages::SendFloat16(buffer, ix, this->temperature);
+  return *ix - startIx;
 }

 void TemperatureSensor::ProcessBinary(char* bytes) {
--- a/Things/TemperatureSensor.h
+++ b/Things/TemperatureSensor.h
@ -7,15 +7,17 @@ namespace RoboidControl {
 /// @brief A temperature sensor
 class TemperatureSensor : public Thing {
 public:
-  /// @brief The measured temperature
-  float temperature = 0;
-
  /// @brief The default constructor
  //TemperatureSensor();
  /// @brief Create a temperature sensor with the given ID
  /// @param networkId The network ID of the sensor
  /// @param thingId The ID of the thing
-  TemperatureSensor(Participant* participant, unsigned char networkId, unsigned char thingId);
+  TemperatureSensor(Participant* participant); //, unsigned char thingId);
+  // TemperatureSensor(Thing* parent);
+  TemperatureSensor(Thing* parent = Thing::LocalRoot());
+
+      /// @brief The measured temperature
+      float temperature = 0;

  /// @brief Manually override the measured temperature
  /// @param temperature The new temperature
@ -24,7 +26,7 @@ class TemperatureSensor : public Thing {
  /// @brief Function to create a binary message with the temperature
  /// @param buffer The byte array for thw binary data
  /// @param ix The starting position for writing the binary data
-  void GenerateBinary(char* bytes, unsigned char* ix) override;
+  int GenerateBinary(char* bytes, unsigned char* ix) override;
  /// @brief Function to extract the temperature received in the binary message
  /// @param bytes The binary data
  virtual void ProcessBinary(char* bytes) override;
--- a/Things/TouchSensor.cpp
+++ b/Things/TouchSensor.cpp
@ -1,21 +1,30 @@
 #include "TouchSensor.h"

 namespace RoboidControl {
-TouchSensor::TouchSensor() : Thing(Thing::Type::TouchSensor) {}

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

-void TouchSensor::GenerateBinary(char* bytes, unsigned char* ix) {}
+void TouchSensor::PrepareForUpdate() {
+  this->touchedSomething = this->externalTouch;
+}
+
+void TouchSensor::Update(bool recursive) {
+  Thing::Update(recursive);
+}
+
+int TouchSensor::GenerateBinary(char* bytes, unsigned char* ix) {
+  bytes[(*ix)++] = this->touchedSomething ? 1 : 0;
+  return 1;
+}

 void TouchSensor::ProcessBinary(char* bytes) {
-  // if (bytes[0] == 1)
-  //   std::cout << this->name << " is Touching something!\n";
-  this->touchedSomething |= (bytes[0] == 1);
+  this->externalTouch = (bytes[0] == 1);
+  if (this->externalTouch)
+    std::cout << "touching!\n";
+  else
+    std::cout << "not touching\n";
 }

 }  // namespace RoboidControl
--- a/Things/TouchSensor.h
+++ b/Things/TouchSensor.h
@ -6,29 +6,30 @@ 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
 public:
-  /// @brief Create a touch sensor with isolated participant
-  TouchSensor();
-  /// @brief Create a touch sensor
-  TouchSensor(Participant* participant);
-  /// @brief Create a temperature sensor with the given ID
-  /// @param networkId The network ID of the sensor
-  /// @param thingId The ID of the thing
-  TouchSensor(Thing* parent);
-  // TouchSensor(RemoteParticipant* participant, unsigned char networkId,
-  // unsigned char thingId);
+  /// @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
+  TouchSensor(Thing* parent = Thing::LocalRoot());

  /// @brief Value which is true when the sensor is touching something, false
  /// otherwise
  bool touchedSomething = false;

-  /// @brief Function to create a binary message with the temperature
+  virtual void PrepareForUpdate() override;
+  virtual void Update(bool recursive) override;
+
+  /// @brief Function used to generate binary data for this touch sensor
  /// @param buffer The byte array for thw binary data
  /// @param ix The starting position for writing the binary data
-  void GenerateBinary(char* bytes, unsigned char* ix) override;
-  /// @brief Function to extract the temperature received in the binary message
-  /// @param bytes The binary data
+  int GenerateBinary(char* bytes, unsigned char* ix) override;
+  /// @brief Function used to process binary data received for this touch sensor
+  /// @param bytes The binary data to process
  virtual void ProcessBinary(char* bytes) override;
+protected:
+  bool externalTouch = false;
 };

 }  // namespace RoboidControl
--- a/Windows/WindowsParticipant.cpp
+++ b/Windows/WindowsParticipant.cpp
@ -4,36 +4,22 @@
 #include <winsock2.h>
 #include <ws2tcpip.h>
 #pragma comment(lib, "ws2_32.lib")
-
-#elif defined(__unix__) || defined(__APPLE__)
-#include <arpa/inet.h>
-#include <fcntl.h>  // For fcntl
-#include <netinet/in.h>
-#include <sys/socket.h>
-#include <unistd.h>
 #endif

 namespace RoboidControl {
 namespace Windows {

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

  // Create a UDP socket
-#if defined(_WIN32) || defined(_WIN64)
  // Windows-specific Winsock initialization
  WSADATA wsaData;
  if (WSAStartup(MAKEWORD(2, 2), &wsaData) != 0) {
    std::cerr << "WSAStartup failed" << std::endl;
    return;
  }
-#endif
-
-#if defined(_WIN32) || defined(_WIN64)
  this->sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
-#elif defined(__unix__) || defined(__APPLE__)
-  this->sock = socket(AF_INET, SOCK_DGRAM, 0);
-#endif

  if (this->sock < 0) {
    std::cerr << "Error creating socket" << std::endl;
@ -41,13 +27,8 @@ void LocalParticipant::Setup(int localPort, const char* remoteIpAddress, int rem
  }

 // Set the socket to non-blocking mode
-#if defined(_WIN32) || defined(_WIN64)
  u_long mode = 1;  // 1 to enable non-blocking socket
  ioctlsocket(this->sock, FIONBIO, &mode);
-#elif defined(__unix__) || defined(__APPLE__)
-  int flags = fcntl(this->sock, F_GETFL, 0);
-  fcntl(this->sock, F_SETFL, flags | O_NONBLOCK);
-#endif

  if (remotePort != 0) {
    // Set up the address to send to
@ -56,12 +37,8 @@ void LocalParticipant::Setup(int localPort, const char* remoteIpAddress, int rem
    remote_addr.sin_port = htons((u_short)remotePort);
    if (inet_pton(AF_INET, remoteIpAddress, &remote_addr.sin_addr) <= 0) {
      std::cerr << "Invalid address" << std::endl;
-#if defined(_WIN32) || defined(_WIN64)
      closesocket(sock);
      WSACleanup();
-#elif defined(__unix__) || defined(__APPLE__)
-      close(sock);
-#endif
      return;
    }
  }
@ -72,30 +49,22 @@ void LocalParticipant::Setup(int localPort, const char* remoteIpAddress, int rem
  server_addr.sin_port = htons((u_short)localPort);
  if (inet_pton(AF_INET, "0.0.0.0", &server_addr.sin_addr) <= 0) {
    std::cerr << "Invalid address" << std::endl;
-#if defined(_WIN32) || defined(_WIN64)
    closesocket(sock);
    WSACleanup();
-#elif defined(__unix__) || defined(__APPLE__)
-    close(sock);
-#endif
    return;
  }

  // Bind the socket to the specified port
  if (bind(this->sock, (const struct sockaddr*)&server_addr, sizeof(server_addr)) < 0) {
    std::cerr << "Bind failed" << std::endl;
-#if defined(_WIN32) || defined(_WIN64)
    closesocket(sock);
    WSACleanup();
-#elif defined(__unix__) || defined(__APPLE__)
-    close(sock);
-#endif
  }

-#endif
+#endif // _WIN32 || _WIN64
 }

-void LocalParticipant::Receive() {
+void ParticipantUDP::Receive() {
 #if defined(_WIN32) || defined(_WIN64)
  // char ip_str[INET_ADDRSTRLEN];
  // inet_ntop(AF_INET, &(server_addr.sin_addr), ip_str, INET_ADDRSTRLEN);
@ -103,30 +72,22 @@ void LocalParticipant::Receive() {
  //           << ntohs(server_addr.sin_port) << "\n";

  sockaddr_in client_addr;
-#if defined(_WIN32) || defined(_WIN64)
  int len = sizeof(client_addr);
-#elif defined(__unix__) || defined(__APPLE__)
-  socklen_t len = sizeof(client_addr);
-#endif
  int packetSize = recvfrom(this->sock, buffer, sizeof(buffer), 0, (struct sockaddr*)&client_addr, &len);
  // std::cout << "received data " << packetSize << "\n";
  if (packetSize < 0) {
-#if defined(_WIN32) || defined(_WIN64)
    int error_code = WSAGetLastError();  // Get the error code on Windows
    if (error_code != WSAEWOULDBLOCK)
      std::cerr << "recvfrom failed with error: " << error_code << std::endl;
-#else
-    // std::cerr << "recvfrom failed with error: " << packetSize << std::endl;
-#endif
  } else if (packetSize > 0) {
    char sender_ipAddress[INET_ADDRSTRLEN];
    inet_ntop(AF_INET, &(client_addr.sin_addr), sender_ipAddress, INET_ADDRSTRLEN);
    unsigned int sender_port = ntohs(client_addr.sin_port);

    ReceiveData(packetSize, sender_ipAddress, sender_port);
-    // RoboidControl::LocalParticipant* remoteParticipant = this->GetParticipant(sender_ipAddress, sender_port);
+    // RoboidControl::ParticipantUDP* remoteParticipant = this->Get(sender_ipAddress, sender_port);
    // if (remoteParticipant == nullptr) {
-    //   remoteParticipant = this->AddParticipant(sender_ipAddress, sender_port);
+    //   remoteParticipant = this->Add(sender_ipAddress, sender_port);
    //   // std::cout << "New sender " << sender_ipAddress << ":"
    //   //           << sender_port << "\n";
    //   // std::cout << "New remote participant " <<
@ -138,16 +99,16 @@ void LocalParticipant::Receive() {

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

-bool LocalParticipant::Send(Participant* remoteParticipant, int bufferSize) {
+bool ParticipantUDP::Send(Participant* remoteParticipant, int bufferSize) {
 #if defined(_WIN32) || defined(_WIN64)
  char ip_str[INET_ADDRSTRLEN];
  inet_ntop(AF_INET, &(remote_addr.sin_addr), ip_str, INET_ADDRSTRLEN);
  std::cout << "Send to " << ip_str << ":" << ntohs(remote_addr.sin_port) << "\n";
  int sent_bytes = sendto(sock, this->buffer, bufferSize, 0, (struct sockaddr*)&remote_addr, sizeof(remote_addr));
-#if defined(_WIN32) || defined(_WIN64)
+
  if (sent_bytes <= SOCKET_ERROR) {
    int error_code = WSAGetLastError();  // Get the error code on Windows
    std::cerr << "sendto failed with error: " << error_code << std::endl;
@ -155,18 +116,11 @@ bool LocalParticipant::Send(Participant* remoteParticipant, int bufferSize) {
    WSACleanup();
    return false;
  }
-#elif defined(__unix__) || defined(__APPLE__)
-  if (sent_bytes < 0) {
-    std::cerr << "sendto failed with error: " << sent_bytes << std::endl;
-    close(sock);
-    return false;
-  }
-#endif
-#endif
+#endif // _WIN32 || _WIN64
  return true;
 }

-bool LocalParticipant::Publish(IMessage* msg) {
+bool ParticipantUDP::Publish(IMessage* msg) {
 #if defined(_WIN32) || defined(_WIN64)
  int bufferSize = msg->Serialize(this->buffer);
  if (bufferSize <= 0)
@ -176,7 +130,7 @@ bool LocalParticipant::Publish(IMessage* msg) {
  inet_ntop(AF_INET, &(broadcast_addr.sin_addr), ip_str, INET_ADDRSTRLEN);
  std::cout << "Publish to " << ip_str << ":" << ntohs(broadcast_addr.sin_port) << "\n";
  int sent_bytes = sendto(sock, this->buffer, bufferSize, 0, (struct sockaddr*)&broadcast_addr, sizeof(broadcast_addr));
-#if defined(_WIN32) || defined(_WIN64)
+
  if (sent_bytes <= SOCKET_ERROR) {
    int error_code = WSAGetLastError();  // Get the error code on Windows
    std::cerr << "sendto failed with error: " << error_code << std::endl;
@ -184,14 +138,7 @@ bool LocalParticipant::Publish(IMessage* msg) {
    WSACleanup();
    return false;
  }
-#elif defined(__unix__) || defined(__APPLE__)
-  if (sent_bytes < 0) {
-    std::cerr << "sendto failed with error: " << sent_bytes << std::endl;
-    close(sock);
-    return false;
-  }
-#endif
-#endif
+#endif // _WIN32 || _WIN64
  return true;
 }

--- a/Windows/WindowsParticipant.h
+++ b/Windows/WindowsParticipant.h
@ -1,11 +1,11 @@
 #pragma once

-#include "../LocalParticipant.h"
+#include "Participants/ParticipantUDP.h"

 namespace RoboidControl {
 namespace Windows {

-class LocalParticipant : public RoboidControl::LocalParticipant {
+class ParticipantUDP : public RoboidControl::ParticipantUDP {
 public:
  void Setup(int localPort, const char* remoteIpAddress, int remotePort);
  void Receive();
--- a/examples/BB2B.cpp
+++ b/examples/BB2B.cpp
@ -17,26 +17,26 @@ using namespace RoboidControl;

 int main() {
  // The robot's propulsion is a differential drive
-  DifferentialDrive* bb2b = new DifferentialDrive();
+  DifferentialDrive bb2b = DifferentialDrive();
  // Is has a touch sensor at the front left of the roboid
-  TouchSensor* touchLeft = new TouchSensor(bb2b);
+  TouchSensor touchLeft = TouchSensor(&bb2b); 
  // and other one on the right
-  TouchSensor* touchRight = new TouchSensor(bb2b);
+  TouchSensor touchRight = TouchSensor(&bb2b);

  // Do forever:
  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.touchedSomething) ? -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.touchedSomething) ? -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);
+    bb2b.SetWheelVelocity(leftWheelSpeed, rightWheelSpeed);

    // Update the roboid state
-    bb2b->Update(true);
+    bb2b.Update(true);

    // and sleep for 100ms
 #if defined(ARDUINO)
--- a/examples/CMakeLists.txt
+++ b/examples/CMakeLists.txt
--- a/examples/README.md
+++ b/examples/README.md
@ -0,0 +1 @@
+Important: this folder has to be names 'examples' exactly to maintain compatibility with Arduino
--- a/test/participant_test.cc
+++ b/test/participant_test.cc
@ -4,111 +4,75 @@
 // not supported using Visual Studio 2022 compiler...
 #include <gtest/gtest.h>

+#include "Participants/SiteServer.h"
+#include "Thing.h"
+
 #include <chrono>
 #include <thread>
-// #include <ws2tcpip.h>
-
-#include "Participant.h"
-#include "SiteServer.h"
-#include "Thing.h"

 using namespace RoboidControl;

-// Function to get the current time in milliseconds as unsigned long
-unsigned long get_time_ms() {
-  auto now = std::chrono::steady_clock::now();
-  auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(
-      now.time_since_epoch());
-  return static_cast<unsigned long>(ms.count());
+TEST(Participant, Participant) {
+  ParticipantUDP* participant = new ParticipantUDP("127.0.0.1", 7682);
+
+  unsigned long milliseconds = Thing::GetTimeMs();
+  unsigned long startTime = milliseconds;
+  while (milliseconds < startTime + 7000) {
+    participant->Update();
+    
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    milliseconds = Thing::GetTimeMs();
+  }
+  SUCCEED();
 }

-// class RoboidControlSuite : public ::testing::test {
-//   TEST_F(RoboidControlSuite, HiddenParticipant) {
-//     Thing thing = Thing();
-  
-//     unsigned long milliseconds = get_time_ms();
-//     unsigned long startTime = milliseconds;
-//     while (milliseconds < startTime + 1000) {
-//       Thing.Update(milliseconds);
-  
-//       milliseconds = get_time_ms();
-//     }
-//     ASSERT_EQ(1, 1);
-//   }
-// }
+TEST(Participant, SiteServer) {
+  SiteServer* siteServer = new SiteServer();

-class ParticipantSuite : public ::testing::Test {
-protected:
-  // SetUp and TearDown can be used to set up and clean up before/after each
-  // test
-  void SetUp() override {
-    // Initialize test data here
+  unsigned long milliseconds = Thing::GetTimeMs();
+  unsigned long startTime = milliseconds;
+  while (milliseconds < startTime + 7000) {
+    siteServer->Update();
+    
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    milliseconds = Thing::GetTimeMs();
  }
+  SUCCEED();
+}

-  void TearDown() override {
-    // Clean up test data here
+TEST(Participant, SiteParticipant) {
+  SiteServer* siteServer = new SiteServer(7681);
+  ParticipantUDP* participant = new ParticipantUDP("127.0.0.1", 7681, 7682);
+
+  unsigned long milliseconds = Thing::GetTimeMs();
+  unsigned long startTime = milliseconds;
+  while (milliseconds < startTime + 7000) {
+    siteServer->Update();
+    participant->Update();
+
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    milliseconds = Thing::GetTimeMs();
  }
-};
+  SUCCEED();
+}

+TEST(Participant, ThingMsg) {
+  SiteServer* siteServer = new SiteServer(7681);
+  ParticipantUDP* participant = new ParticipantUDP("127.0.0.1", 7681, 7682);
+  Thing* thing = new Thing();
+  thing->SetName("First Thing");
+  thing->SetModel("https://passer.life/extras/ant.jpg");
+  
+  unsigned long milliseconds = Thing::GetTimeMs();
+  unsigned long startTime = milliseconds;
+  while (milliseconds < startTime + 7000) {
+    siteServer->Update();
+    participant->Update();

-
-// TEST_F(ParticipantSuite, LocalParticipant) {
-//   LocalParticipant* participant = new LocalParticipant("127.0.0.1", 7681);
-
-//   unsigned long milliseconds = get_time_ms();
-//   unsigned long startTime = milliseconds;
-//   while (milliseconds < startTime + 1000) {
-//     participant->Update(milliseconds);
-
-//     milliseconds = get_time_ms();
-//   }
-//   ASSERT_EQ(1, 1);
-// }
-
-// TEST_F(ParticipantSuite, SiteServer) {
-//   SiteServer site = SiteServer(7681);
-
-//   unsigned long milliseconds = get_time_ms();
-//   unsigned long startTime = milliseconds;
-//   while (milliseconds < startTime + 1000) {
-//     site.Update(milliseconds);
-
-//     milliseconds = get_time_ms();
-//   }
-//   ASSERT_EQ(1, 1);
-// }
-
-// TEST_F(ParticipantSuite, SiteParticipant) {
-//   SiteServer site = SiteServer(7681);
-//   LocalParticipant participant = LocalParticipant("127.0.0.1", 7681);
-
-//   unsigned long milliseconds = get_time_ms();
-//   unsigned long startTime = milliseconds;
-//   while (milliseconds < startTime + 1000) {
-//     site.Update(milliseconds);
-//     participant.Update(milliseconds);
-
-//     std::this_thread::sleep_for(std::chrono::milliseconds(100));
-//     milliseconds = get_time_ms();
-//   }
-//   ASSERT_EQ(1, 1);
-// }
-
-// TEST_F(ParticipantSuite, Thing) {
-//   SiteServer site = SiteServer(7681);
-//   LocalParticipant participant = LocalParticipant("127.0.0.1", 7681);
-//   Thing thing = Thing(&participant);
-
-//   unsigned long milliseconds = get_time_ms();
-//   unsigned long startTime = milliseconds;
-//   while (milliseconds < startTime + 1000) {
-//     site.Update(milliseconds);
-//     participant.Update(milliseconds);
-
-//     std::this_thread::sleep_for(std::chrono::milliseconds(100));
-//     milliseconds = get_time_ms();
-//   }
-//   ASSERT_EQ(1, 1);
-// }
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    milliseconds = Thing::GetTimeMs();
+  }
+  SUCCEED();
+}

 #endif
--- a/test/thing_test.cc
+++ b/test/thing_test.cc
@ -4,7 +4,7 @@
 // not supported using Visual Studio 2022 compiler...
 #include <gtest/gtest.h>

-#include "LocalParticipant.h"
+#include "Participants/ParticipantUDP.h"
 #include "Thing.h"

 using namespace RoboidControl;
@ -15,7 +15,7 @@ TEST(RoboidControlSuite, HiddenParticipant) {
  unsigned long milliseconds = Thing::GetTimeMs();
  unsigned long startTime = milliseconds;
  while (milliseconds < startTime + 1000) {
-    Thing::UpdateThings(milliseconds);
+    Thing::UpdateThings();

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

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

  unsigned long milliseconds = Thing::GetTimeMs();
  unsigned long startTime = milliseconds;
  while (milliseconds < startTime + 1000) {
-    participant->Update(milliseconds);
+    participant->Update();

    milliseconds = Thing::GetTimeMs();
  }
Author	SHA1	Message	Date
Pascal Serrarens	c95d1cfc06	Merge branch 'main' of https://git.passer.life/RoboidControl/RoboidControl-cpp	2025-06-02 12:26:25 +02:00
Pascal Serrarens	627a79f56c	Merge commit '90e89b2ecd49870e84af4b66fe41610e4855673c'	2025-06-02 12:26:07 +02:00
Pascal Serrarens	c3ba44d47a	Merge commit 'e50c8eb9c89c7fc801ed2c543a1d3970011efb2c'	2025-05-26 15:29:28 +02:00
Pascal Serrarens	e50c8eb9c8	Squashed 'LinearAlgebra/' changes from bfa1123..8b6f628 8b6f628 Fix Matrix unit test 28a3064 Fix tests 91f4280 Reduced the use of raw pointers 83539df Fixes to make it work againg, but failing at udp.begin() 3cca327 Optimizations 361807c Performance improvements 7bd83fa Processomg Acc data * propagating 0eb2851 Linking works 06ff5e9 Added ESP-IDF Wifi/UDP support (but it still give linker errors) 7461a1f Implemented integrate function b7f102a Fix matrix2 tests 7d2dd08 First steps implementation Matrix operations f05b411 Cleanup 05a3b09 Update namespace, Windows compatibility 95a248a First udp communcation is working fea15c3 removed namespace Passer 753de2d Fixes 9674f68 Disabled test failing on MacOS 5b89234 Made it work on MacOS d0ba29e Updated LinearAlgebra ebfb4fb Merge commit '0b63a044eb84175cc922f3f1fe0ce039d101bf7b' 0b63a04 Updated documentation, cleanup a0082bd Add direction documentation 770ce32 Merge commit '2b5f5cd175b20fa01e04b696dfe0cfbe2c4ad9da' 2b5f5cd Removed shorthand Deg/Rad aliases... (see below) d212db2 Trying to fix specialization before instantiation error e445e5b Trying to fix specialization before instantiation error f47e504 Trying to fix specialization before instantiation error b877d4d Trying to fix specialization before instantiation error 06e8623 Trying to fix specialization before instantiation error e97aee9 Trying to fix specialization before instantiation error 0e00e5d Merge branch 'main' of http://gitlab.passervr.com/passer/cpp/linear-algebra 52de55d Trying to fix specialization before instantiation error 47ba024 Trying to fix specialization before instantiation error 2402030 Trying to fix specialization before instantiation error 4e3f253 Fixed gitignore 06c70e2 Merge branch 'Experimental' 6f30334 Updated PolarOf, removed default Polar type 94a3105 Cleanup 89a5711 Fixed typo in documentation 64b7f11 Removed default Angle type 58beb36 Completed AngleOf documentation 5f58a2c Updated documentation 97d937e Fixed (copilot) unit tests 585a3b0 Removed default Spherical type d3226fd Added copilot test documentation a7aa679 Added a performance test 2c57c3f Fixed direction for spherical 28ee225 Normalize Spherical at creation 800eb75 improved rounding of discrete angles 536d6ce Add Binary support 18756ba Correct axis on quaternion from swingtwist 06e42c5 Normalized swing twist edbb4b1 Removes Angle::deg90/180 because of issues 0a07b23 SwingTwist == and Angle 9406b57 SwingTwist::AngleAxis using Direction 06da9e5 Removed AngleAxis, Sperhical will be used instead 2e61e1b Fix unit test 54d0318 Extended unit tests (plus fixes) 8286c1c Minor improvements 95a6fb3 Improved Angle quality 9eca318 Fixed inverse 0ebfce4 Spherical direction 136e44e Fixed unit tests fb5cee8 Proper angle implementation (unit tests still fail) 92d5ef0 Normalizing directions 1ea65d5 Made toQuestion const f1c70c7 Fix unit tests 2ad6a5a Fix include 11259a9 Extend functionality 3363388 Extended AngleAxis & Direction 09e25a8 Add Asin and Atan a646e93 Add ACos 407e771 Add WithDistance 69bad8f Added degrees, radians, cos, sin, and tan functions 5c3dfa6 Adde degrees/readians and poc ACos 05c61a3 Add distance between 6a1d043 Added Angle comparison 6b3bcfc Another anglebetween fix b5a6330 Fix anglebetween b975aed Added angle and rotate functions f483439 Moved Polar/Spherical to template class completely e0ef43a Merge branch 'Experimental' of http://gitlab.passervr.com/passer/cpp/linear-algebra into Experimental b460bec Added scaling to SphericalOf<T> e10e010 Added Direction to library sources dedaa31 Fix unit tests 1da93ca Fix ToVector3 9c3503f Added SphericalOf 353cb1b Add conversion from Quaternion cf86ba8 Cleanup template classes ea6894e Fix generic template functions 1ce7234 Bug fixes 38ebb34 Added Direction to replace Axis c72bba4 Change Vector3::Angle return type to AngleOf<float> 18cf653 Added scaling support 51772a1 Added add/subtract for discrete angles 49c6740 Make Angle -> float conversion explicit 608c45c Fix precision error 4591aef Added Spherical16 48828e2 Fix int/float issues 86b7c21 Added Degrees inline function a4b0491 Fix unit tests b81b77b Extend AngleOf support c70c079 Add spherical subtract 2bad384 Added spherical addition a25a8be Fixed namespace issues f8009a7 Updated namespace 91c7b20 Fix gitlab test runner 78468e3 Renamed VectorAlgebra project to LinearAlgebra 47a6368 Fix Vector3 ee62cba XYZ deprecation 62d7439 Extend ulitity functions 95713c8 Cleanup, added utility functions f7d9d97 Cleanup 66e47d7 Cleanup Vector2 and Polar e922a01 fix == operator 5693097 Initial subtractop test c411b43 Textual improvement 0c54276 equality support b1e34b6 Improved unit tests 6ddb074 Improved unit tests 5489b3c Fix test 3d971c1 Improve namespace 791bd78 namespace improvement 9bea94f Use Angle type for Polar 8a2ce21 Swapped polar constructor param order e5f8d73 namespace fix 8e7a8c6 Fix namespaces 64ca768 normalize toAngleAxis result 4385bef Add angle-axis 4b07328 Add conversion from Vector3 fe12c99 Fix Spherical reference c274c3e Add conversion from Spherical 66ff721 Fixed wrong conversion to short 264bcbc Added 32 bit angle f190dd7 Merge branch 'DiscreteAngle' into Experimental ae55a24 Further vector3 conversion fixes ec0cc47 Fix spherical to vector 89b5da8 Fix ambiguity e62fa96 Fix specialization again 430344a Fix specialization error 3e5ede0 Removed old file e62bacc First Discrete Angle functions 87d2c11 Set strict warnings 5141766 Add spherical.toVector3 test (and fixes) 8e3b9e2 Fix Polar test 7b85556 Addedfirst Polar test 395f82d Fix Spherical Unit test 0cbef79 Add first Spherical Unit test dc601a1 Removed Vector2.tofactor 159bdae Added spherical.tovector (but is is buggy) 273ebae Fixes 0468031 Bug fix 3714507 Implemented templated angles 69e7ee1 Added DiscreteAgle 2cbf259 Make negation virtual override 2b50bab Add negation for DiscreteAngle de3a647 Updated negation 254bb27 Added negation bd04285 Make superclass accessable 2bb0009 Initial implementation git-subtree-dir: LinearAlgebra git-subtree-split: 8b6f628d0c6b9e72260ce1d9c0437124c2d7b6d8	2025-05-26 15:26:44 +02:00
Pascal Serrarens	90e89b2ecd	Squashed 'LinearAlgebra/' changes from bfa1123..72f1472 72f1472 Fix missing <chrono> include 54634f0 Backporting some style changes from Python fea15c3 removed namespace Passer 753de2d Fixes 9674f68 Disabled test failing on MacOS 5b89234 Made it work on MacOS d0ba29e Updated LinearAlgebra ebfb4fb Merge commit '0b63a044eb84175cc922f3f1fe0ce039d101bf7b' 0b63a04 Updated documentation, cleanup a0082bd Add direction documentation 770ce32 Merge commit '2b5f5cd175b20fa01e04b696dfe0cfbe2c4ad9da' 2b5f5cd Removed shorthand Deg/Rad aliases... (see below) d212db2 Trying to fix specialization before instantiation error e445e5b Trying to fix specialization before instantiation error f47e504 Trying to fix specialization before instantiation error b877d4d Trying to fix specialization before instantiation error 06e8623 Trying to fix specialization before instantiation error e97aee9 Trying to fix specialization before instantiation error 0e00e5d Merge branch 'main' of http://gitlab.passervr.com/passer/cpp/linear-algebra 52de55d Trying to fix specialization before instantiation error 47ba024 Trying to fix specialization before instantiation error 2402030 Trying to fix specialization before instantiation error 4e3f253 Fixed gitignore 06c70e2 Merge branch 'Experimental' 6f30334 Updated PolarOf, removed default Polar type 94a3105 Cleanup 89a5711 Fixed typo in documentation 64b7f11 Removed default Angle type 58beb36 Completed AngleOf documentation 5f58a2c Updated documentation 97d937e Fixed (copilot) unit tests 585a3b0 Removed default Spherical type d3226fd Added copilot test documentation a7aa679 Added a performance test 2c57c3f Fixed direction for spherical 28ee225 Normalize Spherical at creation 800eb75 improved rounding of discrete angles 536d6ce Add Binary support 18756ba Correct axis on quaternion from swingtwist 06e42c5 Normalized swing twist edbb4b1 Removes Angle::deg90/180 because of issues 0a07b23 SwingTwist == and Angle 9406b57 SwingTwist::AngleAxis using Direction 06da9e5 Removed AngleAxis, Sperhical will be used instead 2e61e1b Fix unit test 54d0318 Extended unit tests (plus fixes) 8286c1c Minor improvements 95a6fb3 Improved Angle quality 9eca318 Fixed inverse 0ebfce4 Spherical direction 136e44e Fixed unit tests fb5cee8 Proper angle implementation (unit tests still fail) 92d5ef0 Normalizing directions 1ea65d5 Made toQuestion const f1c70c7 Fix unit tests 2ad6a5a Fix include 11259a9 Extend functionality 3363388 Extended AngleAxis & Direction 09e25a8 Add Asin and Atan a646e93 Add ACos 407e771 Add WithDistance 69bad8f Added degrees, radians, cos, sin, and tan functions 5c3dfa6 Adde degrees/readians and poc ACos 05c61a3 Add distance between 6a1d043 Added Angle comparison 6b3bcfc Another anglebetween fix b5a6330 Fix anglebetween b975aed Added angle and rotate functions f483439 Moved Polar/Spherical to template class completely e0ef43a Merge branch 'Experimental' of http://gitlab.passervr.com/passer/cpp/linear-algebra into Experimental b460bec Added scaling to SphericalOf<T> e10e010 Added Direction to library sources dedaa31 Fix unit tests 1da93ca Fix ToVector3 9c3503f Added SphericalOf 353cb1b Add conversion from Quaternion cf86ba8 Cleanup template classes ea6894e Fix generic template functions 1ce7234 Bug fixes 38ebb34 Added Direction to replace Axis c72bba4 Change Vector3::Angle return type to AngleOf<float> 18cf653 Added scaling support 51772a1 Added add/subtract for discrete angles 49c6740 Make Angle -> float conversion explicit 608c45c Fix precision error 4591aef Added Spherical16 48828e2 Fix int/float issues 86b7c21 Added Degrees inline function a4b0491 Fix unit tests b81b77b Extend AngleOf support c70c079 Add spherical subtract 2bad384 Added spherical addition a25a8be Fixed namespace issues f8009a7 Updated namespace 91c7b20 Fix gitlab test runner 78468e3 Renamed VectorAlgebra project to LinearAlgebra 47a6368 Fix Vector3 ee62cba XYZ deprecation 62d7439 Extend ulitity functions 95713c8 Cleanup, added utility functions f7d9d97 Cleanup 66e47d7 Cleanup Vector2 and Polar e922a01 fix == operator 5693097 Initial subtractop test c411b43 Textual improvement 0c54276 equality support b1e34b6 Improved unit tests 6ddb074 Improved unit tests 5489b3c Fix test 3d971c1 Improve namespace 791bd78 namespace improvement 9bea94f Use Angle type for Polar 8a2ce21 Swapped polar constructor param order e5f8d73 namespace fix 8e7a8c6 Fix namespaces 64ca768 normalize toAngleAxis result 4385bef Add angle-axis 4b07328 Add conversion from Vector3 fe12c99 Fix Spherical reference c274c3e Add conversion from Spherical 66ff721 Fixed wrong conversion to short 264bcbc Added 32 bit angle f190dd7 Merge branch 'DiscreteAngle' into Experimental ae55a24 Further vector3 conversion fixes ec0cc47 Fix spherical to vector 89b5da8 Fix ambiguity e62fa96 Fix specialization again 430344a Fix specialization error 3e5ede0 Removed old file e62bacc First Discrete Angle functions 87d2c11 Set strict warnings 5141766 Add spherical.toVector3 test (and fixes) 8e3b9e2 Fix Polar test 7b85556 Addedfirst Polar test 395f82d Fix Spherical Unit test 0cbef79 Add first Spherical Unit test dc601a1 Removed Vector2.tofactor 159bdae Added spherical.tovector (but is is buggy) 273ebae Fixes 0468031 Bug fix 3714507 Implemented templated angles 69e7ee1 Added DiscreteAgle 2cbf259 Make negation virtual override 2b50bab Add negation for DiscreteAngle de3a647 Updated negation 254bb27 Added negation bd04285 Make superclass accessable 2bb0009 Initial implementation git-subtree-dir: LinearAlgebra git-subtree-split: 72f1472f2e1d2c9a1fe002460e47606b9aeed548	2025-05-26 14:04:37 +02:00
Pascal Serrarens	0920f99c1e	Fix tests	2025-05-26 12:27:09 +02:00
pascal	6e860be913	Updated BB2B example	2025-05-26 09:22:59 +02:00
Pascal Serrarens	8620023277	Working controlled motor	2025-05-23 17:57:14 +02:00
Pascal Serrarens	140138977a	AR mode works, but reaction time is slow	2025-05-21 15:59:11 +02:00
Pascal Serrarens	1b5fef15e6	More refactoring to get rid of pointers	2025-05-19 15:58:35 +02:00
Pascal Serrarens	da8831a968	Added controlledMotor, ptr to ref for hierarchy	2025-05-19 08:55:00 +02:00
Pascal Serrarens	5b5513971c	Reduced the use of raw pointers	2025-05-18 17:34:52 +02:00
Pascal Serrarens	3f8eadf8cc	Added Root & new Thing ref. constructor	2025-05-17 18:09:38 +02:00
Pascal Serrarens	17de9db0be	Added rel. encoder, removed currentTime in Update()	2025-05-16 11:54:40 +02:00
Pascal Serrarens	26be4557c5	Added thing reconstruct	2025-05-15 19:52:24 +02:00
Pascal Serrarens	31c725b0ca	BB2B with HW works	2025-05-15 15:59:43 +02:00
Pascal Serrarens	169d842143	Step2 of renaming examples map	2025-05-14 12:40:23 +02:00
Pascal Serrarens	ada8973964	step 1 of changing examples map	2025-05-14 12:39:54 +02:00
Pascal Serrarens	38fd8f51c8	Added readme about the examples folder name	2025-05-14 12:36:06 +02:00
Pascal Serrarens	23ea03fbaf	Ultrasonic TouchSensor subclass	2025-05-14 12:04:29 +02:00
Pascal Serrarens	40aab4dc7a	fix NO_STD issues	2025-05-13 09:38:30 +02:00
Pascal Serrarens	6265daea87	Documented and renamed participant registry	2025-05-01 14:36:09 +02:00
Pascal Serrarens	2d77aa2461	Fix issue multiple local participants	2025-05-01 14:28:14 +02:00
Pascal Serrarens	19310377ff	Added participant tests	2025-05-01 14:28:03 +02:00
Pascal Serrarens	81c5d07e39	Fix include error	2025-05-01 11:06:34 +02:00
Pascal Serrarens	8d778ab41e	Aligned Messages	2025-05-01 11:01:51 +02:00
Pascal Serrarens	db1265a135	Aliged Participants	2025-04-30 17:00:01 +02:00
Pascal Serrarens	4db9164f2a	Completed things docs	2025-04-30 11:40:44 +02:00
Pascal Serrarens	75a0cf350d	Aligned Thing implementation	2025-04-29 11:47:15 +02:00
Pascal Serrarens	478d6d9431	Unit test fixes	2025-04-28 18:14:03 +02:00
Pascal Serrarens	df1a769d10	Align participant documentation	2025-04-28 18:13:46 +02:00
Pascal Serrarens	fe69419010	Binary msg sending fix	2025-04-23 17:50:40 +02:00
Pascal Serrarens	693c9b8b33	Object creation/destruction works	2025-04-23 15:05:38 +02:00
Pascal Serrarens	a292179f87	Steps toward hierarchy changes	2025-04-23 12:47:40 +02:00
Pascal Serrarens	e56f25f9a1	Improved network sync	2025-04-22 17:47:06 +02:00
Pascal Serrarens	619695c90d	static participant list	2025-04-22 11:59:52 +02:00
Pascal Serrarens	80127e0041	Merge commit '66793370d4bd5c4bc8439bf0e02a1f7e99816d03' into RoboidControl	2025-04-22 09:39:09 +02:00
Pascal Serrarens	045826d3c2	Fixed parenting	2025-04-18 17:24:33 +02:00
Pascal Serrarens	8ff0fdd78f	Fix crash at udp.begin()	2025-04-18 14:36:07 +02:00
Pascal Serrarens	a17c933908	Fixes to make it work againg, but failing at udp.begin()	2025-04-17 12:50:49 +02:00
Pascal Serrarens	0317ac5094	Merge commit '726f761d1ce08e886c6ec1808f2c427490b397f1' into RoboidControl	2025-04-17 12:50:28 +02:00
Pascal Serrarens	32ca8f5f71	Improve debug logging	2025-04-11 11:55:23 +02:00
Pascal Serrarens	304856ea0a	Improve ESP-IDF defines	2025-04-11 11:23:54 +02:00
Pascal Serrarens	1aed9856aa	Added sendmsg debug messages	2025-04-11 11:15:20 +02:00
Pascal Serrarens	122d087f81	Added sendmsg debug	2025-04-11 11:06:21 +02:00
Pascal Serrarens	e92ecef46f	moved from broadcasting to unicasting	2025-04-11 08:35:58 +02:00
Pascal Serrarens	92be21a9c1	Communication improvements, adding gyro msg	2025-04-10 17:35:06 +02:00
Pascal Serrarens	28e42df771	Added gyroscope	2025-04-09 17:45:55 +02:00
Pascal Serrarens	e0303ff369	Optimizations	2025-04-09 16:07:49 +02:00
Pascal Serrarens	5fddebd1bc	Performance improvements	2025-04-09 12:31:46 +02:00
Pascal Serrarens	2062e4a71c	Pose is synchronized to de sim.env.	2025-04-09 11:07:25 +02:00
Pascal Serrarens	aede0e5cd3	Processomg Acc data * propagating	2025-04-08 17:25:29 +02:00
Pascal Serrarens	bb33724873	Merge commit '12d91378e5fe126b85141123aefb27a4c33f96e2'	2025-04-08 12:55:26 +02:00
Pascal Serrarens	a1fc21dfe1	Receiving no longer crashes	2025-04-08 12:48:52 +02:00
Pascal Serrarens	12d91378e5	Fix refactoring issues	2025-04-08 10:43:20 +02:00
Pascal Serrarens	b594bd59f4	Refactoring	2025-04-08 09:49:52 +02:00
Pascal Serrarens	6353af4a29	Refactoring	2025-04-07 08:35:16 +02:00
Pascal Serrarens	b32ac68966	Simulated Accelerometer can be received	2025-04-03 17:57:08 +02:00
Pascal Serrarens	7b17d8459a	Basic things seems to work, now start doing the tracker stuff	2025-04-03 12:54:40 +02:00
Pascal Serrarens	afc48a1438	Receiving works	2025-04-03 12:36:23 +02:00
Pascal Serrarens	8130a057ea	Sending messages works	2025-04-03 11:41:33 +02:00
Pascal Serrarens	a5cd5c89d3	Wifi connection is set up	2025-04-03 09:40:59 +02:00
Pascal Serrarens	0c826d24be	Linking works	2025-04-02 17:25:11 +02:00
Pascal Serrarens	42d3600cd8	Added ESP-IDF Wifi/UDP support (but it still give linker errors)	2025-04-02 12:45:59 +02:00
Pascal Serrarens	39abc0247b	Implemented integrate function	2025-04-02 10:03:31 +02:00
				`@ -0,0 +1 @@`
				`Important: this folder has to be names 'examples' exactly to maintain compatibility with Arduino`