From b527aeb97bbb37ca77bb962a13a7051ade3cfd7c Mon Sep 17 00:00:00 2001
From: Pascal Serrarens <21057728+passervr@users.noreply.github.com>
Date: Wed, 6 Dec 2023 12:46:08 +0100
Subject: [PATCH] Simplified perception

---
 Perception.cpp | 96 ++++++++++++++++++++++++++++++++++++++++----------
 Perception.h   | 79 ++++++++++++++++++++++-------------------
 2 files changed, 119 insertions(+), 56 deletions(-)

diff --git a/Perception.cpp b/Perception.cpp
index 18ea8e3..c0b203e 100644
--- a/Perception.cpp
+++ b/Perception.cpp
@@ -44,7 +44,7 @@ Sensor* Perception::GetSensor(unsigned int sensorId) {
 
   return nullptr;
 }
-
+/*
 float Perception::DistanceForward(float angle) {
   float minDistance = INFINITY;
   for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
@@ -131,6 +131,61 @@ float Perception::DistanceDown(float angle) {
   }
   return minDistance;
 }
+*/
+
+float Perception::GetDistance(float fromAngle, float toAngle) {
+  float minDistance = INFINITY;
+  if (toAngle < fromAngle)
+    // Hmm. Can't look backward properly for now
+    return minDistance;
+
+  for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
+    Placement placement = sensorPlacements[sensorIx];
+    float angle = placement.horizontalDirection;
+    if (angle > fromAngle && angle < toAngle) {
+      Thing* thing = placement.thing;
+      if (thing == nullptr)
+        continue;
+
+      if (thing->type == Thing::DistanceSensorType) {
+        DistanceSensor* distanceSensor = (DistanceSensor*)thing;
+        if (distanceSensor != nullptr && distanceSensor->IsOn())
+          minDistance = fmin(minDistance, distanceSensor->GetDistance());
+      }
+    }
+  }
+  return minDistance;
+}
+
+float Perception::GetDistance(float fromHorizontalAngle,
+                              float toHorizontalAngle,
+                              float fromVerticalAngle,
+                              float toVerticalAngle) {
+  float minDistance = INFINITY;
+  if (toHorizontalAngle < fromHorizontalAngle ||
+      toVerticalAngle < fromVerticalAngle)
+    // Hmm. Can't look backward properly for now
+    return minDistance;
+
+  for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
+    Placement placement = sensorPlacements[sensorIx];
+    if (placement.horizontalDirection > fromHorizontalAngle &&
+        placement.horizontalDirection < toHorizontalAngle &&
+        placement.verticalDirection > fromVerticalAngle &&
+        placement.verticalDirection < toVerticalAngle) {
+      Thing* thing = placement.thing;
+      if (thing == nullptr)
+        continue;
+
+      if (thing->type == Thing::DistanceSensorType) {
+        DistanceSensor* distanceSensor = (DistanceSensor*)thing;
+        if (distanceSensor != nullptr && distanceSensor->IsOn())
+          minDistance = fmin(minDistance, distanceSensor->GetDistance());
+      }
+    }
+  }
+  return minDistance;
+}
 
 bool Perception::SwitchOn(float fromAngle, float toAngle) {
   if (toAngle < fromAngle)
@@ -165,24 +220,27 @@ unsigned int Perception::ToDepthMapIndex(float angle) {
   return depthMapIx;
 }
 
-float Perception::GetDistance(float angle) {
-  if (depthMap != nullptr) {
-    if (angle < rangeMinimum || angle > rangeMaximum)
-      return INFINITY;
-    unsigned int depthMapIx = ToDepthMapIndex(angle);
-    return depthMap[depthMapIx];
-  } else {
-    for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
-      Placement placement = sensorPlacements[sensorIx];
-      float placementAngle = placement.horizontalDirection;
-      if (placementAngle == angle) {
-        DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing;
-        return distanceSensor->GetDistance();
-      }
-    }
-  }
-  return INFINITY;
-}
+// float Perception::GetDistance(float angle) {
+//   if (depthMap != nullptr) {
+//     if (angle < rangeMinimum || angle > rangeMaximum)
+//       return INFINITY;
+//     unsigned int depthMapIx = ToDepthMapIndex(angle);
+//     return depthMap[depthMapIx];
+//   } else {
+//     for (unsigned int sensorIx = 0; sensorIx < this->sensorCount;
+//     sensorIx++)
+//     {
+//       Placement placement = sensorPlacements[sensorIx];
+//       float placementAngle = placement.horizontalDirection;
+//       if (placementAngle == angle) {
+//         DistanceSensor* distanceSensor =
+//         (DistanceSensor*)placement.thing; return
+//         distanceSensor->GetDistance();
+//       }
+//     }
+//   }
+//   return INFINITY;
+// }
 
 void Perception::SetResolution(unsigned int resolution) {
   this->resolution = resolution;
diff --git a/Perception.h b/Perception.h
index ae5ee0c..66b22aa 100644
--- a/Perception.h
+++ b/Perception.h
@@ -24,46 +24,51 @@ class Perception {
 
   unsigned int GetSensorCount();
   Sensor* GetSensor(unsigned int sensorId);
+  /*
+    float DistanceForward(float angle = 90);
 
-  float DistanceForward(float angle = 90);
+    /// @brief Distance to the closest object on the left
+    /// @return distance in meters, INFINITY when no object is detected.
+    /// @note An object is on the left when the `angle` is between -180 and 0
+    /// degrees.
+    /// @note An object dead straight (0 degrees) is not reported.
+    float DistanceLeft() { return DistanceLeft(180); }
+    /// @brief Distance to the closest object on the left
+    /// @param angle the maximum angle on the left used for detection.
+    /// @return distance in meters, INFINITY when no object is detected.
+    /// @note An object is on the left when the `angle` is between -`angle` and
+    0
+    /// degrees.
+    /// @note An object dead straight (0 degrees) is not reported.
+    /// @note When an object is beyond `angle` meters, it is not reported.
+    float DistanceLeft(float angle);
 
-  /// @brief Distance to the closest object on the left
-  /// @return distance in meters, INFINITY when no object is detected.
-  /// @note An object is on the left when the `angle` is between -180 and 0
-  /// degrees.
-  /// @note An object dead straight (0 degrees) is not reported.
-  float DistanceLeft() { return DistanceLeft(180); }
-  /// @brief Distance to the closest object on the left
-  /// @param angle the maximum angle on the left used for detection.
-  /// @return distance in meters, INFINITY when no object is detected.
-  /// @note An object is on the left when the `angle` is between -`angle` and 0
-  /// degrees.
-  /// @note An object dead straight (0 degrees) is not reported.
-  /// @note When an object is beyond `angle` meters, it is not reported.
-  float DistanceLeft(float angle);
+    /// @brief Distance to the closest object on the right
+    /// @return distance in meters, INFINITY when no object is detected
+    /// @note An object is on the right when the `angle` is between 0 and 180
+    /// degrees
+    /// @note An object dead straight (0 degrees) is not reported
+    float DistanceRight() { return DistanceRight(180); }
+    /// @brief Distance to the closest object on the right
+    /// @param angle the maximum angle on the left used for detection.
+    /// @return distance in meters, INFINITY when no object is detected
+    /// @note An object is on the left when the `angle` is between 0 and `angle`
+    /// degrees.
+    /// @note An object dead straight (0 degrees) is not reported.
+    /// @note When an object is beyond `angle` meters, it is not reported.
+    float DistanceRight(float angle);
 
-  /// @brief Distance to the closest object on the right
-  /// @return distance in meters, INFINITY when no object is detected
-  /// @note An object is on the right when the `angle` is between 0 and 180
-  /// degrees
-  /// @note An object dead straight (0 degrees) is not reported
-  float DistanceRight() { return DistanceRight(180); }
-  /// @brief Distance to the closest object on the right
-  /// @param angle the maximum angle on the left used for detection.
-  /// @return distance in meters, INFINITY when no object is detected
-  /// @note An object is on the left when the `angle` is between 0 and `angle`
-  /// degrees.
-  /// @note An object dead straight (0 degrees) is not reported.
-  /// @note When an object is beyond `angle` meters, it is not reported.
-  float DistanceRight(float angle);
-
-  float DistanceUp() { return DistanceUp(180); }
-  float DistanceUp(float angle);
-  float DistanceDown() { return DistanceDown(180); }
-  float DistanceDown(float angle);
-
-  float Distance(float leftAngle, float rightAngle);
-  float GetDistance(float angle);
+    float DistanceUp() { return DistanceUp(180); }
+    float DistanceUp(float angle);
+    float DistanceDown() { return DistanceDown(180); }
+    float DistanceDown(float angle);
+  */
+  float GetDistance(float fromAngle, float toAngle);
+  float GetDistance(float fromHorizontalAngle,
+                    float toHorizontalAngle,
+                    float fromVerticalAngle,
+                    float toVerticalAngle);
+  //   float GetDistance(float angle);
 
   bool SwitchOn(float fromAngle, float toAngle);