#include "NetworkSync.h"

// #define RC_DEBUG 1

#ifdef RC_DEBUG
#include <Arduino.h>
#if ESP32
#define SERIALPORT Serial
#else
#define SERIALPORT Serial
#endif
#endif

#include "LinearAlgebra/Angle.h"
#include "LinearAlgebra/AngleUsing.h"
#include "LinearAlgebra/Spherical.h"
#include "float16/float16.h"

#include <string.h>

NetworkSync::NetworkSync(Roboid *roboid) {
  this->roboid = roboid;
  this->networkId = 0;
}

void NetworkSync::ReceiveMessage(Roboid *roboid, unsigned char bytecount) {
  networkPerception->ProcessPacket(roboid, buffer, bytecount);

  switch (buffer[0]) {
  case NetworkIdMsg:
    ReceiveNetworkId();
    break;
  }
}

// Actually, this is a kind of investigate...
void NetworkSync::ReceiveNetworkId() {
  this->networkId = buffer[1];
#ifdef RC_DEBUG
  SERIALPORT.printf("Received network Id %d\n", this->networkId);
#endif
  SendThing(roboid);
  SendName(roboid);
  SendModel(roboid);
  for (unsigned char childIx = 0; childIx < roboid->childCount; childIx++) {
    Thing *child = roboid->GetChild(childIx);
    if (child != nullptr)
      PublishRelativeThing(child, true);
  }
}

void NetworkSync::PublishDevice() {
  unsigned char ix = 0;
  buffer[ix++] = DeviceMsg;
  buffer[ix++] = 0; // No network ID
  PublishBuffer(ix);

#ifdef RC_DEBUG
  SERIALPORT.printf("Publish Device\n");
#endif
}

void NetworkSync::PublishState(Roboid *roboid) {
  SendPose(roboid);
  PublishPerception(roboid);
}

void NetworkSync::SendThing(Thing *thing) {
  if (thing == nullptr)
    return;

  unsigned char ix = 0;
  buffer[ix++] = CreateMsg;
  buffer[ix++] = thing->id;
  buffer[ix++] = thing->type;
  SendBuffer(ix);

#ifdef RC_DEBUG
  printf("Sent Thing [%d/%d] %d\n", networkId, buffer[1], buffer[2]);
#endif
}

void NetworkSync::NewObject(InterestingThing *thing) {
  if (thing == nullptr || thing->networkId != 0x00)
    return;

  unsigned char ix = 0;
  buffer[ix++] = CreateMsg;
  buffer[ix++] = thing->id;
  buffer[ix++] = thing->type;
  SendBuffer(ix);

#ifdef RC_DEBUG
  printf("Sent CreateMsg [%d/%d] %d\n", networkId, buffer[1], buffer[2]);
#endif

  thing->updated = true;
  PublishTrackedObject(roboid, thing);
}

void NetworkSync::PublishRelativeThing(Thing *thing, bool recurse) {
  Thing *parentThing = thing->GetParent();

  unsigned char ix = 0;
  buffer[ix++] = RelativePoseMsg;
  buffer[ix++] = thing->id;
  if (parentThing != nullptr)
    buffer[ix++] = parentThing->id;
  else
    buffer[ix++] = 0x00;
  SendSpherical16(
      buffer, &ix,
      thing->position); // Do we need this if we already send a pose?

  SendBuffer(ix);

  SendName(thing);
  SendModel(thing);

  if (recurse) {
    for (unsigned char childIx = 0; childIx < thing->childCount; childIx++) {
      Thing *child = thing->GetChild(childIx);
      if (child != nullptr)
        PublishRelativeThing(child, true);
    }
  }
}

void NetworkSync::SendName(Thing *thing) {
  if (thing->name == nullptr)
    return;

  unsigned char len = strlen(thing->name);
  if (len > 255)
    return;

  unsigned char ix = 0;
  buffer[ix++] = NameMsg;
  buffer[ix++] = thing->id;

  buffer[ix++] = len;
  for (unsigned char nameIx = 0; nameIx < len; nameIx++)
    buffer[ix++] = thing->name[nameIx];

  SendBuffer(ix);

#ifdef RC_DEBUG
  SERIALPORT.printf("Sent Name [%d/%d] %s\n", networkId, buffer[1],
                    thing->name);
#endif
}

void NetworkSync::SendModel(Roboid *roboid) {
  if (roboid->modelUrl == nullptr)
    return;

  unsigned char len = strlen(roboid->modelUrl);
  if (len > 255)
    return;

  unsigned char ix = 0;
  buffer[ix++] = ModelMsg;
  buffer[ix++] = 0x00;               // objectId
  Spherical16 s = Spherical16::zero; //(roboid->modelPosition);
  SendSpherical16(buffer, &ix, s);
  SendFloat16(buffer, &ix, roboid->modelScale);

  buffer[ix++] = len;
  for (int urlIx = 0; urlIx < len; urlIx++)
    buffer[ix++] = roboid->modelUrl[urlIx];

  SendBuffer(ix);
}

void NetworkSync::SendModel(Thing *thing) {
  if (thing->modelUrl == nullptr)
    return;

  unsigned char len = strlen(thing->modelUrl);
  if (len > 255)
    return;

  unsigned char ix = 0;
  buffer[ix++] = ModelMsg;
  buffer[ix++] = thing->id;          // objectId
  Spherical16 s = Spherical16::zero; // Spherical(thing->modelPosition);
  SendSpherical16(buffer, &ix, s);
  SendFloat16(buffer, &ix, 1); // thing->modelScale);

  buffer[ix++] = len;
  for (int urlIx = 0; urlIx < len; urlIx++)
    buffer[ix++] = thing->modelUrl[urlIx];

  SendBuffer(ix);
}

void NetworkSync::SendDestroyThing(InterestingThing *thing) {
  if (networkId == 0) // We're not connected to a site yet
    return;

  unsigned char ix = 0;
  buffer[ix++] = DestroyMsg;
  buffer[ix++] = thing->id;
  SendBuffer(ix);

#if RC_DEBUG
  printf("Sent DestroyMsg [%d/%d]\n", thing->networkId, thing->id);
#endif
}

void NetworkSync::SendPose(Thing *thing, bool recurse) {
  if (this->networkId == 0) // We're not connected to a site yet
    return;

  // if (thing->GetLinearVelocity().distance > 0 ||
  //     thing->GetAngularVelocity().distance > 0) {
  unsigned char ix = 0;
  buffer[ix++] = PoseMsg;
  buffer[ix++] = thing->id;
  buffer[ix++] = Pose_Position | Pose_Orientation;
  SendSpherical16(buffer, &ix, thing->position);
  SendQuat32(buffer, &ix, thing->orientation.ToQuaternion());
  SendBuffer(ix);

#if RC_DEBUG
  // if (thing->id == 0)
  SERIALPORT.printf("Sent PoseMsg Thing [%d/%d] %f\n", this->networkId,
                    buffer[1], thing->position.distance);
#endif
  // }

  if (recurse) {
    for (unsigned char childIx = 0; childIx < thing->childCount; childIx++) {
      Thing *child = thing->GetChild(childIx);
      if (child != nullptr)
        SendPose(thing->GetChild(childIx), true);
    }
  }
}

void NetworkSync::PublishState(Sensor *sensor) {
  float *value = (float *)sensor->GetValue();
  if (value == nullptr)
    return;

  unsigned char ix = 0;
  buffer[ix++] = StateMsg;
  buffer[ix++] = sensor->type;
  SendFloat16(buffer, &ix, *value);
  PublishBuffer(ix);
}

void NetworkSync::PublishPerception(Roboid *roboid) {
  Perception *perception = roboid->perception;
  if (perception == nullptr)
    return;

  for (unsigned int sensorIx = 0; sensorIx < perception->sensorCount;
       sensorIx++) {
    Sensor *sensor = perception->sensors[sensorIx];
    if (sensor == nullptr)
      continue;
    PublishState(sensor);
  }
  PublishTrackedObjects(roboid, roboid->perception->GetTrackedObjects());
}

void NetworkSync::PublishTrackedObjects(Roboid *roboid,
                                        InterestingThing **objects) {
  if (networkId == 0) // We're not connected to a site yet
    return;

  int n = 0;
  for (unsigned char objIx = 0; objIx < Perception::maxObjectCount; objIx++) {
    InterestingThing *obj = objects[objIx];
    if (obj == nullptr)
      continue;

    if (obj->confidence > 0)
      PublishTrackedObject(roboid, obj);

    n++;
  }
}

void NetworkSync::PublishTrackedObject(Roboid *roboid,
                                       InterestingThing *object) {
  if (object == nullptr || object->updated == false ||
      object->networkId != 0x00) {
    return;
  }

  /*
    Spherical16 roboidPosition = roboid->GetPosition();
    SwingTwist16 roboidOrientation = roboid->GetOrientation();

    Spherical16 worldPosition =
        roboidPosition + roboidOrientation * object->position;
    SwingTwist16 worldOrientation = roboidOrientation * object->orientation;
  */

  SwingTwist16 inv_originOrientation;
  Spherical16 originPosition;
  if (roboid->worldOrigin == nullptr) {
    inv_originOrientation = SwingTwist16::identity;
    originPosition = Spherical16::zero;
  } else {
    inv_originOrientation =
        SwingTwist16::Inverse(roboid->worldOrigin->orientation);
    originPosition = roboid->worldOrigin->position;
  }

  // SwingTwist16 inv_originOrientation =
  //     SwingTwist16::Inverse(roboid->worldOrigin->orientation);
  // Spherical16 originPosition = roboid->worldOrigin->position;

  SwingTwist16 worldOrientation = inv_originOrientation * object->orientation;
  Spherical16 worldPosition =
      inv_originOrientation * (object->position - originPosition);

  unsigned char ix = 0;
  buffer[ix++] = PoseMsg;    // Position2DMsg;
  buffer[ix++] = object->id; // objectId;
  buffer[ix++] = Pose_Position | Pose_Orientation;
  SendSpherical16(buffer, &ix, worldPosition);
  SendSwingTwist(buffer, &ix, worldOrientation);
  SendBuffer(ix);

#if RC_DEBUG
  printf("Sent Thing PoseMsg [%d/%d] %d\n", networkId, buffer[1], object->type);
#endif

  object->updated = false;
}

void NetworkSync::SendPoseMsg(Buffer sendBuffer, Roboid *roboid) {
  Spherical16 velocity = roboid->propulsion->GetVelocity();
  Spherical16 worldVelocity = roboid->orientation * velocity;
  Vector3 worldVelocity3 = worldVelocity.ToVector3();

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

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

  sendBuffer(buffer, bufferSize);
#endif
}

void NetworkSync::SendInvestigate(InterestingThing *thing) {
  unsigned char ix = 0;
  buffer[ix++] = InvestigateMsg;
  buffer[ix++] = thing->networkId;
  buffer[ix++] = thing->id;
  SendBuffer(ix);
#ifdef RC_DEBUG
  printf("Sent Investigate [%d/%d]\n", thing->networkId, thing->id);
#endif
}

void NetworkSync::SendText(const char *s) {
  unsigned char length;
  for (length = 0; length < 253; length++) {
    if (s[length] == '\0')
      break;
  }
  if (length >= 253)
    return;

  unsigned char ix = 0;
  buffer[ix++] = 0xB0;
  buffer[ix++] = length;
  for (int urlIx = 0; urlIx < length; urlIx++)
    buffer[ix++] = s[urlIx];

  SendBuffer(ix);
#ifdef RC_DEBUG
  printf("Sent Text %s\n", s);
#endif
}

#if ESP32
#include <Arduino.h>
#endif
void NetworkSync::SendInt(const int x) {
  String s = String(x);
  char length = s.length();

  unsigned char ix = 0;
  buffer[ix++] = 0xB0;
  buffer[ix++] = length;
  for (int urlIx = 0; urlIx < length; urlIx++)
    buffer[ix++] = s[urlIx];

  SendBuffer(ix);
}

// Low-level functions

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

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

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

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

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

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

void NetworkSync::SendAngle8(unsigned char *data, unsigned int startIndex,
                             const float angle) {
  Angle8 packedAngle2 = Angle8::Degrees(angle);
  data[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 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);
}

void NetworkSync::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 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::SendBuffer(unsigned char bufferSize) {}

void NetworkSync::PublishBuffer(unsigned char bufferSize) {}