#pragma once

#include "Motor.h"
#include "Placement.h"
#include "Vector2.h"

namespace Passer {
namespace RoboidControl {

/// @brief The Propulsion module for a Roboid is used to move the Roboid in
/// space
///
/// 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
  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);

  // Velocity control

  /// @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);

 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;
};

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