#include "LowLevelMessages.h"

#include "../float16/float16.h"

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

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

  buffer[(*ix)++] = (binary >> 8) & 0xFF;
  buffer[(*ix)++] = binary & 0xFF;
}

void Passer::Control::LowLevelMessages::SendSpherical16(unsigned 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());
}

void Passer::Control::LowLevelMessages::SendQuat32(unsigned 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;
  }
  // Serial.printf(" (%d) %d:%d:%d:%d ", startIndex, qx, qy, qz, qw);
  buffer[(*ix)++] = qx;
  buffer[(*ix)++] = qy;
  buffer[(*ix)++] = qz;
  buffer[(*ix)++] = qw;
}