#pragma region VEXcode Generated Robot Configuration
// Make sure all required headers are included.
#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include <math.h>
#include <string.h>


#include "vex.h"

using namespace vex;

// Brain should be defined by default
brain Brain;


// START V5 MACROS
#define waitUntil(condition)                                                   \
  do {                                                                         \
    wait(5, msec);                                                             \
  } while (!(condition))

#define repeat(iterations)                                                     \
  for (int iterator = 0; iterator < iterations; iterator++)
// END V5 MACROS


// Robot configuration code.
controller Controller1 = controller(primary);
digital_out gear1 = digital_out(Brain.ThreeWirePort.A);
digital_out gear2 = digital_out(Brain.ThreeWirePort.B);
motor rightAMotorA = motor(PORT1, ratio18_1, false);
motor rightAMotorB = motor(PORT4, ratio18_1, true);
motor_group rightA = motor_group(rightAMotorA, rightAMotorB);

motor rightBMotorA = motor(PORT2, ratio18_1, false);
motor rightBMotorB = motor(PORT11, ratio18_1, true);
motor_group rightB = motor_group(rightBMotorA, rightBMotorB);

motor leftAMotorA = motor(PORT10, ratio18_1, false);
motor leftAMotorB = motor(PORT7, ratio18_1, true);
motor_group leftA = motor_group(leftAMotorA, leftAMotorB);

motor leftBMotorA = motor(PORT8, ratio18_1, false);
motor leftBMotorB = motor(PORT12, ratio18_1, true);
motor_group leftB = motor_group(leftBMotorA, leftBMotorB);




// Helper to make playing sounds from the V5 in VEXcode easier and
// keeps the code cleaner by making it clear what is happening.
void playVexcodeSound(const char *soundName) {
  printf("VEXPlaySound:%s\n", soundName);
  wait(5, msec);
}



// define variable for remote controller enable/disable
bool RemoteControlCodeEnabled = true;

#pragma endregion VEXcode Generated Robot Configuration
//Define the many global variables

//variable for setting curve values to desired axis
int SelectorReadOut = 0;

//universal output for the drive function
float driveOutput = 0.0;

//universal values for each drive factor
float axis1Output = 0.0;
float axis3Output = 0.0;

//universal Boolean for locking the catapult
bool cataLock = false;

//coefficient for output rpm
float rpmCoef = 0.0;

//global values for at output rpm for both
float leftRPM = 0.0;
float rightRPM = 0.0;

//torque for both sides
float leftTorque = 0.0;
float rightTorque = 0.0;

//to lock the shifter when the catapult is going to fire
bool CataFiring = false;

//a gear value to make the gears work
int gear = 0;

//Motor Curve function
void motorCurve(){
    //get controller info and prep for processing.

    //initialize variables first
    double y = 0;
    float coef = 1.28;

    int x = fabs(SelectorReadOut * coef);

    switch(x){
        case 0 ... 60:
            y = pow(1.04029, x * 1.284467);
            break;
        case 61 ... 80:
            y = 0.75 * x -25;
            break;
        case 81 ... 108:
            y = 1.0357 * x - 47.85714857;
            break;
        case 109 ... 127:
            y = 63 + pow(1.232099, x - 108);
            break;
        default:
            y = 128;
            break;
    }
    y = y / coef;

    if(SelectorReadOut < 0){
        y = -y;
    }

    driveOutput = y;
    wait(2.5, msec);


}

//When Started function to initialize everything and set us up for the match
void whenStarted(){
    //intakePneumatics.set(true);
    rightA.setMaxTorque(100, percent);
    rightB.setMaxTorque(100, percent);
    leftA.setMaxTorque(100, percent);
    leftB.setMaxTorque(100, percent);

    rightA.setStopping(hold);
    rightB.setStopping(hold);
    leftA.setStopping(hold);
    leftB.setStopping(hold);

}

//Motor Curve thread
int MotorCurve(){
    while(true){
        //get and set the appropriate values for curves function
SelectorReadOut = Controller1.Axis3.position();
motorCurve();
axis3Output = driveOutput;
SelectorReadOut = Controller1.Axis1.position();
motorCurve();
axis1Output = driveOutput;

//set deadband for controller
if(Controller1.Axis3.position() < 5 && Controller1.Axis3.position() > -5){
    axis3Output = 0.0;
}
if(Controller1.Axis1.position() < 5 && Controller1.Axis1.position() > -5){
    axis1Output = 0.0;
}
//wait so that the processor doesn’t get overloaded
wait(1.0, msec);
}
return 0;
}

//DriveTrain thread
int DriveTrain(){
    while(true){
        //correct the way the drive is spinning
        if(gear1.value() == 1){
            axis3Output = 0 - axis3Output;
        }
        //psuedo if statement for drivetrain vs cata

        //sets the drive based on motor curve outputs
        float leftDrive = axis3Output - axis1Output;
        float rightDrive = axis3Output + axis1Output;

        leftA.setVelocity(leftDrive, percent);
        leftB.setVelocity(leftDrive, percent);
        rightA.setVelocity(rightDrive, percent);
        rightB.setVelocity(rightDrive, percent);


        //wait so that the processor doesn’t get overloaded
        wait(2.5, msec);
  }
  return 0;
}

float DriveRPM = 0.0;
float DriveTorque = 0.0;

//a thread to do all the miscellaneous math and statistics
int Stats(){
    while(true){
        if(gear1.value() == 1){
            rpmCoef = 1.0;
}
if(gear2.value() == 1){
    rpmCoef = 3.0;
}
if(gear2.value() == 0 && gear1.value() == 0){
    rpmCoef = 0.0;
}
leftRPM = fabs(((leftA.velocity(rpm) + leftB.velocity(rpm)) / 2) * rpmCoef);
rightRPM = fabs(((rightA.velocity(rpm) + rightB.velocity(rpm)) / 2) * rpmCoef);
leftTorque = leftA.torque(Nm) + leftB.torque(Nm);
rightTorque = rightA.torque(Nm) + rightB.torque(Nm);
DriveRPM = (fabs(leftRPM) + fabs(rightRPM))/2;
DriveTorque = (fabs(leftTorque) + fabs(rightTorque)) / 2;
}
    return 0;
}

//thread to work the manual shifting

int Shifting(){
    while(true){
        if(Controller1.ButtonR1.pressing()){
            gear++;
            wait(250, msec);
        }
        if(Controller1.ButtonL1.pressing()){
            gear--;
            wait(250, msec);
        }
        switch(gear){
            case -1:
                gear = 0;
                break;
            case 3:
                gear = 2;
                break;
            case 2:
                gear1.set(false);
                gear2.set(true);
                break;
            case 1:
                gear1.set(true);
                gear2.set(false);
                break;
            default:
                gear1.set(false);
                gear2.set(false);
                break;

            wait(5.0, msec);
        }
    }
    return 0;
}

//UI thread
int UI(){

    while(true){
        Controller1.Screen.clearScreen();
        Controller1.Screen.setCursor(1, 1);
        Controller1.Screen.print(DriveRPM);
        Controller1.Screen.print("RPM");
        Controller1.Screen.newLine();
        Controller1.Screen.print(DriveTorque);
        Controller1.Screen.print("NM");
        Controller1.Screen.newLine();
        Controller1.Screen.print("M");
        Controller1.Screen.print(gear);
        Controller1.Screen.setCursor(3, 12);
        Controller1.Screen.print(Brain.Battery.capacity(percent));
        Controller1.Screen.print("%");
        if(DriveRPM > 192 && DriveRPM < 205){
          Controller1.rumble(rumbleShort);
        }
        wait(10, msec);
    }
    return 0;
}

//call all functions
int main(){
    //initialize the print color for the console
    printf("\033[30m");

    //call threads
    whenStarted();

    vex::task ws1(MotorCurve);
    vex::task ws2(DriveTrain);
    vex::task ws5(Stats);
    vex::task ws6(Shifting);
    vex::task ws8(UI);
}