#include "main.h"
#include "EZ-Template/rotation_mechanisms/rotation_calc.hpp"

using namespace ez;

ArmMech armObjRange;

LiftMech::LiftMech() {
    liftHeight = 0;
    liftError = 0;
    liftVelocity = 0;

    liftStates = {0, 0, 0, 0};
}
LiftMech::LiftMech(std::initializer_list<int> _liftStates, int _numLiftStates) {
    liftStates = _liftStates;
    numLiftStates = _numLiftStates;
    currLiftState = 0;
    liftTarget = 0;
    set_lift_kp(0.0173f);
}

void LiftMech::lift_driver_control(ControllerR currController, ButtonsR buttonUsed) {
    switch (currController)
    {
    case ControllerR::master:

        switch (buttonUsed)
        {
        case ButtonsR::arrow_buttons:
            if (master.get_digital(DIGITAL_RIGHT)) {
                lift.move(127);
            }
            else if (master.get_digital(DIGITAL_DOWN)) {
                lift.move(-127);
            }
            break;
        case ButtonsR::Y_B_buttons:
            if (master.get_digital(DIGITAL_Y)) {
                lift.move(127);
            }
            else if (master.get_digital(DIGITAL_B)) {
                lift.move(-127);
            }
            break;
        case ButtonsR::left_shoulder_buttons:
            if (master.get_digital(DIGITAL_L1)) {
                lift.move(127);
            }
            else if (master.get_digital(DIGITAL_L2)) {
                lift.move(-127);
            }
            break;
        case ButtonsR::right_shoulder_buttons:
            if (master.get_digital(DIGITAL_R1)) {
                lift.move(127);
            }
            else if (master.get_digital(DIGITAL_R2)) {
                lift.move(-127);
            }
            break;
        default:
            break;
        }
        break;
    case ControllerR::slave:

        switch (buttonUsed)
        {
        case ButtonsR::arrow_buttons:
            if (slave.get_digital(DIGITAL_RIGHT)) {
                cycle_states();
            }
            else if (slave.get_digital(DIGITAL_DOWN)) {
                reset_state();
            }
            break;
        case ButtonsR::Y_B_buttons:
            if (slave.get_digital(DIGITAL_Y)) {
                cycle_states();
            }
            else if (slave.get_digital(DIGITAL_B)) {
                reset_state();
            }
            break;
        case ButtonsR::left_shoulder_buttons:
            if (slave.get_digital(DIGITAL_L1)) {
                cycle_states();
            }
            else if (slave.get_digital(DIGITAL_L2)) {
                reset_state();
            }
            break;
        case ButtonsR::right_shoulder_buttons:
            if (slave.get_digital(DIGITAL_R1)) {
                cycle_states();
            }
            else if (slave.get_digital(DIGITAL_R2)) {
                reset_state();
            }
            break;
        default:
            break;
        }
        break;
    default:
        break;
    }
}

void LiftMech::lift_auton_control(int pickState) {
    switch (pickState)
    {
    case 1:
        liftTarget = liftStates[0];
        break;
    case 2:
        liftTarget = liftStates[1];
        break;
    case 3:
        liftTarget = liftStates[2];
        break;
    case 4:
        liftTarget = liftStates[3];
        break;
    default:
        liftTarget = liftStates[0];
        break;
    }
}

void LiftMech::cycle_states() {
    currLiftState ++;

    if (!liftStates.empty()) {
        if (currLiftState == numLiftStates) {
            currLiftState = 0;
        }
        liftTarget = liftStates[currLiftState];
    }
}

void LiftMech::reset_state() {
    currLiftState = 0;

    if (!liftStates.empty()) {
        liftTarget = liftStates[currLiftState];
    }
}

void LiftMech::lift_control() {
    liftError = liftTarget - liftRotation.get_position();
    liftVelocity = get_lift_kp() * liftError;
    lift.move(liftVelocity);
}

int LiftMech::lift_range_finder() {
    liftHeight = liftRotation.get_position();

    if (firstRange.size() >= 2 && liftHeight >= firstRange[0] && liftHeight <= firstRange[1]) {
        return armObjRange.targetPos = 0;
    }
    else if (secondRange.size() >= 2 && liftHeight >= secondRange[0] && liftHeight <= secondRange[1]) {
        return armObjRange.targetPos = 1;
    }
    else if (thirdRange.size() >= 2 && liftHeight >= thirdRange[0] && liftHeight <= thirdRange[1]) {
        return armObjRange.targetPos = 2;
    }
    else if (fourthRange.size() >= 2 && liftHeight >= fourthRange[0] && liftHeight <= fourthRange[1]) {
        return armObjRange.targetPos = 3;
    }
}

void LiftMech::set_lift_kp(float _liftKp) {
    liftKP = _liftKp;
}

void LiftMech::set_ranges(std::vector<int> _firstRange, std::vector<int> _secondRange, std::vector<int> _thirdRange, std::vector<int> _fourthRange) {
    firstRange = _firstRange;
    secondRange = _secondRange;
    thirdRange = _thirdRange;
    fourthRange = _fourthRange;
}

int LiftMech::get_lift_state() {
    if (!liftStates.empty()) {
        return liftStates[currLiftState];
    }
}
int LiftMech::get_num_lift_states() {return numLiftStates;}
float LiftMech::get_lift_kp() {return liftKP;}