#pragma once

#include "Thing.h"

namespace RoboidControl {

/// @brief A thing which can move itself using a differential drive system
///
/// @sa @link https://en.wikipedia.org/wiki/Differential_wheeled_robot @endlink
class DifferentialDrive : public Thing {
 public:
  /// @brief Create a differential drive without networking support
  DifferentialDrive();
  /// @brief Create a differential drive with networking support
  /// @param participant The local participant
  DifferentialDrive(Participant* participant);

  /// @brief Configures the dimensions of the drive
  /// @param wheelDiameter The diameter of the wheels in meters
  /// @param wheelSeparation The distance between the wheels in meters
  ///
  /// These values are used to compute the desired wheel speed from the set
  /// linear and angular velocity.
  /// @sa SetLinearVelocity SetAngularVelocity
  void SetDriveDimensions(float wheelDiameter, float wheelSeparation);
  /// @brief Congures the motors for the wheels
  /// @param leftWheel The motor for the left wheel
  /// @param rightWheel The motor for the right wheel
  void SetMotors(Thing* leftWheel, Thing* rightWheel);

  /// @brief Directly specify the speeds of the motors
  /// @param speedLeft The speed of the left wheel in degrees per second.
  /// Positive moves the robot in the forward direction.
  /// @param speedRight The speed of the right wheel in degrees per second.
  /// Positive moves the robot in the forward direction.

  void SetWheelVelocity(float speedLeft, float speedRight);

  /// @copydoc RoboidControl::Thing::Update(unsigned long)
  virtual void Update(unsigned long currentMs, bool recursive = true) override;

 protected:
  /// @brief The radius of a wheel in meters
  float wheelRadius = 1.0f;
  /// @brief The distance between the wheels in meters
  float wheelSeparation = 1.0f;

  /// @brief Convert revolutions per second to meters per second
  float rpsToMs = 1.0f;

  /// @brief The left wheel
  Thing* leftWheel = nullptr;
  /// @brief The right wheel
  Thing* rightWheel = nullptr;
};

}  // namespace RoboidControl