From 9a70b3d3d229b60a74f1a0a1c144287a989d5915 Mon Sep 17 00:00:00 2001
From: Pascal Serrarens <pascal@passervr.com>
Date: Sat, 30 Dec 2023 09:28:26 +0100
Subject: [PATCH] Add sensor reference for perceived objects

---
 Perception.cpp | 31 ++++++++++++++++++++++---------
 Perception.h   |  9 ++++++---
 2 files changed, 28 insertions(+), 12 deletions(-)

diff --git a/Perception.cpp b/Perception.cpp
index 9a33ad2..10aadab 100644
--- a/Perception.cpp
+++ b/Perception.cpp
@@ -30,6 +30,16 @@ Sensor *Perception::GetSensor(unsigned int sensorId) {
   return nullptr;
 }
 
+Sensor *Perception::FindSensorOfType(unsigned int sensorType) {
+  for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
+    Sensor *sensor = (Sensor *)this->sensorPlacements[sensorIx].thing;
+    if (sensor->type == sensorType)
+      return sensor;
+  }
+
+  return nullptr;
+}
+
 float Perception::GetDistance(float direction, float range) {
   float minDistance = INFINITY;
   if (range < 0)
@@ -129,8 +139,9 @@ PerceivedObject::PerceivedObject() {
   this->radius = INFINITY;
 }
 
-PerceivedObject::PerceivedObject(Polar position, float radius)
+PerceivedObject::PerceivedObject(Sensor *sensor, Polar position, float radius)
     : PerceivedObject() {
+  this->sensor = sensor;
   this->position = position;
   this->radius = radius;
 }
@@ -168,11 +179,11 @@ void PerceivedObject::Refresh(Polar position, float radius) {
   this->confidence = maxConfidence;
 }
 
-void Perception::AddPerceivedObject(Polar position) {
+void Perception::AddPerceivedObject(Sensor *sensor, Polar position) {
   // int objCount = PerceivedObjectCount();
   // printf("perc obj count %d\n");
 
-  PerceivedObject *obj = new PerceivedObject(position);
+  PerceivedObject *obj = new PerceivedObject(sensor, position);
   // objCount = PerceivedObjectCount();
   // printf("perc obj count %d\n");
   //   AddPerceivedObject(obj);
@@ -189,10 +200,10 @@ void Perception::AddPerceivedObject(Polar position) {
     }
     // Do we see the same object?
     else {
-      printf("(%d) my %f %f =^= received %f %f\n", objIx,
-             this->perceivedObjects[objIx]->position.distance,
-             this->perceivedObjects[objIx]->position.angle,
-             obj->position.distance, obj->position.angle);
+      // printf("(%d) my %f %f =^= received %f %f\n", objIx,
+      //        this->perceivedObjects[objIx]->position.distance,
+      //        this->perceivedObjects[objIx]->position.angle,
+      //        obj->position.distance, obj->position.angle);
       if (obj->IsTheSameAs(this->perceivedObjects[objIx])) {
         printf("[%d] Updating...\n", objIx);
         this->perceivedObjects[objIx]->Refresh(obj->position, obj->radius);
@@ -252,19 +263,21 @@ void Perception::Update(float currentTimeMs) {
 
     if (thing->type == Thing::DistanceSensorType) {
       DistanceSensor *distanceSensor = (DistanceSensor *)thing;
+
       printf("S%d: %d %f\n", sensorIx, (int)distanceSensor,
              distanceSensor->GetDistance());
+             
       if (distanceSensor != nullptr && distanceSensor->ObjectNearby()) {
         Polar position = Polar(thingPlacement.horizontalDirection,
                                distanceSensor->GetDistance());
-        AddPerceivedObject(position);
+        AddPerceivedObject(distanceSensor, position);
       }
     } else if (thing->type == Thing::SwitchType) {
       Switch *switchSensor = (Switch *)thing;
       if (switchSensor != nullptr && switchSensor->IsOn()) {
         Polar position =
             Polar(thingPlacement.horizontalDirection, nearbyDistance);
-        AddPerceivedObject(position);
+        AddPerceivedObject(switchSensor, position);
       }
     }
   }
diff --git a/Perception.h b/Perception.h
index d4e6c57..5e6f36b 100644
--- a/Perception.h
+++ b/Perception.h
@@ -11,7 +11,7 @@ namespace RoboidControl {
 class PerceivedObject {
 public:
   PerceivedObject();
-  PerceivedObject(Polar position, float radius = 0.1F);
+  PerceivedObject(Sensor *sensor, Polar position, float radius = 0.1F);
 
   static constexpr float equalityDistance = 0.3F;
   static constexpr float equalityAngle = 5.0F;
@@ -19,8 +19,9 @@ public:
 
   char id;
 
-  Polar position;
+  Polar position = Polar::zero;
   float radius;
+  Sensor *sensor = nullptr;
 
   static constexpr char maxConfidence = 255;
   static constexpr char confidenceDropSpeed = 2;
@@ -52,6 +53,8 @@ public:
   unsigned int GetSensorCount();
   Sensor *GetSensor(unsigned int sensorId);
 
+  Sensor *FindSensorOfType(unsigned int sensorType);
+
   /// @brief Gets the distance to the closest object
   /// @param direction The direction to look for objects
   /// @param range The range in which objects should be looked for
@@ -95,7 +98,7 @@ public:
 
   // Object Perception
 
-  void AddPerceivedObject(Polar position);
+  void AddPerceivedObject(Sensor *sensor, Polar position);
   // void AddPerceivedObject(PerceivedObject *obj);
   unsigned char PerceivedObjectCount();
   PerceivedObject **GetPerceivedObjects();