#include "NetworkPerception.h"

#include "NetworkSync.h"

#define RC_DEBUG true
#if RC_DEBUG
#include <Arduino.h>
#endif

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

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

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::ReceiveInvestigateMsg(unsigned char *data,
                                              Roboid *roboid) {
  unsigned char networkId = data[1];
  unsigned char objectId = data[2];

#if RC_DEBUG
  printf("Received InvestigateMsg [%d/%d]\n", networkId, objectId);
#endif

  if (networkId != roboid->networkSync->networkId)
    // We only response to investigation requests for our own objects
    return;

  if (objectId == 0x00) {
    // They are investigating the roboid itself!
    if (roboid->modelUrl != nullptr)
      roboid->networkSync->SendModel(roboid);
  } else {
    InterestingThing *thing =
        roboid->perception->FindTrackedObject(0x00, objectId);
    if (thing == nullptr)
      return;

    roboid->networkSync->NewObject(thing);
    roboid->networkSync->SendModel(thing);
  }
}

void NetworkPerception::ReceivePlane(unsigned char *data, Roboid *roboid) {
  unsigned char networkId = data[1];
  unsigned char poseType = data[3];
  if ((poseType & NetworkSync::Pose_Position) == 0 ||
      (poseType & NetworkSync::Pose_Orientation) == 0)

    // both position and orientation are required
    return;

  Spherical16 worldPosition = Spherical16::FromVector3(ReceiveVector3(data, 4));
  Spherical16 roboidPosition = roboid->GetPosition();
  Spherical16 deltaPosition = worldPosition - roboidPosition;

  float distance = deltaPosition.distance; // magnitude();
  if (roboid->perception->IsInteresting(distance) == false) {
    // printf("  plane not interesting\n");
    return;
  }

  SwingTwist16 roboidOrientation = roboid->GetOrientation();
  Spherical16 localPosition =
      SwingTwist16::Inverse(roboidOrientation) * deltaPosition;
  // float angle =
  //     Vector3::SignedAngle(Vector3::forward, localPosition, Vector3::up);

  // Polar position = Polar(angle, distance);
  Spherical16 position =
      localPosition; // Spherical16::FromVector3(localPosition);

  // printf("Received plane (%f %f %f) (%f %f %f) %f %f %f\n", worldPosition.x,
  //        worldPosition.y, worldPosition.z, roboidPosition.x,
  //        roboidPosition.y, roboidPosition.z, position.distance,
  //        (float)position.horizontalAngle, (float)position.verticalAngle);
  // printf("Received plane %f (%f %f)\n", position.distance,
  //        (float)position.horizontalAngle, (float)position.verticalAngle);
  roboid->perception->AddTrackedObject(this, position, SwingTwist16::identity,
                                       0x80, 0x80, networkId);
}

void NetworkPerception::ReceiveSphere(unsigned char *data, Roboid *roboid) {
  unsigned char networkId = data[1];

  float radius = ReceiveFloat100(data, 3);

  Spherical16 worldPosition = Spherical16::FromVector3(ReceiveVector3(data, 7));
  Spherical16 roboidPosition = roboid->GetPosition();
  Spherical16 deltaPosition = worldPosition - roboidPosition;

  SwingTwist16 roboidOrientation = roboid->GetOrientation();
  Spherical16 localPosition =
      SwingTwist16::Inverse(roboidOrientation) * deltaPosition;

  Spherical16 position =
      localPosition; // Spherical16::FromVector3(localPosition);

  roboid->perception->AddTrackedObject(this, position, SwingTwist16::identity,
                                       0x81, 0x81, networkId);
}

void NetworkPerception::ReceivePoseMsg(unsigned char *data, Roboid *roboid) {
  if (roboid->networkSync->networkId == 0) // We're not connected to a site yet
    return;

  unsigned char networkId = data[1];
  unsigned char objectId = data[2];
  unsigned char poseType = data[3];

  if (networkId == roboid->networkSync->networkId)
    networkId = 0x00;

  if (objectId == 0x80)
    return ReceivePlane(data, roboid);
  else if (objectId == 0x81)
    return ReceiveSphere(data, roboid);

#if RC_DEBUG
    // printf("Received PoseMsg [%d/%d]\n", networkId, objectId);
#endif

  InterestingThing *thing =
      roboid->perception->FindTrackedObject(networkId, objectId);

  SwingTwist16 roboidOrientation = roboid->GetOrientation();
  Spherical16 position = Spherical16::zero;
  SwingTwist16 orientation = SwingTwist16::identity;

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

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

      Spherical16 localPosition = SwingTwist16::Inverse(roboidOrientation) *
                                  (worldPosition - roboidPosition);
      position = localPosition; // Spherical16::FromVector3(localPosition);
      // printf("  worldPosition (%f %f %f) localPosition (%f %f %f)\n",
      //        worldPosition.Right(), worldPosition.Up(),
      //        worldPosition.Forward(), localPosition.Right(),
      //        localPosition.Up(), localPosition.Forward());
      // printf("  position: %f (%f %f)\n", position.distance,
      //        position.horizontal.InDegrees(), position.vertical.InDegrees());
      if (thing != nullptr)
        thing->position = position;
    }
  }

  if ((poseType & NetworkSync::Pose_Orientation) != 0) {
    Vector3 worldAngles = ReceiveVector3(data, 16);
    SwingTwist16 worldOrientation = SwingTwist16(
        Angle16::Degrees(worldAngles.Up()),
        Angle16::Degrees(worldAngles.Right()),
        Angle16::Degrees(
            worldAngles.Forward())); // Quaternion::Euler(worldAngles);
    if (objectId == 0) {
      roboid->SetOrientation(worldOrientation);
    } else {
      orientation = SwingTwist16::Inverse(roboidOrientation) * worldOrientation;
      if (thing != nullptr) {
        thing->orientation = orientation;
      }
    }
  }

  if (thing == nullptr) {
    thing = roboid->perception->AddTrackedObject(this, position, orientation,
                                                 0xFF, objectId, networkId);
    if (thing->networkId != 0x00) {
      // Unknown thing
      roboid->networkSync->SendInvestigate(thing);
    }
  }
}

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