#include "LowLevelMessages.h"

#include "LinearAlgebra/float16.h"
// #include <iostream>

namespace RoboidControl {

void LowLevelMessages::SendAngle8(char* buffer,
                                  unsigned char* ix,
                                  const float angle) {
  Angle8 packedAngle2 = Angle8::Degrees(angle);
  buffer[(*ix)++] = packedAngle2.GetBinary();
}
Angle8 LowLevelMessages::ReceiveAngle8(const char* buffer,
                                       unsigned char* startIndex) {
  unsigned char binary = buffer[(*startIndex)++];

  Angle8 angle = Angle8::Binary(binary);

  return angle;
}

void LowLevelMessages::SendFloat16(char* buffer,
                                   unsigned char* ix,
                                   float value) {
  float16 value16 = float16(value);
  short binary = value16.getBinary();

  buffer[(*ix)++] = (binary >> 8) & 0xFF;
  buffer[(*ix)++] = binary & 0xFF;
}
float LowLevelMessages::ReceiveFloat16(const char* buffer,
                                       unsigned char* startIndex) {
  unsigned char ix = *startIndex;
  unsigned char msb = buffer[ix++];
  unsigned char lsb = buffer[ix++];
  unsigned short value = msb << 8 | lsb;
  float16 f = float16();
  f.setBinary(value);

  *startIndex = ix;
  return (float)f.toFloat();
}

void LowLevelMessages::SendSpherical16(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 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 LowLevelMessages::SendQuat32(char* buffer,
                                  unsigned char* ix,
                                  SwingTwist16 rotation) {
  Quaternion q = rotation.ToQuaternion();
  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;
  }
  // std::cout << (int)qx << "," << (int)qy << "," << (int)qz << "," << (int)qw
  // << "\n";
  buffer[(*ix)++] = qx;
  buffer[(*ix)++] = qy;
  buffer[(*ix)++] = qz;
  buffer[(*ix)++] = qw;
}

SwingTwist16 LowLevelMessages::ReceiveQuat32(const 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;
}

}  // namespace RoboidControl