#include "Perception.h"
#include "DistanceSensor.h"
#include "NetworkSync.h"
#include "Switch.h"
#include "VectorAlgebra/Angle.h"

#include <math.h>

#ifdef RC_DEBUG2
#include <Arduino.h>
#endif

unsigned char Perception::maxObjectCount = 7; // 7 is typically the maximum
                                              // number of object which can
                                              // be tracked by a human

Perception::Perception() {
  this->trackedObjects = new InterestingThing *[maxObjectCount];
  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++)
    this->trackedObjects[objIx] = nullptr;
}

Perception::Perception(Sensor **sensors, unsigned int sensorCount)
    : Perception() {
  this->sensorCount = sensorCount;
  this->sensors = new Sensor *[this->sensorCount];
  for (unsigned char sensorIx = 0; sensorIx < this->sensorCount; sensorIx++)
    this->sensors[sensorIx] = sensors[sensorIx];

  this->trackedObjects = new InterestingThing *[maxObjectCount];
  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++)
    this->trackedObjects[objIx] = nullptr;
}

unsigned int Perception::GetSensorCount() { return this->sensorCount; }

Sensor *Perception::GetSensor(unsigned int sensorId) {
  if (sensorId >= this->sensorCount)
    return nullptr;

  Sensor *sensor = this->sensors[sensorId];
  return sensor;
}

unsigned int Perception::AddSensor(Sensor *newSensor) {
  unsigned int newSensorCount = this->sensorCount + 1;
  Sensor **newSensors = new Sensor *[newSensorCount];
  for (unsigned char sensorIx = 0; sensorIx < this->sensorCount; sensorIx++)
    newSensors[sensorIx] = sensors[sensorIx];

  unsigned int sensorId = this->sensorCount;
  newSensors[sensorId] = newSensor;

  Sensor **oldSensors = this->sensors;
  this->sensors = newSensors;
  this->sensorCount = newSensorCount;
  delete[] oldSensors;
  return sensorId;
}

Sensor *Perception::FindSensorOfType(unsigned int sensorType) {
  for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
    Sensor *sensor = this->sensors[sensorIx];
    if (sensor->type == sensorType)
      return sensor;
  }

  return nullptr;
}

float Perception::GetDistance(float horizontalDirection, float range) {
  float minDistance = INFINITY;
  if (range < 0)
    range = -range;

  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
    InterestingThing *obj = trackedObjects[objIx];
    if (obj == nullptr)
      continue;
    if (obj->position.horizontalAngle > horizontalDirection - range &&
        obj->position.horizontalAngle < horizontalDirection + range) {

      minDistance = fminf(minDistance, obj->position.magnitude);
    }
  }
  return minDistance;
}

float Perception::GetDistance(float horizontalDirection,
                              float verticalDirection, float range) {
  float minDistance = INFINITY;
  if (range < 0)
    range = -range;

  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
    InterestingThing *obj = trackedObjects[objIx];
    if (obj == nullptr)
      continue;
    if (obj->position.horizontalAngle > horizontalDirection - range &&
        obj->position.horizontalAngle < horizontalDirection + range) {

      minDistance = fminf(minDistance, obj->position.magnitude);
    }
  }

  return minDistance;
}
bool Perception::ObjectNearby(float direction, float range) {
  if (range < 0)
    range = -range;

  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
    InterestingThing *obj = trackedObjects[objIx];
    if (obj == nullptr)
      continue;

    if (obj->position.horizontalAngle > direction - range &&
        obj->position.horizontalAngle < direction + range) {
      if (obj->position.magnitude <= nearbyDistance)
        return true;
    }
  }
  return false;
}

void Perception::AddTrackedObject(Sensor *sensor, Polar position) {
  InterestingThing *obj = new InterestingThing(sensor, position);

  unsigned char farthestObjIx = 0;
  unsigned char availableSlotIx = 0;
  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
    if (this->trackedObjects[objIx] == nullptr) {
      availableSlotIx = objIx;
    }
    // Do we see the same object?
    else {
      if (obj->IsTheSameAs(this->trackedObjects[objIx])) {
#ifdef RC_DEBUG2
        Serial.print((int)this->trackedObjects[objIx]->id);
        Serial.println(": update tracked object");
#endif

        this->trackedObjects[objIx]->Refresh(obj->position);
        delete obj;
        return;
      }
      // Is this the fartest object we see?
      else if (this->trackedObjects[farthestObjIx] == nullptr ||
               (this->trackedObjects[objIx]->position.magnitude >
                this->trackedObjects[farthestObjIx]->position.magnitude)) {
        farthestObjIx = objIx;
      }
    }
  }

  // Check if an perception slot is available (we currently see less than the
  // max number of objects)
  if (availableSlotIx < maxObjectCount) {
    // a slot is available
    this->trackedObjects[availableSlotIx] = obj;
    obj->id = availableSlotIx;
#ifdef RC_DEBUG2
    Serial.print((int)obj->id);
    Serial.println(": new tracked object");
#endif

  }
  // If this object is closer than the farthest object, then replace it
  else if (obj->position.magnitude <
           this->trackedObjects[farthestObjIx]->position.magnitude) {
    delete this->trackedObjects[farthestObjIx];
    this->trackedObjects[farthestObjIx] = obj;
    obj->id = availableSlotIx;
#ifdef RC_DEBUG2
    Serial.print((int)obj->id);
    Serial.println(": replaced tracked object");
#endif
  } else {
#ifdef RC_DEBUG2
    Serial.print((int)obj->id);
    Serial.println(": delete tracked object");
#endif
    // No available slot, delete trackedobject
    delete obj;
  }
}

void Perception::AddTrackedObject(Sensor *sensor, Spherical position) {
  InterestingThing *obj = new InterestingThing(sensor, position);

  unsigned char farthestObjIx = 0;
  unsigned char availableSlotIx = 0;
  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
    if (this->trackedObjects[objIx] == nullptr) {
      availableSlotIx = objIx;
    }
    // Do we see the same object?
    else {
      if (obj->IsTheSameAs(this->trackedObjects[objIx])) {
#ifdef RC_DEBUG2
        Serial.print((int)this->trackedObjects[objIx]->id);
        Serial.println(": update tracked object");
#endif

        this->trackedObjects[objIx]->Refresh(obj->position);
        delete obj;
        return;
      }
      // Is this the fartest object we see?
      else if (this->trackedObjects[farthestObjIx] == nullptr ||
               (this->trackedObjects[objIx]->position.magnitude >
                this->trackedObjects[farthestObjIx]->position.magnitude)) {
        farthestObjIx = objIx;
      }
    }
  }

  // Check if an perception slot is available (we currently see less than the
  // max number of objects)
  if (availableSlotIx < maxObjectCount) {
    // a slot is available
    this->trackedObjects[availableSlotIx] = obj;
    obj->id = availableSlotIx;
#ifdef RC_DEBUG2
    Serial.print((int)obj->id);
    Serial.println(": new tracked object");
#endif

  }
  // If this object is closer than the farthest object, then replace it
  else if (obj->position.magnitude <
           this->trackedObjects[farthestObjIx]->position.magnitude) {
    delete this->trackedObjects[farthestObjIx];
    this->trackedObjects[farthestObjIx] = obj;
    obj->id = availableSlotIx;
#ifdef RC_DEBUG2
    Serial.print((int)obj->id);
    Serial.println(": replaced tracked object");
#endif
  } else {
#ifdef RC_DEBUG2
    Serial.print((int)obj->id);
    Serial.println(": delete tracked object");
#endif
    // No available slot, delete trackedobject
    delete obj;
  }
}

unsigned char Perception::TrackedObjectCount() {
  unsigned char objectCount = 0;
  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
    if (this->trackedObjects[objIx] != nullptr)
      objectCount++;
  }
  return objectCount;
}

InterestingThing **Perception::GetTrackedObjects() {
  return this->trackedObjects;
}

InterestingThing *Perception::GetMostInterestingThing() {
  InterestingThing *closestObject = nullptr;
  float closestDistance = INFINITY;
  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
    InterestingThing *obj = this->trackedObjects[objIx];
    if (obj != nullptr) {
      if (obj->position.magnitude < closestDistance) {
        closestObject = obj;
        closestDistance = obj->position.magnitude;
      }
    }
  }
  return closestObject;
}

#include <Arduino.h>

void Perception::Update(float currentTimeMs) {
  float deltaTime = (currentTimeMs - lastUpdateTimeMs) / 1000;
  if (deltaTime <= 0)
    return;

  lastUpdateTimeMs = currentTimeMs;

  // Update sensing
  for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
    Sensor *sensor = sensors[sensorIx];
    if (sensor == nullptr)
      continue;

    if (sensor->type == Thing::DistanceSensorType) {
      DistanceSensor *distanceSensor = (DistanceSensor *)sensor;

      float distance = distanceSensor->GetDistance();
      if (distance >= 0) {
        float angle = sensor->position.angle;
        Polar position = Polar(angle, distance);
        AddTrackedObject(distanceSensor, position);
      }

    } else if (sensor->type == Thing::SwitchType) {
      Switch *switchSensor = (Switch *)sensor;
      if (switchSensor->IsOn()) {
        Polar position = Polar(sensor->position.angle, nearbyDistance);
        AddTrackedObject(switchSensor, position);
      }
    } else {
      sensor->Update();
    }
  }

  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
    InterestingThing *obj = trackedObjects[objIx];
    if (obj == nullptr)
      continue;

    if (obj->DegradeConfidence(deltaTime) == false) {
      // delete obj
      if (roboid != nullptr && roboid->networkSync != nullptr)
        roboid->networkSync->DestroyObject(obj);
      this->trackedObjects[objIx] = nullptr;
      delete obj;
    }
  }
}

void Perception::UpdatePose(Polar translation) {
  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
    InterestingThing *obj = trackedObjects[objIx];
    if (obj == nullptr)
      continue;

    // We only support translations on polars at this moment...
    // This needs Spherical operator- to be implemented to work in 3d space
    Polar horizontalPosition = obj->position.ProjectOnHorizontalPlane();
    Spherical newPosition = Spherical(horizontalPosition - translation);
    obj->position = newPosition;
  }
}

void Perception::UpdatePose(Quaternion rotation) {
  // only rotation around vertical axis is supported for now
  float rotationAngle;
  Vector3 rotationAxis;
  rotation.ToAngleAxis(&rotationAngle, &rotationAxis);
  // Make sure rotation axis is positive
  if (rotationAxis.y < 0)
    rotationAngle = -rotationAngle;

  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
    InterestingThing *obj = trackedObjects[objIx];
    if (obj == nullptr)
      continue;

    float updatedAngle =
        Angle::Normalize(obj->position.horizontalAngle - rotationAngle);
    obj->position.horizontalAngle = updatedAngle;
  }
}