#include "NetworkPerception.h"

#include "RoboidControl/NetworkSync.h"

void NetworkPerception::ProcessPacket(Roboid* roboid,
                                      unsigned char* buffer,
                                      int packetsize) {
  // printf("packet received, type = 0x%02x %d %d %d %d\n", buffer[0],
  // buffer[2], buffer[3], buffer[4], buffer[5]);

  if (buffer[0] == NetworkSync::PoseMsg) {
    ReceiveObject(buffer, roboid);
  }
}

Int32 NetworkPerception::ReceiveInt32(unsigned char* data, int startIndex) {
  Int32 a = Int32((UInt32)(data[startIndex + 3]) << 24 |
                  (UInt32)(data[startIndex + 2]) << 16 |
                  (UInt32)(data[startIndex + 1]) << 8 |
                  (UInt32)(data[startIndex + 0]));
  return a;
}

float NetworkPerception::ReceiveFloat100(unsigned char* data, int startIndex) {
  Int32 intValue = ReceiveInt32(data, startIndex);
  float f = (float)intValue / 100.0F;
  return f;
}

Vector3 NetworkPerception::ReceiveVector3(unsigned char* data, int startIndex) {
  float x = ReceiveFloat100(data, startIndex);
  float y = ReceiveFloat100(data, startIndex + 4);
  float z = ReceiveFloat100(data, startIndex + 8);
  Vector3 v = Vector3(x, y, z);
  return v;
}

void NetworkPerception::ReceiveObject(unsigned char* data, Roboid* roboid) {
  unsigned char objectId = data[1];
  // char poseType = data[2];
  Vector3 worldPosition = ReceiveVector3(data, 3);
  Vector3 worldAngles = ReceiveVector3(data, 15);
  Quaternion worldOrientation = Quaternion::Euler(worldAngles);
  Vector3 worldDirection = worldOrientation * Vector3::forward;
  if (objectId == 0) {
    roboid->SetPosition(worldPosition);
    roboid->SetOrientation(worldOrientation);
    // printf("Received pose (%f, %f) (%f, %f)\n", worldPosition.x,
    //        worldPosition.z, worldDirection.x, worldDirection.z);

  } else {
    Vector3 myPosition = roboid->GetPosition();  // Vector3::zero;
    Vector2 myPosition2 = Vector3::ProjectHorizontalPlane(myPosition);
    Quaternion myOrientation = roboid->GetOrientation();
    Vector3 myDirection = myOrientation * Vector3::forward;
    Vector2 myDirection2 = Vector3::ProjectHorizontalPlane(myDirection);

    Vector3 localPosition =
        Quaternion::Inverse(myOrientation) * (worldPosition - myPosition);

    // float distance = Vector3::Distance(myPosition, worldPosition);
    // float angle = Vector3::SignedAngle(myDirection, localPosition,
    // Vector3::up);
    float distance = Vector3::Magnitude(localPosition);
    float angle =
        Vector3::SignedAngle(Vector3::forward, localPosition, Vector3::up);

    Polar position = Polar(angle, distance);

    // int objCount = roboid->perception->TrackedObjectCount();

    // printf("object received at (%f, %f)/(%f, %f) -> (%f %f)\n",
    // worldPosition.x,
    //        worldPosition.z, localPosition.x, localPosition.z, distance,
    //        angle);

    roboid->perception->AddTrackedObject(this, position);
  }
}