Migrated to ControlCore

CoreThing.h

@@ -1,7 +1,5 @@
 #pragma once
 
-#include "Participant.h"
-
 namespace Passer::Control {
 
 class CoreThing {
@@ -31,3 +29,4 @@ protected:
 };
 
 } // namespace Passer::Control
+using namespace Passer::Control;

LowLevelMessages.cpp

@@ -7,6 +7,14 @@ void LowLevelMessages::SendAngle8(unsigned char *buffer, unsigned char *ix,
   Angle8 packedAngle2 = Angle8::Degrees(angle);
   buffer[(*ix)++] = packedAngle2.GetBinary();
 }
+Angle8 LowLevelMessages::ReceiveAngle8(const unsigned char *buffer,
+                                       unsigned char *startIndex) {
+  unsigned char binary = buffer[(*startIndex)++];
+
+  Angle8 angle = Angle8::Binary(binary);
+
+  return angle;
+}
 
 void LowLevelMessages::SendFloat16(unsigned char *buffer, unsigned char *ix,
                                    float value) {
@@ -16,14 +24,36 @@ void LowLevelMessages::SendFloat16(unsigned char *buffer, unsigned char *ix,
   buffer[(*ix)++] = (binary >> 8) & 0xFF;
   buffer[(*ix)++] = binary & 0xFF;
 }
+float LowLevelMessages::ReceiveFloat16(const unsigned char *buffer,
+                                       unsigned char *startIndex) {
+  unsigned char ix = *startIndex;
+  unsigned short value = buffer[ix++] << 8 | buffer[ix++];
+  float16 f = float16();
+  f.setBinary(value);
 
-void Passer::Control::LowLevelMessages::SendSpherical16(unsigned char *buffer,
+  *startIndex = ix;
-                                                        unsigned char *ix,
+  return f.toDouble();
+}
+
+void LowLevelMessages::SendSpherical16(unsigned char *buffer, unsigned char *ix,
                                        Spherical16 s) {
   SendFloat16(buffer, ix, s.distance);
   SendAngle8(buffer, ix, s.direction.horizontal.InDegrees());
   SendAngle8(buffer, ix, s.direction.vertical.InDegrees());
 }
+Spherical16 LowLevelMessages::ReceiveSpherical16(const unsigned char *buffer,
+                                                 unsigned char *startIndex) {
+  float distance = ReceiveFloat16(buffer, startIndex);
+
+  Angle8 horizontal8 = ReceiveAngle8(buffer, startIndex);
+  Angle16 horizontal = Angle16::Binary(horizontal8.GetBinary() * 256);
+
+  Angle8 vertical8 = ReceiveAngle8(buffer, startIndex);
+  Angle16 vertical = Angle16::Binary(vertical8.GetBinary() * 256);
+
+  Spherical16 s = Spherical16(distance, horizontal, vertical);
+  return s;
+}
 
 void Passer::Control::LowLevelMessages::SendQuat32(unsigned char *buffer,
                                                    unsigned char *ix,
@@ -45,3 +75,14 @@ void Passer::Control::LowLevelMessages::SendQuat32(unsigned char *buffer,
   buffer[(*ix)++] = qz;
   buffer[(*ix)++] = qw;
 }
+
+SwingTwist16 LowLevelMessages::ReceiveQuat32(const unsigned char *buffer,
+                                             unsigned char *ix) {
+  float qx = (buffer[(*ix)++] - 128.0F) / 127.0F;
+  float qy = (buffer[(*ix)++] - 128.0F) / 127.0F;
+  float qz = (buffer[(*ix)++] - 128.0F) / 127.0F;
+  float qw = buffer[(*ix)++] / 255.0F;
+  Quaternion q = Quaternion(qx, qy, qz, qw);
+  SwingTwist16 s = SwingTwist16::FromQuaternion(q);
+  return s;
+}

LowLevelMessages.h

@@ -7,12 +7,23 @@ class LowLevelMessages {
 public:
   static void SendAngle8(unsigned char *buffer, unsigned char *ix,
                          const float angle);
+  static Angle8 ReceiveAngle8(const unsigned char *buffer,
+                              unsigned char *startIndex);
+
   static void SendFloat16(unsigned char *buffer, unsigned char *ix,
                           float value);
+  static float ReceiveFloat16(const unsigned char *buffer,
+                              unsigned char *startIndex);
+
   static void SendSpherical16(unsigned char *buffer, unsigned char *ix,
                               Spherical16 s);
+  static Spherical16 ReceiveSpherical16(const unsigned char *buffer,
+                                        unsigned char *startIndex);
+
   static void SendQuat32(unsigned char *buffer, unsigned char *ix,
                          SwingTwist16 q);
+  static SwingTwist16 ReceiveQuat32(const unsigned char *buffer,
+                                    unsigned char *ix);
 };
 
 } // namespace Passer::Control

Messages.cpp

@@ -1,21 +1,81 @@
 #include "Messages.h"
 
 #include "LowLevelMessages.h"
+#include "Participant.h"
 #include "string.h"
 
 #pragma region IMessage
 
+IMessage::IMessage() {}
+
+// IMessage::IMessage(unsigned char *buffer) { Deserialize(buffer); }
+
 unsigned char IMessage::Serialize(unsigned char *buffer) { return 0; }
 
-bool IMessage::SendMsg(Participant *client, IMessage msg) {
+// void IMessage::Deserialize(unsigned char *buffer) {}
-  // return SendMsg(client, client.buffer, );nameLength
+
-  return client->SendBuffer(msg.Serialize(client->buffer));
+// bool IMessage::SendMsg(Participant *client, IMessage msg) {
+//   // return SendMsg(client, client.buffer, );nameLength
+//   return client->SendBuffer(msg.Serialize(client->buffer));
+// }
+
+unsigned char *IMessage::ReceiveMsg(unsigned char packetSize) {
+  return nullptr;
 }
 
+bool IMessage::Publish(Participant *participant) {
+  return participant->PublishBuffer(Serialize(participant->buffer));
+}
+bool IMessage::Send(Participant *participant) {
+  return participant->SendBuffer(Serialize(participant->buffer));
+}
+
+// IMessage
+#pragma endregion
+
+#pragma region Client
+
+ClientMsg::ClientMsg(unsigned char networkId) { this->networkId = networkId; }
+
+unsigned char ClientMsg::Serialize(unsigned char *buffer) {
+  unsigned char ix = 0;
+  buffer[ix++] = this->id;
+  buffer[ix++] = this->networkId;
+  return ix;
+}
+
+// bool ClientMsg::Send(Participant *participant, unsigned char networkId) {
+//   ClientMsg msg = ClientMsg()
+// }
+// Client Msg
+#pragma endregion
+
+#pragma region Network Id
+
+NetworkIdMsg::NetworkIdMsg(unsigned char *buffer) {
+  this->networkId = buffer[1];
+}
+
+// void NetworkIdMsg::Deserialize(unsigned char *buffer) {
+//   this->networkId = buffer[1];
+// }
+
+NetworkIdMsg NetworkIdMsg::Receive(unsigned char *buffer,
+                                   unsigned char bufferSize) {
+  NetworkIdMsg msg = NetworkIdMsg(buffer);
+  return msg;
+}
+
+// Network Id
 #pragma endregion
 
 #pragma region Investigate
 
+InvestigateMsg::InvestigateMsg(unsigned char *buffer) {
+  unsigned ix = 1; // first byte is msgId
+  this->networkId = buffer[ix++];
+  this->thingId = buffer[ix++];
+}
 InvestigateMsg::InvestigateMsg(unsigned char networkId, unsigned char thingId) {
   this->networkId = networkId;
   this->thingId = thingId;
@@ -29,17 +89,25 @@ unsigned char InvestigateMsg::Serialize(unsigned char *buffer) {
   return ix;
 }
 
-bool InvestigateMsg::Send(Participant *client, unsigned char networkId,
+// bool InvestigateMsg::Send(Participant *participant, unsigned char networkId,
-                          unsigned char thingId) {
+//                           unsigned char thingId) {
-  InvestigateMsg msg = InvestigateMsg(networkId, thingId);
+//   InvestigateMsg msg = InvestigateMsg(networkId, thingId);
-  return SendMsg(client, msg);
+//   return msg.Send(participant);
-}
+// }
 
 // Investigate
 #pragma endregion
 
 #pragma region Thing
 
+ThingMsg::ThingMsg(const unsigned char *buffer) {
+  unsigned char ix = 1; // first byte is msg id
+  this->networkId = buffer[ix++];
+  this->thingId = buffer[ix++];
+  this->thingType = buffer[ix++];
+  this->parentId = buffer[ix++];
+}
+
 ThingMsg::ThingMsg(unsigned char networkId, unsigned char thingId,
                    unsigned char thingType, unsigned char parentId) {
   this->networkId = networkId;
@@ -58,12 +126,12 @@ unsigned char ThingMsg::Serialize(unsigned char *buffer) {
   return ix;
 }
 
-bool ThingMsg::Send(Participant *client, unsigned char networkId,
+// bool ThingMsg::Send(Participant *participant, unsigned char networkId,
-                    unsigned char thingId, unsigned char thingType,
+//                     unsigned char thingId, unsigned char thingType,
-                    unsigned char parentId) {
+//                     unsigned char parentId) {
-  ThingMsg msg = ThingMsg(networkId, thingId, thingType, parentId);
+//   ThingMsg msg = ThingMsg(networkId, thingId, thingType, parentId);
-  return SendMsg(client, msg);
+//   return msg.Send(participant);
-}
+// }
 
 // Thing
 #pragma endregion
@@ -90,17 +158,17 @@ unsigned char NameMsg::Serialize(unsigned char *buffer) {
   return ix;
 }
 
-bool NameMsg::Send(Participant *client, CoreThing *thing,
+// bool NameMsg::Send(Participant *participant, CoreThing *thing,
-                   unsigned char nameLength) {
+//                    unsigned char nameLength) {
-  if (thing->name == nullptr)
+//   if (thing->name == nullptr)
-    return true; // nothing sent, but still a success!
+//     return true; // nothing sent, but still a success!
 
-  if (strlen(thing->name) == 0)
+//   if (strlen(thing->name) == 0)
-    return true;
+//     return true;
 
-  NameMsg msg = NameMsg(thing->networkId, thing->id, thing->name, nameLength);
+//   NameMsg msg = NameMsg(thing->networkId, thing->id, thing->name,
-  return SendMsg(client, msg);
+//   nameLength); return msg.Send(participant);
-}
+// }
 
 // Name
 #pragma endregion
@@ -134,7 +202,6 @@ unsigned char ModelUrlMsg::Serialize(unsigned char *buffer) {
 
 #pragma region PoseMsg
 
-#include <Arduino.h>
 PoseMsg::PoseMsg(unsigned char networkId, unsigned char thingId,
                  unsigned char poseType, Spherical16 position,
                  SwingTwist16 orientation) {
@@ -144,6 +211,14 @@ PoseMsg::PoseMsg(unsigned char networkId, unsigned char thingId,
   this->orientation = orientation;
   this->poseType = poseType;
 }
+PoseMsg::PoseMsg(const unsigned char *buffer) {
+  unsigned char ix = 1; // First byte is msg id
+  this->networkId = buffer[ix++];
+  this->thingId = buffer[ix++];
+  this->poseType = buffer[ix++];
+  this->position = LowLevelMessages::ReceiveSpherical16(buffer, &ix);
+  this->orientation = LowLevelMessages::ReceiveQuat32(buffer, &ix);
+}
 
 unsigned char PoseMsg::Serialize(unsigned char *buffer) {
   unsigned char ix = 0;
@@ -152,11 +227,66 @@ unsigned char PoseMsg::Serialize(unsigned char *buffer) {
   buffer[ix++] = this->thingId;
   buffer[ix++] = this->poseType;
   LowLevelMessages::SendSpherical16(buffer, &ix, this->position);
-  printf("send spherical %f %f\n", this->position.distance,
-         this->position.direction.horizontal.InDegrees());
   LowLevelMessages::SendQuat32(buffer, &ix, this->orientation);
   return ix;
 }
 
 // Pose
 #pragma endregion
+
+#pragma region CustomMsg
+
+CustomMsg::CustomMsg(unsigned char *buffer) {
+  unsigned char ix;
+  this->networkId = buffer[ix++];
+  this->thingId = buffer[ix++];
+  this->dataSize = buffer[ix++];
+  this->data =
+      buffer + ix; // This is only valid because the code ensures the the msg
+                   // lifetime is shorter than the buffer lifetime...
+}
+
+CustomMsg::CustomMsg(unsigned char networkId, unsigned char thingId,
+                     unsigned char *data, unsigned char dataSize) {
+  this->networkId = networkId;
+  this->thingId = thingId;
+  this->dataSize = dataSize;
+  this->data = data;
+}
+
+unsigned char CustomMsg::Serialize(unsigned char *buffer) {
+  unsigned char ix = 0;
+  buffer[ix++] = this->id;
+  buffer[ix++] = this->networkId;
+  buffer[ix++] = this->thingId;
+  for (int dataIx = 0; dataIx < this->dataSize; dataIx++)
+    buffer[ix++] = this->data[dataIx];
+  return ix;
+}
+
+// void CustomMsg::Deserialize(unsigned char *buffer) {
+//   unsigned char ix;
+//   this->networkId = buffer[ix++];
+//   this->thingId = buffer[ix++];
+//   this->dataSize = buffer[ix++];
+//   this->data = buffer + ix; // challenging: point directly into the buffer!
+
+//   // this->data = new unsigned char[this->dataSize]; // memory leak!
+//   // for (unsigned char dataIx = 0; dataIx < this->dataSize; dataIx++)
+//   //   this->data[dataIx] = buffer[ix++];
+// }
+
+// bool CustomMsg::Send(Participant *participant, unsigned char networkId,
+//                      unsigned char thingId, unsigned char *data,
+//                      unsigned char dataSize) {
+//   CustomMsg msg = CustomMsg(networkId, thingId, data, dataSize);
+//   return msg.Send(participant);
+// }
+
+CustomMsg CustomMsg::Receive(unsigned char *buffer, unsigned char bufferSize) {
+  CustomMsg msg = CustomMsg(buffer);
+  return msg;
+}
+
+// CustomMsg
+#pragma endregion

Messages.h

@@ -1,17 +1,43 @@
 #pragma once
+
 #include "../LinearAlgebra/Spherical.h"
 #include "../LinearAlgebra/SwingTwist.h"
 #include "../float16/float16.h"
 #include "CoreThing.h"
-#include "Participant.h"
 
 namespace Passer::Control {
 
+class Participant;
+
 class IMessage {
 public:
+  IMessage();
   virtual unsigned char Serialize(unsigned char *buffer);
 
-  static bool SendMsg(Participant *client, IMessage msg);
+  static unsigned char *ReceiveMsg(unsigned char packetSize);
+
+  bool Publish(Participant *participant);
+  bool Send(Participant *participant);
+};
+
+class ClientMsg : public IMessage {
+public:
+  static const unsigned char id = 0xA0;
+  unsigned char networkId;
+
+  ClientMsg(unsigned char networkId);
+  virtual unsigned char Serialize(unsigned char *buffer) override;
+};
+
+class NetworkIdMsg : public IMessage {
+public:
+  static const unsigned char id = 0xA1;
+  static const unsigned char length = 2;
+  unsigned char networkId;
+
+  NetworkIdMsg(unsigned char *buffer);
+
+  static NetworkIdMsg Receive(unsigned char *buffer, unsigned char bufferSize);
 };
 
 class InvestigateMsg : public IMessage {
@@ -21,12 +47,10 @@ public:
   unsigned char networkId;
   unsigned char thingId;
 
+  InvestigateMsg(unsigned char *buffer);
   InvestigateMsg(unsigned char networkId, unsigned char thingId);
 
   virtual unsigned char Serialize(unsigned char *buffer) override;
-
-  static bool Send(Participant *client, unsigned char networkId,
-                   unsigned char thingId);
 };
 
 class ThingMsg : public IMessage {
@@ -38,14 +62,11 @@ public:
   unsigned char thingType;
   unsigned char parentId;
 
+  ThingMsg(const unsigned char *buffer);
   ThingMsg(unsigned char networkId, unsigned char thingId,
            unsigned char thingType, unsigned char parentId);
 
   virtual unsigned char Serialize(unsigned char *buffer) override;
-
-  static bool Send(Participant *client, unsigned char networkId,
-                   unsigned char thingId, unsigned char thingType,
-                   unsigned char parentId);
 };
 
 class NameMsg : public IMessage {
@@ -61,9 +82,6 @@ public:
           unsigned char nameLength);
 
   virtual unsigned char Serialize(unsigned char *buffer) override;
-
-  static bool Send(Participant *client, CoreThing *thing,
-                   unsigned char nameLength);
 };
 
 class ModelUrlMsg : public IMessage {
@@ -92,8 +110,10 @@ public:
   unsigned char thingId;
 
   unsigned char poseType;
-  const unsigned char Pose_Position = 0x01;
+  static const unsigned char Pose_Position = 0x01;
-  const unsigned char Pose_Orientation = 0x02;
+  static const unsigned char Pose_Orientation = 0x02;
+  static const unsigned char Pose_LinearVelocity = 0x04;  // For future use
+  static const unsigned char Pose_AngularVelocity = 0x08; // For future use
 
   Spherical16 position;
   SwingTwist16 orientation;
@@ -101,9 +121,31 @@ public:
   PoseMsg(unsigned char networkId, unsigned char thingId,
           unsigned char poseType, Spherical16 position,
           SwingTwist16 orientation);
+  PoseMsg(const unsigned char *buffer);
 
   virtual unsigned char Serialize(unsigned char *buffer) override;
 };
 
+class CustomMsg : public IMessage {
+public:
+  static const unsigned char id = 0xB1;
+  static const unsigned length = 3;
+
+  unsigned char networkId;
+  unsigned char thingId;
+
+  unsigned char dataSize;
+  unsigned char *data;
+
+  CustomMsg(unsigned char *buffer);
+  CustomMsg(unsigned char networkId, unsigned char thingId, unsigned char *data,
+            unsigned char dataSize);
+
+  virtual unsigned char Serialize(unsigned char *buffer) override;
+
+  static CustomMsg Receive(unsigned char *buffer, unsigned char bufferSize);
+};
+
 } // namespace Passer::Control
+
 using namespace Passer::Control;

Participant.cpp

@@ -1,10 +1,40 @@
 #include "Participant.h"
 
-// bool Passer::Control::ControlClient::SendMsg(unsigned char *buffer,
-//                                              unsigned char bufferSize) {
-//   return false;
-// }
-
 bool Participant::SendBuffer(unsigned char bufferSize) { return false; }
 
 bool Participant::PublishBuffer(unsigned char bufferSize) { return false; }
+
+void Participant::ReceiveData(unsigned char bufferSize) {
+  unsigned char msgId = this->buffer[0];
+  switch (msgId) {
+  case NetworkIdMsg::id: {
+    // NetworkIdMsg msg = NetworkIdMsg::Receive(this->buffer, bufferSize);
+    NetworkIdMsg msg = NetworkIdMsg(this->buffer);
+    ProcessNetworkIdMsg(msg);
+  } break;
+  case InvestigateMsg::id: {
+    InvestigateMsg msg = InvestigateMsg(this->buffer);
+    ProcessInvestigateMsg(msg);
+  } break;
+  case ThingMsg::id: {
+    ThingMsg msg = ThingMsg(this->buffer);
+    ProcessThingMsg(msg);
+  } break;
+  case PoseMsg::id: {
+    PoseMsg msg = PoseMsg(this->buffer);
+    ProcessPoseMsg(msg);
+  } break;
+  case CustomMsg::id: {
+    CustomMsg msg = CustomMsg(this->buffer);
+    ProcessCustomMsg(msg);
+  } break;
+  };
+}
+
+void Participant::ProcessNetworkIdMsg(NetworkIdMsg msg) {}
+
+void Participant::ProcessInvestigateMsg(InvestigateMsg msg) {}
+
+void Participant::ProcessThingMsg(ThingMsg msg) {}
+
+void Participant::ProcessCustomMsg(CustomMsg msg) {}

Participant.h

@@ -1,15 +1,24 @@
 #pragma once
 
+#include "Messages.h"
+
 namespace Passer::Control {
 
 class Participant {
 public:
-  // unsigned char *buffer;
-  // bool SendMsg(unsigned char *buffer, unsigned char bufferSize);
-
   unsigned char buffer[1024];
+
   virtual bool SendBuffer(unsigned char bufferSize);
   virtual bool PublishBuffer(unsigned char bufferSize);
+
+  void ReceiveData(unsigned char bufferSize);
+
+protected:
+  virtual void ProcessNetworkIdMsg(NetworkIdMsg msg);
+  virtual void ProcessInvestigateMsg(InvestigateMsg msg);
+  virtual void ProcessThingMsg(ThingMsg msg);
+  virtual void ProcessPoseMsg(PoseMsg msg);
+  virtual void ProcessCustomMsg(CustomMsg msg);
 };
 
 } // namespace Passer::Control