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

namespace RoboidControl {

PoseMsg::PoseMsg(unsigned char networkId, Thing* thing, bool force) {
  this->networkId = networkId;
  this->thingId = thing->id;

  this->poseType = 0;
  if (thing->positionUpdated || force) {
    this->position = thing->GetPosition();
    this->poseType |= Pose_Position;
  }
  if (thing->orientationUpdated || force) {
    this->orientation = thing->GetOrientation();
    this->poseType |= Pose_Orientation;
  }
  if (thing->linearVelocityUpdated) {
    this->linearVelocity = thing->GetLinearVelocity();
    this->poseType |= Pose_LinearVelocity;
    thing->linearVelocityUpdated = false;
  }
  if (thing->angularVelocityUpdated) {
    this->angularVelocity = thing->GetAngularVelocity();
    this->poseType |= Pose_AngularVelocity;
    thing->angularVelocityUpdated = false;
  }
}

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::ReceiveSpherical(buffer, &ix);
  this->orientation = LowLevelMessages::ReceiveQuat32(buffer, &ix);
  // linearVelocity
  // angularVelocity
}

PoseMsg::~PoseMsg() {}

unsigned char PoseMsg::Serialize(char* buffer) {
  if (this->poseType == 0)
    return 0;

#if defined(DEBUG)
  std::cout << "Send PoseMsg [" << (int)this->networkId << "/"
            << (int)this->thingId << "] " << (int)this->poseType << std::endl;
#endif
  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::SendSpherical(buffer, &ix, this->position);
  if ((this->poseType & Pose_Orientation) != 0)
    LowLevelMessages::SendQuat32(buffer, &ix, this->orientation);
  if ((this->poseType & Pose_LinearVelocity) != 0)
    LowLevelMessages::SendSpherical(buffer, &ix, this->linearVelocity);
  if ((this->poseType & Pose_AngularVelocity) != 0)
    LowLevelMessages::SendSpherical(buffer, &ix, this->angularVelocity);
  return ix;
}

}  // namespace RoboidControl