Arduino/Things/DRV8833.cpp

@@ -7,9 +7,7 @@ namespace Arduino {
 
 #pragma region DRV8833
 
-DRV8833::DRV8833(Configuration config, Thing* parent) : Thing(parent) {
-  this->type = Type::Undetermined;
-  this->name = "DRV8833";
+DRV8833::DRV8833(Configuration config, Thing* parent) : Thing(Type::Undetermined, parent) {
   this->pinStandby = config.standby;
   if (pinStandby != 255)
     pinMode(pinStandby, OUTPUT);

Arduino/Things/DigitalInput.cpp

@@ -7,9 +7,8 @@ namespace Arduino {
 
 #pragma region Digital input
 
-DigitalInput::DigitalInput(unsigned char pin, Thing* parent) : Thing(parent) {
-  this->type = Type::Switch;
-  this->name = "Digital Input";
+DigitalInput::DigitalInput(unsigned char pin, Thing* parent)
+    : Thing(Type::Undetermined, parent) {
   this->pin = pin;
   pinMode(this->pin, INPUT);
   std::cout << "digital input start\n";

Arduino/Things/UltrasonicSensor.cpp

@@ -7,8 +7,7 @@ namespace RoboidControl {
 namespace Arduino {
 
 UltrasonicSensor::UltrasonicSensor(Configuration config, Thing* parent)
-    : Thing(parent) {
-      this->type = Type::DistanceSensor;
+    : Thing(Type::Undetermined, parent) {
   this->name = "Ultrasonic sensor";
   this->pinTrigger = config.trigger;
   this->pinEcho = config.echo;

Participant.cpp

@@ -1,10 +1,6 @@
 #include "Participant.h"
 
 #include <string.h>
-#include "Arduino/ArduinoParticipant.h"
-#include "EspIdf/EspIdfParticipant.h"
-#include "Posix/PosixParticipant.h"
-#include "Windows/WindowsParticipant.h"
 
 namespace RoboidControl {
 
@@ -20,13 +16,17 @@ void Participant::ReplaceLocalParticipant(Participant& newParticipant) {
 }
 
 Participant::Participant() {
-  Thing::CreateRoot(this);
-  //this->Add(this->root);
+  //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) {
-  Thing::CreateRoot(this);
-  //this->Add(this->root);
+  // 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);
@@ -56,34 +56,6 @@ void Participant::Update() {
   }
 }
 
-bool Participant::Send(IMessage* msg) {
-  int bufferSize = msg->Serialize(this->buffer);
-  if (bufferSize <= 0)
-    return true;
-
-  // std::cout << "send msg " << (static_cast<int>(this->buffer[0]) & 0xff)
-  //           << " to " << 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(this, bufferSize);
-#elif defined(IDF_VER)
-  EspIdf::ParticipantUDP* thisEspIdf =
-      static_cast<EspIdf::ParticipantUDP*>(this);
-  return thisEspIdf->Send(remoteParticipant, bufferSize);
-#else
-  return false;
-#endif
-}
-
 Thing* Participant::Get(unsigned char thingId) {
   for (Thing* thing : this->things) {
     if (thing->id == thingId)

Participant.h

@@ -1,6 +1,5 @@
 #pragma once
 
-#include "Messages/IMessage.h"
 #include "Thing.h"
 
 namespace RoboidControl {
@@ -60,48 +59,30 @@ class ParticipantRegistry {
 /// participant. It is used as a basis for the local participant, but also as a
 /// reference to remote participants.
 class Participant {
-#pragma region Init
-
- public:
-  /// @brief Create a generic participant
-  Participant();
-  /// @brief Create a new participant with the given communcation info
-  /// @param ipAddress The IP address of the participant
-  /// @param port The UDP port of the participant
-  /// @remarks This does not belong here, it should move to ParticipantUDP or
-  /// something like that in the future
-  Participant(const char* ipAddress, int port);
-  /// @brief Destructor for the participant
-  ~Participant();
-
-  /// @brief The local participant for this application
-  static Participant* LocalParticipant;
-  /// @brief Replace the local participant
-  /// @param newParticipant The new local Participant
-  static void ReplaceLocalParticipant(Participant& newParticipant);
-
-#pragma endregion Init
-
-#pragma region Properties
-
  public:
   /// @brief The name of the participant
   const char* name = "Participant";
 
   /// @brief The Ip Address of a participant.
-  /// @remarks This does not belong here, it should move to ParticipantUDP or
-  /// something like that in the future
   const char* ipAddress = "0.0.0.0";
   /// @brief The port number for UDP communication with the participant.
-  /// @remarks This does not belong here, it should move to ParticipantUDP or
-  /// something like that in the future
   unsigned int port = 0;
 
   /// @brief The network Id to identify the participant
   unsigned char networkId = 0;
 
-  /// @brief The root thing for this participant
-  Thing* root = nullptr;
+  Participant();
+  /// @brief Create a new participant with the given communcation info
+  /// @param ipAddress The IP address of the participant
+  /// @param port The UDP port of the participant
+  Participant(const char* ipAddress, int port);
+  /// @brief Destructor for the participant
+  ~Participant();
+
+  static Participant* LocalParticipant;
+  static void ReplaceLocalParticipant(Participant& newParticipant);
+
+  Thing* root = new Thing(this);
 
  public:
 #if defined(NO_STD)
@@ -123,31 +104,12 @@ class Participant {
   /// @param thing The thing to remove
   void Remove(Thing* thing);
 
-#pragma endregion Properties
-
-#pragma region Update
-
- public:
   /// @brief Update all things for this participant
+  /// @param currentTimeMs The current time in milliseconds (optional)
   virtual void Update();
 
-#pragma endregion Update
-
-#pragma region Send
-
- public:
-  char buffer[1024];
-
-  virtual bool Send(IMessage* msg);
-
-#pragma endregion Send
-
-#pragma region Participant Registry
-
  public:
   static ParticipantRegistry registry;
-
-#pragma endregion Participant Registry
 };
 
 }  // namespace RoboidControl

Participants/ParticipantUDP.cpp

@@ -46,6 +46,14 @@ ParticipantUDP::ParticipantUDP(const char* ipAddress, int port, int localPort)
   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;
@@ -97,8 +105,7 @@ void ParticipantUDP::Update() {
       if (this->remoteSite == nullptr)
         this->Publish(msg);
       else
-        this->remoteSite->Send(msg);
-
+        this->Send(this->remoteSite, msg);
       delete msg;
 
       this->nextPublishMe = currentTimeMs + this->publishInterval;
@@ -115,7 +122,7 @@ void ParticipantUDP::PrepMyThings() {
   for (Thing* thing : this->things) {
     if (thing == nullptr)
       continue;
-
+      
     thing->PrepareForUpdate();
   }
 }
@@ -130,12 +137,12 @@ void ParticipantUDP::UpdateMyThings() {
     if (thing->hierarchyChanged) {
       if (!(this->isIsolated || this->networkId == 0)) {
         ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
-        this->remoteSite->Send(thingMsg);
+        this->Send(this->remoteSite, thingMsg);
         delete thingMsg;
 
         if (thing->nameChanged) {
           NameMsg* nameMsg = new NameMsg(this->networkId, thing);
-          this->remoteSite->Send(nameMsg);
+          this->Send(this->remoteSite, nameMsg);
           delete nameMsg;
         }
       }
@@ -152,20 +159,20 @@ void ParticipantUDP::UpdateMyThings() {
     if (!(this->isIsolated || this->networkId == 0)) {
       if (thing->terminate) {
         DestroyMsg* destroyMsg = new DestroyMsg(this->networkId, thing);
-        this->remoteSite->Send(destroyMsg);
+        this->Send(this->remoteSite, destroyMsg);
         delete destroyMsg;
       } else {
         //  Send to remote site
         if (thing->nameChanged) {
           NameMsg* nameMsg = new NameMsg(this->networkId, thing);
-          this->remoteSite->Send(nameMsg);
+          this->Send(this->remoteSite, nameMsg);
           delete nameMsg;
         }
         PoseMsg* poseMsg = new PoseMsg(this->networkId, thing);
-        this->remoteSite->Send(poseMsg);
+        this->Send(this->remoteSite, poseMsg);
         delete poseMsg;
         BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing);
-        this->remoteSite->Send(binaryMsg);
+        this->Send(this->remoteSite, binaryMsg);
         delete binaryMsg;
       }
     }
@@ -196,10 +203,10 @@ void ParticipantUDP::UpdateOtherThings() {
 
     for (Thing* thing : participant->things) {
       PoseMsg* poseMsg = new PoseMsg(participant->networkId, thing);
-      participant->Send(poseMsg);
+      this->Send(participant, poseMsg);
       delete poseMsg;
       BinaryMsg* binaryMsg = new BinaryMsg(participant->networkId, thing);
-      participant->Send(binaryMsg);
+      this->Send(participant, binaryMsg);
       delete binaryMsg;
     }
   }
@@ -214,50 +221,49 @@ void ParticipantUDP::SendThingInfo(Participant* remoteParticipant,
                                    Thing* thing) {
   // std::cout << "Send thing info [" << (int)thing->id << "] \n";
   ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
-  remoteParticipant->Send(thingMsg);
+  this->Send(remoteParticipant, thingMsg);
   delete thingMsg;
   NameMsg* nameMsg = new NameMsg(this->networkId, thing);
-  remoteParticipant->Send(nameMsg);
+  this->Send(remoteParticipant, nameMsg);
   delete nameMsg;
   ModelUrlMsg* modelMsg = new ModelUrlMsg(this->networkId, thing);
-  remoteParticipant->Send(modelMsg);
+  this->Send(remoteParticipant, modelMsg);
   delete modelMsg;
   PoseMsg* poseMsg = new PoseMsg(this->networkId, thing, true);
-  remoteParticipant->Send(poseMsg);
+  this->Send(remoteParticipant, poseMsg);
   delete poseMsg;
-  BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing);
-  remoteParticipant->Send(binaryMsg);
-  delete binaryMsg;
+  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;
+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;
+  // 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
-// }
+#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";
@@ -396,18 +402,6 @@ void ParticipantUDP::ReceiveData(unsigned char bufferSize,
       Process(sender, msg);
       delete msg;
     } break;
-    case TextMsg::id: {
-      TextMsg* msg = new TextMsg(this->buffer);
-      bufferSize -= msg->length + msg->textLength;
-      Process(sender, msg);
-      delete msg;
-    } break;
-    case DestroyMsg::id: {
-      DestroyMsg* msg = new DestroyMsg(this->buffer);
-      bufferSize -= msg->length;
-      Process(sender, msg);
-      delete msg;
-    } break;
   };
 
 // Check if the buffer has been read completely
@@ -538,24 +532,6 @@ void ParticipantUDP::Process(Participant* sender, BinaryMsg* msg) {
   }
 }
 
-void ParticipantUDP::Process(Participant* sender, TextMsg* msg) {
-#if defined(DEBUG)
-  std::cout << this->name << ": process TextMsg " << (int)msg->textLength << " "
-            << (int)msg->text << "\n";
-#endif
-}
-
-void ParticipantUDP::Process(Participant* sender, DestroyMsg* msg) {
-#if defined(DEBUG)
-  std::cout << this->name << ": process Destroy [" << (int)msg->networkId << "/"
-            << (int)msg->thingId << "]\n";
-#endif
-
-  Thing* thing = sender->Get(msg->thingId);
-  if (thing != nullptr)
-    this->Remove(thing);
-}
-
 // Receive
 #pragma endregion
 

Participants/ParticipantUDP.h

@@ -9,7 +9,6 @@
 #include "Messages/PoseMsg.h"
 #include "Messages/NetworkIdMsg.h"
 #include "Messages/ThingMsg.h"
-#include "Messages/TextMsg.h"
 #include "Participant.h"
 
 #if !defined(NO_STD)
@@ -44,7 +43,6 @@ constexpr int MAX_SENDER_COUNT = 256;
 /// RoboidControl::IsolatedParticipant::Isolated().
 /// @sa RoboidControl::Thing::Thing()
 class ParticipantUDP : public Participant {
-
 #pragma region Init
 
  public:
@@ -60,15 +58,19 @@ class ParticipantUDP : public Participant {
   /// @param localPort The port used by the local participant
   ParticipantUDP(const char* ipAddress, int port = 7681, int localPort = 7681);
 
+  /// @brief Isolated participant is used when the application is run without
+  /// networking
+  /// @return A participant without networking support
+  static ParticipantUDP* Isolated();
+
+  /// @brief True if the participant is running isolated.
+  /// Isolated participants do not communicate with other participants
+
 #pragma endregion Init
 
-#pragma region Properties
-
-public:
   /// @brief True if the participant is running isolated.
   /// Isolated participants do not communicate with other participants
   bool isIsolated = false;
-
   /// @brief The remote site when this participant is connected to a site
   Participant* remoteSite = nullptr;
 
@@ -92,8 +94,6 @@ public:
   void begin();
   bool connected = false;
 
-#pragma endregion Properties
-
 #pragma region Update
 
  public:
@@ -114,7 +114,7 @@ public:
   void SendThingInfo(Participant* remoteParticipant, Thing* thing);
   void PublishThingInfo(Thing* thing);
 
-  //bool Send(Participant* remoteParticipant, IMessage* msg);
+  bool Send(Participant* remoteParticipant, IMessage* msg);
   bool Publish(IMessage* msg);
 
 #pragma endregion Send
@@ -139,8 +139,6 @@ protected:
   virtual void Process(Participant* sender, ModelUrlMsg* msg);
   virtual void Process(Participant* sender, PoseMsg* msg);
   virtual void Process(Participant* sender, BinaryMsg* msg);
-  virtual void Process(Participant* sender, TextMsg* msg);
-  virtual void Process(Participant* sender, DestroyMsg* msg);
 
 #pragma endregion Receive
 

Participants/SiteServer.cpp

@@ -42,10 +42,10 @@ void SiteServer::UpdateMyThings() {
           continue;
 
         PoseMsg* poseMsg = new PoseMsg(this->networkId, thing);
-        participant->Send(poseMsg);
+        this->Send(participant, poseMsg);
         delete poseMsg;
         BinaryMsg* binaryMsg = new BinaryMsg(this->networkId, thing);
-        participant->Send(binaryMsg);
+        this->Send(participant, binaryMsg);
         delete binaryMsg;
       }
     }
@@ -62,7 +62,7 @@ void SiteServer::Process(Participant* sender, ParticipantMsg* msg) {
     // sender->ipAddress
     //           << ":" << (int)sender->port << "\n";
     NetworkIdMsg* msg = new NetworkIdMsg(sender->networkId);
-    sender->Send(msg);
+    this->Send(sender, msg);
     delete msg;
   }
 }

Participants/SiteServer.h

@@ -2,11 +2,11 @@
 
 #include "ParticipantUDP.h"
 
-// #if !defined(NO_STD)
-// #include <functional>
-// #include <memory>
-// #include <unordered_map>
-// #endif
+#if !defined(NO_STD)
+#include <functional>
+#include <memory>
+#include <unordered_map>
+#endif
 
 namespace RoboidControl {
 

Thing.cpp

@@ -16,277 +16,337 @@
 #include <list>
 #endif
 
-namespace RoboidControl {
+namespace RoboidControl
+{
 
 #pragma region Init
 
-Thing* Thing::LocalRoot() {
-  Participant* p = Participant::LocalParticipant;
-  Thing* localRoot = p->root;
-  return localRoot;
-}
+  Thing *Thing::LocalRoot()
+  {
+    Participant *p = Participant::LocalParticipant;
+    Thing *localRoot = p->root;
+    return localRoot;
+  }
 
-// Only use this for root things
-Thing::Thing(Participant* owner) {
-  this->type = Type::Root;
-  this->name = "Root";
+  // Only use this for root things
+  Thing::Thing(Participant *owner)
+  {
+    this->type = Type::Root; // should become root
 
-  this->position = Spherical::zero;
-  this->positionUpdated = true;
-  this->orientation = SwingTwist::identity;
-  this->orientationUpdated = true;
-  this->hierarchyChanged = true;
+    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;
+    this->linearVelocity = Spherical::zero;
+    this->angularVelocity = Spherical::zero;
 
-  this->owner = owner;
-  this->owner->Add(this);
-  // std::cout << this->owner->name << ": New root thing " << std::endl;
-}
+    this->owner = owner;
+    // this->owner->Add(this, true);
+    //std::cout << this->owner->name << ": New root thing " << std::endl;
+  }
 
-void Thing::CreateRoot(Participant* owner) {
-  owner->root = new Thing(owner);
-}
+  Thing::Thing(Thing *parent)
+  {
+    this->type = Type::Undetermined;
 
-Thing::Thing(Thing* parent) {
-  this->type = Type::Undetermined;
+    this->position = Spherical::zero;
+    this->positionUpdated = true;
+    this->orientation = SwingTwist::identity;
+    this->orientationUpdated = true;
+    this->hierarchyChanged = true;
 
-  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;
 
-  this->linearVelocity = Spherical::zero;
-  this->angularVelocity = Spherical::zero;
+    this->owner = parent->owner;
+    this->owner->Add(this, true);
+    this->SetParent(parent);
 
-  this->owner = parent->owner;
-  this->owner->Add(this, true);
-  this->SetParent(parent);
+    std::cout << this->owner->name << ": New thing for " << parent->name
+              << std::endl;
+  }
 
-  std::cout << this->owner->name << ": New thing for " << parent->name
-            << std::endl;
-}
-
-Thing::~Thing() {
-  std::cout << "Destroy thing " << this->name << std::endl;
-}
+  Thing::~Thing()
+  {
+    std::cout << "Destroy thing " << this->name << std::endl;
+  }
 
 #pragma endregion Init
 
-void Thing::SetName(const char* name) {
-  this->name = name;
-  this->nameChanged = true;
-}
+  void Thing::SetName(const char *name)
+  {
+    this->name = name;
+    this->nameChanged = true;
+  }
 
-const char* Thing::GetName() const {
-  return this->name;
-}
+  const char *Thing::GetName() const
+  {
+    return this->name;
+  }
 
-void Thing::SetModel(const char* url) {
-  this->modelUrl = url;
-}
+  void Thing::SetModel(const char *url)
+  {
+    this->modelUrl = url;
+  }
 
 #pragma region Hierarchy
 
-void Thing::SetParent(Thing* parent) {
-  if (parent == nullptr) {
-    Thing* parentThing = this->parent;
-    if (parentThing != nullptr)
-      parentThing->RemoveChild(this);
-    this->parent = nullptr;
-  } else
-    parent->AddChild(this);
-  this->hierarchyChanged = true;
-}
-
-bool Thing::IsRoot() const {
-  return this == LocalRoot() || this->parent == nullptr;
-}
-
-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];
-
-  for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
-    newChildren[childIx] = this->children[childIx];
-    if (this->children[childIx] == child) {
-      // child is already present, stop copying do not update the children
-      delete[] newChildren;
-      return;
+  void Thing::SetParent(Thing *parent)
+  {
+    if (parent == nullptr)
+    {
+      Thing *parentThing = this->parent;
+      if (parentThing != nullptr)
+        parentThing->RemoveChild(this);
+      this->parent = nullptr;
     }
+    else
+      parent->AddChild(this);
+    this->hierarchyChanged = true;
   }
 
-  newChildren[this->childCount] = child;
-  child->parent = this;
+  // void Thing::SetParent(Thing* parent) {
+  //   parent->AddChild(this);
+  //   this->hierarchyChanged = true;
+  // }
 
-  if (this->children != nullptr)
-    delete[] this->children;
+  // const Thing& Thing::GetParent() {
+  //   return *this->parent;
+  // }
 
-  this->children = newChildren;
-  this->childCount = newChildCount;
-}
+  bool Thing::IsRoot() const
+  {
+    return this == LocalRoot() || this->parent == nullptr; //&Thing::Root;
+  }
 
-Thing* Thing::RemoveChild(Thing* child) {
-  unsigned char newChildCount = this->childCount - 1;
-  Thing** newChildren = new Thing*[newChildCount];
+  // void Thing::SetParent(Thing* root, const char* name) {
+  //   Thing* thing = root->FindChild(name);
+  //   if (thing != nullptr)
+  //     this->SetParent(thing);
+  // }
 
-  unsigned char newChildIx = 0;
-  for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
-    if (this->children[childIx] != child) {
-      if (newChildIx == newChildCount) {  // We did not find the child
-        // stop copying and return nothing
+  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];
+
+    for (unsigned char childIx = 0; childIx < this->childCount; childIx++)
+    {
+      newChildren[childIx] = this->children[childIx];
+      if (this->children[childIx] == child)
+      {
+        // child is already present, stop copying do not update the children
         delete[] newChildren;
-        return nullptr;
-      } else
-        newChildren[newChildIx++] = this->children[childIx];
+        return;
+      }
     }
+
+    newChildren[this->childCount] = child;
+    child->parent = this;
+
+    if (this->children != nullptr)
+      delete[] this->children;
+
+    this->children = newChildren;
+    this->childCount = newChildCount;
   }
 
-  child->parent = Thing::LocalRoot();
+  Thing *Thing::RemoveChild(Thing *child)
+  {
+    unsigned char newChildCount = this->childCount - 1;
+    Thing **newChildren = new Thing *[newChildCount];
 
-  delete[] this->children;
-  this->children = newChildren;
-  this->childCount = newChildCount;
+    unsigned char newChildIx = 0;
+    for (unsigned char childIx = 0; childIx < this->childCount; childIx++)
+    {
+      if (this->children[childIx] != child)
+      {
+        if (newChildIx == newChildCount)
+        { // We did not find the child
+          // stop copying and return nothing
+          delete[] newChildren;
+          return nullptr;
+        }
+        else
+          newChildren[newChildIx++] = this->children[childIx];
+      }
+    }
 
-  return child;
-}
+    child->parent = Thing::LocalRoot();
 
-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)
-      continue;
-    if (child->id == id)
-      return child;
+    delete[] this->children;
+    this->children = newChildren;
+    this->childCount = newChildCount;
 
-    if (recurse) {
-      Thing* foundChild = child->GetChild(id, recurse);
+    return child;
+  }
+
+  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)
+        continue;
+      if (child->id == id)
+        return child;
+
+      if (recurse)
+      {
+        Thing *foundChild = child->GetChild(id, recurse);
+        if (foundChild != nullptr)
+          return foundChild;
+      }
+    }
+    return nullptr;
+  }
+
+  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;
+
+      if (strcmp(child->name, name) == 0)
+        return child;
+
+      Thing *foundChild = child->FindChild(name);
       if (foundChild != nullptr)
         return foundChild;
     }
+    return nullptr;
   }
-  return nullptr;
-}
-
-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;
-
-    if (strcmp(child->name, name) == 0)
-      return child;
-
-    Thing* foundChild = child->FindChild(name);
-    if (foundChild != nullptr)
-      return foundChild;
-  }
-  return nullptr;
-}
 
 #pragma endregion Hierarchy
 
 #pragma region Pose
 
-void Thing::SetPosition(Spherical position) {
-  this->position = position;
-  this->positionUpdated = true;
-}
-Spherical Thing::GetPosition() {
-  return this->position;
-}
-
-void Thing::SetOrientation(SwingTwist orientation) {
-  this->orientation = orientation;
-  this->orientationUpdated = true;
-}
-
-SwingTwist Thing::GetOrientation() {
-  return this->orientation;
-}
-
-void Thing::SetLinearVelocity(Spherical linearVelocity) {
-  if (this->linearVelocity.distance != linearVelocity.distance) {
-    this->linearVelocity = linearVelocity;
-    this->linearVelocityUpdated = true;
+  void Thing::SetPosition(Spherical position)
+  {
+    this->position = position;
+    this->positionUpdated = true;
   }
-}
-
-Spherical Thing::GetLinearVelocity() {
-  return this->linearVelocity;
-}
-
-void Thing::SetAngularVelocity(Spherical angularVelocity) {
-  if (this->angularVelocity.distance != angularVelocity.distance) {
-    this->angularVelocity = angularVelocity;
-    this->angularVelocityUpdated = true;
+  Spherical Thing::GetPosition()
+  {
+    return this->position;
   }
-}
 
-Spherical Thing::GetAngularVelocity() {
-  return this->angularVelocity;
-}
+  void Thing::SetOrientation(SwingTwist orientation)
+  {
+    this->orientation = orientation;
+    this->orientationUpdated = true;
+  }
+
+  SwingTwist Thing::GetOrientation()
+  {
+    return this->orientation;
+  }
+
+  void Thing::SetLinearVelocity(Spherical linearVelocity)
+  {
+    if (this->linearVelocity.distance != linearVelocity.distance)
+    {
+      this->linearVelocity = linearVelocity;
+      this->linearVelocityUpdated = true;
+    }
+  }
+
+  Spherical Thing::GetLinearVelocity()
+  {
+    return this->linearVelocity;
+  }
+
+  void Thing::SetAngularVelocity(Spherical angularVelocity)
+  {
+    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() {
+  unsigned long Thing::GetTimeMs()
+  {
 #if defined(ARDUINO)
-  unsigned long ms = millis();
-  return ms;
+    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());
+    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(bool recursive) {
+  //   Update(GetTimeMs(), recursive);
+  // }
 
-void Thing::PrepareForUpdate() {}
+  void Thing::PrepareForUpdate() {}
 
-void Thing::Update(bool recursive) {
-  this->positionUpdated = false;
-  this->orientationUpdated = false;
-  this->linearVelocityUpdated = false;
-  this->angularVelocityUpdated = false;
-  this->hierarchyChanged = false;
-  this->nameChanged = false;
+  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);
+    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;
-};
+  int Thing::GenerateBinary(char *buffer, unsigned char *ix)
+  {
+    (void)buffer;
+    (void)ix;
+    return 0;
+  }
+  void Thing::ProcessBinary(char *bytes)
+  {
+    (void)bytes;
+  };
 
-}  // namespace RoboidControl
+} // namespace RoboidControl

Thing.h

@@ -43,35 +43,32 @@ class Thing {
   };
 
 #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
+  /// @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 connected to the root thing.
+  /// without a parent will be a root thing.
   Thing(Thing* parent = LocalRoot());
 
- private:
-  /// @brief Constructor to create a root thing
-  /// @param owner The participant who will own this root thing
-  /// @remarks This function is private because CreateRoot() should be used
-  /// instead
-  Thing(Participant* owener);
+  /// @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
 
- public:
-  /// @brief Destructor for a Thing
   ~Thing();
 
-  /// @brief Create a root thing for a participant
-  /// @param owner The participant who will own this root thing
-  static void CreateRoot(Participant* owner);
-
-  /// @brief The root thing for the local participant
-  /// @return The root thing for the local participant
-  static Thing* LocalRoot();
-
 #pragma endregion Init
 
  public:
@@ -80,7 +77,9 @@ class Thing {
 
 #pragma region Properties
 
- public:
+  /// @brief The participant managing this thing
+  Participant* owner = nullptr;
+
   /// @brief The ID of the thing
   unsigned char id = 0;
 
@@ -88,12 +87,10 @@ class Thing {
   /// This can be either a Thing::Type of a byte value for custom types
   unsigned char type = Type::Undetermined;
 
-  /// @brief The participant owning this thing
-  Participant* owner = nullptr;
-
   /// @brief The name of the thing
   const char* name = nullptr;
 
+ public:
   void SetName(const char* name);
   const char* GetName() const;
   bool nameChanged = false;
@@ -102,12 +99,14 @@ class Thing {
   /// loaded from
   /// @param url The url of the model
   /// @remark Although the roboid implementation is not dependent on the model,
-  /// the only official supported model formats are .png (sprite), .gltf and .glb
+  /// 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
 
@@ -115,13 +114,13 @@ class Thing {
 
   /// @brief Sets the parent of this Thing
   /// @param parent The Thing which should become the parent
+  // virtual void SetParent(Thing* parent);
   void SetParent(Thing* parent);
   /// @brief Gets the parent of this Thing
   /// @return The parent Thing
+  // Thing* GetParent();
   Thing* GetParent();
 
-  /// @brief Check if this is a root thing
-  /// @return True is this thing is a root
   bool IsRoot() const;
 
   /// @brief The number of children
@@ -226,9 +225,13 @@ class Thing {
   virtual void PrepareForUpdate();
 
   /// @brief Updates the state of the thing
+  /// @param currentTimeMs The current clock time in milliseconds; if this is
+  /// zero, the current time is retrieved automatically
   /// @param recurse When true, this will Update the descendants recursively
   virtual void Update(bool recurse = false);
 
+  static void UpdateThings();
+
   /// @brief Get the current time in milliseconds
   /// @return The current time in milliseconds
   static unsigned long GetTimeMs();