#pragma once

#include "LinearAlgebra/AngleAxis.h"
#include "Perception.h"
#include "Propulsion.h"
#include "ServoMotor.h"

namespace Passer {
namespace RoboidControl {

class NetworkSync;

/// @brief A Roboid is used to control autonomous robots
class Roboid : public Thing {
public:
  /// @brief Default constructor for a Roboid
  Roboid();
  /// @brief Creates a Roboid with Perception and Propulsion abilities
  /// @param perception The Perception implementation to use for this Roboid
  /// @param propulsion The Propulsion implementation to use for this Roboid
  Roboid(Propulsion *propulsion);

  char *name = nullptr;
  void SetName(char *name);

  /// @brief Update the state of the Roboid
  /// @param currentTimeMs The time in milliseconds when calling this
  /// function
  void Update(unsigned long currentTimeMs);

  /// @brief The Perception module of this Roboid
  Perception *perception = nullptr;
  /// @brief The Propulsion module of this Roboid
  Propulsion *propulsion = nullptr;
  // ServoMotor *actuation = nullptr;

  /// @brief The reference to the module to synchronize states across a network
  NetworkSync *networkSync = nullptr;

  /// @brief Retrieve the current position of the roboid
  /// @return The position in carthesian coordinates in world space
  /// @details The origin and units of the position depends on the position
  /// tracking system used. This value will be Vector3::zero unless a position
  /// is received through network synchronisation
  virtual Spherical16 GetPosition();
  // Vector2 GetPosition2D();
  /// @brief Retrieve the current orientation of the roboid
  /// @return The orientation quaternion in world space
  /// @details The origin orientation depends on the position tracking system
  /// used. This value will be Quaternion::identity unless an orientation is
  /// received though network synchronization
  virtual SwingTwist16 GetOrientation();

  /// @brief Update the current position of the roboid
  /// @param worldPosition The position of the roboid in carthesian coordinates
  /// in world space
  /// @details The use of this function will also update the positions and
  /// orientations of the perceived objects by the roboid
  /// (roboid->perception->perceivedObjects), as these are local to the
  /// roboid's position.
  virtual void SetPosition(Spherical16 worldPosition);
  /// @brief Update the current orientation of the roboid
  /// @param worldOrientation The orientation of the roboid in world space
  /// @details The use of this function will also update the orientations of the
  /// perceived objects by the roboid (roboid->perception->perceivedObjets),
  /// as these are local to the roboid' orientation.
  virtual void SetOrientation(SwingTwist16 worldOrientation);

  virtual void AddChild(Thing *child) override;

private:
  /// @brief The position of the roboid in carthesian coordinates in world space
  /// @details This position may be set when NetworkSync is used to receive
  /// positions from an external tracking system. These values should not be set
  /// directly, but SetPosition should be used instead.
  // Spherical16 worldPosition = Spherical16::zero;
  /// @brief The orientation of the roboid in world space
  /// @details The position may be set when NetworkSync is used to receive
  /// orientations from an external tracking system. This value should not be
  /// set directly, but SetOrientation should be used instead.
  // Quaternion worldOrientation = Quaternion::identity;

  // AngleAxis worldAngleAxis = AngleAxis();

  unsigned long lastUpdateTimeMs = 0;
};

} // namespace RoboidControl
} // namespace Passer

using namespace Passer::RoboidControl;