#include "Sensing.h"
#include "DistanceSensor.h"
#include "Switch.h"

#include <math.h>

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(Placement *things, unsigned int thingCount) {
  sensorCount = 0;
  for (unsigned int thingIx = 0; thingIx < thingCount; thingIx++) {
    Thing *thing = things[thingIx].thing;
    if ((thing->type & Thing::SensorType) != 0)
      sensorCount++;
  }

  sensorPlacements = new Placement[sensorCount];

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

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

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

  Thing *thing = this->sensorPlacements[sensorId].thing;
  if (thing->type & Thing::SensorType != 0)
    return (Sensor *)thing;

  return nullptr;
}

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->type & Thing::SensorType != 0)
      continue;

    DistanceSensor *distanceSensor = (DistanceSensor *)placement.thing;
    float sensorAngle = placement.direction.z;
    if (sensorAngle > 0 && 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->type & Thing::SensorType != 0)
      continue;

    DistanceSensor *distanceSensor = (DistanceSensor *)placement.thing;
    float sensorAngle = placement.direction.y;
    // 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;
  Serial.printf("  distance sensor count: %d\n", sensorCount);
  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.direction.y;
    Serial.printf("  distance sensor: 0 %f %f\n", sensorAngle, angle);
    if (sensorAngle > 0 && sensorAngle < angle) {
      minDistance = fmin(minDistance, distanceSensor->GetDistance());
    }
  }
  return minDistance;
}

float Sensing::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->type & Thing::SensorType != 0)
      continue;

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

float Sensing::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->type & Thing::SensorType != 0)
      continue;

    DistanceSensor *distanceSensor = (DistanceSensor *)placement.thing;
    float sensorAngle = placement.direction.y; // not correct!
    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.y;
    if (angle > fromAngle && angle < toAngle) {
      Thing *thing = placement.thing;
      if (thing == nullptr)
        continue;

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

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

float Sensing::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.direction.x;
      if (placementAngle == angle) {
        DistanceSensor *distanceSensor = (DistanceSensor *)placement.thing;
        return distanceSensor->GetDistance();
      }
    }
  }
  return INFINITY;
}

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

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

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

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

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