From cf31f2099abd77d066589a26567136826c5f500d Mon Sep 17 00:00:00 2001
From: Pascal Serrarens <pascal@passervr.com>
Date: Mon, 24 Feb 2025 12:54:06 +0100
Subject: [PATCH] First udp communcation is working

---
 Arduino/ArduinoUtils.h                 |  2 +-
 Arduino/Participant.cpp                |  4 ++--
 LinearAlgebra/Direction.h              |  6 ++++++
 LinearAlgebra/Spherical.h              |  6 ++++++
 Messages/NetworkIdMsg.cpp              | 22 -------------------
 Messages/SiteMsg.cpp                   | 22 +++++++++++++++++++
 Messages/{NetworkIdMsg.h => SiteMsg.h} |  8 +++----
 Participant.cpp                        | 22 +++++++++----------
 Participant.h                          | 16 +++-----------
 Sensors/DigitalSensor.cpp              |  4 ++--
 Sensors/DigitalSensor.h                |  4 ++--
 Sensors/TemperatureSensor.cpp          |  6 +++---
 Sensors/TemperatureSensor.h            |  4 ++--
 Sensors/TouchSensor.cpp                | 15 +++++++++++++
 Sensors/TouchSensor.h                  | 29 ++++++++++++++++++++++++++
 SiteServer.cpp                         |  8 +++----
 SiteServer.h                           |  8 +++----
 Thing.cpp                              | 13 +++++++-----
 Thing.h                                |  4 ++--
 19 files changed, 126 insertions(+), 77 deletions(-)
 delete mode 100644 Messages/NetworkIdMsg.cpp
 create mode 100644 Messages/SiteMsg.cpp
 rename Messages/{NetworkIdMsg.h => SiteMsg.h} (82%)
 create mode 100644 Sensors/TouchSensor.cpp
 create mode 100644 Sensors/TouchSensor.h

diff --git a/Arduino/ArduinoUtils.h b/Arduino/ArduinoUtils.h
index f2676ed..1961739 100644
--- a/Arduino/ArduinoUtils.h
+++ b/Arduino/ArduinoUtils.h
@@ -3,7 +3,7 @@
 
 #include <Arduino.h>
 bool StartWifi(const char *wifiSsid, const char *wifiPassword,
-               bool hotspotFallback);
+               bool hotspotFallback = false);
 
 void CheckFirmware(String url, String FIRMWARE_NAME, int FIRMWARE_VERSION);
 
diff --git a/Arduino/Participant.cpp b/Arduino/Participant.cpp
index 30f22f6..daa99d7 100644
--- a/Arduino/Participant.cpp
+++ b/Arduino/Participant.cpp
@@ -73,8 +73,8 @@ bool Participant::Send(RemoteParticipant* remoteParticipant, int bufferSize) {
   udp.write((unsigned char*)buffer, bufferSize);
   udp.endPacket();
 
-  //   std::cout << "Sent to " << this->remoteIpAddress << ":"
-  //             << this->remotePort << "\n";
+    std::cout << "Sent to " << remoteParticipant->ipAddress << ":"
+              << remoteParticipant->port << "\n";
 #endif
   return true;
 }
diff --git a/LinearAlgebra/Direction.h b/LinearAlgebra/Direction.h
index b614a04..1120c6b 100644
--- a/LinearAlgebra/Direction.h
+++ b/LinearAlgebra/Direction.h
@@ -91,6 +91,12 @@ protected:
 using DirectionSingle = DirectionOf<float>;
 using Direction16 = DirectionOf<signed short>;
 
+#if defined(ARDUINO)
+using Direction = Direction16;
+#else
+using Direction = DirectionSingle;
+#endif
+
 } // namespace LinearAlgebra
 } // namespace Passer
 using namespace Passer::LinearAlgebra;
diff --git a/LinearAlgebra/Spherical.h b/LinearAlgebra/Spherical.h
index e4d6682..3131a53 100644
--- a/LinearAlgebra/Spherical.h
+++ b/LinearAlgebra/Spherical.h
@@ -180,6 +180,12 @@ using SphericalSingle = SphericalOf<float>;
 /// hardware
 using Spherical16 = SphericalOf<signed short>;
 
+#if defined(ARDUINO)
+using Spherical = Spherical16;
+#else
+using Spherical = SphericalSingle;
+#endif
+
 } // namespace LinearAlgebra
 } // namespace Passer
 using namespace Passer::LinearAlgebra;
diff --git a/Messages/NetworkIdMsg.cpp b/Messages/NetworkIdMsg.cpp
deleted file mode 100644
index cfa36b8..0000000
--- a/Messages/NetworkIdMsg.cpp
+++ /dev/null
@@ -1,22 +0,0 @@
-#include "NetworkIdMsg.h"
-
-namespace RoboidControl {
-
-NetworkIdMsg::NetworkIdMsg(const char* buffer) {
-  this->networkId = buffer[1];
-}
-
-NetworkIdMsg::NetworkIdMsg(unsigned char networkId) {
-  this->networkId = networkId;
-}
-
-NetworkIdMsg::~NetworkIdMsg() {}
-
-unsigned char NetworkIdMsg::Serialize(char* buffer) {
-  unsigned char ix = 0;
-  buffer[ix++] = this->id;
-  buffer[ix++] = this->networkId;
-  return NetworkIdMsg::length;
-}
-
-}  // namespace RoboidControl
diff --git a/Messages/SiteMsg.cpp b/Messages/SiteMsg.cpp
new file mode 100644
index 0000000..d9f7afd
--- /dev/null
+++ b/Messages/SiteMsg.cpp
@@ -0,0 +1,22 @@
+#include "SiteMsg.h"
+
+namespace RoboidControl {
+
+SiteMsg::SiteMsg(const char* buffer) {
+  this->networkId = buffer[1];
+}
+
+SiteMsg::SiteMsg(unsigned char networkId) {
+  this->networkId = networkId;
+}
+
+SiteMsg::~SiteMsg() {}
+
+unsigned char SiteMsg::Serialize(char* buffer) {
+  unsigned char ix = 0;
+  buffer[ix++] = this->id;
+  buffer[ix++] = this->networkId;
+  return SiteMsg::length;
+}
+
+}  // namespace RoboidControl
diff --git a/Messages/NetworkIdMsg.h b/Messages/SiteMsg.h
similarity index 82%
rename from Messages/NetworkIdMsg.h
rename to Messages/SiteMsg.h
index 083439b..b6e433f 100644
--- a/Messages/NetworkIdMsg.h
+++ b/Messages/SiteMsg.h
@@ -3,7 +3,7 @@
 namespace RoboidControl {
 
 /// @brief A message communicating the network ID for that participant
-class NetworkIdMsg : public IMessage {
+class SiteMsg : public IMessage {
 public:
   /// @brief The message ID
   static const unsigned char id = 0xA1;
@@ -14,11 +14,11 @@ public:
 
   /// @brief Create a new message for sending
   /// @param networkId The network ID for the participant
-  NetworkIdMsg(unsigned char networkId);
+  SiteMsg(unsigned char networkId);
   /// @copydoc RoboidControl::IMessage::IMessage(char*)
-  NetworkIdMsg(const char *buffer);
+  SiteMsg(const char *buffer);
   /// @brief Destructor for the message
-  virtual ~NetworkIdMsg();
+  virtual ~SiteMsg();
 
   /// @copydoc RoboidControl::IMessage::Serialize
   virtual unsigned char Serialize(char *buffer) override;
diff --git a/Participant.cpp b/Participant.cpp
index 014e3e3..59a7a4d 100644
--- a/Participant.cpp
+++ b/Participant.cpp
@@ -21,7 +21,7 @@
 
 namespace RoboidControl {
 
-Participant::Participant() {}
+//Participant::Participant() {}
 
 Participant::Participant(int port) {
   this->ipAddress = "0.0.0.0";
@@ -82,7 +82,7 @@ void Participant::Update(unsigned long currentTimeMs) {
     ParticipantMsg* msg = new ParticipantMsg(this->networkId);
     this->Publish(msg);
     delete msg;
-    std::cout << this->name << " published ParticipantMsg\n";
+    //std::cout << this->name << " published ParticipantMsg\n";
     this->nextPublishMe = currentTimeMs + this->publishInterval;
   }
   this->ReceiveUDP();
@@ -129,12 +129,12 @@ void Participant::SendThingInfo(RemoteParticipant* remoteParticipant, Thing* thi
   ThingMsg* thingMsg = new ThingMsg(this->networkId, thing);
   this->Send(remoteParticipant, thingMsg);
   delete thingMsg;
-  NameMsg* nameMsg = new NameMsg(this->networkId, thing);
-  this->Send(remoteParticipant, nameMsg);
-  delete nameMsg;
-  ModelUrlMsg* modelMsg = new ModelUrlMsg(this->networkId, thing);
-  this->Send(remoteParticipant, modelMsg);
-  delete modelMsg;
+  // NameMsg* nameMsg = new NameMsg(this->networkId, thing);
+  // this->Send(remoteParticipant, nameMsg);
+  // delete nameMsg;
+  // ModelUrlMsg* modelMsg = new ModelUrlMsg(this->networkId, thing);
+  // this->Send(remoteParticipant, modelMsg);
+  // delete modelMsg;
 }
 
 void Participant::PublishThingInfo(Thing* thing) {
@@ -199,8 +199,8 @@ void Participant::ReceiveData(unsigned char bufferSize, RemoteParticipant* remot
       Process(remoteParticipant, msg);
       delete msg;
     } break;
-    case NetworkIdMsg::id: {
-      NetworkIdMsg* msg = new NetworkIdMsg(this->buffer);
+    case SiteMsg::id: {
+      SiteMsg* msg = new SiteMsg(this->buffer);
       Process(remoteParticipant, msg);
       delete msg;
     } break;
@@ -234,7 +234,7 @@ void Participant::ReceiveData(unsigned char bufferSize, RemoteParticipant* remot
 
 void Participant::Process(RemoteParticipant* sender, ParticipantMsg* msg) {}
 
-void Participant::Process(RemoteParticipant* sender, NetworkIdMsg* msg) {
+void Participant::Process(RemoteParticipant* sender, SiteMsg* msg) {
   std::cout << this->name << ": process NetworkId [" << (int)this->networkId << "/" << (int)msg->networkId << "]\n";
   if (this->networkId != msg->networkId) {
     this->networkId = msg->networkId;
diff --git a/Participant.h b/Participant.h
index 9a7aea2..feaa1aa 100644
--- a/Participant.h
+++ b/Participant.h
@@ -4,7 +4,7 @@
 #include "Messages/InvestigateMsg.h"
 #include "Messages/ModelUrlMsg.h"
 #include "Messages/NameMsg.h"
-#include "Messages/NetworkIdMsg.h"
+#include "Messages/SiteMsg.h"
 #include "Messages/ParticipantMsg.h"
 #include "Messages/PoseMsg.h"
 #include "Messages/ThingMsg.h"
@@ -30,12 +30,9 @@ class Participant : public RemoteParticipant {
  public:
   char buffer[1024];
   long publishInterval = 3000;  // 3 seconds
-  // unsigned char networkId = 0;
 
   const char* name = "Participant";
 
-  // const char *ipAddress = "0.0.0.0";
-  // int port = 0;
   int localPort = 0;
 
 #if defined(ARDUINO)
@@ -57,8 +54,7 @@ class Participant : public RemoteParticipant {
 
 #endif
 
-  Participant();
-  Participant(int port);
+  Participant(int port = 7681);
   Participant(const char* ipAddress, int port);
 
   void begin();
@@ -66,12 +62,6 @@ class Participant : public RemoteParticipant {
 
   virtual void Update(unsigned long currentTimeMs = 0);
 
-  // std::list<Thing *> things;
-  // Thing *Get(unsigned char networkId, unsigned char thingId);
-  // int Add(Thing *thing);
-  // void Remove(Thing *thing);
-  // void UpdateAll(unsigned long currentTimeMs);
-
   void SendThingInfo(RemoteParticipant* remoteParticipant, Thing* thing);
   void PublishThingInfo(Thing* thing);
 
@@ -93,7 +83,7 @@ class Participant : public RemoteParticipant {
   void ReceiveUDP();
 
   virtual void Process(RemoteParticipant* sender, ParticipantMsg* msg);
-  virtual void Process(RemoteParticipant* sender, NetworkIdMsg* msg);
+  virtual void Process(RemoteParticipant* sender, SiteMsg* msg);
   virtual void Process(RemoteParticipant* sender, InvestigateMsg* msg);
   virtual void Process(RemoteParticipant* sender, ThingMsg* msg);
   virtual void Process(RemoteParticipant* sender, NameMsg* msg);
diff --git a/Sensors/DigitalSensor.cpp b/Sensors/DigitalSensor.cpp
index ad14f3b..97a2197 100644
--- a/Sensors/DigitalSensor.cpp
+++ b/Sensors/DigitalSensor.cpp
@@ -2,8 +2,8 @@
 
 namespace RoboidControl {
 
-DigitalSensor::DigitalSensor() {}
+//DigitalSensor::DigitalSensor() {}
 
-DigitalSensor::DigitalSensor(unsigned char networkId, unsigned char thingId) {}
+DigitalSensor::DigitalSensor(RemoteParticipant* participant, unsigned char networkId, unsigned char thingId) : Thing(participant) {}
 
 }  // namespace RoboidControl
diff --git a/Sensors/DigitalSensor.h b/Sensors/DigitalSensor.h
index ed0a973..fabf99d 100644
--- a/Sensors/DigitalSensor.h
+++ b/Sensors/DigitalSensor.h
@@ -11,11 +11,11 @@ class DigitalSensor : public Thing {
   bool state = 0;
 
   /// @brief The default constructor
-  DigitalSensor();
+  //DigitalSensor();
   /// @brief Create a temperature sensor with the given ID
   /// @param networkId The network ID of the sensor
   /// @param thingId The ID of the thing
-  DigitalSensor(unsigned char networkId, unsigned char thingId);
+  DigitalSensor(RemoteParticipant* participant, unsigned char networkId, unsigned char thingId);
 };
 
 }  // namespace RoboidControl
diff --git a/Sensors/TemperatureSensor.cpp b/Sensors/TemperatureSensor.cpp
index 1852ef5..498575c 100644
--- a/Sensors/TemperatureSensor.cpp
+++ b/Sensors/TemperatureSensor.cpp
@@ -6,10 +6,10 @@ namespace RoboidControl {
 
 // TemperatureSensor::TemperatureSensor() : Thing(Type::TemperatureSensor) {}
 
-TemperatureSensor::TemperatureSensor() : Thing(Type::TemperatureSensor) {}
+//TemperatureSensor::TemperatureSensor() : Thing(Type::TemperatureSensor) {}
 
-TemperatureSensor::TemperatureSensor(unsigned char networkId, unsigned char thingId)
-    : Thing(nullptr, networkId, thingId, Type::TemperatureSensor) {}
+TemperatureSensor::TemperatureSensor(RemoteParticipant* participant, unsigned char networkId, unsigned char thingId)
+    : Thing(participant, networkId, thingId, Type::TemperatureSensor) {}
 
 void TemperatureSensor::SetTemperature(float temp) {
   this->temperature = temp;
diff --git a/Sensors/TemperatureSensor.h b/Sensors/TemperatureSensor.h
index 5ecec8f..da06786 100644
--- a/Sensors/TemperatureSensor.h
+++ b/Sensors/TemperatureSensor.h
@@ -11,11 +11,11 @@ class TemperatureSensor : public Thing {
   float temperature = 0;
 
   /// @brief The default constructor
-  TemperatureSensor();
+  //TemperatureSensor();
   /// @brief Create a temperature sensor with the given ID
   /// @param networkId The network ID of the sensor
   /// @param thingId The ID of the thing
-  TemperatureSensor(unsigned char networkId, unsigned char thingId);
+  TemperatureSensor(RemoteParticipant* participant, unsigned char networkId, unsigned char thingId);
 
   /// @brief Manually override the measured temperature
   /// @param temperature The new temperature
diff --git a/Sensors/TouchSensor.cpp b/Sensors/TouchSensor.cpp
new file mode 100644
index 0000000..86c3590
--- /dev/null
+++ b/Sensors/TouchSensor.cpp
@@ -0,0 +1,15 @@
+#include "TouchSensor.h"
+
+namespace RoboidControl {
+
+TouchSensor::TouchSensor(RemoteParticipant* participant) : Thing(participant) {
+    this->touchedSomething = false;
+}
+
+// TouchSensor::TouchSensor(RemoteParticipant* participant, unsigned char networkId, unsigned char thingId) : Thing(participant, networkId, thingId) {
+//     this->touchedSomething = false;
+// }
+
+void TouchSensor::GenerateBinary(char* bytes, unsigned char* ix) {}
+void TouchSensor::ProcessBinary(char* bytes) {}
+}  // namespace RoboidControl
\ No newline at end of file
diff --git a/Sensors/TouchSensor.h b/Sensors/TouchSensor.h
new file mode 100644
index 0000000..fdbf627
--- /dev/null
+++ b/Sensors/TouchSensor.h
@@ -0,0 +1,29 @@
+#pragma once
+
+#include "Thing.h"
+
+namespace RoboidControl {
+
+/// @brief A sensor which can detect touches
+class TouchSensor : public Thing {
+ public:
+  /// @brief Value which is true when the sensor is touching something, false otherwise
+  bool touchedSomething = false;
+
+  /// @brief Create a touch sensor
+  TouchSensor(RemoteParticipant* participant);
+  /// @brief Create a temperature sensor with the given ID
+  /// @param networkId The network ID of the sensor
+  /// @param thingId The ID of the thing
+  // TouchSensor(RemoteParticipant* participant, unsigned char networkId, unsigned char thingId);
+
+  /// @brief Function to create a binary message with the temperature
+  /// @param buffer The byte array for thw binary data
+  /// @param ix The starting position for writing the binary data
+  void GenerateBinary(char* bytes, unsigned char* ix) override;
+  /// @brief Function to extract the temperature received in the binary message
+  /// @param bytes The binary data
+  virtual void ProcessBinary(char* bytes) override;
+};
+
+}  // namespace RoboidControl
diff --git a/SiteServer.cpp b/SiteServer.cpp
index c6acb88..6aa4d17 100644
--- a/SiteServer.cpp
+++ b/SiteServer.cpp
@@ -25,14 +25,14 @@ void SiteServer::Process(RemoteParticipant *sender, ParticipantMsg *msg) {
   if (msg->networkId == 0) {
     std::cout << this->name << " received New Client -> " << sender->ipAddress
               << "  " << (int)sender->networkId << "\n";
-    NetworkIdMsg *msg = new NetworkIdMsg(sender->networkId);
+    SiteMsg *msg = new SiteMsg(sender->networkId);
     //sender->Send(msg);
     this->Send(sender, msg);
     delete msg;
   }
 }
 
-void SiteServer::Process(RemoteParticipant *sender, NetworkIdMsg *msg) {}
+void SiteServer::Process(RemoteParticipant *sender, SiteMsg *msg) {}
 
 void SiteServer::Process(RemoteParticipant *sender, ThingMsg *msg) {
   Thing *thing = sender->Get(msg->networkId, msg->thingId);
@@ -40,11 +40,11 @@ void SiteServer::Process(RemoteParticipant *sender, ThingMsg *msg) {
     auto thingMsgProcessor = thingMsgProcessors.find(msg->thingType);
     Thing *newThing;
     if (thingMsgProcessor != thingMsgProcessors.end()) // found item
-      newThing = thingMsgProcessor->second(msg->networkId, msg->thingId);
+      newThing = thingMsgProcessor->second(sender, msg->networkId, msg->thingId);
     else
       newThing = new Thing(sender, msg->networkId, msg->thingId,
                            (Thing::Type)msg->thingType);
-    sender->Add(newThing);
+    //sender->Add(newThing);
   }
 }
 
diff --git a/SiteServer.h b/SiteServer.h
index 03c8223..e54e15f 100644
--- a/SiteServer.h
+++ b/SiteServer.h
@@ -17,8 +17,8 @@ class SiteServer : public Participant {
 
   template <typename ThingClass>
   void Register(unsigned char thingType) {
-    thingMsgProcessors[thingType] = [](unsigned char networkId, unsigned char thingId) {
-      return new ThingClass(networkId, thingId);
+    thingMsgProcessors[thingType] = [](RemoteParticipant* participant, unsigned char networkId, unsigned char thingId) {
+      return new ThingClass(participant, networkId, thingId);
     };
   };
 
@@ -26,10 +26,10 @@ class SiteServer : public Participant {
   unsigned long nextPublishMe = 0;
 
   virtual void Process(RemoteParticipant* sender, ParticipantMsg* msg) override;
-  virtual void Process(RemoteParticipant* sender, NetworkIdMsg* msg) override;
+  virtual void Process(RemoteParticipant* sender, SiteMsg* msg) override;
   virtual void Process(RemoteParticipant* sender, ThingMsg* msg) override;
 
-  using ThingConstructor = std::function<Thing*(unsigned char networkId, unsigned char thingId)>;
+  using ThingConstructor = std::function<Thing*(RemoteParticipant* participant, unsigned char networkId, unsigned char thingId)>;
   std::unordered_map<unsigned char, ThingConstructor> thingMsgProcessors;
 };
 
diff --git a/Thing.cpp b/Thing.cpp
index f1cca3e..361be03 100644
--- a/Thing.cpp
+++ b/Thing.cpp
@@ -8,18 +8,21 @@
 
 namespace RoboidControl {
 
-Thing::Thing(Type thingType) : Thing((unsigned char)thingType) {}
-
-Thing::Thing(unsigned char thingType) {
-  // this->position = Spherical16::zero;
-  // this->orientation = SwingTwist16::identity;
+Thing::Thing(RemoteParticipant* participant, Type thingType) : Thing(participant, (unsigned char)thingType) {}
 
+Thing::Thing(RemoteParticipant* participant, unsigned char thingType) {
+  this->participant = participant;
   this->id = 0;
   this->type = thingType;
   this->networkId = 0;
 
+  this->position = Spherical16::zero;
+  this->orientation = SwingTwist16::identity;
+
   this->linearVelocity = Spherical16::zero;
   this->angularVelocity = Spherical16::zero;
+
+  participant->Add(this);
 }
 
 Thing::Thing(RemoteParticipant* participant, unsigned char networkId, unsigned char thingId, Type thingType) {
diff --git a/Thing.h b/Thing.h
index 28faea1..a8bdc76 100644
--- a/Thing.h
+++ b/Thing.h
@@ -42,10 +42,10 @@ class Thing {
 
   /// @brief Create a new thing of the given type
   /// @param thingType The predefined type of thing
-  Thing(Type thingType = Type::Undetermined);
+  Thing(RemoteParticipant* participant, Type thingType = Type::Undetermined);
   /// @brief Create a new thing of the give type
   /// @param thingType The custom type of the thing
-  Thing(unsigned char thingType);
+  Thing(RemoteParticipant* participant, unsigned char thingType);
   /// @brief Create a new thing for the given participant
   /// @param participant The participant for which this thing is created
   /// @param networkId The network ID of the thing