#pragma once

#include "Sensor.h"
#include "TrackedObject.h"
#include "VectorAlgebra/Polar.h"
#include "VectorAlgebra/Quaternion.h"
#include "VectorAlgebra/Spherical.h"

// #include <vector.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(Sensor **sensors, unsigned int sensorCount);

  /// @brief The roboid of this perception system
  Roboid *roboid = nullptr;

  unsigned int AddSensor(Sensor *sensor);

  /// @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 Find the first sensor of the given type
  /// @param sensorType The type of sensor as is defined in the Thing class (for
  /// example Thing::SensorType)
  /// @return The first sensor found or a nullptr which no sensor has been found
  /// of the given type
  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
  /// @details 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
  /// @details 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
  /// @details 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);

  /// @brief Add or update an object detected by the given sensor
  /// @param sensor The sensor which has detected the object
  /// @param position The position of the sensor in polar coordinates local to
  /// the roboid
  void AddTrackedObject(Sensor *sensor, Polar position);
  void AddTrackedObject(Sensor *sensor, Spherical position);

  /// @brief Retrieve the number of objects currently being tracked by the
  /// roboid
  /// @return The object of objects, which is always lower than maxObjectCount
  unsigned char TrackedObjectCount();
  /// @brief Retreive the objects currently tracked by the roboid
  /// @return An array of current objects
  /// @details The returned array this should never be a nullptr, but
  /// each array entry may be a nullptr when less than maxObjectCount objects is
  /// currently being tracked.
  InterestingThing **GetTrackedObjects();

  InterestingThing *GetMostInterestingThing();

  // mainly used for confidence update

  /// @brief Update the state of the perception.
  /// @param currentTimeMs The current time in milliseconds
  /// @details This will update the perceptoin of object. It will retrieve the
  /// latest state for each sensor and update the confidence of the tracked
  /// objects.
  void Update(float currentTimeMs);

  /// @brief Update the position/orientation of the preceived objects from the
  /// given roboid translation
  /// @param translation The translation of the roboid in world space in polar
  /// coordinates
  /// @details This function will be called through Roboid::SetPosition. It
  /// is advised to use that function to update the roboid position instead of
  /// this function.
  void UpdatePose(Polar translation);
  /// @brief Update the orientation of the perceived objecst from the given
  /// roboid rotation
  /// @param rotation The rotation of the roboid in world space
  void UpdatePose(Quaternion rotation);

  /// @brief Objects with a distance closed that this value will be considered
  /// nearby.
  /// @details This value is used by the ObjectNearby function to select the
  /// objects
  float nearbyDistance = 0.3F;

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

  float lastUpdateTimeMs = 0;

  static unsigned char maxObjectCount; // = 7; // 7 is typically the maximum
                                       // number of object which can
                                       // be tracked by a human
  InterestingThing **trackedObjects;
};

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