#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();
  Sensor *GetSensor(unsigned int sensorId);

  /// @brief Gets the distance to the closest object
  /// @param direction The direction to look for objects
  /// @param range The range in which objects should be looked for
  /// @return The distance to the closest object in meters
  float GetDistance(float direction, float range = 10.0F);
  /// @brief Gets the distance to the closest object
  /// @param horizontalDirection The direction in the horizontal plane to look
  /// for objects
  /// @param verticalDirection The direction in the vertical plane to look for
  /// objects
  /// @param range The range in which objects should be looked for
  /// @return The distance to the closest object in meters
  /// The directions can be thought of as the polar angle (vertical) and
  /// azimuthal angle (horizontal) in the spherical coordinate system.
  float GetDistance(float horizontalDirection, float verticalDirection,
                    float range = 10.0F);

  /// @brief Checks if an object is nearby
  /// @param direction The direction to look for objects
  /// @param range The range in which objects should be looked for
  /// @return True when an object is close, False otherwise
  /// Wether an object is closeby depends on the sensor. This can be a sensor
  /// like a Switch or a DistanceSensor. The latter uses the
  /// DistanceSensor::triggerDistance to check if an object is nearby.
  bool ObjectNearby(float direction, float range = 10.0F);
  /// @brief Checks if an object is nearby
  /// @param horizontalDirection The direction in the horizontal plane to look
  /// for objects
  /// @param verticalDirection The direction in the vertical plane to look for
  /// objects
  /// @param range The range in which objects should be looked for
  /// @return True when an object is close, False otherwise
  /// Wether an object is closeby depends on the sensor. This can be a sensor
  /// like a Switch or a DistanceSensor. The latter uses the
  /// DistanceSensor::triggerDistance to check if an object is nearby.
  ///
  /// The directions can be thought of as the polar angle (vertical) and
  /// azimuthal angle (horizontal) in the spherical coordinate system.
  bool ObjectNearby(float horizontalDirection, float verticalDirection,
                    float range = 10.0F);

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;