#include <DistanceSensor.h>
#include <Sensing.h>
#include <Switch.h>

#include <math.h>
#include <algorithm>

SensorPlacement::SensorPlacement(DistanceSensor* distanceSensor, Vector2 direction) {
  this->distanceSensor = distanceSensor;
  this->switchSensor = nullptr;
  this->direction = direction;
}
SensorPlacement::SensorPlacement(Switch* switchSensor, Vector2 direction) {
  this->distanceSensor = nullptr;
  this->switchSensor = switchSensor;
  this->direction = direction;
}

Sensing::Sensing() {}

// void Sensing::AddSensors(SensorPlacement* sensors, unsigned int sensorCount) {
//   this->sensors = sensors;
//   this->sensorCount = sensorCount;
// }

void Sensing::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];
  }
}

float Sensing::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->isDistanceSensor == false)
      continue;

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

float Sensing::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->isDistanceSensor == false)
      continue;

    DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing;
    float sensorAngle = placement.direction.x;
    // Serial.printf("  distance sensor: %f %f 0\n", -angle, sensorAngle);
    if (sensorAngle < 0 && sensorAngle > -angle) {
      minDistance = fmin(minDistance, distanceSensor->GetDistance());
    }
  }
  return minDistance;
}

float Sensing::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->isDistanceSensor == false)
      continue;

    DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing;
    float sensorAngle = placement.direction.x;
    // Serial.printf("  distance sensor: 0 %f %f\n", sensorAngle, angle);
    if (sensorAngle > 0 && sensorAngle < angle) {
      minDistance = fmin(minDistance, distanceSensor->GetDistance());
    }
  }
  return minDistance;
}

bool Sensing::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.direction.x;
    if (angle > fromAngle && angle < toAngle) {
      DistanceSensor* distanceSensor = (DistanceSensor*)placement.thing;

      // if (placement.switchSensor != nullptr && placement.switchSensor->IsOn())
      //   return true;
      // else
      if (distanceSensor != nullptr && distanceSensor->IsOn())
        return true;
    }
  }
  return false;
}