#include "Messages.h"

#include "float16/float16.h"

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

  Angle8 angle = Angle8::Binary(binary);

  return angle;
}

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

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

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

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

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

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