#include "Messages.h"

#include "float16/float16.h"

Angle8 Messages::ReceiveAngle8(unsigned char *buffer,
                               unsigned char *startIndex) {
  unsigned char binary = buffer[(*startIndex)++];

  Angle8 angle = Angle8::Binary(binary);

  return angle;
}

float Messages::ReceiveFloat16(unsigned char *buffer,
                               unsigned char *startIndex) {
  unsigned char ix = *startIndex;
  unsigned short value = buffer[ix++] << 8 | buffer[ix];
  float16 f = float16();
  f.setBinary(value);

  *startIndex = ix;
  return f.toDouble();
}

Spherical16 Messages::ReceiveSpherical16(unsigned char *buffer,
                                         unsigned char *startIndex) {
  float distance = ReceiveFloat16(buffer, startIndex);

  Angle8 horizontal8 = ReceiveAngle8(buffer, startIndex);
  Angle16 horizontal = Angle16::Binary(horizontal8.GetBinary() * 255);

  Angle8 vertical8 = ReceiveAngle8(buffer, startIndex);
  Angle16 vertical = Angle16::Binary(vertical8.GetBinary() * 255);

  Spherical16 s = Spherical16(distance, horizontal, vertical);
  return s;
}

void Messages::SendVector3(unsigned char *buffer, unsigned char *startIndex,
                           const Vector3 v) {
  SendSingle100(buffer, *startIndex, v.Right());
  (*startIndex) += 4;
  SendSingle100(buffer, *startIndex, v.Up());
  (*startIndex) += 4;
  SendSingle100(buffer, *startIndex, v.Forward());
  (*startIndex) += 4;
}

void Messages::SendQuaternion(unsigned char *buffer, const int startIndex,
                              const Quaternion q) {
  Vector3 angles = Quaternion::ToAngles(q);
  int ix = startIndex;
  SendAngle8(buffer, ix++, angles.Right());
  SendAngle8(buffer, ix++, angles.Up());
  SendAngle8(buffer, ix++, angles.Forward());
}

void Messages::SendPolar(unsigned char *buffer, unsigned char *startIndex,
                         Polar p) {
  SendAngle8(buffer, *startIndex, (const float)p.angle.InDegrees());
  SendSingle100(buffer, (*startIndex) + 1, p.distance);
}

void Messages::SendSpherical16(unsigned char *buffer, unsigned char *startIndex,
                               Spherical16 s) {
  SendAngle8(buffer, (*startIndex)++, s.direction.horizontal.InDegrees());
  SendAngle8(buffer, (*startIndex)++, s.direction.vertical.InDegrees());
  SendFloat16(buffer, startIndex, s.distance);
}

void Messages::SendSwingTwist(unsigned char *buffer, unsigned char *ix,
                              const SwingTwist16 r) {
  Quaternion q = r.ToQuaternion();
  SendQuat32(buffer, ix, q);
}

void Messages::SendQuat32(unsigned char *buffer, unsigned char *startIndex,
                          const Quaternion q) {
  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;
  }
  // Serial.printf(" (%d) %d:%d:%d:%d ", startIndex, qx, qy, qz, qw);
  buffer[(*startIndex)++] = qx;
  buffer[(*startIndex)++] = qy;
  buffer[(*startIndex)++] = qz;
  buffer[(*startIndex)++] = qw;
}

void Messages::SendAngle8(unsigned char *buffer, unsigned int startIndex,
                          const float angle) {
  Angle8 packedAngle2 = Angle8::Degrees(angle);
  buffer[startIndex] = packedAngle2.GetBinary();
}

// void NetworkSync::SendAngle16(unsigned char *data, unsigned int startIndex,
//                               const float angle) {
//   AngleUsing<signed short> packedAngle = AngleUsing<signed short>(angle);
//   signed short value = packedAngle.GetValue();
//   data[startIndex] = value >> 8;
//   data[startIndex + 1] = value & 0xFF;
// }

// void NetworkSync::SendAngle32(unsigned char *data, unsigned int startIndex,
//                               const float angle) {
//   AngleUsing<signed long> packedAngle = AngleUsing<signed long>(angle);
//   unsigned long value = packedAngle.GetValue();
//   data[startIndex] = value >> 24 & 0xFF;
//   data[startIndex + 1] = value >> 16 & 0xFF;
//   data[startIndex + 2] = value >> 8 & 0xFF;
//   data[startIndex + 3] = value & 0xFF;
//   // Serial.printf(" %lu=%d:%d:%d:%d ", value, data[startIndex],
//   //               data[startIndex + 1], data[startIndex + 2],
//   //               data[startIndex + 3]);
// }

// void Messages::SendSingle100(unsigned char *data, unsigned int startIndex,
//                              float value) {
//   // Sends a float with truncated 2 decimal precision
//   Int32 intValue = value * 100;
//   SendInt32(data, startIndex, intValue);
// }

void Messages::SendFloat16(unsigned char *data, unsigned char *startIndex,
                           float value) {
  float16 value16 = float16(value);
  short binary = value16.getBinary();

  data[(*startIndex)++] = (binary >> 8) & 0xFF;
  data[(*startIndex)++] = binary & 0xFF;
}

// void Messages::SendInt32(unsigned char *data, unsigned int startIndex,
//                          Int32 value) {
//   for (unsigned char ix = 0; ix < 4; ix++) {
//     data[startIndex++] = ((unsigned char *)&value)[ix];
//   }
// }