#include "AdafruitServoMotor.hpp"
#include "StateMachine.hpp"

// Motor hardware angle must be at least changed by that much before
// the motion is commited to the motor (prevent motor rattle)
constexpr float MIN_DELTA_FOR_MOTION_COMMIT = 0.02f;

AdafruitServoMotor::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,
    const uint16_t minPulseLength,
    const uint16_t maxPulseLength,
    const float initialPosition
) noexcept:
    driver(driver),
    motorIndex(motorIndex),
    RangedMotor(
        gearboxRatio,
        scaleCalibration,
        zeroOffset,
        minSafeAngle,
        maxSafeAngle
    ),
    currentHardwarePosition      (virtualAngleToHardwareAngle(initialPosition)),
    lastCommittedHardwarePosition(virtualAngleToHardwareAngle(initialPosition)),
    specRangeDeg(specRangeDeg),
    minPulseLength(minPulseLength),
    maxPulseLength(maxPulseLength)
{
    if (motorIndex >= 16) {
        StateMachine::getInstance().raiseError("Servo motor index out of range for adafruit pwm board!");
    }

    if (!driver.isGood()) {
        StateMachine::getInstance().raiseError("Servo motor received offline driver board!");
    }


    if (!driver.occupyMotorSlot(motorIndex)) {
        StateMachine::getInstance().raiseError("Servo motor index already occupied for adafruit pwm board!");
    }
}

bool AdafruitServoMotor::moveTo(const float target) noexcept
{
    if (!driver.isGood()) {
        StateMachine::getInstance().raiseError("Servo motor driver board gone offline!");
    }

    // delta = abs(old_hardware_pos - new_hardware_pos)
    float delta = currentHardwarePosition;
    currentHardwarePosition = virtualAngleToHardwareAngle(target);
    delta = abs(delta - currentHardwarePosition);

    if (currentHardwarePosition >= safeHardwareMinAngle &&
        currentHardwarePosition <= safeHardwareMaxAngle) {
        
        // Only move the motor if it would turn it at least n degrees 
        if (abs(lastCommittedHardwarePosition - currentHardwarePosition) >= MIN_DELTA_FOR_MOTION_COMMIT) {
            commitMoveImmediately();
        }
        
        return true;
    }
    else {
        return false;
    }
}

void AdafruitServoMotor::commitMoveImmediately() noexcept
{
    driver.get().setPWM(motorIndex, 0, hardwareAngleToPulse(currentHardwarePosition));
    lastCommittedHardwarePosition = currentHardwarePosition;
    return false;
}

uint16_t AdafruitServoMotor::hardwareAngleToPulse(const float hardwareAngle) const noexcept
{
    return map(hardwareAngle, 0, specRangeDeg, minPulseLength, maxPulseLength);
}