#pragma once

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

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

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

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

  void Update(float currentTimeMs);

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

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

  bool Drive(float distance);

  Motor *motor;
  Encoder *encoder;

protected:
  float lastUpdateTime;
  float targetVelocity;
  float actualVelocity;
  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
} // namespace Passer
using namespace Passer::RoboidControl;