#pragma once

#include "PerceivedObject.h"
#include "Placement.h"
#include "Polar.h"
#include "Quaternion.h"
#include "Sensor.h"

namespace Passer {
namespace RoboidControl {

class Roboid;

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

  /// @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);

  Roboid *roboid = nullptr;

  /// @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);

  Sensor *FindSensorOfType(unsigned int sensorType);

  /// @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);

  // Object Perception

  void AddPerceivedObject(Sensor *sensor, Polar position);
  // void AddPerceivedObject(PerceivedObject *obj);
  unsigned char PerceivedObjectCount();
  PerceivedObject **GetPerceivedObjects();

  // mainly used for confidence update
  void Update(float currentTimeMs);

  void UpdatePose(Polar translation);
  void UpdatePose(Quaternion rotation);

  float nearbyDistance = 0.3F;

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

  float lastUpdateTimeMs = 0;

  static const unsigned char maxObjectCount = 7;
  PerceivedObject
      *perceivedObjects[maxObjectCount]; // 7 is typically the maximum number of
                                         // object which can be tracked by a
                                         // human
};

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