RoboidControl-cpp/ControlCore_old/Messages.cpp

#include "Messages.h"

#include "LowLevelMessages.h"
#include "Participant.h"
#include "string.h"

#pragma region IMessage

IMessage::IMessage() {}

unsigned char IMessage::Serialize(unsigned char *buffer) { return 0; }

unsigned char *IMessage::ReceiveMsg(unsigned char packetSize) {
  return nullptr;
}

bool IMessage::Publish(Participant *participant) {
  return participant->PublishBuffer(Serialize(participant->buffer));
}
bool IMessage::SendTo(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++] = ClientMsg::id;
  buffer[ix++] = this->networkId;
  return ix;
}

// Client Msg
#pragma endregion

#pragma region Network Id

NetworkIdMsg::NetworkIdMsg(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;
}

unsigned char InvestigateMsg::Serialize(unsigned char *buffer) {
  unsigned char ix = 0;
  buffer[ix++] = InvestigateMsg::id;
  buffer[ix++] = this->networkId;
  buffer[ix++] = this->thingId;
  return ix;
}

// 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;
  this->thingId = thingId;
  this->thingType = thingType;
  this->parentId = parentId;
}

unsigned char ThingMsg::Serialize(unsigned char *buffer) {
  unsigned char ix = 0;
  buffer[ix++] = ThingMsg::id;
  buffer[ix++] = this->networkId;
  buffer[ix++] = this->thingId;
  buffer[ix++] = this->thingType;
  buffer[ix++] = this->parentId;
  return ix;
}

// Thing
#pragma endregion

#pragma region Name

NameMsg::NameMsg(unsigned char networkId, unsigned char thingId,
                 const char *name, unsigned char nameLength) {
  this->networkId = networkId;
  this->thingId = thingId;
  this->name = name;
  this->nameLength = nameLength;
}

unsigned char NameMsg::Serialize(unsigned char *buffer) {
  unsigned char ix = 0;
  buffer[ix++] = NameMsg::id;
  buffer[ix++] = this->networkId;
  buffer[ix++] = this->thingId;
  buffer[ix++] = this->nameLength;
  for (int nameIx = 0; nameIx < this->nameLength; nameIx++)
    buffer[ix++] = this->name[nameIx];

  return ix;
}

// Name
#pragma endregion

#pragma region ModelUrl

ModelUrlMsg::ModelUrlMsg(unsigned char networkId, unsigned char thingId,
                         unsigned char urlLength, const char *url,
                         float scale) {
  this->networkId = networkId;
  this->thingId = thingId;
  this->urlLength = urlLength;
  this->url = url;
  this->scale = scale;
}

unsigned char ModelUrlMsg::Serialize(unsigned char *buffer) {
  unsigned char ix = 0;
  buffer[ix++] = ModelUrlMsg::id;
  buffer[ix++] = this->networkId;
  buffer[ix++] = this->thingId;
  LowLevelMessages::SendFloat16(buffer, &ix, this->scale);
  buffer[ix++] = this->urlLength;
  for (int urlIx = 0; urlIx < this->urlLength; urlIx++)
    buffer[ix++] = url[urlIx];
  return ix;
}

// Model Url
#pragma endregion

#pragma region PoseMsg

PoseMsg::PoseMsg(unsigned char networkId, unsigned char thingId,
                 unsigned char poseType, Spherical16 position,
                 SwingTwist16 orientation, Spherical16 linearVelocity,
                 Spherical16 angularVelocity) {
  this->networkId = networkId;
  this->thingId = thingId;

  this->poseType = poseType;
  this->position = position;
  this->orientation = orientation;
  this->linearVelocity = linearVelocity;
  this->angularVelocity = angularVelocity;
}
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;
  buffer[ix++] = PoseMsg::id;
  buffer[ix++] = this->networkId;
  buffer[ix++] = this->thingId;
  buffer[ix++] = this->poseType;
  if (this->poseType & Pose_Position)
    LowLevelMessages::SendSpherical16(buffer, &ix, this->position);
  if (this->poseType & Pose_Orientation)
    LowLevelMessages::SendQuat32(buffer, &ix, this->orientation);
  if (this->poseType & Pose_LinearVelocity)
    LowLevelMessages::SendSpherical16(buffer, &ix, this->linearVelocity);
  if (this->poseType & Pose_AngularVelocity)
    LowLevelMessages::SendSpherical16(buffer, &ix, this->angularVelocity);
  return ix;
}

// Pose
#pragma endregion

#pragma region CustomMsg

CustomMsg::CustomMsg(unsigned char *buffer) {
  unsigned char ix = 1;
  this->networkId = buffer[ix++];
  this->thingId = 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, Thing *thing) {
  this->networkId = networkId;
  this->thingId = thing->id;
  this->thing = thing;
}

unsigned char CustomMsg::Serialize(unsigned char *buffer) {
  unsigned char ix = this->length;
  this->thing->SendBytes(buffer, &ix);
  if (ix <= this->length) // in this case, no data is actually sent
    return 0;

  buffer[0] = CustomMsg::id;
  buffer[1] = this->networkId;
  buffer[2] = this->thingId;
  return ix;
}

CustomMsg CustomMsg::Receive(unsigned char *buffer, unsigned char bufferSize) {
  CustomMsg msg = CustomMsg(buffer);
  return msg;
}

// CustomMsg
#pragma endregion

#pragma region DestroyMsg

DestroyMsg::DestroyMsg(unsigned char networkId, Thing *thing) {
  this->networkId = networkId;
  this->thingId = thing->id;
}

unsigned char DestroyMsg::Serialize(unsigned char *buffer) {
  unsigned char ix = 0;
  buffer[ix++] = DestroyMsg::id;
  buffer[ix++] = this->networkId;
  buffer[ix++] = this->thingId;
  return ix;
}

// DestroyMsg
#pragma endregion