#pragma once

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

namespace Passer {
namespace RoboidContol {

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

  Vector3 rotationAxis = Vector3::up;
  Angle minAngle = -90.0F;
  Angle maxAngle = 90.0F;

  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 = 0.0F;

 protected:
  bool hasTargetAngle = false;
  Angle16 targetAngle = 0.0F;
  Angle16 actualAngle = 0.0F;

  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;