#pragma once

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

namespace Passer {
namespace RoboidControl {

/// @brief Module to which keeps track of objects around the roboid
class Perception {
 public:
  /// @brief Default Constructor
  Perception();

  /// @brief Template to make it possible to leave out ths sensorCount
  /// @tparam sensorCount
  /// @param sensors An array of sensor placements
  template <unsigned int sensorCount>
  inline Perception(Placement (&sensors)[sensorCount]) {
    Perception(sensors, sensorCount);
  }
  /// @brief Create a perception setup with the given Sensors
  /// @param sensors The Placement of Sensors on the Roboid
  /// @param sensorCount The number of sensors in the placement array
  Perception(Placement* sensors, unsigned int sensorCount);

  /// @brief Get the number of Sensors
  /// @return The number of sensors, zero when no sensors are present
  unsigned int GetSensorCount();
  /// @brief Get a specific Sensor on the Roboid
  /// @param sensorId The index of the Sensor
  /// @return The requested Sensor or a nullptr when no Sensor with the given
  /// index could be found
  Sensor* GetSensor(unsigned int sensorIx);
  /// @brief Get the placement of a specific Sensor
  /// @param motorIx The index of the Sensor
  /// @return Returns the Placement or a nullptr when no Placement with the
  /// given index could be found
  Placement* GetSensorPlacement(unsigned int sensorIx);

  /// @brief Distance to the closest object on the front of the Roboid
  /// @param direction The direction relative to the forward direction in which
  /// the object should be present
  /// @return The distance to the closest object within the given range,
  /// INFINITY when no object is detected
  float DistanceForward(float direction = 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);

  /// @brief Distance to the closest object above the Roboid
  /// @return distance in meters, INFINITY when no object is detected
  float DistanceUp() { return DistanceUp(180); }
  /// @brief Distance to the closest object above the Roboid within the give
  /// range
  /// @param angle The angle relative to the upward direction to indicate the
  /// range
  /// @return distance in meters, INFINITY when no object is detected
  float DistanceUp(float angle);
  /// @brief Disatnce to the closest object under the Roboid
  /// @return distance in meters, INFINITY when no object is detected
  float DistanceDown() { return DistanceDown(180); }
  /// @brief Distance to the closest object under th4e Roboid within the given
  /// range
  /// @param angle The angle relative to the downward direciton to indicate the
  /// range
  /// @return distance in meters, INFINITY when no object is detected
  float DistanceDown(float angle);

  /// @brief Checks if an object is close within the give range in the
  /// horizontal plane
  /// @param fromAngle Start angle in the horizontal plane
  /// @param toAngle End angle in the horizontal plane
  /// @return True is an object is closeby
  /// @note Whether an object is closeby depends on the Distance Sensor
  /// @remark This function is likely to change in the near future
  bool SwitchOn(float fromAngle, float toAngle);

 protected:
  /// @brief The Placement of the Sensors used for Perception
  Placement* sensorPlacements = nullptr;
  /// @brief The number of Sensors used for Perception
  unsigned int sensorCount = 0;
};

}  // namespace RoboidControl
}  // namespace Passer
using namespace Passer::RoboidControl;