From b81b236ef410792a32218102dd3ff5d97d41feb0 Mon Sep 17 00:00:00 2001
From: Pascal Serrarens <pascal@passervr.com>
Date: Tue, 14 May 2024 11:58:42 +0200
Subject: [PATCH] Bugfixes

---
 LinearAlgebra  |  2 +-
 Perception.cpp | 26 +++++++++++++++-----------
 Roboid.cpp     |  1 -
 3 files changed, 16 insertions(+), 13 deletions(-)

diff --git a/LinearAlgebra b/LinearAlgebra
index ee62cba..78468e3 160000
--- a/LinearAlgebra
+++ b/LinearAlgebra
@@ -1 +1 @@
-Subproject commit ee62cbae3ec63b7847df242e02b453a43defa5de
+Subproject commit 78468e38cd8d398f36213f8205b87ad41f798f74
diff --git a/Perception.cpp b/Perception.cpp
index b38c9a5..0f4780b 100644
--- a/Perception.cpp
+++ b/Perception.cpp
@@ -74,7 +74,7 @@ float GetPlaneDistance(InterestingThing *plane, float horizontalAngle,
   float distance = plane->position.distance;
   float deltaAngle = Angle::Normalize((float)plane->position.horizontalAngle -
                                       horizontalAngle);
-  if (abs(deltaAngle) < abs(range)) {
+  if (fabsf(deltaAngle) < fabsf(range)) {
     // distance = distance
     // printf("   plane distance = %f (%f-%f)+%f=%f", distance,
     //        (float)plane->position.horizontalAngle, horizontalAngle, range,
@@ -188,7 +188,7 @@ bool Perception::ObjectNearby(float direction, float range) {
   return false;
 }
 
-#include <WifiSync.h>
+// #include <WifiSync.h>
 void Perception::AddTrackedObject(Sensor *sensor, Polar position,
                                   unsigned char thingType,
                                   unsigned char networkId) {
@@ -235,8 +235,10 @@ void Perception::AddTrackedObject(Sensor *sensor, Polar position,
     printf("%d: new tracked object [%d/%d]\n", availableSlotIx, obj->networkId,
            obj->id);
 #endif
-    roboid->networkSync->NewObject(obj);
-    ((WifiSync *)roboid->networkSync)->PublishTrackedObject(roboid, obj);
+    if (roboid->networkSync != nullptr) {
+      roboid->networkSync->NewObject(obj);
+      // ((WifiSync *)roboid->networkSync)->PublishTrackedObject(roboid, obj);
+    }
   }
   // If this object is closer than the farthest object, then replace it
   else if (obj->position.distance <
@@ -250,8 +252,10 @@ void Perception::AddTrackedObject(Sensor *sensor, Polar position,
     // Serial.println(": replaced tracked object");
     printf("%d: replaced tracked object [/%d]\n", farthestObjIx, obj->id);
 #endif
-    roboid->networkSync->NewObject(obj);
-    ((WifiSync *)roboid->networkSync)->PublishTrackedObject(roboid, obj);
+    if (roboid->networkSync != nullptr) {
+      roboid->networkSync->NewObject(obj);
+      // ((WifiSync *)roboid->networkSync)->PublishTrackedObject(roboid, obj);
+    }
   } else {
 #ifdef RC_DEBUG2
     // Serial.print((int)obj->id);
@@ -418,17 +422,17 @@ unsigned char Perception::ThingsOfType(unsigned char thingType,
     if (thing == nullptr)
       continue;
 
-    printf("[%d/%d]%d  ", thing->networkId, thing->id, thing->type);
+    // printf("[%d/%d]%d  ", thing->networkId, thing->id, thing->type);
     if (thing->type == thingType) {
       buffer[thingCount] = thing;
       thingCount++;
       if (thingCount >= bufferSize) {
-        printf("\n");
+        // printf("\n");
         return bufferSize;
       }
     }
   }
-  printf("\n");
+  // printf("\n");
   return thingCount;
 }
 
@@ -480,7 +484,7 @@ void Perception::Update(float currentTimeMs) {
         float angle = sensor->position.angle;
         // Polar position = Polar(angle, distance);
         Polar position = Polar(distance, angle);
-        // AddTrackedObject(distanceSensor, position);
+        AddTrackedObject(distanceSensor, position);
       }
 
     } else if (sensor->type == Thing::SwitchType) {
@@ -525,7 +529,7 @@ void Perception::UpdatePose(Polar translation) {
       //        (float)thing->position.verticalAngle);
       // Update the closest point to the plane
       float angle = (float)thing->position.horizontalAngle + translation.angle;
-      angle = abs(angle);
+      angle = fabsf(angle);
 
       float deltaDistance = translation.distance * cosf(angle * Angle::Deg2Rad);
       // printf(" | translate %f %f %f | ", (float)translation.distance,
diff --git a/Roboid.cpp b/Roboid.cpp
index 9d7b4ce..20af082 100644
--- a/Roboid.cpp
+++ b/Roboid.cpp
@@ -75,7 +75,6 @@ Vector2 Roboid::GetPosition2D() {
   return Vector2(this->worldPosition.Right(), this->worldPosition.Forward());
 }
 
-#include <Arduino.h>
 Quaternion Roboid::GetOrientation() {
   Vector3 axis = this->worldAngleAxis.axis.ToVector3();
   Quaternion q = Quaternion::AngleAxis(this->worldAngleAxis.angle, axis);