#include "Messages.h"

#include "LowLevelMessages.h"
// #include "Messages/CustomMsg.h"
#include "Participant.h"
#include "string.h"

#pragma region IMessage

IMessage::IMessage() {}

// IMessage::IMessage(unsigned char *buffer) { Deserialize(buffer); }

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

// 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::SendTo(Participant *participant) {
//   return participant->SendBuffer(Serialize(participant->buffer));
// }

// IMessage
#pragma endregion

#pragma region Investigate

InvestigateMsg::InvestigateMsg(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(char *buffer) {
  unsigned char ix = 0;
  buffer[ix++] = this->id;
  buffer[ix++] = this->networkId;
  buffer[ix++] = this->thingId;
  return ix;
}

// 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 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 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(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) != 0)
    LowLevelMessages::SendSpherical16(buffer, &ix, this->position);
  if ((this->poseType & Pose_Orientation) != 0)
    LowLevelMessages::SendQuat32(buffer, &ix, this->orientation);
  if ((this->poseType & Pose_LinearVelocity) != 0)
    LowLevelMessages::SendSpherical16(buffer, &ix, this->linearVelocity);
  if ((this->poseType & Pose_AngularVelocity) != 0)
    LowLevelMessages::SendSpherical16(buffer, &ix, this->angularVelocity);
  return ix;
}

// Pose
#pragma endregion

#pragma region DestroyMsg

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

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

// DestroyMsg
#pragma endregion