#pragma once

#include "Placement.h"
#include "Sensor.h"

#include <list>

class DistanceSensor;
class Switch;

class NewSensorPlacement : public Placement {
};

struct SensorPlacement {
  DistanceSensor* distanceSensor;
  Switch* switchSensor;
  Vector2 direction;

  SensorPlacement(DistanceSensor* distanceSensor, Vector2 direction);
  SensorPlacement(Switch* switchSensor, Vector2 direction);
};

// class Perception : public Sensing {
// }

class Sensing {
 public:
  /// @brief Setup sensing
  Sensing();

  // void AddSensors(SensorPlacement* sensors, unsigned int sensorCount);
  void AddSensors(Placement* sensors, unsigned int sensorCount);

  unsigned int GetSensorCount();
  Sensor* GetSensor(unsigned int sensorId);

  float DistanceForward(float angle = 90);

  /// @brief Distance to the closest object on the left
  /// @return distance in meters, INFINITY when no object is detected.
  /// @note An object is on the left when the `angle` is between -180 and 0 degrees.
  /// @note An object dead straight (0 degrees) is not reported.
  float DistanceLeft() { return DistanceLeft(180); }
  /// @brief Distance to the closest object on the left
  /// @param angle the maximum angle on the left used for detection.
  /// @return distance in meters, INFINITY when no object is detected.
  /// @note An object is on the left when the `angle` is between -`angle` and 0 degrees.
  /// @note An object dead straight (0 degrees) is not reported.
  /// @note When an object is beyond `angle` meters, it is not reported.
  float DistanceLeft(float angle);

  /// @brief Distance to the closest object on the right
  /// @return distance in meters, INFINITY when no object is detected
  /// @note An object is on the right when the `angle` is between 0 and 180 degrees
  /// @note An object dead straight (0 degrees) is not reported
  float DistanceRight() { return DistanceRight(180); }
  /// @brief Distance to the closest object on the right
  /// @param angle the maximum angle on the left used for detection.
  /// @return distance in meters, INFINITY when no object is detected
  /// @note An object is on the left when the `angle` is between 0 and `angle` degrees.
  /// @note An object dead straight (0 degrees) is not reported.
  /// @note When an object is beyond `angle` meters, it is not reported.
  float DistanceRight(float angle);

  float DistanceUp() { return DistanceUp(180); }
  float DistanceUp(float angle);
  float DistanceDown() { return DistanceDown(180); }
  float DistanceDown(float angle);

  float Distance(float leftAngle, float rightAngle);
  float GetDistance(float angle);

  bool SwitchOn(float fromAngle, float toAngle);

  void SetResolution(unsigned int resolution);
  void SetRange(float min, float max);
  float* GetDepthMap();
  unsigned int ToDepthMapIndex(float angle);
  void SetDepthMap(float angle, float distance);

 protected:
  // SensorPlacement* sensors = nullptr;
  Placement* sensorPlacements = nullptr;
  unsigned int sensorCount = 0;

  unsigned int resolution;
  float rangeMinimum;
  float rangeMaximum;
  float* depthMap = nullptr;
};