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

#include <math.h>

Perception::Perception() {}

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

void Perception::AddSensors(Placement* things, unsigned int thingCount) {
  sensorCount = 0;
  for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) {
    Thing* thing = things[thingIx].thing;
    if (thing->IsSensor())
      sensorCount++;
  }

  sensorPlacements = new Placement[sensorCount];

  unsigned int sensorIx = 0;
  for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) {
    Thing* thing = things[thingIx].thing;
    if (thing->IsSensor())
      sensorPlacements[sensorIx++] = things[thingIx];
  }
}

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::DistanceForward(float angle) {
  float minDistance = INFINITY;
  for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
    Placement placement = sensorPlacements[sensorIx];
    Sensor* sensor = (Sensor*)placement.thing;
    if (sensor->IsSensor())
      continue;

    DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing;
    if (placement.verticalDirection > -angle &&
        placement.verticalDirection < angle &&
        placement.horizontalDirection > -angle &&
        placement.horizontalDirection < angle) {
      minDistance = fmin(minDistance, distanceSensor->GetDistance());
    }
  }
  return minDistance;
}

float Perception::DistanceLeft(float angle) {
  float minDistance = INFINITY;
  for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
    Placement placement = sensorPlacements[sensorIx];
    Sensor* sensor = (Sensor*)placement.thing;
    if (sensor->IsSensor())
      continue;

    DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing;
    float sensorAngle = placement.horizontalDirection;
    if (sensorAngle < 0 && sensorAngle > -angle) {
      minDistance = fmin(minDistance, distanceSensor->GetDistance());
    }
  }
  return minDistance;
}

float Perception::DistanceRight(float angle) {
  float minDistance = INFINITY;
  for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
    Placement placement = sensorPlacements[sensorIx];
    Sensor* sensor = (Sensor*)placement.thing;
    if (sensor->type != Thing::DistanceSensorType)
      continue;

    DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing;
    float sensorAngle = placement.horizontalDirection;
    if (sensorAngle > 0 && sensorAngle < angle) {
      minDistance = fmin(minDistance, distanceSensor->GetDistance());
    }
  }
  return minDistance;
}

float Perception::DistanceUp(float angle) {
  float minDistance = INFINITY;
  for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
    Placement placement = sensorPlacements[sensorIx];
    Sensor* sensor = (Sensor*)placement.thing;
    if (sensor->IsSensor())
      continue;

    DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing;
    float sensorAngle = placement.verticalDirection;
    if (sensorAngle > 0 && sensorAngle < angle) {
      minDistance = fmin(minDistance, distanceSensor->GetDistance());
    }
  }
  return minDistance;
}

float Perception::DistanceDown(float angle) {
  float minDistance = INFINITY;
  for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
    Placement placement = sensorPlacements[sensorIx];
    Sensor* sensor = (Sensor*)placement.thing;
    if (sensor->IsSensor())
      continue;

    DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing;
    float sensorAngle = placement.verticalDirection;
    if (sensorAngle < 0 && sensorAngle > -angle) {
      minDistance = fmin(minDistance, distanceSensor->GetDistance());
    }
  }
  return minDistance;
}
*/

float Perception::GetDistance(float fromAngle, float toAngle) {
  float minDistance = INFINITY;
  if (toAngle < fromAngle)
    // Hmm. Can't look backward properly for now
    return minDistance;

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

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

float Perception::GetDistance(float fromHorizontalAngle,
                              float toHorizontalAngle,
                              float fromVerticalAngle,
                              float toVerticalAngle) {
  float minDistance = INFINITY;
  if (toHorizontalAngle < fromHorizontalAngle ||
      toVerticalAngle < fromVerticalAngle)
    // Hmm. Can't look backward properly for now
    return minDistance;

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

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

bool Perception::SwitchOn(float fromAngle, float toAngle) {
  if (toAngle < fromAngle)
    return false;

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

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

unsigned int Perception::ToDepthMapIndex(float angle) {
  unsigned int depthMapIx =
      (unsigned int)(((angle - rangeMinimum) / (rangeMaximum - rangeMinimum)) *
                     (float)resolution);
  return depthMapIx;
}

// float Perception::GetDistance(float angle) {
//   if (depthMap != nullptr) {
//     if (angle < rangeMinimum || angle > rangeMaximum)
//       return INFINITY;
//     unsigned int depthMapIx = ToDepthMapIndex(angle);
//     return depthMap[depthMapIx];
//   } else {
//     for (unsigned int sensorIx = 0; sensorIx < this->sensorCount;
//     sensorIx++)
//     {
//       Placement placement = sensorPlacements[sensorIx];
//       float placementAngle = placement.horizontalDirection;
//       if (placementAngle == angle) {
//         DistanceSensor* distanceSensor =
//         (DistanceSensor*)placement.thing; return
//         distanceSensor->GetDistance();
//       }
//     }
//   }
//   return INFINITY;
// }

void Perception::SetResolution(unsigned int resolution) {
  this->resolution = resolution;
  this->depthMap = new float[this->resolution];
}

void Perception::SetRange(float min, float max) {
  this->rangeMinimum = min;
  this->rangeMaximum = max;
}

float* Perception::GetDepthMap() {
  return this->depthMap;
}

void Perception::SetDepthMap(float angle, float distance) {
  if (angle < rangeMinimum || angle > rangeMaximum)
    return;

  unsigned int depthMapIx = ToDepthMapIndex(angle);
  depthMap[depthMapIx] = distance;
}

Sensor* Perception::GetSensor(float angle) {
  angle = Angle::Normalize(angle);

  for (unsigned int ix = 0; ix < this->sensorCount; ix++) {
    Placement placement = this->sensorPlacements[ix];
    float delta = placement.horizontalDirection - angle;
    if (delta > -0.01F && delta < 0.01F)
      return (Sensor*)placement.thing;
  }

  return nullptr;
}