#pragma once

#include "Motor.h"
#include "RelativeEncoder.h"

namespace RoboidControl {

/// @brief A motor with speed control
/// It uses a feedback loop from an encoder to regulate the speed
/// The speed is measured in revolutions per second.
class ControlledMotor : public Motor {
 public:
  ControlledMotor(Motor* motor, RelativeEncoder* encoder, Thing* parent = Thing::LocalRoot());

  float pidP = 1;
  float pidD = 0;
  float pidI = 0;

  /// @brief The actual velocity in revolutions per second
  float actualVelocity;

  enum Direction { Forward = 1, Reverse = -1 };
  Direction rotationDirection;

  virtual void Update(bool recurse = false) override;

  /// @brief Set the target verlocity for the motor controller
  /// @param speed the target velocity in revolutions per second
  virtual void SetTargetVelocity(float velocity) override;

  Motor* motor;
  RelativeEncoder* encoder;

 protected:
  float lastUpdateTime;
};

}  // namespace RoboidControl