#include "Perception.h"
#include "Angle.h"
#include "DistanceSensor.h"
#include "Switch.h"

#include <Arduino.h>
#include <math.h>

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

Perception::Perception(Placement *sensors, unsigned int sensorCount)
    : Perception() {
  this->sensorCount = sensorCount;
  this->sensorPlacements = (Placement *)sensors;
}

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

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

  Thing *thing = this->sensorPlacements[sensorId].thing;
  if (thing->IsSensor())
    return (Sensor *)thing;

  return nullptr;
}

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

  for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
    Placement placement = sensorPlacements[sensorIx];
    // This still needs support for angles wrapping around 180 degrees !!!!
    if (placement.horizontalDirection > direction - range &&
        placement.horizontalDirection < direction + range) {
      Thing *thing = placement.thing;
      if (thing == nullptr)
        continue;

      if (thing->type == Thing::DistanceSensorType) {
        DistanceSensor *distanceSensor = (DistanceSensor *)thing;
        if (distanceSensor != nullptr && distanceSensor->ObjectNearby())
          minDistance = fmin(minDistance, distanceSensor->GetDistance());
      }
    }
  }
  return minDistance;
}

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

  for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
    Placement placement = sensorPlacements[sensorIx];
    // This still needs support for angles wrapping around 180 degrees !!!!
    if (placement.horizontalDirection > horizontalDirection - range &&
        placement.horizontalDirection < horizontalDirection + range &&
        placement.verticalDirection > verticalDirection - range &&
        placement.verticalDirection < verticalDirection + range) {
      Thing *thing = placement.thing;
      if (thing == nullptr)
        continue;

      if (thing->type == Thing::DistanceSensorType) {
        DistanceSensor *distanceSensor = (DistanceSensor *)thing;
        if (distanceSensor != nullptr && distanceSensor->ObjectNearby())
          minDistance = fmin(minDistance, distanceSensor->GetDistance());
      }
    }
  }
  return minDistance;
}
bool Perception::ObjectNearby(float direction, float range) {
  if (range < 0)
    range = -range;

  for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
    Placement placement = sensorPlacements[sensorIx];
    if (placement.horizontalDirection > direction - range &&
        placement.horizontalDirection < direction + range) {
      Thing *thing = placement.thing;
      if (thing == nullptr)
        continue;

      if (thing->type == Thing::DistanceSensorType) {
        DistanceSensor *distanceSensor = (DistanceSensor *)thing;
        if (distanceSensor != nullptr && distanceSensor->ObjectNearby())
          return true;
      } else if (thing->type == Thing::SwitchType) {
        Switch *switchSensor = (Switch *)thing;
        if (switchSensor != nullptr && switchSensor->IsOn())
          return true;
      }
    }
  }
  return false;
}

/***
 * Oject perception
 ***/

PerceivedObject::PerceivedObject() {
  this->id = 0;
  this->confidence = maxConfidence;
  this->position = Polar(0, INFINITY);
  this->radius = INFINITY;
}

PerceivedObject::PerceivedObject(Polar position, float radius)
    : PerceivedObject() {
  this->position = position;
  this->radius = radius;
}

bool PerceivedObject::IsTheSameAs(PerceivedObject *otherObj) {
  if (id != 0 && id == otherObj->id)
    return true;
  if (abs(position.distance - otherObj->position.distance) > equalityDistance)
    return false;
  if (abs(position.angle - otherObj->position.angle) > equalityAngle)
    return false;
  return true;
}

bool PerceivedObject::DegradeConfidence(float deltaTime) {
  unsigned char confidenceDrop =
      (unsigned char)((float)confidenceDropSpeed * deltaTime);
  // Make sure the confidence always drops
  if (confidenceDrop == 0)
    confidenceDrop = 1;

  if (confidence <= confidenceDrop) {
    // object is dead
    confidence = 0;
    return false;
  } else {
    confidence -= confidenceDrop;
    return true;
  }
}

void PerceivedObject::Refresh(Polar position, float radius) {
  this->position = position;
  this->radius = radius;
  this->confidence = maxConfidence;
}

void Perception::AddPerceivedObject(PerceivedObject *obj) {
  unsigned char farthestObjIx = 0;
  unsigned char availableSlotIx = 0;
  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
    // Is this slot available?
    if (perceivedObjects[objIx] == nullptr) {
      availableSlotIx = objIx;
    }
    // Do we see the same object?
    else if (obj->IsTheSameAs(perceivedObjects[objIx])) {
      perceivedObjects[objIx]->Refresh(obj->position, obj->radius);
      return;
    }
    // Is this the fartest object we see?
    else if (perceivedObjects[farthestObjIx] == nullptr ||
             (perceivedObjects[objIx]->position.distance >
              perceivedObjects[farthestObjIx]->position.distance)) {
      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
    perceivedObjects[availableSlotIx] = obj;
  }
  // If this object is closer than the farthest object, then replace it
  else if (obj->position.distance <
           perceivedObjects[farthestObjIx]->position.distance) {
    perceivedObjects[farthestObjIx] = obj;
    // we may want to destroy the fartest object, but if it is created
    // externally, other links may still exist...
  }
}

unsigned char Perception::PerceivedObjectCount() {
  unsigned char objectCount = 0;
  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
    if (perceivedObjects[objIx] != nullptr)
      objectCount++;
  }
  return objectCount;
}

PerceivedObject **Perception::GetPerceivedObjects() { return perceivedObjects; }

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

  lastUpdateTimeMs = currentTimeMs;

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

    if (obj->DegradeConfidence(deltaTime) == false) {
      // delete obj
      perceivedObjects[objIx] = nullptr;
    } else {
      Serial.printf("[%d] confidence: %d\n", objIx,
                    perceivedObjects[objIx]->confidence);
    }
  }
  if (perceivedObjects[0] != nullptr) {
  }
}