#pragma once

#include "Motor.h"
#include "VectorAlgebra/Polar.h"
#include "VectorAlgebra/Quaternion.h"
#include "VectorAlgebra/Spherical.h"
#include "VectorAlgebra/Vector2.h"
#include "VectorAlgebra/Vector3.h"

namespace Passer {
namespace RoboidControl {

class Roboid;

/// @brief The Propulsion module for a Roboid is used to move the Roboid in
/// space
/// @details Usually, a specific implementation of the propulsion module is used
/// for a robot. This base class does not implement the functions to move the
/// Roboid around.
class Propulsion {
public:
  /// @brief Default Constructor for Propulsion
  Propulsion();

  /// @brief Update the propulsion state of the Roboid
  /// @param currentTimeMs  The time in milliseconds when calling this
  virtual void Update(float currentTimeMs);

  /// @brief Get the number of motors in this roboid
  /// @return The number of motors. Zero when no motors are present
  unsigned int GetMotorCount();
  /// @brief Get a specific motor
  /// @param motorIx The index of the motor
  /// @return Returns the motor or a nullptr when no motor with the given index
  /// could be found
  Motor *GetMotor(unsigned int motorIx);
  /// @brief Get the Placement of a specific Motor
  /// @param motorIx The index of the Motor
  /// @return Returns the Placement or a nullptr when no Placement with the give
  /// index could be found
  // Placement *GetMotorPlacement(unsigned int motorIx);

  /*
    /// @brief Sets the forward and rotation speed of a (grounded) Roboid
    /// @param forward The target forward speed
    /// @param yaw The target rotation speed around the vertical axis
    /// This function is typically used for Roboid which are driving on the
    /// ground.
    virtual void SetTwistSpeed(float forward, float yaw = 0.0F);
    /// @brief Sets the forward, sideward and rotation speed of a (grounded)
    /// Roboid
    /// @param linear The target linear (forward, sideward) speed
    /// @param yaw The target rotation speed around the vertical axis
    /// This function is typically used for Roboid which are driving on the
    ground
    /// which have to ability to move sideward
    virtual void SetTwistSpeed(Vector2 linear, float yaw = 0.0F);
    /// @brief Set the target 3D linear and 3D rotation speed of a (flying)
    Roboid
    /// @param linear The target linear speed
    /// @param yaw The target rotation speed around the vertical axis
    /// @param pitch The target rotation speed around the sideward axis
    /// @param roll The target rotation speed around hte forward axis
    virtual void SetTwistSpeed(Vector3 linear,
                               float yaw = 0.0F,
                               float pitch = 0.0F,
                               float roll = 0.0F);
  */
  virtual void SetTargetVelocity(float velocity);
  // virtual void SetTargetVelocity(Polar velocity);
  // virtual void SetTargetVelocity(Spherical velocity);
  virtual void SetTargetVelocity(Vector2 velocity);
  virtual void SetTargetVelocity(Vector3 velocity);

  virtual void SetTargetYawVelocity(float yaw);
  // virtual void SetTargetAngularVelocity(float yaw, float pitch, float roll);

  virtual float GetTargetForwardVelocity();

  virtual float GetActualForwardVelocity();
  /// @brief Retrieve the current velocity of the roboid
  /// @return The velocity in polar coordinates
  /// The actual units of the velocity depend on the implementation
  virtual Polar GetActualVelocity();
  /// @brief Retrieve the current angular velocity of the roboid
  /// @return The angular velocity
  /// The actual unit of the angular velocity depend on the implementation
  virtual float GetActualYawVelocity();

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

protected:
  /// @brief The number of motors used for Propulsion
  unsigned int motorCount = 0;
  /// @brief The Placement of the motors used for Propulsion
  // Placement *placement = nullptr;
  Motor **motors = nullptr;

  Vector3 targetVelocity;
  float targetYawVelocity;
};

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