#pragma once

#include "Encoder.h"
#include "Motor.h"

/// @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 Thing {
 public:
  ControlledMotor();
  ControlledMotor(Motor* motor, Encoder* encoder);

  float velocity;

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

  void Update(float timeStep);

  /// @brief Set the target speed for the motor controller
  /// @param speed the target in revolutions per second.
  void SetTargetSpeed(float speed);

  /// @brief Get the actual speed from the encoder
  /// @return The speed in revolutions per second
  float GetActualSpeed() {
    return (int)rotationDirection * encoder->GetRevolutionsPerSecond();
  }

  bool Drive(float distance) {
    if (!driving) {
      targetDistance = distance;
      encoder->StartCountingRevolutions();
      driving = true;
    } else
      targetDistance -= encoder->RestartCountingRevolutions();

    return (targetDistance <= 0);
  }

 protected:
  float targetVelocity;
  Motor* motor;
  Encoder* encoder;
  enum Direction { Forward = 1, Reverse = -1 };

  Direction rotationDirection;

  bool driving = false;
  float targetDistance = 0;
  float lastEncoderPosition = 0;
};