#pragma once

#include "LinearAlgebra/AngleAxis.h"
#include "LinearAlgebra/Polar.h"
#include "LinearAlgebra/Quaternion.h"
#include "LinearAlgebra/Spherical.h"
#include "Sensor.h"

namespace Passer {
namespace RoboidControl {

/// @brief An object tracked by the roboid
class InterestingThing {
 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
  InterestingThing(Sensor* sensor, Polar position);
  InterestingThing(Sensor* sensor,
                   Spherical position,
                   Quaternion orientation = Quaternion::identity);

  /// @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);
  void Refresh(Spherical position,
               Quaternion orientation = Quaternion::identity);

  /// @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(InterestingThing* 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;

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

  char parentId = 0;
  /// @brief The current position of the object
  Spherical position = Spherical::zero;
  /// @brief The current orientation of the object
  AngleAxis<float> orientation = AngleAxis<float>();
  // Quaternion orientation = Quaternion::identity;
  /// @brief The sensor which provided that lastet pose this object
  Sensor* sensor = nullptr;

  unsigned char type = 0x00;
  unsigned char confidence;
  bool updated = false;

 protected:
  static constexpr unsigned char maxConfidence = 255;
  static constexpr unsigned char confidenceDropSpeed = 10;  // 150; // 2;
};

}  // namespace RoboidControl
}  // namespace Passer