#include "NetworkPerception.h"

#include "ControlCore/Messages.h"
#include "NetworkSync.h"
#include "float16/float16.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::ThingMsg:
    ReceiveThingMsg(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::ReceiveThingMsg(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;
  }
  printf("complete\n");
}

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

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

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

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

      roboid->networkSync->SendThingInfo(interestingThing, false);
    }
  }
}

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);
  */
  SwingTwist16 originOrientation;
  Spherical16 originPosition;
  if (roboid->worldOrigin == nullptr) {
    originOrientation = SwingTwist16::identity;
    originPosition = Spherical16::zero;
  } else {
    originOrientation = roboid->worldOrigin->orientation;
    originPosition = roboid->worldOrigin->position;
  }
  Spherical16 position = originPosition + originOrientation * worldPosition;
  // 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);
  */
  SwingTwist16 originOrientation;
  Spherical16 originPosition;
  if (roboid->worldOrigin == nullptr) {
    originOrientation = SwingTwist16::identity;
    originPosition = Spherical16::zero;
  } else {
    originOrientation = roboid->worldOrigin->orientation;
    originPosition = roboid->worldOrigin->position;
  }
  Spherical16 position = originPosition + originOrientation * worldPosition;

  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);
  // if (thing == nullptr) {
  //   thing = roboid->perception->AddTrackedObject(this, position, orientation,
  //                                                0xFF, objectId, networkId);
  //   if (thing->networkId != 0x00) {
  //     // Unknown thing
  //     InvestigateMsg::Send(thing);
  //     // roboid->networkSync->SendInvestigate(thing);
  //   }
  // }

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

  // Vector3 worldAngles = ReceiveVector3(data, 16);
  // SwingTwist16 worldOrientation = SwingTwist16(
  //     Angle16::Degrees(worldAngles.Up()),
  //     Angle16::Degrees(worldAngles.Right()), Angle16::Degrees(
  //         worldAngles.Forward())); // Quaternion::Euler(worldAngles);

  // if ((poseType & NetworkSync::Pose_Orientation) != 0) {
  //   if (objectId == 0) {
  //     // roboid->SetOrientation(worldOrientation);
  //     if (roboid->worldOrigin == nullptr) {
  //       printf("creating new origin\n");
  //       roboid->worldOrigin = new Thing(0);
  //       printf("created\n");
  //     }

  //     roboid->worldOrigin->orientation =
  //         SwingTwist16::Inverse(worldOrientation);
  //   } else {
  //     // orientation = SwingTwist16::Inverse(roboidOrientation) *
  //     // worldOrientation; if (thing != nullptr) {
  //     //   thing->orientation = orientation;
  //     // }
  //     SwingTwist16 originOrientation;
  //     Spherical16 originPosition;
  //     if (roboid->worldOrigin == nullptr) {
  //       originOrientation = SwingTwist16::identity;
  //       originPosition = Spherical16::zero;
  //     } else {
  //       originOrientation = roboid->worldOrigin->orientation;
  //       originPosition = roboid->worldOrigin->position;
  //     }

  //     thing->orientation = originOrientation * worldOrientation;
  //   }
  // }

  // if ((poseType & NetworkSync::Pose_Position) != 0) {
  //   Vector3 worldVector3 = ReceiveVector3(data, 4);
  //   Spherical16 worldPosition = Spherical16::FromVector3(worldVector3);
  //   if (objectId == 0) {
  //     // roboid->SetPosition(worldPosition);
  //     if (roboid->worldOrigin == nullptr) {
  //       printf("creating new origin again\n");
  //       roboid->worldOrigin = new Thing(0);
  //     }

  //     roboid->worldOrigin->position =
  //         roboid->worldOrigin->orientation * -worldPosition;
  //   } else {
  //     SwingTwist16 originOrientation;
  //     Spherical16 originPosition;
  //     if (roboid->worldOrigin == nullptr) {
  //       originOrientation = SwingTwist16::identity;
  //       originPosition = Spherical16::zero;
  //     } else {
  //       originOrientation = roboid->worldOrigin->orientation;
  //       originPosition = roboid->worldOrigin->position;
  //     }

  //     // SwingTwist16 roboidLocalOrientation =
  //     //     originOrientation * worldOrientation;

  //     thing->position = originPosition + originOrientation * worldPosition;

  //     /*
  //     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);
  //     Vector3 roboidVector3 = roboidPosition.ToVector3();
  //     printf("  roboid position (%f %f %f)\n", roboidVector3.Right(),
  //            roboidVector3.Up(), roboidVector3.Forward());
  //     printf("  [%d/%d] worldPosition (%f %f %f) localPosition %f (%f %f)\n
  //     ",
  //            networkId, objectId, worldVector3.Right(), worldVector3.Up(),
  //            worldVector3.Forward(), localPosition.distance,
  //            localPosition.direction.horizontal.InDegrees(),
  //            localPosition.direction.vertical.InDegrees());

  //     thing->position = localPosition;
  //     */
  //   }
  // }
}

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

float NetworkPerception::ReceiveFloat16(unsigned char *data,
                                        unsigned char *startIndex) {
  byte ix = *startIndex;
  unsigned short value = data[ix++] << 8 | data[ix];
  float16 f = float16();
  f.setBinary(value);

  *startIndex = ix;
  return f.toDouble();
}