#include "PoseMsg.h"
#include "LowLevelMessages.h"

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);
}

PoseMsg::~PoseMsg() {}

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;
}