#include "AdafruitServoMotor.hpp"
#include "StateMachine.hpp"
#include "defs.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.01f;

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 float initialPosition,
    const uint16_t minPulseLength,
    const uint16_t maxPulseLength
) 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!");
    }

    const float hardwareTarget = virtualAngleToHardwareAngle(target);
    currentHardwarePosition = constrain(hardwareTarget, safeHardwareMinAngle, safeHardwareMaxAngle);
        
    // Only move the motor if it would turn it at least n degrees 
    if (abs(lastCommittedHardwarePosition - currentHardwarePosition) >= MIN_DELTA_FOR_MOTION_COMMIT) {
        commitMoveImmediately();
    }

    // True if they are the same, e.g. no clamping was applied
    return (abs(currentHardwarePosition - hardwareTarget) < COMP_EPSILON);
}

void AdafruitServoMotor::commitMoveImmediately() noexcept
{
    if (!driver.isGood()) {
        StateMachine::getInstance().raiseError("Servo motor driver board gone offline!");
    }

    driver.get().setPWM(motorIndex, 0, hardwareAngleToPulse(currentHardwarePosition));
    lastCommittedHardwarePosition = currentHardwarePosition;
    return false;
}

uint16_t AdafruitServoMotor::hardwareAngleToPulse(const float hardwareAngle) const noexcept
{
    const float t = hardwareAngle / specRangeDeg;
    const uint16_t pulse = minPulseLength + t * (maxPulseLength - minPulseLength);

    return pulse;
}