#pragma once

#include "ControlledMotor.h"
#include "LinearAlgebra/Angle.h"

namespace RoboidControl {

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

  Direction16 rotationAxis = Direction16::up;
  Angle16 minAngle = Angle16::Degrees(-90);
  Angle16 maxAngle = Angle16::Degrees(90);

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

  Thing* target = nullptr;

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

  virtual void SetMaximumVelocity(float maxVelocity);

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

  virtual void Update(unsigned long currentTimeMs, bool recurse) override;

  Angle16 limitedTargetAngle = Angle16();

 protected:
  bool hasTargetAngle = false;
  Angle16 targetAngle = Angle16();
  Angle16 actualAngle = Angle16();

  float maxSpeed = 0.0F;

  float targetVelocity = 0.0F;

  float lastUpdateTimeMs = 0.0F;

  virtual void SetAngle(Angle16 angle);
};

}  // namespace RoboidContol