#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);

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

  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;

  /// @brief Get the actual velocity from the encoder
  /// @return The velocity in revolutions per second
  //   float GetActualVelocity();

  //   bool Drive(float distance);

  Motor* motor;
  RelativeEncoder* encoder;

 protected:
  float lastUpdateTime;


  //float targetVelocity;

//   float netDistance = 0;
//   float startDistance = 0;
//   bool driving = false;
//   float targetDistance = 0;
//   float lastEncoderPosition = 0;
};

}  // namespace RoboidControl