#pragma once

#include "ControlledMotor.h"

namespace Passer {
namespace RoboidContol {

class ServoMotor : public Thing {
public:
  ServoMotor();

  float minAngle = -90.0F;
  float maxAngle = 90.0F;

  enum ControlMode { Position, Velocity };
  ControlMode controlMode = ControlMode::Position;

  virtual void SetTargetAngle(float angle);
  virtual float GetTargetAngle();

  virtual void SetMaximumVelocity(float maxVelocity);

  virtual void SetTargetVelocity(float velocity);
  virtual float GetTargetVelocity();

  virtual void Update(float currentTimeMs);

protected:
  bool hasTargetAngle = false;
  float targetAngle = 0.0F;

  float maxVelocity = 0.0F;

  float targetVelocity = 0.0F;
  float limitedTargetAngle = 0.0F;

  float lastUpdateTimeMs = 0.0F;

  virtual void SetAngle(float angle) = 0;
};

} // namespace RoboidContol
} // namespace Passer
using namespace Passer::RoboidContol;