#include "RangedMotor.hpp"
#include "StateMachine.hpp"
#include "defs.hpp"

RangedMotor::RangedMotor(
    const float gearboxRatio,
    const float scaleCalibration,
    const float zeroOffset,
    const float minSafeAngle,
    const float maxSafeAngle
) noexcept
{
    // Using setters for validation
    setGearboxRatio(gearboxRatio);
    setScaleCalibration(scaleCalibration);
    setZeroOffset(zeroOffset);
    setSafeMinAngle(minSafeAngle);
    setSafeMaxAngle(maxSafeAngle);
}

// Getters
float RangedMotor::getGearboxRatio() const noexcept
{
    return gearboxRatio;
}

float RangedMotor::getScaleCalibration() const noexcept
{
    return scaleCalibration;
}

float RangedMotor::getZeroOffset() const noexcept
{
    return zeroOffset;
}

float RangedMotor::getSafeMinAngle() const noexcept
{
    return hardwareAngleToVirtualAngle(safeHardwareMinAngle);
}

float RangedMotor::getSafeMaxAngle() const noexcept
{
    return hardwareAngleToVirtualAngle(safeHardwareMaxAngle);
}

float RangedMotor::getSafeHardwareMinAngle() const noexcept
{
    return safeHardwareMinAngle;
}

float RangedMotor::getSafeHardwareMaxAngle() const noexcept
{
    return safeHardwareMaxAngle;
}

// Setters
void RangedMotor::setGearboxRatio(float gearboxRatio) noexcept
{
    if (abs(gearboxRatio) < COMP_EPSILON) {
        StateMachine::getInstance().raiseError("Zero-gearbox-ratio was set on ranged motor!");
        return;
    }
    this->gearboxRatio = gearboxRatio;
}

void RangedMotor::setScaleCalibration(float scaleCalibration) noexcept
{
    if (abs(gearboxRatio) < COMP_EPSILON) {
        StateMachine::getInstance().raiseError("Zero-scale-calibration was set on ranged motor!");
        return;
    }
    this->scaleCalibration = scaleCalibration;
}

void RangedMotor::setZeroOffset(float zeroOffset) noexcept
{
    this->zeroOffset = zeroOffset;
}

void RangedMotor::setSafeMinAngle(float safeMinAngle) noexcept
{
    safeHardwareMinAngle = virtualAngleToHardwareAngle(safeMinAngle);
}

void RangedMotor::setSafeMaxAngle(float safeMaxAngle) noexcept
{
    safeHardwareMaxAngle = virtualAngleToHardwareAngle(safeMaxAngle);
}

void RangedMotor::setSafeHardwareMinAngle(float safeHardwareMinAngle) noexcept
{
    this->safeHardwareMinAngle = safeHardwareMinAngle;
}

void RangedMotor::setSafeHardwareMaxAngle(float safeHardwareMaxAngle) noexcept
{
    this->safeHardwareMaxAngle = safeHardwareMaxAngle;
}

float RangedMotor::virtualAngleToHardwareAngle(float virtualAngle) const noexcept
{
    // Apply scaling correction
    virtualAngle *= scaleCalibration;
    
    // Apply offset
    virtualAngle += zeroOffset;
    
    // Apply gearbox reduction
    virtualAngle *= gearboxRatio;
    
    // Return the now no-longer virtual angle
    return virtualAngle;
}

float RangedMotor::hardwareAngleToVirtualAngle(float hardwareAngle) const noexcept
{
    // Undo gearbox ration
    hardwareAngle /= gearboxRatio;

    // Undo offset
    hardwareAngle -= zeroOffset;

    // Undo scaling correction
    hardwareAngle /= scaleCalibration;

    return hardwareAngle;
}

float RangedMotor::calculateScaleCalibration(const float virtualRange, const float hardwareRange)
{
    return virtualRange / hardwareRange;
}