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

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->ObjectNearby())
//           minDistance = fmin(minDistance, distanceSensor->GetDistance());
//       }
//     }
//   }
//   return minDistance;
// }

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