#pragma once

#include <Encoder.h>
#include <Motor.h>

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

  void SetTargetVelocity(float rotationsPerSecond);
  float GetActualVelocity() {
    return rotationDirection * encoder->GetRotationsPerSecond();
  }  // in rotations per second

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

  Direction rotationDirection;
};