#include "BoomControl.h"

#define COIL_MIN_CURRENT 300
#define COIL_MAX_CURRENT 700

#define MINIMUM_JOYSTICK_VOLTAGE 500
#define MAXIMUM_JOYSTICK_VOLTAGE 4500
#define JOYSTICK_DEADBAND 20

int OutputScaling(int scaledInput);
int InputScaling(unsigned int axisVoltage);

int OutputScaling(int scaledInput)
{
    int input = scaledInput;
    if (input < -100) {
        input = -100;
    }
    if (input > 100) {
        input = 100;
    }
    return input * (COIL_MAX_CURRENT - COIL_MIN_CURRENT) / 100 + COIL_MIN_CURRENT;
}

int InputScaling(unsigned int axisVoltage)
{
    return ((int)axisVoltage - MINIMUM_JOYSTICK_VOLTAGE - JOYSTICK_DEADBAND / 2) * 200 /
               (MAXIMUM_JOYSTICK_VOLTAGE - MINIMUM_JOYSTICK_VOLTAGE - JOYSTICK_DEADBAND) -
           100;
}

void BoomControl(unsigned int joystickInput, HydraulicValveOutput_ts *output)
{
    int scaledInput = InputScaling(joystickInput);
    if (scaledInput > 0)
    {
        output->coilUp = OutputScaling(scaledInput);
        output->coilDown = 0;
    }
    else if (scaledInput < 0)
    {
        output->coilUp = 0;
        output->coilDown = OutputScaling(-scaledInput);
    }
    else
    {
        output->coilUp = 0;
        output->coilDown = 0;
    }
}
