#pragma once
#include <Arduino.h>
#include "RangedMotor.hpp"
#include "AdafruitPWMBoard.hpp"

class AdafruitServoMotor : public RangedMotor
{
    public:
        AdafruitServoMotor(
            AdafruitPWMBoard& driver,
            const uint8_t motorIndex,
            const float gearboxRatio,
            const float scaleCalibration,
            const float zeroOffset,
            const float minSafeAngle,
            const float maxSafeAngle,
            const float specRangeDeg, // 180 for a 180-deg-servo, 270 for a 270-deg-servo, etc ...
            const float initialPosition = 0,
            const uint16_t minPulseLength = 125,
            const uint16_t maxPulseLength = 625
        ) noexcept;
        AdafruitServoMotor(const AdafruitServoMotor& other) = delete;
        AdafruitServoMotor(AdafruitServoMotor&& other) noexcept = default;

        // Returns true if the motion is accepted, false if it is outside of safe bounds.
        // If out of safe bounds, maximum or minimun safe value will be assumen
        bool moveTo(const float target) noexcept override;
        float getPosition() const noexcept override { return hardwareAngleToVirtualAngle(currentHardwarePosition); };

        // Will apply the current position immediately, no matter how small the move might be.
        void commitMoveImmediately() noexcept;

    protected:
        uint16_t hardwareAngleToPulse(const float hardwareAngle) const noexcept;
        AdafruitPWMBoard& driver;
        const uint8_t motorIndex;
        float currentHardwarePosition = 0;
        float lastCommittedHardwarePosition = 0;
        float specRangeDeg;
        const uint16_t minPulseLength;
        const uint16_t maxPulseLength;
};