From 1aa4520646009307a607192031015c9da826cd43 Mon Sep 17 00:00:00 2001
From: Pascal Serrarens <pascal@passervr.com>
Date: Fri, 17 May 2024 11:57:47 +0200
Subject: [PATCH] Inverstigation response

---
 NetworkPerception.cpp | 29 +++++++++++++++++++++++++++++
 NetworkPerception.h   |  1 +
 2 files changed, 30 insertions(+)

diff --git a/NetworkPerception.cpp b/NetworkPerception.cpp
index 22032a3..a258f06 100644
--- a/NetworkPerception.cpp
+++ b/NetworkPerception.cpp
@@ -2,6 +2,11 @@
 
 #include "RoboidControl/NetworkSync.h"
 
+// #define DEBUG true
+#if DEBUG
+#include <Arduino.h>
+#endif
+
 void NetworkPerception::ProcessPacket(Roboid *roboid, unsigned char *buffer,
                                       int packetsize) {
   // printf("packet received, type = 0x%02x %d %d %d %d %d\n", buffer[0],
@@ -11,6 +16,9 @@ void NetworkPerception::ProcessPacket(Roboid *roboid, unsigned char *buffer,
   case NetworkSync::CreateMsg:
     ReceiveCreateMsg(buffer, roboid);
     break;
+  case NetworkSync::InvestigateMsg:
+    ReceiveInvestigateMsg(buffer, roboid);
+    break;
   case NetworkSync::PoseMsg:
     ReceivePoseMsg(buffer, roboid);
     break;
@@ -35,6 +43,27 @@ void NetworkPerception::ReceiveCreateMsg(unsigned char *data, Roboid *roboid) {
   }
 }
 
+void NetworkPerception::ReceiveInvestigateMsg(unsigned char *data,
+                                              Roboid *roboid) {
+  unsigned char networkId = data[1];
+  unsigned char objectId = data[2];
+
+#if DEBUG
+  printf("Received InvestigateMsg [%d/%d]\n", networkId, objectId);
+#endif
+
+  if (networkId != roboid->networkSync->networkId)
+    // We only response to investigation requests for our own objects
+    return;
+
+  InterestingThing *thing =
+      roboid->perception->FindTrackedObject(0x00, objectId);
+  if (thing == nullptr)
+    return;
+
+  roboid->networkSync->NewObject(thing);
+}
+
 void NetworkPerception::ReceivePlane(unsigned char *data, Roboid *roboid) {
   unsigned char networkId = data[1];
   unsigned char poseType = data[3];
diff --git a/NetworkPerception.h b/NetworkPerception.h
index 0917ad4..77e60e1 100644
--- a/NetworkPerception.h
+++ b/NetworkPerception.h
@@ -13,6 +13,7 @@ public:
 
 protected:
   void ReceiveCreateMsg(unsigned char *data, Roboid *roboid);
+  void ReceiveInvestigateMsg(unsigned char *data, Roboid *roboid);
   void ReceivePoseMsg(unsigned char *data, Roboid *roboid);
   void ReceiveTypedObject(unsigned char *data, Roboid *roboid);