#pragma once

#include "Sensor.h"
#include "VectorAlgebra/Polar.h"

namespace Passer {
namespace RoboidControl {

/// @brief An object tracked by the roboid
class TrackedObject {
public:
  /// @brief An object tracked by the roboid
  /// @param sensor The Sensor which detected this object
  /// @param position The position in polar coordinates local to the roboid
  TrackedObject(Sensor *sensor, Polar position);

  /// @brief Update the position of the object
  /// @param position The latest known position of the object
  /// @details This will also update the confidence of the object to the
  /// maxConfidence value
  void Refresh(Polar position);

  /// @brief Decrease the confidence based on the elapsed time
  /// @param deltaTime The time since the last DegradeConfidence call
  /// @return Returns false when the object's confidence has reached zero, true
  /// otherwise
  /// @details When this function returns false, the object should no longer be
  /// tracked.
  bool DegradeConfidence(float deltaTime);

  /// @brief Determine whether this object is the same as another object
  /// @param otherObj The other object to compare to
  /// @return Returns true when both objects are considered the same
  /// The result of this check depends on the equalityDistance and equalityAngle
  /// value.
  bool IsTheSameAs(TrackedObject *otherObj);
  /// @brief The maximum difference in distance from the roboid in which two
  /// objects may be considered the same
  /// @details When the difference in distance is exactly this
  /// value, the objects can be the same.
  /// With a value of 0.3, objects with coordinates position->distance
  /// = 1.2F and position->distance = 1.6 will be considered different, but
  /// objects with coordinates position->distance = 1.2 and position->distance
  /// = 1.0 can be the same.
  static constexpr float equalityDistance = 0.3F;
  /// @brief The maximum difference in angle from the roboids orientation in
  /// which two objects may be considered the same
  /// @details When the difference in angle is exactly this value, the objects
  /// can be the same.
  /// With a value of 5.0, object with coordinates position->angle = 30
  /// and position->angle = 36 will be considered different, but object with
  /// coordinated position->angle = 30 and position->angle = 27 can be the same.
  static constexpr float equalityAngle = 5.0F;

  /// @brief The id of the tracked object
  char id;

  /// @brief The current position of the object
  Polar position = Polar::zero;
  /// @brief The sensor which provided that lastet pose this object
  Sensor *sensor = nullptr;

protected:
  static constexpr unsigned char maxConfidence = 255;
  static constexpr unsigned char confidenceDropSpeed = 1; // 2;
  unsigned char confidence;
};

} // namespace RoboidControl
} // namespace Passer