#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 %d\n", buffer[0],
  //        buffer[2], buffer[2], buffer[3], buffer[4], buffer[5]);

  switch (buffer[0]) {
  case NetworkSync::CreateMsg:
    ReceiveCreateMsg(buffer, roboid);
    break;
  case NetworkSync::PoseMsg:
    ReceivePoseMsg(buffer, roboid);
    break;
  case NetworkSync::PoseTypeMsg:
    ReceiveTypedObject(buffer, roboid);
    break;
  }
}

#include <Arduino.h>
void NetworkPerception::ReceiveCreateMsg(unsigned char *data, Roboid *roboid) {
  unsigned char networkId = data[1];
  unsigned char objectId = data[2];
  unsigned char objectType = data[3];
  if (networkId == roboid->networkSync->networkId)
    return;

  // printf("Received create message [%d/%d]\n", networkId, objectId);
  InterestingThing *thing =
      roboid->perception->FindTrackedObject(networkId, objectId);
  if (thing != nullptr) {
    thing->type = objectType;
  }
}

void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) {
  unsigned char networkId = data[1];
  unsigned char objectId = data[2];
  unsigned char poseType = data[3];
  // printf("My network id == %d\n", roboid->networkSync->networkId);
  if (networkId == roboid->networkSync->networkId)
    networkId = 0x00;
  printf("Received pose message [%d/%d]\n", networkId, objectId);

  if ((poseType & NetworkSync::Pose_Position) != 0) {
    Vector3 worldPosition = ReceiveVector3(data, 4);
    if (objectId == 0) {
      roboid->SetPosition(worldPosition);
    } else {
      Vector3 roboidPosition = roboid->GetPosition();

      float distance = Vector3::Distance(roboidPosition, worldPosition);
      if (roboid->perception->IsInteresting(distance) == false) {
        printf("  not interesting\n");
        return;
      }

      Quaternion roboidOrientation = roboid->GetOrientation();
      Vector3 localPosition = Quaternion::Inverse(roboidOrientation) *
                              (worldPosition - roboidPosition);

      float angle =
          Vector3::SignedAngle(Vector3::forward, localPosition, Vector3::up);

      Polar position = Polar(angle, distance);
      /*
      Vector2 worldPosition2D = Vector2(worldPosition);

      Vector2 myPosition = roboid->GetPosition2D();
      float myOrientation = roboid->GetOrientation2D();

      Vector2 localPosition =
          Vector2::Rotate(worldPosition2D - myPosition, -myOrientation);

      float distance = Vector2::Magnitude(localPosition);
      float angle = Vector2::SignedAngle(Vector3::forward, localPosition);
      Polar position = Polar(angle, distance);
      */

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

      InterestingThing *thing = roboid->perception->AddTrackedObject(
          this, networkId, objectId, position);
      if (thing->type == 0xFF) {
        // Unknown thing
        roboid->networkSync->SendInvestigateThing(thing);
      }
    }
  }

  /*
    Vector3 worldAngles = ReceiveVector3(data, 16);
    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);
    }
  */
}

// HACK!!!
// #include "../../../src/LighthouseTracker.h"

void NetworkPerception::ReceiveTypedObject(unsigned char *data,
                                           Roboid *roboid) {
  unsigned char objectType = data[1];
  unsigned char objectId = data[2];
  if (objectType == 0x02) { // lighthouse
    // We require position and orientation for lighthouses
    if (data[3] !=
        (NetworkSync::Pose_Position | NetworkSync::Pose_Orientation)) {
      return;
    }

    Vector3 worldPosition = ReceiveVector3(data, 4);
    Vector3 worldAngles = ReceiveVector3(data, 16);
    Quaternion worldOrientation = Quaternion::Euler(worldAngles);

    // roboid->perception->AddTrackedObject(this, objectType, worldPosition,
    //                                      worldOrientation);
    // Sensor *sensor =
    //     roboid->perception->FindSensorOfType(0x82); // Lighthouse type
    //     sensor
    // LighthouseTracker *lhSensor = (LighthouseTracker *)sensor;
    // lhSensor->UpdateGeometry(objectId, worldPosition, worldOrientation);
  }
}

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;
}