???

#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;

competition Competition;

//define global variables
int axis3Drive = 0;
int axis1Drive = 0;
float coef = 1.28;

// 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);
distance Distance1 = distance(PORT2);
motor leftMotorA = motor(PORT5, ratio6_1, false);
motor leftMotorB = motor(PORT7, ratio6_1, false);
motor_group LeftDriveSmart = motor_group(leftMotorA, leftMotorB);
motor rightMotorA = motor(PORT9, ratio6_1, true);
motor rightMotorB = motor(PORT4, ratio6_1, true);
motor_group RightDriveSmart = motor_group(rightMotorA, rightMotorB);
drivetrain Drivetrain = drivetrain(LeftDriveSmart, RightDriveSmart, 219.44, 295, 40, mm, 1);

motor cataMotorA = motor(PORT14, ratio36_1, true);
motor cataMotorB = motor(PORT10, ratio36_1, false);
motor_group cata = motor_group(cataMotorA, cataMotorB);

motor intake = motor(PORT11, ratio36_1, true);

digital_out DigitalOutB = digital_out(Brain.ThreeWirePort.B);
digital_out DigitalOutH = digital_out(Brain.ThreeWirePort.H);

digital_in DigitalInD = digital_in(Brain.ThreeWirePort.D);

// define variable for remote controller enable/disable
bool RemoteControlCodeEnabled = true;
// define variables used for controlling motors based on controller inputs
bool DrivetrainLNeedsToBeStopped_Controller1 = true;
bool DrivetrainRNeedsToBeStopped_Controller1 = true;

int axis1Position = 0;
int axis3Posititon = 0;

int axis3calc() {
    while(true){
    //get controller info and prep for processing

    //initalize variables first
    double y = 0;

    int x = fabs(axis3Position * 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(Controller1.Axis3.position() < 0){
        y = -y;
    }

    axis3Drive = -y;
    wait(2.5, msec);
    }
    return 0;
}
int axis1calc(){
    while(true){
    //get controller info and prep for processing

    //initalize variables first
    double y = 0;

    int x = fabs(axis3Position * 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(Controller1.Axis1.position() < 0){
        y = -y;
    }
    axis1Drive = y;
    wait(2.5, msec);
    }
    return 0;
}

// define a task that will handle monitoring inputs from Controller1
int rc_auto_loop_function_Controller1() {
  // process the controller input every 20 milliseconds
  // update the motors based on the input values
  while(true) {

    axis3Position = Controller1.Axis3.position();
    axis1Position = Controller1.Axis1.position();

    vex::task ws4(axis1calc);

    vex::task ws3(axis3calc);

    if(RemoteControlCodeEnabled) {
      int drivetrainLeftSideSpeed = axis3Drive - axis1Drive;
      int drivetrainRightSideSpeed = axis3Drive + axis1Drive;

      // check if the value is inside of the deadband range
      if (drivetrainLeftSideSpeed < 5 && drivetrainLeftSideSpeed > -5) {
        // check if the left motor has already been stopped
        if (DrivetrainLNeedsToBeStopped_Controller1) {
          // stop the left drive motor
          LeftDriveSmart.stop();
          // tell the code that the left motor has been stopped
          DrivetrainLNeedsToBeStopped_Controller1 = false;
        }
      } else {
        // reset the toggle so that the deadband code knows to stop the left motor nexttime the input is in the deadband range
        DrivetrainLNeedsToBeStopped_Controller1 = true;
      }
      // check if the value is inside of the deadband range
      if (drivetrainRightSideSpeed < 5 && drivetrainRightSideSpeed > -5) {
        // check if the right motor has already been stopped
        if (DrivetrainRNeedsToBeStopped_Controller1) {
          // stop the right drive motor
          RightDriveSmart.stop();
          // tell the code that the right motor has been stopped
          DrivetrainRNeedsToBeStopped_Controller1 = false;
        }
      } else {
        // reset the toggle so that the deadband code knows to stop the right motor next time the input is in the deadband range
        DrivetrainRNeedsToBeStopped_Controller1 = true;
      }

      // only tell the left drive motor to spin if the values are not in the deadband range
      if (DrivetrainLNeedsToBeStopped_Controller1) {
        LeftDriveSmart.setVelocity(drivetrainLeftSideSpeed, percent);
        LeftDriveSmart.spin(forward);
      }
      // only tell the right drive motor to spin if the values are not in the deadband range
      if (DrivetrainRNeedsToBeStopped_Controller1) {
        RightDriveSmart.setVelocity(drivetrainRightSideSpeed, percent);
        RightDriveSmart.spin(forward);
      }
    }
    // wait before repeating the process
    wait(5, msec);
  }
  return 0;
}

task rc_auto_loop_task_Controller1(rc_auto_loop_function_Controller1);

#pragma endregion VEXcode Generated Robot Configuration
bool intakeInterlock = false;
bool cataInterlock = false;
int switchpoint = 0;


int Interlock(){
    while(true){
        if(DigitalInD.value() == 0){
            intakeInterlock = true;
        } else {
            intakeInterlock = false;
        }
        if(DigitalOutB.value() == 1){
            cataInterlock = false;
        } else {
            cataInterlock = true;
        }
    wait(5, msec);
    }
    return 0;
}

int Intake(){
    bool intakeToggle = false;
      while(true){
        intake.setMaxTorque(100, percent);
        intake.spin(forward);
        if(Controller1.ButtonR1.pressing()){
            intakeToggle = !intakeToggle;
            wait(250, msec);
        }
        if(Distance1.objectDistance(inches) < 4.0){
        intake.setStopping(hold);
          intake.stop();
          intake.setVelocity(0, percent);
        } else {
            intake.setVelocity(60, percent);
        }
    wait(5, msec);
    }
    return 0;
}

int cataFire(){
  cata.setMaxTorque(100, percent);
  cata.setVelocity(100, percent);
  cata.setStopping(hold);
  float i = 0;
  while(true){
    i = cata.position(degrees) - (floor(cata.position(turns)) * 360);
    if(cataInterlock == false && !Controller1.ButtonR1.pressing()){
      if(Distance1.objectDistance(inches) < 3.0 || Controller1.ButtonA.pressing()){
      intake.setVelocity(0, percent);
      cata.spin(forward);
      wait(250, msec);
      while(DigitalInD.value() == 1){
        cata.spin(forward);
      }
      cata.stop();
      } else {
      cata.stop();
      intake.setVelocity(60, percent);
    }
   }
    wait(1, msec);
  }
  return 0;
}

int wings(){
  while(true){
    if(Controller1.ButtonL1.pressing()){
      DigitalOutH.set(true);
    }else{
      DigitalOutH.set(false);
    }
    wait(5, msec);
    }
  return 0;
}

int encoder(){
  int counter = 0;
  int tempA = 0;
  int tempB = 0;
  int tempL1 = 0;
  int tempR1 = 0;
  int tempU = 0;
  int tempD = 0;
  int tempR = 0;
  int tempL = 0;
  int temp3 = 0;
  int temp1 = 0;
  while(true){
    if(Controller1.ButtonA.pressing()){
        tempA = 1;
    } else{
        tempA = 0;
    }
    if(Controller1.ButtonB.pressing()){
        tempB = 1;
    } else{
        tempB = 0;
    }
    if(Controller1.ButtonL1.pressing()){
        tempL1 = 1;
    } else{
        tempL1 = 0;
    }
    if(Controller1.ButtonR1.pressing()){
        tempR1 = 1;
    } else{
        tempR1 = 0;
    }
    if(Controller1.ButtonUp.pressing()){
        tempU = 1;
    } else{
        tempU = 0;
    }
    if(Controller1.ButtonDown.pressing()){
        tempD = 1;
    } else{
        tempD = 0;
    }
    if(Controller1.ButtonRight.pressing()){
        tempR = 1;
    } else{
        tempR = 0;
    }
    if(Controller1.ButtonLeft.pressing()){
        tempL = 1;
    } else{
        tempL = 0;
    }
    temp3 = Controller1.Axis3.position();
    temp1 = Controller1.Axis1.position();
    printf("%d,\t%d,\t%d,\t%d,\t%d,\t%d,\t%d,\t%d,\t%d,\t%d,\n",tempA,tempB,tempL1,tempR1,tempU,tempD,tempR,tempL,temp3,temp1);
    wait(5, msec);
  }
}

int onauton_autonomous(){
//array setup
float values[] = {};

//the y-int for the algebra
int mathA = -10;
int mathB = -9;
int mathL1 = -8;
int mathR1 = -7;
int mathU = -6;
int mathD = -5;
int mathR = -4;
int mathL = -3;
int math3 = -2;
int math1 = -1;

int outA = 0;
int outB = 0;
int outL1 = 0;
int outR1 = 0;
int outU = 0;
int outD = 0;
int outR = 0;
int outL = 0;
int out3 = 0;
int out1 = 0;

bool buttonA = false;
bool buttonB = false;
bool buttonL1 = false;
bool buttonR1 = false;
bool buttonUp = false;
bool buttonDown = false;
bool buttonRight = false;
bool buttonLeft = false;

  while (true) {
    //regular code goes here
    //make sure to replace Controller1.ButtonA.pressing() with “buttonA”…

    //math at the start before reading the values
    mathA = mathA + 10;
    mathB = mathB + 10;
    mathL1 = mathL1 + 10;
    mathR1 = mathR1 + 10;
    mathU = mathU + 10;
    mathD = mathD + 10;
    mathR = mathR + 10;
    mathL = mathL + 10;
    math3 = math3 + 10;
    math1 = math1 + 10;

    //read the values off of the array and set the values accordingly
    outA = values[mathA];
    outB = values[mathB];
    outL1 = values[mathL1];
    outR1 = values[mathR1];
    outU =  values[mathU];
    outD = values[mathD];
    outR = values[mathR];
    outL = values[mathL];
    out3 = values[math3];
    out1 = values[math1];

    //do stuff on the controller
    if(outA == 1){
        buttonA = true;
    } else {
        buttonA = false;
    }
    if(outB == 1){
        buttonB = true;
    } else {
        buttonB = false;
    }
    if(outL1 == 1){
        buttonL1 = true;
    } else {
        buttonL1 = false;
    }
    if(outR1 == 1){
        buttonR1 = true;
    } else {
        buttonR1 = false;
    }
    if(outU == 1){
        buttonUp = true;
    } else {
        buttonUp = false;
    }
    if(outD == 1){
        buttonDown = true;
    } else {
        buttonDown = false;
    }
    if(outL == 1){
        buttonLeft = true;
    } else {
        buttonLeft = false;
    }
    int axis3 = out3;
    int axis1 = out1;

    axis1Position = axis1;
    axis3Position = axis3;
    vex::task ws3;
    vex::task
    RightDriveSmart.setVelocity(axis3Drive - axis1Drive, percent);
    LeftDriveSmart.setVelocity(axis3Drive + axis1Drive, percent);
    RightDriveSmart.spin(forward);
    LeftDriveSmart.spin(forward);

  axis3 = out3;
  axis1 = out1;
    cata.setStopping(hold);
    cata.stop();
 if(wingState == true){
    set.DigitalOutD(true);
 }

    wait(5, msec);
  }

  return 0;
}
}

void whenStarted(){
    DigitalOutB.set(true);
  cata.setVelocity(40, percent);
    while(DigitalInD.value() == 1){
      cata.spin(forward);
    }
    cata.setStopping(hold);
    cata.stop();
    //cata.setVelocity(30, percent);
    //cata.setMaxTorque(100, percent);
    //while(DigitalInD.value() == 1){
        //cata.spin(forward);
    //}
    //cata.setVelocity(0, percent);
    //cata.setStopping(hold);
    //cata.stop();
}

int main() {
  // post event registration

  // set default print color to black
  printf("\033[30m");

  // wait for rotation sensor to fully initialize
  wait(30, msec);

  //run the important things
  whenStarted();
  vex::task ws5(Interlock);
  vex::task ws6(wings);
  vex::task ws1(Intake);
  vex::task ws2(cataFire);
  vex::task ws7(encoder);
}