#pragma once

#include "Propulsion.h"

namespace Passer {
namespace RoboidControl {

/// @brief A two-wheeled Propulsion method
///
/// The wheels are put at either side of the roboid with the following behaviour
/// * When both wheels spin forward, the Roboid moves forward
/// * When both wheels spin backward, the Roboid moves backward
/// * When both wheels are spinning in opposite directions, the Roboid rotates
/// wihout moving forward or backward
/// * When just one wheel is spinning, the Roboid turnes while moving forward or
/// backward.
class DifferentialDrive : public Propulsion {
public:
  /// @brief Default constructor
  DifferentialDrive();
  /// @brief Setup of the DifferentialDrive with the Placement of the motors
  /// @param leftMotorPlacement Placement of the left Motor
  /// @param rightMotorPlacement Placement of the right Motor
  /// In this setup, the left motor Direction will be CounterClockWise when
  /// driving forward, while the right motor will turn Clockwise.
  /// @note When not using controlled motors, the placement of the motors is
  /// irrelevant.
  // DifferentialDrive(Placement leftMotorPlacement,
  //                   Placement rightMotorPlacement);

  DifferentialDrive(Motor *leftMotor, Motor *rightMotor);

  void SetDimensions(float wheelDiameter, float wheelSeparation);

  /// @brief Set the target speeds of the motors directly
  /// @param leftSpeed The target speed of the left Motor
  /// @param rightSpeed The target speed of the right Motor
  void SetMotorTargetSpeeds(float leftSpeed, float rightSpeed);

  /// @brief Controls the motors through forward and rotation speeds
  /// @param forward The target forward speed of the Roboid
  /// @param yaw The target rotation speed of the Roboid
  virtual void SetTwistSpeed(float forward, float yaw) override;
  /// @brief Controls the motors through forward and rotation speeds
  /// @param linear The target linear speed of the Roboid
  /// @param yaw The target rotation speed of the Roboid
  /// @note As a DifferentialDrive cannot move sideward, this function has the
  /// same effect as using the void SetTwistSpeed(float forward, float yaw)
  /// function.
  virtual void SetTwistSpeed(Vector2 linear, float yaw = 0.0F);
  /// @brief Controls the motors through forward and rotation speeds
  /// @param linear The target linear speed
  /// @param yaw The target rotation speed around the vertical axis
  /// @param pitch Pitch is not supported and is ignored
  /// @param roll Roll is not supported and is ignores
  /// @note As a DifferentialDrive cannot move sideward or vertical, this
  /// function has the same effect as using the void SetTwistSpeed(float
  /// forward, float yaw) function.
  virtual void SetTwistSpeed(Vector3 linear, float yaw = 0.0F,
                             float pitch = 0.0F, float roll = 0.0F);

  // virtual void SetVelocity(Polar velocity);

  /// @brief Calculate the linear velocity of the roboid based on the wheel
  /// velocities
  /// @return The velocity of the roboid in local space
  /// @details The actual values may not be accurate, depending on the available
  /// information
  /// @remark This will be more expanded/detailed in a future version of Roboid
  /// Control
  // virtual Spherical16 GetVelocity() override;
  /// @brief Calculate the angular velocity of the roboid based on the wheel
  /// velocities
  /// @return The angular speed of the roboid in local space
  /// @details The actual value may not be accurate, depending on the available
  /// information
  /// @remark This will be more expanded/detailed in a future version of Roboid
  /// Control
  // virtual float GetAngularVelocity() override;

protected:
  float wheelDiameter = 1.0F;   // in meters
  float wheelSeparation = 1.0F; // in meters;

  float rpsToMs = 1.0F; // convert revolutions per second to meters per second
};

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