From c2daeb26576458dbed7c072049cb969c93a0fcb6 Mon Sep 17 00:00:00 2001
From: Pascal Serrarens <pascal@passervr.com>
Date: Tue, 27 Feb 2024 12:51:34 +0100
Subject: [PATCH] Add typed object receiver

---
 NetworkPerception.cpp | 29 +++++++++++++++++++++++++++++
 NetworkPerception.h   |  1 +
 NetworkSync.h         |  5 +++--
 Perception.cpp        |  5 +++++
 Roboid.cpp            |  4 ++--
 5 files changed, 40 insertions(+), 4 deletions(-)

diff --git a/NetworkPerception.cpp b/NetworkPerception.cpp
index dfda5d8..982e7c1 100644
--- a/NetworkPerception.cpp
+++ b/NetworkPerception.cpp
@@ -9,6 +9,8 @@ void NetworkPerception::ProcessPacket(Roboid *roboid, unsigned char *buffer,
 
   if (buffer[0] == NetworkSync::PoseMsg) {
     ReceiveObject(buffer, roboid);
+  } else if (buffer[0] == NetworkSync::PoseTypeMsg) {
+    ReceiveTypedObject(buffer, roboid);
   }
 }
 
@@ -76,3 +78,30 @@ void NetworkPerception::ReceiveObject(unsigned char *data, Roboid *roboid) {
     roboid->perception->AddTrackedObject(this, position);
   }
 }
+
+#include "../../../src/LighthouseTracker.h"
+#include <Arduino.h>
+
+void NetworkPerception::ReceiveTypedObject(unsigned char *data,
+                                           Roboid *roboid) {
+  unsigned char objectType = data[1];
+  unsigned char objectId = data[2];
+  if (objectType == 0x02) { // lighthouse
+    // We require position and orientation for lighthouses
+    if (data[3] != NetworkSync::Pose_Position | NetworkSync::Pose_Orientation)
+      return;
+
+    Vector3 worldPosition = ReceiveVector3(data, 4);
+    Vector3 worldAngles = ReceiveVector3(data, 16);
+    Quaternion worldOrientation = Quaternion::Euler(worldAngles);
+
+    // roboid->perception->AddTrackedObject(this, objectType, worldPosition,
+    //                                      worldOrientation);
+    Sensor *sensor =
+        roboid->perception->FindSensorOfType(0x82); // Lighthouse type sensor
+    LighthouseTracker *lhSensor = (LighthouseTracker *)sensor;
+    lhSensor->UpdateGeometry(objectId, worldPosition, worldOrientation);
+  }
+
+  Serial.printf("Typed Object received\n");
+}
diff --git a/NetworkPerception.h b/NetworkPerception.h
index 60e32ce..b6da74d 100644
--- a/NetworkPerception.h
+++ b/NetworkPerception.h
@@ -13,6 +13,7 @@ public:
 
 protected:
   void ReceiveObject(unsigned char *data, Roboid *roboid);
+  void ReceiveTypedObject(unsigned char *data, Roboid *roboid);
 
   Int32 ReceiveInt32(unsigned char *data, int startIndex);
   float ReceiveFloat100(unsigned char *data, int startIndex);
diff --git a/NetworkSync.h b/NetworkSync.h
index 980fcfe..2805685 100644
--- a/NetworkSync.h
+++ b/NetworkSync.h
@@ -20,6 +20,7 @@ public:
 
   /// @brief The id of a Pose message
   static const char PoseMsg = 0x10;
+  static const char PoseTypeMsg = 0x11;
   /// @brief A bit pattern for the pose, stating that this message contains a
   /// position in world coordinates
   static const char Pose_Position = 0x01;
@@ -42,8 +43,8 @@ public:
 
   void PublishTrackedObjects(SendBuffer sendBuffer, InterestingThing **objects);
 
-  virtual void SendPosition(Vector3 worldPosition) {};
-  virtual void SendPose(Vector3 worldPosition, Quaternion worldOrientation) {};
+  virtual void SendPosition(Vector3 worldPosition){};
+  virtual void SendPose(Vector3 worldPosition, Quaternion worldOrientation){};
 
 protected:
   NetworkPerception *networkPerception;
diff --git a/Perception.cpp b/Perception.cpp
index 1f5e6b4..731d064 100644
--- a/Perception.cpp
+++ b/Perception.cpp
@@ -247,6 +247,11 @@ void Perception::AddTrackedObject(Sensor *sensor, Spherical position) {
   }
 }
 
+// void Perception::AddTrackedObject(Sensor *sensor, uint8_t objectType,
+//                                   Vector3 position, Quaternion orientation) {
+
+//                                   }
+
 unsigned char Perception::TrackedObjectCount() {
   unsigned char objectCount = 0;
   for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
diff --git a/Roboid.cpp b/Roboid.cpp
index 34d4f7e..d6f1606 100644
--- a/Roboid.cpp
+++ b/Roboid.cpp
@@ -68,8 +68,8 @@ void Roboid::SetPosition(Vector3 newWorldPosition) {
   this->worldPosition = newWorldPosition;
 
   if (networkSync != nullptr)
-    // networkSync->SendPosition(this->worldPosition);
-    networkSync->SendPose(this->worldPosition, this->worldOrientation);
+    networkSync->SendPosition(this->worldPosition);
+  // networkSync->SendPose(this->worldPosition, this->worldOrientation);
 }
 
 void Roboid::SetOrientation(Quaternion worldOrientation) {