diff --git a/ControlCore/CoreThing.h b/ControlCore/CoreThing.h
index 8b36634..025f372 100644
--- a/ControlCore/CoreThing.h
+++ b/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;
\ No newline at end of file
diff --git a/ControlCore/LowLevelMessages.cpp b/ControlCore/LowLevelMessages.cpp
index e80ebb1..247368f 100644
--- a/ControlCore/LowLevelMessages.cpp
+++ b/ControlCore/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,
-                                                        unsigned char *ix,
-                                                        Spherical16 s) {
+  *startIndex = 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;
+}
\ No newline at end of file
diff --git a/ControlCore/LowLevelMessages.h b/ControlCore/LowLevelMessages.h
index 4af565d..65dcb9b 100644
--- a/ControlCore/LowLevelMessages.h
+++ b/ControlCore/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
diff --git a/ControlCore/Messages.cpp b/ControlCore/Messages.cpp
index 459c59e..e5562b5 100644
--- a/ControlCore/Messages.cpp
+++ b/ControlCore/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) {
-  // return SendMsg(client, client.buffer, );nameLength
-  return client->SendBuffer(msg.Serialize(client->buffer));
+// void IMessage::Deserialize(unsigned char *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,
-                          unsigned char thingId) {
-  InvestigateMsg msg = InvestigateMsg(networkId, thingId);
-  return SendMsg(client, msg);
-}
+// bool InvestigateMsg::Send(Participant *participant, unsigned char networkId,
+//                           unsigned char thingId) {
+//   InvestigateMsg msg = InvestigateMsg(networkId, thingId);
+//   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,
-                    unsigned char thingId, unsigned char thingType,
-                    unsigned char parentId) {
-  ThingMsg msg = ThingMsg(networkId, thingId, thingType, parentId);
-  return SendMsg(client, msg);
-}
+// bool ThingMsg::Send(Participant *participant, unsigned char networkId,
+//                     unsigned char thingId, unsigned char thingType,
+//                     unsigned char parentId) {
+//   ThingMsg msg = ThingMsg(networkId, thingId, thingType, parentId);
+//   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,
-                   unsigned char nameLength) {
-  if (thing->name == nullptr)
-    return true; // nothing sent, but still a success!
+// bool NameMsg::Send(Participant *participant, CoreThing *thing,
+//                    unsigned char nameLength) {
+//   if (thing->name == nullptr)
+//     return true; // nothing sent, but still a success!
 
-  if (strlen(thing->name) == 0)
-    return true;
+//   if (strlen(thing->name) == 0)
+//     return true;
 
-  NameMsg msg = NameMsg(thing->networkId, thing->id, thing->name, nameLength);
-  return SendMsg(client, msg);
-}
+//   NameMsg msg = NameMsg(thing->networkId, thing->id, thing->name,
+//   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
\ No newline at end of file
+#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
diff --git a/ControlCore/Messages.h b/ControlCore/Messages.h
index f62eb1e..8436455 100644
--- a/ControlCore/Messages.h
+++ b/ControlCore/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;
-  const unsigned char Pose_Orientation = 0x02;
+  static const unsigned char Pose_Position = 0x01;
+  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;
\ No newline at end of file
diff --git a/ControlCore/Participant.cpp b/ControlCore/Participant.cpp
index 1056dd0..233ac74 100644
--- a/ControlCore/Participant.cpp
+++ b/ControlCore/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) {}
\ No newline at end of file
diff --git a/ControlCore/Participant.h b/ControlCore/Participant.h
index 1ab3190..ca033d7 100644
--- a/ControlCore/Participant.h
+++ b/ControlCore/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
diff --git a/DirectionalSensor.cpp b/DirectionalSensor.cpp
index cc988c8..6679169 100644
--- a/DirectionalSensor.cpp
+++ b/DirectionalSensor.cpp
@@ -1,6 +1,6 @@
 #include "DirectionalSensor.h"
 
-#include "Messages.h"
+#include "ControlCore/LowLevelMessages.h"
 
 DirectionalSensor::DirectionalSensor() : Sensor() {
   this->type = DirectionalSensor::Type;
@@ -11,5 +11,5 @@ Spherical16 DirectionalSensor::GetVector() { return Spherical16::zero; }
 
 void DirectionalSensor::ProcessBytes(unsigned char *bytes) {
   unsigned char ix = 0;
-  this->vector = Messages::ReceiveSpherical16(bytes, &ix);
+  this->vector = LowLevelMessages::ReceiveSpherical16(bytes, &ix);
 }
diff --git a/DistanceSensor.cpp b/DistanceSensor.cpp
index 2aa06f9..a2a759b 100644
--- a/DistanceSensor.cpp
+++ b/DistanceSensor.cpp
@@ -1,7 +1,7 @@
 #include "DistanceSensor.h"
 
 // #include "NetworkPerception.h"
-#include "Messages.h"
+#include "ControlCore/LowLevelMessages.h"
 #include <math.h>
 
 DistanceSensor::DistanceSensor() {
@@ -33,5 +33,5 @@ bool DistanceSensor::ObjectNearby() {
 
 void DistanceSensor::ProcessBytes(unsigned char *bytes) {
   unsigned char ix = 0;
-  this->distance = Messages::ReceiveFloat16(bytes, &ix);
+  this->distance = LowLevelMessages::ReceiveFloat16(bytes, &ix);
 }
diff --git a/Messages.cpp b/Messages.cpp
deleted file mode 100644
index 5577ca4..0000000
--- a/Messages.cpp
+++ /dev/null
@@ -1,146 +0,0 @@
-#include "Messages.h"
-
-#include "float16/float16.h"
-
-Angle8 Messages::ReceiveAngle8(unsigned char *buffer,
-                               unsigned char *startIndex) {
-  unsigned char binary = buffer[(*startIndex)++];
-
-  Angle8 angle = Angle8::Binary(binary);
-
-  return angle;
-}
-
-float Messages::ReceiveFloat16(unsigned char *buffer,
-                               unsigned char *startIndex) {
-  unsigned char ix = *startIndex;
-  unsigned short value = buffer[ix++] << 8 | buffer[ix++];
-  float16 f = float16();
-  f.setBinary(value);
-
-  *startIndex = ix;
-  return f.toDouble();
-}
-
-Spherical16 Messages::ReceiveSpherical16(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 Messages::SendVector3(unsigned char *buffer, unsigned char *startIndex,
-                           const Vector3 v) {
-  SendSingle100(buffer, *startIndex, v.Right());
-  (*startIndex) += 4;
-  SendSingle100(buffer, *startIndex, v.Up());
-  (*startIndex) += 4;
-  SendSingle100(buffer, *startIndex, v.Forward());
-  (*startIndex) += 4;
-}
-
-void Messages::SendQuaternion(unsigned char *buffer, const int startIndex,
-                              const Quaternion q) {
-  Vector3 angles = Quaternion::ToAngles(q);
-  int ix = startIndex;
-  SendAngle8(buffer, ix++, angles.Right());
-  SendAngle8(buffer, ix++, angles.Up());
-  SendAngle8(buffer, ix++, angles.Forward());
-}
-
-void Messages::SendPolar(unsigned char *buffer, unsigned char *startIndex,
-                         Polar p) {
-  SendAngle8(buffer, *startIndex, (const float)p.angle.InDegrees());
-  SendSingle100(buffer, (*startIndex) + 1, p.distance);
-}
-
-// void Messages::SendSpherical16(unsigned char *buffer, unsigned char
-// *startIndex,
-//                                Spherical16 s) {
-//   // Receive first does distance...
-//   SendAngle8(buffer, (*startIndex)++, s.direction.horizontal.InDegrees());
-//   SendAngle8(buffer, (*startIndex)++, s.direction.vertical.InDegrees());
-//   SendFloat16(buffer, startIndex, s.distance);
-// }
-
-void Messages::SendSwingTwist(unsigned char *buffer, unsigned char *ix,
-                              const SwingTwist16 r) {
-  Quaternion q = r.ToQuaternion();
-  SendQuat32(buffer, ix, q);
-}
-
-void Messages::SendQuat32(unsigned char *buffer, unsigned char *startIndex,
-                          const Quaternion q) {
-  unsigned char qx = (char)(q.x * 127 + 128);
-  unsigned char qy = (char)(q.y * 127 + 128);
-  unsigned char qz = (char)(q.z * 127 + 128);
-  unsigned char qw = (char)(q.w * 255);
-  if (q.w < 0) {
-    qx = -qx;
-    qy = -qy;
-    qz = -qz;
-    qw = -qw;
-  }
-  // Serial.printf(" (%d) %d:%d:%d:%d ", startIndex, qx, qy, qz, qw);
-  buffer[(*startIndex)++] = qx;
-  buffer[(*startIndex)++] = qy;
-  buffer[(*startIndex)++] = qz;
-  buffer[(*startIndex)++] = qw;
-}
-
-void Messages::SendAngle8(unsigned char *buffer, unsigned int startIndex,
-                          const float angle) {
-  Angle8 packedAngle2 = Angle8::Degrees(angle);
-  buffer[startIndex] = packedAngle2.GetBinary();
-}
-
-// void NetworkSync::SendAngle16(unsigned char *data, unsigned int startIndex,
-//                               const float angle) {
-//   AngleUsing<signed short> packedAngle = AngleUsing<signed short>(angle);
-//   signed short value = packedAngle.GetValue();
-//   data[startIndex] = value >> 8;
-//   data[startIndex + 1] = value & 0xFF;
-// }
-
-// void NetworkSync::SendAngle32(unsigned char *data, unsigned int startIndex,
-//                               const float angle) {
-//   AngleUsing<signed long> packedAngle = AngleUsing<signed long>(angle);
-//   unsigned long value = packedAngle.GetValue();
-//   data[startIndex] = value >> 24 & 0xFF;
-//   data[startIndex + 1] = value >> 16 & 0xFF;
-//   data[startIndex + 2] = value >> 8 & 0xFF;
-//   data[startIndex + 3] = value & 0xFF;
-//   // Serial.printf(" %lu=%d:%d:%d:%d ", value, data[startIndex],
-//   //               data[startIndex + 1], data[startIndex + 2],
-//   //               data[startIndex + 3]);
-// }
-
-// void Messages::SendSingle100(unsigned char *data, unsigned int startIndex,
-//                              float value) {
-//   // Sends a float with truncated 2 decimal precision
-//   Int32 intValue = value * 100;
-//   SendInt32(data, startIndex, intValue);
-// }
-
-void Messages::SendFloat16(unsigned char *data, unsigned char *startIndex,
-                           float value) {
-  float16 value16 = float16(value);
-  short binary = value16.getBinary();
-
-  data[(*startIndex)++] = (binary >> 8) & 0xFF;
-  data[(*startIndex)++] = binary & 0xFF;
-}
-
-// void Messages::SendInt32(unsigned char *data, unsigned int startIndex,
-//                          Int32 value) {
-//   for (unsigned char ix = 0; ix < 4; ix++) {
-//     data[startIndex++] = ((unsigned char *)&value)[ix];
-//   }
-// }
diff --git a/Messages.h b/Messages.h
deleted file mode 100644
index ab79673..0000000
--- a/Messages.h
+++ /dev/null
@@ -1,46 +0,0 @@
-#pragma once
-
-#include "Roboid.h"
-
-namespace Passer {
-namespace RoboidControl {
-
-class Messages {
-public:
-  static Angle8 ReceiveAngle8(unsigned char *buffer, unsigned char *startIndex);
-  static float ReceiveFloat16(unsigned char *buffer, unsigned char *startIndex);
-  static Spherical16 ReceiveSpherical16(unsigned char *buffer,
-                                        unsigned char *startIndex);
-
-  static void SendSingle100(unsigned char *buffer, unsigned int startIndex,
-                            float value);
-  static void SendFloat16(unsigned char *buffer, unsigned char *startIndex,
-                          float value);
-
-  // void SendInt32(unsigned char *buffer, unsigned int startIndex, Int32
-  // value);
-  static void SendAngle8(unsigned char *buffer, unsigned int startIndex,
-                         const float value);
-  // void SendAngle16(unsigned char *buffer, unsigned int startIndex,
-  //                  const float value);
-  // void SendAngle32(unsigned char *buffer, unsigned int startIndex,
-  //                  const float value);
-  static void SendVector3(unsigned char *buffer, unsigned char *startIndex,
-                          const Vector3 v);
-  static void SendQuaternion(unsigned char *buffer, const int startIndex,
-                             const Quaternion q);
-
-  static void SendPolar(unsigned char *buffer, unsigned char *startIndex,
-                        Polar p);
-  // static void SendSpherical16(unsigned char *buffer, unsigned char
-  // *startIndex,
-  //                             Spherical16 s);
-  static void SendSwingTwist(unsigned char *buffer, unsigned char *startIndex,
-                             const SwingTwist16 r);
-  static void SendQuat32(unsigned char *buffer, unsigned char *startIndex,
-                         const Quaternion q);
-};
-
-} // namespace RoboidControl
-} // namespace Passer
-using namespace Passer::RoboidControl;
\ No newline at end of file
diff --git a/NetworkPerception.cpp b/NetworkPerception.cpp
deleted file mode 100644
index ff3906c..0000000
--- a/NetworkPerception.cpp
+++ /dev/null
@@ -1,350 +0,0 @@
-#include "NetworkPerception.h"
-
-#include "ControlCore/Messages.h"
-#include "NetworkSync.h"
-#include "float16/float16.h"
-
-#define RC_DEBUG true
-#if RC_DEBUG
-#include <Arduino.h>
-#endif
-
-void NetworkPerception::ProcessPacket(Roboid *roboid, unsigned char *buffer,
-                                      int packetsize) {
-#if RC_DEBUG
-  // printf("packet received, type = 0x%02x %d %d %d %d %d\n", buffer[0],
-  //        buffer[2], buffer[2], buffer[3], buffer[4], buffer[5]);
-#endif
-
-  switch (buffer[0]) {
-  case NetworkSync::ThingMsg:
-    ReceiveThingMsg(buffer, roboid);
-    break;
-  case NetworkSync::InvestigateMsg:
-    ReceiveInvestigateMsg(buffer, roboid);
-    break;
-  case NetworkSync::PoseMsg:
-    ReceivePoseMsg(buffer, roboid);
-    break;
-  case NetworkSync::PoseTypeMsg:
-    ReceiveTypedObject(buffer, roboid);
-    break;
-  }
-}
-
-void NetworkPerception::ReceiveThingMsg(unsigned char *data, Roboid *roboid) {
-  unsigned char networkId = data[1];
-  unsigned char objectId = data[2];
-  unsigned char objectType = data[3];
-  if (networkId == roboid->networkSync->networkId)
-    return;
-
-  printf("Received create message [%d/%d]\n", networkId, objectId);
-  InterestingThing *thing =
-      roboid->perception->FindTrackedObject(networkId, objectId);
-  if (thing != nullptr) {
-    thing->type = objectType;
-  }
-  printf("complete\n");
-}
-
-void NetworkPerception::ReceiveInvestigateMsg(unsigned char *data,
-                                              Roboid *roboid) {
-  unsigned char networkId = data[1];
-  unsigned char thingId = data[2];
-
-  // #if RC_DEBUG
-  printf("Received InvestigateMsg [%d/%d]\n", networkId, thingId);
-  // #endif
-
-  if (networkId != roboid->networkSync->networkId)
-    // We only response to investigation requests for our own objects
-    return;
-
-  if (thingId == 0x00) {
-    // They are investigating the roboid itself!
-    if (roboid->modelUrl != nullptr)
-      // roboid->networkSync->SendModel(roboid);
-      roboid->networkSync->SendThingInfo(roboid);
-  } else {
-    Thing *thing = roboid->FindChild(thingId, true);
-    if (thing != nullptr)
-      roboid->networkSync->SendThingInfo(thing);
-    else {
-      InterestingThing *interestingThing =
-          roboid->perception->FindTrackedObject(0x00, thingId);
-      if (interestingThing == nullptr)
-        return;
-
-      roboid->networkSync->SendThingInfo(interestingThing, false);
-    }
-  }
-}
-
-void NetworkPerception::ReceivePlane(unsigned char *data, Roboid *roboid) {
-  unsigned char networkId = data[1];
-  unsigned char poseType = data[3];
-  if ((poseType & NetworkSync::Pose_Position) == 0 ||
-      (poseType & NetworkSync::Pose_Orientation) == 0)
-
-    // both position and orientation are required
-    return;
-
-  Spherical16 worldPosition = Spherical16::FromVector3(ReceiveVector3(data, 4));
-  /*
-  Spherical16 roboidPosition = roboid->GetPosition();
-  Spherical16 deltaPosition = worldPosition - roboidPosition;
-
-  float distance = deltaPosition.distance; // magnitude();
-  if (roboid->perception->IsInteresting(distance) == false) {
-    // printf("  plane not interesting\n");
-    return;
-  }
-
-  SwingTwist16 roboidOrientation = roboid->GetOrientation();
-  Spherical16 localPosition =
-      SwingTwist16::Inverse(roboidOrientation) * deltaPosition;
-  // float angle =
-  //     Vector3::SignedAngle(Vector3::forward, localPosition, Vector3::up);
-
-  // Polar position = Polar(angle, distance);
-  Spherical16 position =
-      localPosition; // Spherical16::FromVector3(localPosition);
-  */
-  SwingTwist16 originOrientation;
-  Spherical16 originPosition;
-  if (roboid->worldOrigin == nullptr) {
-    originOrientation = SwingTwist16::identity;
-    originPosition = Spherical16::zero;
-  } else {
-    originOrientation = roboid->worldOrigin->orientation;
-    originPosition = roboid->worldOrigin->position;
-  }
-  Spherical16 position = originPosition + originOrientation * worldPosition;
-  // printf("Received plane (%f %f %f) (%f %f %f) %f %f %f\n",
-  // worldPosition.x,
-  //        worldPosition.y, worldPosition.z, roboidPosition.x,
-  //        roboidPosition.y, roboidPosition.z, position.distance,
-  //        (float)position.horizontalAngle, (float)position.verticalAngle);
-  // printf("Received plane %f (%f %f)\n", position.distance,
-  //        (float)position.horizontalAngle, (float)position.verticalAngle);
-  roboid->perception->AddTrackedObject(this, position, SwingTwist16::identity,
-                                       0x80, 0x80, networkId);
-}
-
-void NetworkPerception::ReceiveSphere(unsigned char *data, Roboid *roboid) {
-  unsigned char networkId = data[1];
-
-  float radius = ReceiveFloat100(data, 3);
-
-  Spherical16 worldPosition = Spherical16::FromVector3(ReceiveVector3(data, 7));
-  /*
-  Spherical16 roboidPosition = roboid->GetPosition();
-  Spherical16 deltaPosition = worldPosition - roboidPosition;
-
-  SwingTwist16 roboidOrientation = roboid->GetOrientation();
-  Spherical16 localPosition =
-      SwingTwist16::Inverse(roboidOrientation) * deltaPosition;
-
-  Spherical16 position =
-      localPosition; // Spherical16::FromVector3(localPosition);
-  */
-  SwingTwist16 originOrientation;
-  Spherical16 originPosition;
-  if (roboid->worldOrigin == nullptr) {
-    originOrientation = SwingTwist16::identity;
-    originPosition = Spherical16::zero;
-  } else {
-    originOrientation = roboid->worldOrigin->orientation;
-    originPosition = roboid->worldOrigin->position;
-  }
-  Spherical16 position = originPosition + originOrientation * worldPosition;
-
-  roboid->perception->AddTrackedObject(this, position, SwingTwist16::identity,
-                                       0x81, 0x81, networkId);
-}
-
-void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) {
-  if (roboid->networkSync->networkId == 0) // We're not connected to a site yet
-    return;
-
-  unsigned char networkId = data[1];
-  unsigned char objectId = data[2];
-  unsigned char poseType = data[3];
-
-  if (networkId == roboid->networkSync->networkId)
-    networkId = 0x00;
-
-    // if (objectId == 0x80)
-    //   return ReceivePlane(data, roboid);
-    // else if (objectId == 0x81)
-    //   return ReceiveSphere(data, roboid);
-
-#if RC_DEBUG
-    // printf("Received PoseMsg [%d/%d]\n", networkId, objectId);
-#endif
-
-  // InterestingThing *thing =
-  //     roboid->perception->FindTrackedObject(networkId, objectId);
-  // if (thing == nullptr) {
-  //   thing = roboid->perception->AddTrackedObject(this, position, orientation,
-  //                                                0xFF, objectId, networkId);
-  //   if (thing->networkId != 0x00) {
-  //     // Unknown thing
-  //     InvestigateMsg::Send(thing);
-  //     // roboid->networkSync->SendInvestigate(thing);
-  //   }
-  // }
-
-  // SwingTwist16 roboidOrientation = roboid->GetOrientation();
-  // Spherical16 position = Spherical16::zero;
-  // SwingTwist16 orientation = SwingTwist16::identity;
-
-  // Vector3 worldAngles = ReceiveVector3(data, 16);
-  // SwingTwist16 worldOrientation = SwingTwist16(
-  //     Angle16::Degrees(worldAngles.Up()),
-  //     Angle16::Degrees(worldAngles.Right()), Angle16::Degrees(
-  //         worldAngles.Forward())); // Quaternion::Euler(worldAngles);
-
-  // if ((poseType & NetworkSync::Pose_Orientation) != 0) {
-  //   if (objectId == 0) {
-  //     // roboid->SetOrientation(worldOrientation);
-  //     if (roboid->worldOrigin == nullptr) {
-  //       printf("creating new origin\n");
-  //       roboid->worldOrigin = new Thing(0);
-  //       printf("created\n");
-  //     }
-
-  //     roboid->worldOrigin->orientation =
-  //         SwingTwist16::Inverse(worldOrientation);
-  //   } else {
-  //     // orientation = SwingTwist16::Inverse(roboidOrientation) *
-  //     // worldOrientation; if (thing != nullptr) {
-  //     //   thing->orientation = orientation;
-  //     // }
-  //     SwingTwist16 originOrientation;
-  //     Spherical16 originPosition;
-  //     if (roboid->worldOrigin == nullptr) {
-  //       originOrientation = SwingTwist16::identity;
-  //       originPosition = Spherical16::zero;
-  //     } else {
-  //       originOrientation = roboid->worldOrigin->orientation;
-  //       originPosition = roboid->worldOrigin->position;
-  //     }
-
-  //     thing->orientation = originOrientation * worldOrientation;
-  //   }
-  // }
-
-  // if ((poseType & NetworkSync::Pose_Position) != 0) {
-  //   Vector3 worldVector3 = ReceiveVector3(data, 4);
-  //   Spherical16 worldPosition = Spherical16::FromVector3(worldVector3);
-  //   if (objectId == 0) {
-  //     // roboid->SetPosition(worldPosition);
-  //     if (roboid->worldOrigin == nullptr) {
-  //       printf("creating new origin again\n");
-  //       roboid->worldOrigin = new Thing(0);
-  //     }
-
-  //     roboid->worldOrigin->position =
-  //         roboid->worldOrigin->orientation * -worldPosition;
-  //   } else {
-  //     SwingTwist16 originOrientation;
-  //     Spherical16 originPosition;
-  //     if (roboid->worldOrigin == nullptr) {
-  //       originOrientation = SwingTwist16::identity;
-  //       originPosition = Spherical16::zero;
-  //     } else {
-  //       originOrientation = roboid->worldOrigin->orientation;
-  //       originPosition = roboid->worldOrigin->position;
-  //     }
-
-  //     // SwingTwist16 roboidLocalOrientation =
-  //     //     originOrientation * worldOrientation;
-
-  //     thing->position = originPosition + originOrientation * worldPosition;
-
-  //     /*
-  //     Spherical16 roboidPosition = roboid->GetPosition();
-
-  //     // float distance = Vector3::Distance(roboidPosition, worldPosition);
-  //     // if (roboid->perception->IsInteresting(distance) == false) {
-  //     //   // printf("  not interesting\n");
-  //     //   return;
-  //     // }
-
-  //     Spherical16 localPosition = SwingTwist16::Inverse(roboidOrientation) *
-  //                                 (worldPosition - roboidPosition);
-  //     Vector3 roboidVector3 = roboidPosition.ToVector3();
-  //     printf("  roboid position (%f %f %f)\n", roboidVector3.Right(),
-  //            roboidVector3.Up(), roboidVector3.Forward());
-  //     printf("  [%d/%d] worldPosition (%f %f %f) localPosition %f (%f %f)\n
-  //     ",
-  //            networkId, objectId, worldVector3.Right(), worldVector3.Up(),
-  //            worldVector3.Forward(), localPosition.distance,
-  //            localPosition.direction.horizontal.InDegrees(),
-  //            localPosition.direction.vertical.InDegrees());
-
-  //     thing->position = localPosition;
-  //     */
-  //   }
-  // }
-}
-
-void NetworkPerception::ReceiveTypedObject(unsigned char *data,
-                                           Roboid *roboid) {
-  unsigned char objectType = data[1];
-  unsigned char objectId = data[2];
-  if (objectType == 0x02) { // lighthouse
-    // We require position and orientation for lighthouses
-    if (data[3] !=
-        (NetworkSync::Pose_Position | NetworkSync::Pose_Orientation)) {
-      return;
-    }
-
-    Vector3 worldPosition = ReceiveVector3(data, 4);
-    Vector3 worldAngles = ReceiveVector3(data, 16);
-    Quaternion worldOrientation = Quaternion::Euler(worldAngles);
-
-    // roboid->perception->AddTrackedObject(this, objectType, worldPosition,
-    //                                      worldOrientation);
-    // Sensor *sensor =
-    //     roboid->perception->FindSensorOfType(0x82); // Lighthouse type
-    //     sensor
-    // LighthouseTracker *lhSensor = (LighthouseTracker *)sensor;
-    // lhSensor->UpdateGeometry(objectId, worldPosition, worldOrientation);
-  }
-}
-
-Int32 NetworkPerception::ReceiveInt32(unsigned char *data, int startIndex) {
-  Int32 a = Int32((UInt32)(data[startIndex + 3]) << 24 |
-                  (UInt32)(data[startIndex + 2]) << 16 |
-                  (UInt32)(data[startIndex + 1]) << 8 |
-                  (UInt32)(data[startIndex + 0]));
-  return a;
-}
-
-float NetworkPerception::ReceiveFloat100(unsigned char *data, int startIndex) {
-  Int32 intValue = ReceiveInt32(data, startIndex);
-  float f = (float)intValue / 100.0F;
-  return f;
-}
-
-Vector3 NetworkPerception::ReceiveVector3(unsigned char *data, int startIndex) {
-  float x = ReceiveFloat100(data, startIndex);
-  float y = ReceiveFloat100(data, startIndex + 4);
-  float z = ReceiveFloat100(data, startIndex + 8);
-  Vector3 v = Vector3(x, y, z);
-  return v;
-}
-
-float NetworkPerception::ReceiveFloat16(unsigned char *data,
-                                        unsigned char *startIndex) {
-  byte ix = *startIndex;
-  unsigned short value = data[ix++] << 8 | data[ix];
-  float16 f = float16();
-  f.setBinary(value);
-
-  *startIndex = ix;
-  return f.toDouble();
-}
\ No newline at end of file
diff --git a/NetworkPerception.h b/NetworkPerception.h
deleted file mode 100644
index 68f975f..0000000
--- a/NetworkPerception.h
+++ /dev/null
@@ -1,32 +0,0 @@
-#pragma once
-
-#include "Roboid.h"
-#include "Sensor.h"
-#include "Types.h"
-
-namespace Passer {
-namespace RoboidControl {
-
-class NetworkPerception : public Sensor {
-public:
-  void ProcessPacket(Roboid *roboid, unsigned char *buffer, int packetsize);
-
-protected:
-  void ReceiveThingMsg(unsigned char *data, Roboid *roboid);
-  void ReceiveInvestigateMsg(unsigned char *data, Roboid *roboid);
-  void ReceivePoseMsg(unsigned char *data, Roboid *roboid);
-  void ReceiveTypedObject(unsigned char *data, Roboid *roboid);
-
-  void ReceivePlane(unsigned char *data, Roboid *roboid);
-  void ReceiveSphere(unsigned char *data, Roboid *roboid);
-
-  Int32 ReceiveInt32(unsigned char *data, int startIndex);
-  float ReceiveFloat100(unsigned char *data, int startIndex);
-  Vector3 ReceiveVector3(unsigned char *data, int startIndex);
-
-public:
-  static float ReceiveFloat16(unsigned char *data, unsigned char *startIndex);
-};
-
-} // namespace RoboidControl
-} // namespace Passer
\ No newline at end of file
diff --git a/NetworkSync.cpp b/NetworkSync.cpp
index a3a0028..b7c5af7 100644
--- a/NetworkSync.cpp
+++ b/NetworkSync.cpp
@@ -1,7 +1,6 @@
 #include "NetworkSync.h"
-#include "Messages.h"
 
-// #define RC_DEBUG 1
+#define RC_DEBUG 1
 
 #ifdef RC_DEBUG
 #include <Arduino.h>
@@ -30,29 +29,32 @@ NetworkSync::NetworkSync(Roboid *roboid) {
 void NetworkSync::ReceiveMessage(Roboid *roboid, unsigned char bytecount) {
   // printf("Received msgId %d, length %d\n", buffer[0], bytecount);
 
-  networkPerception->ProcessPacket(roboid, buffer, bytecount);
-
+  ReceiveData(bytecount);
   switch (buffer[0]) {
-  case NetworkIdMsg:
-    ReceiveNetworkId();
-    break;
-  case CustomMsg:
+  case CustomMsg::id:
     ReceiveCustom(bytecount);
     break;
   }
 }
 
 void NetworkSync::PublishClient() {
-  unsigned char ix = 0;
-  buffer[ix++] = ClientMsg;
-  buffer[ix++] = this->networkId;
-  PublishBuffer(ix);
+  ClientMsg msg = ClientMsg(this->networkId);
+  msg.Publish(this);
 
 #ifdef RC_DEBUG
   SERIALPORT.printf("Publish Device\n");
 #endif
 }
 
+void NetworkSync::ProcessNetworkIdMsg(Passer::Control::NetworkIdMsg msg) {
+#ifdef RC_DEBUG
+  SERIALPORT.printf("Received network Id %d\n", msg.networkId);
+#endif
+  this->networkId = msg.networkId;
+  SendThingInfo(roboid, true);
+  printf("completed\n");
+}
+
 void NetworkSync::ReceiveNetworkId() {
   this->networkId = buffer[1];
 #ifdef RC_DEBUG
@@ -62,16 +64,50 @@ void NetworkSync::ReceiveNetworkId() {
   printf("completed\n");
 }
 
-// void NetworkSync::SendInvestigate(InterestingThing *thing) {
-//   unsigned char ix = 0;
-//   buffer[ix++] = InvestigateMsg;
-//   buffer[ix++] = thing->networkId;
-//   buffer[ix++] = thing->id;
-//   SendBuffer(ix);
-// #ifdef RC_DEBUG
-//   printf("Sent Investigate [%d/%d]\n", thing->networkId, thing->id);
-// #endif
-// }
+void NetworkSync::ProcessInvestigateMsg(InvestigateMsg msg) {
+  // #if RC_DEBUG
+  printf("Received InvestigateMsg [%d/%d]\n", msg.networkId, msg.thingId);
+  // #endif
+
+  if (networkId != roboid->networkSync->networkId)
+    // We only response to investigation requests for our own objects
+    return;
+
+  if (msg.thingId == 0x00) {
+    // They are investigating the roboid itself!
+    if (roboid->modelUrl != nullptr)
+      // roboid->networkSync->SendModel(roboid);
+      roboid->networkSync->SendThingInfo(roboid);
+  } else {
+    Thing *thing = roboid->FindChild(msg.thingId, true);
+    if (thing != nullptr)
+      roboid->networkSync->SendThingInfo(thing);
+    else {
+      InterestingThing *interestingThing =
+          roboid->perception->FindTrackedObject(0x00, msg.thingId);
+      if (interestingThing == nullptr)
+        return;
+
+      roboid->networkSync->SendThingInfo(interestingThing, false);
+    }
+  }
+}
+
+void NetworkSync::ProcessThingMsg(ThingMsg msg) {
+  if (msg.networkId == roboid->networkSync->networkId)
+    // Do not process messages for you own things
+    return;
+
+  printf("Received Thing message [%d/%d]\n", msg.networkId, msg.thingId);
+  InterestingThing *thing =
+      roboid->perception->FindTrackedObject(msg.networkId, msg.thingId);
+  if (thing != nullptr) {
+    thing->type = msg.thingType;
+  }
+  printf("complete\n");
+}
+
+void NetworkSync::ProcessPoseMsg(PoseMsg msg) {}
 
 void NetworkSync::ReceiveCustom(unsigned char packetSize) {
   unsigned char ix = 1; // first byte is the msgId
@@ -124,23 +160,10 @@ void NetworkSync::SendThing(Thing *thing) {
   if (thing == nullptr)
     return;
 
-  // unsigned char ix = 0;
-  // buffer[ix++] = ThingMsg;
-  // buffer[ix++] = this->networkId;
-  // buffer[ix++] = thing->id;
-  // buffer[ix++] = (unsigned char)thing->type;
-  // Thing *parentThing = thing->GetParent();
-  // if (parentThing != nullptr)
-  //   buffer[ix++] = parentThing->id;
-  // else
-  //   buffer[ix++] = 0x00;
-  // SendBuffer(ix);
-
   Thing *parent = thing->GetParent();
-  Passer::Control::ThingMsg msg =
-      Passer::Control::ThingMsg(this->networkId, thing->id, thing->type,
-                                parent == nullptr ? 0 : parent->id);
-  SendBuffer(msg.Serialize(this->buffer));
+  ThingMsg msg = ThingMsg(this->networkId, thing->id, thing->type,
+                          parent == nullptr ? 0 : parent->id);
+  msg.Send(this);
 
 #ifdef RC_DEBUG
   printf("Sent Thing [%d/%d] %d\n", networkId, thing->id, (byte)thing->type);
@@ -155,21 +178,8 @@ void NetworkSync::SendName(Thing *thing) {
   if (len > 255)
     return;
 
-  // unsigned char ix = 0;
-  // buffer[ix++] = NameMsg;
-  // buffer[ix++] = this->networkId;
-  // buffer[ix++] = thing->id;
-
-  // buffer[ix++] = len;
-  // for (unsigned char nameIx = 0; nameIx < len; nameIx++)
-  //   buffer[ix++] = thing->name[nameIx];
-
-  // SendBuffer(ix);
-  Passer::Control::NameMsg msg =
-      Passer::Control::NameMsg(this->networkId, thing->id, thing->name, len);
-  SendBuffer(msg.Serialize(this->buffer));
-
-  // Passer::Control::NameMsg::Send(this, thing);
+  NameMsg msg = NameMsg(this->networkId, thing->id, thing->name, len);
+  msg.Send(this);
 
 #ifdef RC_DEBUG
   SERIALPORT.printf("Sent Name [%d/%d] %s\n", networkId, buffer[1],
@@ -185,27 +195,14 @@ void NetworkSync::SendModel(Thing *thing) {
   if (len > 255)
     return;
 
-  // unsigned char ix = 0;
-  // buffer[ix++] = ModelMsg;
-  // buffer[ix++] = this->networkId;
-  // buffer[ix++] = thing->id; // objectId
-  // // Spherical16 s = Spherical16::zero; // Spherical(thing->modelPosition);
-  // // Messages::SendSpherical16(buffer, &ix, s);
-  // Messages::SendFloat16(buffer, &ix, thing->modelScale);
-
-  // buffer[ix++] = len;
-  // for (int urlIx = 0; urlIx < len; urlIx++)
-  //   buffer[ix++] = thing->modelUrl[urlIx];
-
-  // SendBuffer(ix);
-  Passer::Control::ModelUrlMsg msg = Passer::Control::ModelUrlMsg(
-      this->networkId, thing->id, len, thing->modelUrl, thing->modelScale);
-  SendBuffer(msg.Serialize(this->buffer));
+  ModelUrlMsg msg = ModelUrlMsg(this->networkId, thing->id, len,
+                                thing->modelUrl, thing->modelScale);
+  msg.Send(this);
 }
 
 void NetworkSync::SendCustom(Thing *thing) {
   unsigned char ix = 0;
-  buffer[ix++] = NetworkSync::CustomMsg;
+  buffer[ix++] = CustomMsg::id;
   buffer[ix++] = this->networkId;
   buffer[ix++] = thing->id;
   thing->SendBytes(buffer, &ix);
@@ -236,23 +233,14 @@ void NetworkSync::SendPose(Thing *thing, bool force, bool recurse) {
   thing->positionUpdated |= thing->GetLinearVelocity().distance > 0;
   thing->orientationUpdated |= thing->GetAngularVelocity().distance > 0;
   if (force || thing->positionUpdated || thing->orientationUpdated) {
-    // unsigned char ix = 0;
-    // buffer[ix++] = PoseMsg;
-    // buffer[ix++] = this->networkId;
-    // buffer[ix++] = thing->id;
     unsigned char poseType = 0;
     if (force || thing->positionUpdated)
-      poseType |= Pose_Position;
+      poseType |= PoseMsg::Pose_Position;
     if (force || thing->orientationUpdated)
-      poseType |= Pose_Orientation;
-    // buffer[ix++] = poseType;
-    //  Messages::SendSpherical16(buffer, &ix, thing->position);
-    //  Messages::SendQuat32(buffer, &ix, thing->orientation.ToQuaternion());
-    // SendBuffer(ix);
-    Passer::Control::PoseMsg msg =
-        Passer::Control::PoseMsg(this->networkId, thing->id, poseType,
-                                 thing->position, thing->orientation);
-    SendBuffer(msg.Serialize(this->buffer));
+      poseType |= PoseMsg::Pose_Orientation;
+    PoseMsg msg = PoseMsg(this->networkId, thing->id, poseType, thing->position,
+                          thing->orientation);
+    msg.Send(this);
 
     thing->positionUpdated = false;
     thing->orientationUpdated = false;
@@ -278,30 +266,17 @@ void NetworkSync::SendPose(Thing *thing, bool force, bool recurse) {
   }
 }
 
-void NetworkSync::PublishState(Sensor *sensor) {
-  float *value = (float *)sensor->GetValue();
-  if (value == nullptr)
-    return;
-
-  unsigned char ix = 0;
-  buffer[ix++] = StateMsg;
-  buffer[ix++] = sensor->type;
-  Messages::SendFloat16(buffer, &ix, *value);
-  PublishBuffer(ix);
-}
-
 void NetworkSync::PublishPerception(Roboid *roboid) {
-  Perception *perception = roboid->perception;
-  if (perception == nullptr)
-    return;
+  // Perception *perception = roboid->perception;
+  // if (perception == nullptr)
+  //   return;
 
-  for (unsigned int sensorIx = 0; sensorIx < perception->sensorCount;
-       sensorIx++) {
-    Sensor *sensor = perception->sensors[sensorIx];
-    if (sensor == nullptr)
-      continue;
-    PublishState(sensor);
-  }
+  // for (unsigned int sensorIx = 0; sensorIx < perception->sensorCount;
+  //      sensorIx++) {
+  //   Sensor *sensor = perception->sensors[sensorIx];
+  //   if (sensor == nullptr)
+  //     continue;
+  // }
   PublishTrackedObjects(roboid, roboid->perception->GetTrackedObjects());
 }
 
@@ -328,16 +303,6 @@ void NetworkSync::PublishTrackedObject(Roboid *roboid,
   if (thing == nullptr || thing->updated == false || thing->networkId != 0x00) {
     return;
   }
-  // printf("publish tracked\n");
-  /*
-    Spherical16 roboidPosition = roboid->GetPosition();
-    SwingTwist16 roboidOrientation = roboid->GetOrientation();
-
-    Spherical16 worldPosition =
-        roboidPosition + roboidOrientation * object->position;
-    SwingTwist16 worldOrientation = roboidOrientation * object->orientation;
-  */
-
   SwingTwist16 inv_originOrientation;
   Spherical16 originPosition;
   if (roboid->worldOrigin == nullptr) {
@@ -357,18 +322,9 @@ void NetworkSync::PublishTrackedObject(Roboid *roboid,
   Spherical16 worldPosition =
       inv_originOrientation * (thing->position - originPosition);
 
-  // unsigned char ix = 0;
-  // buffer[ix++] = PoseMsg; // Position2DMsg;
-  // buffer[ix++] = this->networkId;
-  // buffer[ix++] = thing->id; // objectId;
-  // buffer[ix++] = Pose_Position | Pose_Orientation;
-  // Messages::SendSpherical16(buffer, &ix, worldPosition);
-  // Messages::SendSwingTwist(buffer, &ix, worldOrientation);
-  // SendBuffer(ix);
-
-  Passer::Control::PoseMsg msg = Passer::Control::PoseMsg(
-      this->networkId, thing->id, Pose_Position | Pose_Orientation,
-      worldPosition, worldOrientation);
+  PoseMsg msg = PoseMsg(this->networkId, thing->id,
+                        PoseMsg::Pose_Position | PoseMsg::Pose_Orientation,
+                        worldPosition, worldOrientation);
   SendBuffer(msg.Serialize(this->buffer));
 
 #if RC_DEBUG
@@ -414,13 +370,3 @@ void NetworkSync::SendInt(const int x) {
 
   SendBuffer(ix);
 }
-
-// Low-level functions
-
-// bool NetworkSync::SendBuffer(unsigned char bufferSize) { return false; }
-
-// bool NetworkSync::PublishBuffer(unsigned char bufferSize) { return false; }
-
-// void NetworkSync::SendCBuffer(unsigned char bufferSize, unsigned char
-// *buffer) {
-//}
\ No newline at end of file
diff --git a/NetworkSync.h b/NetworkSync.h
index a20191b..36d3bec 100644
--- a/NetworkSync.h
+++ b/NetworkSync.h
@@ -1,8 +1,6 @@
 #pragma once
 
-#include "ControlCore/Messages.h"
 #include "ControlCore/Participant.h"
-#include "NetworkPerception.h"
 #include "Perception.h"
 #include "Roboid.h"
 #include "Types.h"
@@ -31,52 +29,20 @@ public:
   void SendModel(Thing *thing);
   void SendCustom(Thing *thing);
 
-  /// @brief The id of a Pose message
-  static const unsigned char PoseMsg = 0x10;
-  static const unsigned char PoseTypeMsg = 0x11;
-  static const unsigned char RelativePoseMsg = 0x12;
-  static const unsigned char StateMsg = 0x18;
-  /// @brief A bit pattern for the pose, stating that this message contains a
-  /// position in world coordinates
-  static const unsigned char Pose_Position = 0x01;
-  /// @brief A bit pattern for the pose, stating that this message contains an
-  /// orientation in world coordinates
-  static const unsigned char Pose_Orientation = 0x02;
-  /// @brief A bit pattern for the pose, stating that this messsage contains a
-  /// linear velocity in world coordinates
-  static const unsigned char Pose_LinearVelocity = 0x04;
-  /// @brief A bit pattern for the pose, stating that this message contains an
-  /// angular velocity in world coordinates
-  static const char Pose_AngularVelocity = 0x08;
-
   static const unsigned char DestroyMsg = 0x20;
 
-  static const unsigned char AngVelocity2DMsg = 0x46;
-  static const unsigned char Position2DMsg = 0x61;
-  static const unsigned char Velocity2DMsg = 0x62;
-  static const unsigned char ThingMsg = 0x80;
-  static const unsigned char InvestigateMsg = 0x81;
-  static const unsigned char ModelMsg = 0x90;
-  static const unsigned char NameMsg = 0x91;
-  static const unsigned char ClientMsg = 0xA0;
-  static const unsigned char NetworkIdMsg = 0xA1;
-  static const unsigned char CustomMsg = 0xB1;
-
   typedef void (*Buffer)(UInt8 *buffer, UInt16 bufferSize);
 
   void ReceiveMessage(Roboid *roboid, unsigned char bytecount);
 
   void PublishState(Roboid *roboid);
 
-  // void SendInvestigate(InterestingThing *thing);
-
   void PublishPerception(Roboid *roboid);
   void PublishTrackedObjects(Roboid *roboid, InterestingThing **objects);
 
   virtual void SendPosition(Spherical16 worldPosition) {};
   virtual void SendPose(Spherical16 worldPosition,
                         SwingTwist16 worldOrientation) {};
-  // void SendPose(Roboid* roboid, bool recurse = true);
   void SendPose(Thing *thing, bool force = 0, bool recurse = true);
 
   virtual void SendText(const char *s);
@@ -86,23 +52,16 @@ public:
 
 protected:
   Roboid *roboid;
-  NetworkPerception *networkPerception;
+
+  virtual void ProcessNetworkIdMsg(Passer::Control::NetworkIdMsg msg) override;
+  virtual void ProcessInvestigateMsg(InvestigateMsg msg) override;
+  virtual void ProcessThingMsg(ThingMsg msg) override;
+  virtual void ProcessPoseMsg(PoseMsg msg) override;
 
   void ReceiveNetworkId();
   void ReceiveCustom(unsigned char packetSize);
 
-  void PublishState(Sensor *sensor);
-
   void PublishTrackedObject(Roboid *roboid, InterestingThing *object);
-  // void PublishRelativeObject(Buffer sendBuffer,
-  //                            UInt8 parentId,
-  //                            InterestingThing* object);
-
-  // unsigned char buffer[256];
-  // virtual bool SendBuffer(unsigned char bufferSize);
-  // virtual bool PublishBuffer(unsigned char bufferSize);
-
-  // virtual void SendCBuffer(unsigned char bufferSize, unsigned char *buffer);
 
   void PublishClient();
 };
diff --git a/Roboid.cpp b/Roboid.cpp
index ee02826..8b4ac5f 100644
--- a/Roboid.cpp
+++ b/Roboid.cpp
@@ -31,7 +31,6 @@ Roboid::Roboid(Propulsion *propulsion) : Roboid() {
     propulsion->roboid = this;
 }
 
-#include <Arduino.h>
 void Roboid::Update(unsigned long currentTimeMs) {
   if (this->lastUpdateTimeMs == 0)
     this->lastUpdateTimeMs = currentTimeMs;
@@ -42,10 +41,6 @@ void Roboid::Update(unsigned long currentTimeMs) {
   if (propulsion != nullptr) {
     propulsion->Update(currentTimeMs);
 
-    printf("orientation %f velocity %f\n",
-           orientation.swing.horizontal.InDegrees(),
-           linearVelocity.direction.horizontal);
-
     float deltaTime = (float)(currentTimeMs - lastUpdateTimeMs) / 1000;
 
     this->angularVelocity = this->propulsion->GetAngularVelocity();
@@ -55,15 +50,9 @@ void Roboid::Update(unsigned long currentTimeMs) {
     SetOrientation(this->orientation * rotation);
 
     this->linearVelocity = this->propulsion->GetVelocity();
-    printf("    orientation %f velocity %f\n",
-           orientation.swing.horizontal.InDegrees(),
-           linearVelocity.direction.horizontal);
     Spherical16 translation =
         this->orientation * this->linearVelocity * deltaTime;
     SetPosition(this->position + translation);
-    printf("        orientation %f velocity %f\n",
-           orientation.swing.horizontal.InDegrees(),
-           linearVelocity.direction.horizontal);
   }
 
   if (children != nullptr) {