From 0d3678350b5b59d740e2e1513bb822d11a60445f Mon Sep 17 00:00:00 2001
From: Pascal Serrarens <pascal@passervr.com>
Date: Fri, 26 Apr 2024 12:43:01 +0200
Subject: [PATCH] Debug info improvement

---
 NetworkPerception.cpp | 6 ++++--
 Perception.cpp        | 4 ++--
 2 files changed, 6 insertions(+), 4 deletions(-)

diff --git a/NetworkPerception.cpp b/NetworkPerception.cpp
index 909c7ee..6f96700 100644
--- a/NetworkPerception.cpp
+++ b/NetworkPerception.cpp
@@ -40,10 +40,10 @@ void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) {
   unsigned char networkId = data[1];
   unsigned char objectId = data[2];
   unsigned char poseType = data[3];
-  // printf("Received pose message [%d/%d]\n", networkId, objectId);
   // printf("My network id == %d\n", roboid->networkSync->networkId);
   if (networkId == roboid->networkSync->networkId)
     networkId = 0x00;
+  printf("Received pose message [%d/%d]\n", networkId, objectId);
 
   if ((poseType & NetworkSync::Pose_Position) != 0) {
     Vector3 worldPosition = ReceiveVector3(data, 4);
@@ -53,8 +53,10 @@ void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) {
       Vector3 roboidPosition = roboid->GetPosition();
 
       float distance = Vector3::Distance(roboidPosition, worldPosition);
-      if (roboid->perception->IsInteresting(distance) == false)
+      if (roboid->perception->IsInteresting(distance) == false) {
+        printf("  not interesting\n");
         return;
+      }
 
       Quaternion roboidOrientation = roboid->GetOrientation();
       Vector3 localPosition = Quaternion::Inverse(roboidOrientation) *
diff --git a/Perception.cpp b/Perception.cpp
index f7c1fbf..ae36d27 100644
--- a/Perception.cpp
+++ b/Perception.cpp
@@ -269,7 +269,7 @@ InterestingThing *Perception::AddTrackedObject(Sensor *sensor,
 #ifdef RC_DEBUG2
     // Serial.print((int)obj->id);
     // Serial.println(": delete tracked object");
-    printf("%d: delete tracked object {/%d}\n", -1, obj->id);
+    printf("%d: delete tracked object {%d/%d}\n", -1, obj->networkId, obj->id);
 #endif
     // No available slot, delete trackedobject
     delete obj;
@@ -297,7 +297,7 @@ bool Perception::IsInteresting(float distance) {
   for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
     InterestingThing *thing = this->trackedObjects[objIx];
     if (thing == nullptr)
-      continue;
+      return true;
     if (thing->position.distance > distance)
       return true;
   }