#pragma once

#include "IncrementalEncoder.h"
#include "Motor.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();
  ControlledMotor(Motor* motor, IncrementalEncoder* encoder);

  inline static bool CheckType(Thing* thing) {
    return (thing->type & (int)Thing::Type::ControlledMotor) != 0;
  }
  float velocity;

  float pidP = 0.1F;
  float pidD = 0.0F;
  float pidI = 0.0F;

  void Update(unsigned long currentTimeMs) override;

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

  /// @brief Get the actual speed from the encoder
  /// @return The speed in revolutions per second
  virtual float GetActualSpeed() override;

  bool Drive(float distance);

  Motor* motor;
  IncrementalEncoder* encoder;

 protected:
  float lastUpdateTime = 0;
  float lastError = 0;
  // float targetSpeed;
  float actualSpeed;
  float netDistance = 0;
  float startDistance = 0;

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

  // Direction rotationDirection;

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

}  // namespace RoboidControl