#include "NetworkSync.h"

#define RC_DEBUG 1

#ifdef RC_DEBUG
#include <Arduino.h>
#endif

#include "LinearAlgebra/Angle.h"
// #include "LinearAlgebra/Angle16.h"
// #include "LinearAlgebra/Angle32.h"
// #include "LinearAlgebra/AngleUsing.h"
#include "LinearAlgebra/Spherical.h"

void NetworkSync::SendVector3(unsigned char *data, unsigned int startIndex,
                              const Vector3 v) {
  SendSingle100(data, startIndex, v.x);
  SendSingle100(data, startIndex += 4, v.y);
  SendSingle100(data, startIndex += 4, v.z);
}

void NetworkSync::SendQuaternion(unsigned char *data, const int startIndex,
                                 const Quaternion q) {
  Vector3 angles = Quaternion::ToAngles(q);
  int ix = startIndex;
  SendAngle(data, ix++, angles.x);
  SendAngle(data, ix++, angles.y);
  SendAngle(data, ix++, angles.z);
}

void NetworkSync::SendSpherical(unsigned char *data, int startIndex,
                                Spherical s) {
  SendAngle(data, startIndex++, s.horizontalAngle);
  SendAngle(data, startIndex++, s.verticalAngle);
  SendAngle(data, startIndex++, s.distance);
}

void NetworkSync::SendSpherical16(unsigned char *data, int startIndex,
                                  Spherical s) {
  SendAngle16(data, startIndex, s.horizontalAngle);
  SendAngle16(data, startIndex += 2, s.verticalAngle);
  SendSingle100(data, startIndex += 2, s.distance);
}

void NetworkSync::SendSpherical32(unsigned char *data, int startIndex,
                                  Spherical s) {
  SendAngle32(data, startIndex, s.horizontalAngle);
  SendAngle32(data, startIndex += 4, s.verticalAngle);
  SendSingle100(data, startIndex += 4, s.distance);
}

void NetworkSync::SendQuat32(unsigned char *data, int 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);
  data[startIndex++] = qx;
  data[startIndex++] = qy;
  data[startIndex++] = qz;
  data[startIndex++] = qw;
}

// void NetworkSync::SendAngle(unsigned char *data, unsigned int startIndex,
//                             const float angle) {
//   AngleUsing<unsigned char> packedAngle = AngleUsing<unsigned char>(angle);
//   data[startIndex] = packedAngle.GetValue();
// }

// 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 NetworkSync::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);
  // for (unsigned char ix = 0; ix < 4; ix++) {
  //   data[startIndex + ix] = ((unsigned char *)&intValue)[ix];
  // }
}

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

void NetworkSync::PublishTrackedObjects(SendBuffer sendBuffer,
                                        InterestingThing **objects) {
  for (unsigned char objIx = 0; objIx < Perception::maxObjectCount; objIx++) {
    InterestingThing *obj = objects[objIx];
    if (obj == nullptr)
      continue;
    // if (obj->sensor->type == Thing::ExternalType)
    //   continue;

    // tmp
    obj->id = objIx;
    PublishTrackedObject(sendBuffer, obj);
  }
}

void NetworkSync::PublishTrackedObject(SendBuffer sendBuffer,
                                       InterestingThing *object) {
  Vector2 worldPosition2 =
      Vector2::Rotate(Vector2::forward * object->position.distance,
                      -object->position.horizontalAngle);
  Vector3 worldPosition3 = Vector3(worldPosition2.x, 0, worldPosition2.y);

#ifdef RC_DEBUG
  Serial.print("Send Pose ");
  Serial.print((int)object->id);
  Serial.print("  Position  ");
  Serial.print(worldPosition3.x);
  Serial.print(", ");
  Serial.print(worldPosition3.y);
  Serial.print(", ");
  Serial.println(worldPosition3.z);
#else
  const UInt16 bufferSize = 3 + 12;
  UInt8 buffer[bufferSize] = {
      PoseMsg,
      (UInt8)object->id,
      Pose_Position,
  };
  unsigned int ix = 3;
  SendVector3(buffer, ix, worldPosition3);
  sendBuffer(buffer, bufferSize);
#endif
}

void NetworkSync::PublishRelativeObject(SendBuffer sendBuffer, UInt8 parentId,
                                        InterestingThing *object) {
  const UInt16 bufferSize = 4 + 12;
  UInt8 buffer[bufferSize] = {
      RelativePoseMsg,
      (UInt8)parentId,
      (UInt8)object->id,
      Pose_Position,
  };
  unsigned int ix = 4;
  SendSpherical32(buffer, ix, object->position);
  // SendVector3(buffer, ix, worldPosition3);
  sendBuffer(buffer, bufferSize);
}

void NetworkSync::SendPoseMsg(SendBuffer sendBuffer, Roboid *roboid) {
  Polar velocity = roboid->propulsion->GetVelocity();
  Vector2 worldVelocity2 =
      Vector2::Rotate(Vector2::forward * velocity.distance, velocity.angle);
  Vector3 worldVelocity3 = Vector3(worldVelocity2.x, 0, worldVelocity2.y);

  float angularVelocity = roboid->propulsion->GetAngularVelocity();
  Vector3 worldAngularVelocity = Vector3(0, angularVelocity, 0);

#ifdef RC_DEBUG
  Serial.print("Send Pose 0  LinearVelocity  ");
  Serial.print(worldVelocity3.x);
  Serial.print(", ");
  Serial.print(worldVelocity3.y);
  Serial.print(", ");
  Serial.print(worldVelocity3.z);
  Serial.print("  AngularVelocity ");
  Serial.print(worldAngularVelocity.x);
  Serial.print(", ");
  Serial.print(worldAngularVelocity.y);
  Serial.print(", ");
  Serial.println(worldAngularVelocity.z);
#else
  const unsigned int bufferSize = 3 + 12 + 12;
  unsigned char buffer[bufferSize] = {
      PoseMsg,
      0, // objectId;
      Pose_LinearVelocity | Pose_AngularVelocity,
  };
  unsigned int ix = 3;
  SendVector3(buffer, ix, worldVelocity3);
  SendVector3(buffer, ix, worldAngularVelocity);

  sendBuffer(buffer, bufferSize);
#endif
}

void NetworkSync::SendDestroyObject(SendBuffer sendBuffer,
                                    InterestingThing *obj) {
#ifdef RC_DEBUG
  Serial.print("Send Destroy ");
  Serial.println((int)obj->id);
#else
  unsigned char buffer[2] = {DestroyMsg, (unsigned char)obj->id};
  sendBuffer(buffer, 2);
#endif
}