From 51e605ca6d43ad9323a14abe4a2c472d5adcebb0 Mon Sep 17 00:00:00 2001
From: Pascal Serrarens <pascal@passervr.com>
Date: Thu, 14 Nov 2024 11:29:08 +0100
Subject: [PATCH] Thing Name support

---
 NetworkSync.cpp | 44 ++++++++++++++++++++++++--------------------
 NetworkSync.h   |  2 +-
 Roboid.cpp      |  2 --
 Roboid.h        |  3 ---
 Thing.cpp       |  2 ++
 Thing.h         |  3 +++
 6 files changed, 30 insertions(+), 26 deletions(-)

diff --git a/NetworkSync.cpp b/NetworkSync.cpp
index a74409d..ad4697c 100644
--- a/NetworkSync.cpp
+++ b/NetworkSync.cpp
@@ -108,9 +108,13 @@ void NetworkSync::PublishRelativeThing(Thing *thing, bool recurse) {
     buffer[ix++] = parentThing->id;
   else
     buffer[ix++] = 0x00;
-  SendSpherical16(buffer, &ix, thing->position);
+  SendSpherical16(
+      buffer, &ix,
+      thing->position); // Do we need this if we already send a pose?
+
   SendBuffer(ix);
 
+  SendName(thing);
   SendModel(thing);
 
   if (recurse) {
@@ -122,27 +126,27 @@ void NetworkSync::PublishRelativeThing(Thing *thing, bool recurse) {
   }
 }
 
-void NetworkSync::SendName(Roboid *roboid) {
-  if (roboid->name == nullptr)
+void NetworkSync::SendName(Thing *thing) {
+  if (thing->name == nullptr)
     return;
 
-  unsigned char len = strlen(roboid->name);
+  unsigned char len = strlen(thing->name);
   if (len > 255)
     return;
 
   unsigned char ix = 0;
   buffer[ix++] = NameMsg;
-  buffer[ix++] = 0x00; // objectId
+  buffer[ix++] = thing->id;
 
   buffer[ix++] = len;
   for (unsigned char nameIx = 0; nameIx < len; nameIx++)
-    buffer[ix++] = roboid->name[nameIx];
+    buffer[ix++] = thing->name[nameIx];
 
   SendBuffer(ix);
 
 #ifdef RC_DEBUG
   SERIALPORT.printf("Sent Name [%d/%d] %s\n", networkId, buffer[1],
-                    roboid->name);
+                    thing->name);
 #endif
 }
 
@@ -208,22 +212,22 @@ void NetworkSync::SendPose(Thing *thing, bool recurse) {
   if (this->networkId == 0) // We're not connected to a site yet
     return;
 
-  if (thing->GetLinearVelocity().distance > 0 ||
-      thing->GetAngularVelocity().distance > 0) {
-    unsigned char ix = 0;
-    buffer[ix++] = PoseMsg;
-    buffer[ix++] = thing->id;
-    buffer[ix++] = Pose_Position | Pose_Orientation;
-    SendSpherical16(buffer, &ix, thing->position);
-    SendQuat32(buffer, &ix, thing->orientation.ToQuaternion());
-    SendBuffer(ix);
+  // if (thing->GetLinearVelocity().distance > 0 ||
+  //     thing->GetAngularVelocity().distance > 0) {
+  unsigned char ix = 0;
+  buffer[ix++] = PoseMsg;
+  buffer[ix++] = thing->id;
+  buffer[ix++] = Pose_Position | Pose_Orientation;
+  SendSpherical16(buffer, &ix, thing->position);
+  SendQuat32(buffer, &ix, thing->orientation.ToQuaternion());
+  SendBuffer(ix);
 
 #if RC_DEBUG
-    if (thing->id == 0)
-      SERIALPORT.printf("Sent PoseMsg Thing [%d/%d] %f\n", this->networkId,
-                        buffer[1], thing->position.distance);
+  // if (thing->id == 0)
+  SERIALPORT.printf("Sent PoseMsg Thing [%d/%d] %f\n", this->networkId,
+                    buffer[1], thing->position.distance);
 #endif
-  }
+  // }
 
   if (recurse) {
     for (unsigned char childIx = 0; childIx < thing->childCount; childIx++) {
diff --git a/NetworkSync.h b/NetworkSync.h
index 75286a8..4347d6c 100644
--- a/NetworkSync.h
+++ b/NetworkSync.h
@@ -24,7 +24,7 @@ public:
   virtual void SendDestroyThing(InterestingThing *obj);
   virtual void NewObject(InterestingThing *obj);
   void SendThing(Thing *thing);
-  void SendName(Roboid *roboid);
+  void SendName(Thing *roboid);
   virtual void SendModel(Roboid *obj);
   void SendModel(Thing *thing);
 
diff --git a/Roboid.cpp b/Roboid.cpp
index dedef27..ec04aad 100644
--- a/Roboid.cpp
+++ b/Roboid.cpp
@@ -30,8 +30,6 @@ Roboid::Roboid(Propulsion *propulsion) : Roboid() {
     propulsion->roboid = this;
 }
 
-void Roboid::SetName(const char *name) { this->name = name; }
-
 void Roboid::Update(unsigned long currentTimeMs) {
   if (perception != nullptr)
     perception->Update(currentTimeMs);
diff --git a/Roboid.h b/Roboid.h
index e7cd8a6..c2cb2d8 100644
--- a/Roboid.h
+++ b/Roboid.h
@@ -20,9 +20,6 @@ public:
   /// @param propulsion The Propulsion implementation to use for this Roboid
   Roboid(Propulsion *propulsion);
 
-  const char *name = nullptr;
-  void SetName(const char *name);
-
   /// @brief Update the state of the Roboid
   /// @param currentTimeMs The time in milliseconds when calling this
   /// function
diff --git a/Thing.cpp b/Thing.cpp
index 2a94cc6..4515c81 100644
--- a/Thing.cpp
+++ b/Thing.cpp
@@ -10,6 +10,8 @@ Thing::Thing(unsigned char id) : id(id) {
   this->children = nullptr;
 }
 
+void Thing::SetName(const char *name) { this->name = name; }
+
 const unsigned int Thing::SwitchType = SensorType | (unsigned int)Type::Switch;
 const unsigned int Thing::DistanceSensorType =
     SensorType | (unsigned int)Type::DistanceSensor;
diff --git a/Thing.h b/Thing.h
index b660528..f5fb7d0 100644
--- a/Thing.h
+++ b/Thing.h
@@ -19,6 +19,9 @@ public:
   /// @brief The type of Thing
   unsigned int type;
 
+  const char *name = nullptr;
+  void SetName(const char *name);
+
   // I hate this, better is to have an additional field stating the thing
   // classificaton Motor, Sensor etc.
   /// @brief The type of a switch sensor