#pragma once

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

namespace Passer {
namespace RoboidContol {

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

  Vector3 rotationAxis = Vector3::up;
  Angle minAngle = Angle::Degrees(-90);
  Angle maxAngle = Angle::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) 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
}  // namespace Passer
using namespace Passer::RoboidContol;