From 28bf3b4682e14e74ebf2b842234f3bde4a4d96c2 Mon Sep 17 00:00:00 2001
From: Pascal Serrarens <pascal@passervr.com>
Date: Mon, 8 Apr 2024 17:26:24 +0200
Subject: [PATCH] Fixed issues

---
 NetworkSync.cpp   |  6 +++---
 Perception.cpp    | 27 ++++++++++++++++++---------
 Perception.h      |  2 ++
 TrackedObject.cpp |  3 ++-
 TrackedObject.h   |  3 ++-
 5 files changed, 27 insertions(+), 14 deletions(-)

diff --git a/NetworkSync.cpp b/NetworkSync.cpp
index fe649fa..07340bc 100644
--- a/NetworkSync.cpp
+++ b/NetworkSync.cpp
@@ -89,9 +89,9 @@ void NetworkSync::SendAngle32(unsigned char *data, unsigned int startIndex,
   data[startIndex + 1] = value >> 16 & 0xFF;
   data[startIndex + 2] = value >> 8 & 0xFF;
   data[startIndex + 3] = value & 0xFF;
-  Serial.printf(" %lu=%d:%d:%d:%d ", value, data[startIndex],
-                data[startIndex + 1], data[startIndex + 2],
-                data[startIndex + 3]);
+  // Serial.printf(" %lu=%d:%d:%d:%d ", value, data[startIndex],
+  //               data[startIndex + 1], data[startIndex + 2],
+  //               data[startIndex + 3]);
 }
 
 void NetworkSync::SendSingle100(unsigned char *data, unsigned int startIndex,
diff --git a/Perception.cpp b/Perception.cpp
index 3c47616..9fb797e 100644
--- a/Perception.cpp
+++ b/Perception.cpp
@@ -6,7 +6,7 @@
 
 #include <math.h>
 
-// #define RC_DEBUG2
+#define RC_DEBUG2
 #ifdef RC_DEBUG2
 #include <Arduino.h>
 #endif
@@ -252,10 +252,17 @@ InterestingThing *Perception::AddTrackedObject(Sensor *sensor,
   }
 }
 
-// void Perception::AddTrackedObject(Sensor *sensor, uint8_t objectType,
-//                                   Vector3 position, Quaternion orientation) {
-
-//                                   }
+InterestingThing *Perception::FindTrackedObject(char objectId) {
+  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
+    InterestingThing *thing = this->trackedObjects[objIx];
+    if (thing == nullptr)
+      continue;
+    if (thing->id == objectId) {
+      return thing;
+    }
+  }
+  return nullptr;
+}
 
 unsigned char Perception::TrackedObjectCount() {
   unsigned char objectCount = 0;
@@ -326,10 +333,12 @@ void Perception::Update(float currentTimeMs) {
 
     if (obj->DegradeConfidence(deltaTime) == false) {
       // delete obj
-      if (roboid != nullptr && roboid->networkSync != nullptr)
-        roboid->networkSync->DestroyObject(obj);
-      this->trackedObjects[objIx] = nullptr;
-      delete obj;
+      // if (roboid != nullptr && roboid->networkSync != nullptr) {
+      //   printf("Cleanup obj %d\n", obj->id);
+      //   roboid->networkSync->DestroyObject(obj);
+      // }
+      // this->trackedObjects[objIx] = nullptr;
+      // delete obj;
     }
   }
 }
diff --git a/Perception.h b/Perception.h
index c3c6556..5cad413 100644
--- a/Perception.h
+++ b/Perception.h
@@ -90,6 +90,8 @@ public:
   AddTrackedObject(Sensor *sensor, Spherical position,
                    Quaternion orientation = Quaternion::identity);
 
+  InterestingThing *FindTrackedObject(char objectId);
+
   /// @brief Retrieve the number of objects currently being tracked by the
   /// roboid
   /// @return The object of objects, which is always lower than maxObjectCount
diff --git a/TrackedObject.cpp b/TrackedObject.cpp
index e75c01b..a7a4ccb 100644
--- a/TrackedObject.cpp
+++ b/TrackedObject.cpp
@@ -54,7 +54,8 @@ void InterestingThing::Refresh(Polar position) {
   this->confidence = maxConfidence;
 }
 
-void InterestingThing::Refresh(Spherical position) {
+void InterestingThing::Refresh(Spherical position, Quaternion orientation) {
   this->position = position;
+  this->orientation = orientation;
   this->confidence = maxConfidence;
 }
diff --git a/TrackedObject.h b/TrackedObject.h
index b34ea8c..65ed968 100644
--- a/TrackedObject.h
+++ b/TrackedObject.h
@@ -23,7 +23,8 @@ public:
   /// @details This will also update the confidence of the object to the
   /// maxConfidence value
   void Refresh(Polar position);
-  void Refresh(Spherical position);
+  void Refresh(Spherical position,
+               Quaternion orientation = Quaternion::identity);
 
   /// @brief Decrease the confidence based on the elapsed time
   /// @param deltaTime The time since the last DegradeConfidence call