#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);

  float GetDistance(float fromAngle, float toAngle);
  float GetDistance(float fromHorizontalAngle,
                    float toHorizontalAngle,
                    float fromVerticalAngle,
                    float toVerticalAngle);
  //   float GetDistance(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 ObjectNearby(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;