/*----------------------------------------------------------------------------*/
/*                                                                            */
/*    Module:       main.cpp                                                  */
/*    Author:       VEX                                                       */
/*    Created:      Thu Sep 26 2019                                           */
/*    Description:  Competition Template                                      */
/*                                                                            */
/*----------------------------------------------------------------------------*/

// ---- START VEXCODE CONFIGURED DEVICES ----
// Robot Configuration:
// [Name]               [Type]        [Port(s)]
// Drivetrain           drivetrain    1, 2
// LiftMotor            motor         18
// LiftMotor2           motor         12
// TiltMotor            motor         3
// Controller1          controller
// IntakeR              motor         17
// IntakeL              motor         8
// ---- END VEXCODE CONFIGURED DEVICES ----

#include "vex.h"
#include <cstdio>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <string>


using namespace vex;
bool speedin = true;
double driveMultiplier = 1.0;

bool apressed = false;
bool bpressed = false;
// A global instance of competition
competition Competition;

motor LeftDriveSmart = motor(PORT1, ratio18_1, false);
motor RightDriveSmart = motor(PORT2, ratio18_1, true);
drivetrain Drivetrain =
    drivetrain(LeftDriveSmart, RightDriveSmart, 319.19, 295, 130, mm, 1);
motor LiftMotor = motor(PORT18, ratio18_1, true);
motor LiftMotor2 = motor(PORT12, ratio18_1, false);
motor TiltMotor = motor(PORT3, ratio18_1, false);
motor TiltMotor2 = motor(PORT10, ratio18_1, true);
controller Controller1 = controller(primary);
motor IntakeR = motor(PORT15, ratio18_1, false);
motor IntakeL = motor(PORT8, ratio18_1, true);

bool Controller1LeftShoulderControlMotorsStopped = true;
bool Controller1RightShoulderControlMotorsStopped = true;
bool Controller1UpDownButtonsControlMotorsStopped = true;
bool DrivetrainLNeedsToBeStopped_Controller1 = true;
bool DrivetrainRNeedsToBeStopped_Controller1 = true;
bool IntakeStop = true;
void controllerBaseInput();
void STOPCODE_DEBUG();
// define your global instances of motors and other devices here

/*---------------------------------------------------------------------------*/
/*                          Pre-Autonomous Functions                         */
/*                                                                           */
/*  You may want to perform some actions before the competition starts.      */
/*  Do them in the following function.  You must return from this function   */
/*  or the autonomous and usercontrol tasks will not be started.  This       */
/*  function is only called once after the V5 has been powered on and        */
/*  not every time that the robot is disabled.                               */
/*---------------------------------------------------------------------------*/

void pre_auton(void) {
  // Initializing Robot Configuration. DO NOT REMOVE!
  vexcodeInit();

  // All activities that occur before the competition starts
  // Example: clearing encoders, setting servo positions, ...
}

/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              Autonomous Task                              */
/*                                                                           */
/*  This task is used to control your robot during the autonomous phase of   */
/*  a VEX Competition.                                                       */
/*                                                                           */
/*  You must modify the code to add your own robot specific commands here.   */
/*---------------------------------------------------------------------------*/
std::ifstream ifs;
void goToPos(motor m, double vel, double pos);
void playback();
int filenum = 1;
std::string instructions;
void autonomous(void) {
  // ..........................................................................
  // Insert autonomous user code here.
  // ..........................................................................
  /*STOPCODE_DEBUG();
  LiftMotor.setVelocity(100, percent);
  LiftMotor2.setVelocity(100, percent);
  TiltMotor.setVelocity(30, percent);
  //Drivetrain.driveFor(6, inches);*/
  LiftMotor.resetRotation();
  LiftMotor2.resetRotation();
  TiltMotor.resetRotation();
  IntakeL.resetRotation();
  IntakeR.resetRotation();
  LeftDriveSmart.resetRotation();
  RightDriveSmart.resetRotation();

  LiftMotor.setStopping(hold);
  LiftMotor2.setStopping(hold);
  // STOPCODE_DEBUG();
  // uint8_t * instructions = nullptr;
  // Brain.SDcard.loadfile("recording.bin", instructions, 2500); // size in
  // bytes
  char filename[1] = "";
  snprintf(filename, 2, "%d", filenum);
  std::string fn = filename;
  fn += ".txt";
  ifs.open(fn, std::ofstream::in);
  Controller1.Screen.print("File opened");
  timer tim;
  tim.clear();
  while (std::getline(ifs, instructions)) {
    while (tim.value() < 0.075) {
      wait(1, msec);
    }
    tim.clear();
    playback();
  }
  ifs.close();
  STOPCODE_DEBUG();
  Drivetrain.setDriveVelocity(40, percent);
  Drivetrain.setTurnVelocity(50, percent);
  // Drivetrain.turnFor(5, degrees);
  Drivetrain.driveFor(15, inches, false);
  wait(2, sec);
  Drivetrain.setDriveVelocity(80, percent);
  Drivetrain.driveFor(reverse, 16, inches);
  wait(1, sec);
  // Drivetrain.turnFor(-45, degrees);
  STOPCODE_DEBUG();
  LiftMotor.startSpinTo(40 * 7, degrees); // unlock everything
  LiftMotor2.startSpinTo(40 * 7, degrees);
  waitUntil(LiftMotor.isDone() && LiftMotor2.isDone());
  TiltMotor.startSpinTo(45 * 7, degrees);
  LiftMotor.startSpinTo(10 * 7, degrees);
  LiftMotor2.startSpinTo(10 * 7, degrees);
  waitUntil(LiftMotor.isDone() && LiftMotor2.isDone() && TiltMotor.isDone());
  IntakeL.setVelocity(100, percent);
  IntakeR.setVelocity(100, percent);
  IntakeL.startSpinTo(-5, rev);
  IntakeR.startSpinTo(-5, rev);
  TiltMotor.startSpinTo(5, degrees);
  waitUntil(TiltMotor.isDone());
  // STOPCODE_DEBUG();
  STOPCODE_DEBUG();
  IntakeL.setVelocity(100, percent);
  IntakeR.setVelocity(100, percent);
  // IntakeL.startSpinTo(5, rev); // grab preload
  // IntakeR.startSpinTo(5, rev);
  Drivetrain.setDriveVelocity(80, percent);
  Drivetrain.setTurnVelocity(100, percent);

  // Drivetrain.driveFor(24 * 3.5, inches); // drive to the row of 4 (if on that
  // tile)
  waitUntil(IntakeL.isDone() && IntakeR.isDone());
  // Drivetrain.turnFor(-90, degrees); // turn LEFT
  IntakeL.spin(forward);
  IntakeR.spin(forward);
  Drivetrain.setDriveVelocity(50, percent);
  Drivetrain.driveFor(24 * 0.8, inches); // grab 4 cubes
  wait(1000, msec);
  IntakeL.stop(hold);
  IntakeR.stop(hold); // hold the cubes
  STOPCODE_DEBUG();
  Drivetrain.driveFor(reverse, 24 * 1.6, inches); // go back
  Drivetrain.setTurnVelocity(50, percent);
  Drivetrain.turnFor(130, degrees); // turn RIGHT towards corner
  Drivetrain.setDriveVelocity(33, percent);
  Drivetrain.driveFor(24 * 0.8, inches); // go to corner
  TiltMotor.spinTo(50 * 7, degrees);     // tilt cubes
  IntakeL.setVelocity(20, percent);
  IntakeR.setVelocity(20, percent);
  IntakeL.startSpinFor(-3, rev);
  IntakeR.startSpinFor(-3, rev); // start pushing out cubes
  Drivetrain.setDriveVelocity(25, percent);
  Drivetrain.driveFor(reverse, 24 * 0.8, inches); // back away slowly
}
void playback() {
  int y = 0;
  y = instructions.find(" ", 5);
  double vel = strtod(instructions.substr(0, y).c_str(), NULL);
  instructions = instructions.substr(y + 1);
  y = instructions.find(" ", 5);
  double pos = strtod(instructions.substr(0, y).c_str(), NULL);
  instructions = instructions.substr(y + 1);
  Controller1.Screen.print(vel);
  
  // LeftDriveSmart.setVelocity(vel, percent);
  // LeftDriveSmart.startSpinTo(pos, degrees);

  y = instructions.find(" ", 5);
  double vel2 = strtod(instructions.substr(0, y).c_str(), NULL);
  instructions = instructions.substr(y + 1);
  y = instructions.find(" ", 5);
  double pos2 = strtod(instructions.substr(0, y).c_str(), NULL);
  instructions = instructions.substr(y + 1);
  //Controller1.Screen.print(vel);
  goToPos(LeftDriveSmart, vel, pos);
  goToPos(RightDriveSmart, vel2, pos2);

  y = instructions.find(" ", 5);
  vel = strtod(instructions.substr(0, y).c_str(), NULL);
  instructions = instructions.substr(y + 1);
  IntakeL.spin(forward, vel, percent);
  IntakeR.spin(forward, vel, percent);

  y = instructions.find(" ", 5);
  vel = strtod(instructions.substr(0, y).c_str(), NULL);
  instructions = instructions.substr(y + 1);
  y = instructions.find(" ", 5);
  pos = strtod(instructions.substr(0, y).c_str(), NULL);
  instructions = instructions.substr(y + 1);
  goToPos(LiftMotor, vel, pos);
  goToPos(LiftMotor2, vel, pos);
  //LiftMotor2.rotateTo(pos, degrees, vel, velocityUnits::pct, false);

  y = instructions.find(" ", 5);
  vel = strtod(instructions.substr(0, y).c_str(), NULL);
  instructions = instructions.substr(y + 1);
  y = instructions.find(" ", 5);
  pos = strtod(instructions.substr(0, y).c_str(), NULL);
  goToPos(TiltMotor, vel, pos);
  goToPos(TiltMotor2, -vel, pos);
  //TiltMotor.rotateTo(pos, degrees, vel, velocityUnits::pct, false);
  //wait(10, msec);
}
/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              User Control Task                            */
/*                                                                           */
/*  This task is used to control your robot during the user control phase of */
/*  a VEX Competition.                                                       */
/*                                                                           */
/*  You must modify the code to add your own robot specific commands here.   */
/*---------------------------------------------------------------------------*/
std::ofstream ofs;

void usercontrol(void) {
  Controller1.Screen.setCursor(4, 0);
        // Controller1.Screen.clearLine();
  Controller1.Screen.print(filenum);
  LiftMotor.setVelocity(100, percent);
  LiftMotor.stop();
  LiftMotor2.setVelocity(100, percent);
  LiftMotor2.stop();
  TiltMotor.setVelocity(30, percent);
  TiltMotor2.setVelocity(30, percent);
  TiltMotor.stop();
  TiltMotor2.stop();
  LeftDriveSmart.stop();
  RightDriveSmart.stop();
  Controller1.Screen.clearScreen();
  Controller1.Screen.setCursor(0, 0);
  Controller1.Screen.clearLine();
  Controller1.Screen.print("Normal Driving ");
  bool recording = false;
  bool donerecording = false;
  timer tim2;
  // User control code here, inside the loop
  while (1) {
    // This is the main execution loop for the user control program.
    // Each time through the loop your program should update motor + servo
    // values based on feedback from the joysticks.

    // ........................................................................
    // Insert user code here. This is where you use the joystick values to
    // update your motors, etc.
    // ........................................................................
    if (Controller1.ButtonY.pressing()) {
      speedin = false;
      driveMultiplier = 0.33;
      Controller1.Screen.setCursor(0, 0);
      Controller1.Screen.clearLine();
      Controller1.Screen.print("Precise Driving ");
    } else if (Controller1.ButtonX.pressing()) {
      speedin = true;
      driveMultiplier = 1;
      Controller1.Screen.setCursor(0, 0);
      Controller1.Screen.clearLine();
      Controller1.Screen.print("Normal Driving  ");
    }
    if (Controller1.ButtonA.pressing() && !apressed) {
      speedin = !speedin;
      apressed = true;
    } else if (!Controller1.ButtonA.pressing())
      apressed = false;

    if (Controller1.ButtonB.pressing() && !bpressed) {
      if (driveMultiplier == 1.0) {
        driveMultiplier = 0.33;
        Controller1.Screen.setCursor(0, 0);
        // Controller1.Screen.clearLine();
        Controller1.Screen.print("Precise Driving");
      } else if (driveMultiplier == 0.33) {
        driveMultiplier = 1;
        Controller1.Screen.setCursor(0, 0);
        // Controller1.Screen.clearLine();
        Controller1.Screen.print("Normal Driving ");
      }
      bpressed = true;
    } else if (!Controller1.ButtonB.pressing())
      bpressed = false;
    // uint8_t * data = (uint8_t*)malloc(sizeof(uint8_t) * 5);
    if (Controller1.ButtonLeft.pressing() && !recording) {
      recording = true;

      LeftDriveSmart.resetRotation();
      RightDriveSmart.resetRotation();
      LiftMotor.resetRotation();
      LiftMotor2.resetRotation();
      TiltMotor.resetRotation();
      TiltMotor2.resetRotation();
      IntakeL.resetRotation();
      IntakeR.resetRotation();
      if (Brain.SDcard.isInserted()) {
        // create a file with long filename
        char filename[1] = "";
        snprintf(filename, 2, "%d", filenum);
        std::string fn = filename;
        fn += ".txt";
        ofs.open(fn, std::ofstream::out);
      }
      tim2.clear();
    }
    if (Controller1.ButtonRight.pressing()) {
      donerecording = true;
    }
    if (Controller1.ButtonUp.pressing()) {
      filenum ++;
      Controller1.Screen.setCursor(4, 0);
        // Controller1.Screen.clearLine();
      Controller1.Screen.print(filenum);
      wait(250, msec);
    }
    if (Controller1.ButtonDown.pressing()) {
      filenum --;
      Controller1.Screen.setCursor(4, 0);
        // Controller1.Screen.clearLine();
      Controller1.Screen.print(filenum);
      wait(250, msec);
    }
    controllerBaseInput();
    if (recording) {
      while (tim2.value() < 0.075) {
        wait(1, msec);
      }
      tim2.clear();
      // count ++;
      std::string thisReading = "";
      char output[12];
      snprintf(output, 12, "%f", LeftDriveSmart.velocity(percent));
      thisReading += output;
      thisReading += " ";
      // output[12] = "";
      snprintf(output, 12, "%f", LeftDriveSmart.rotation(degrees));
      thisReading += output;
      thisReading += " ";

      // output[12] = "";
      snprintf(output, 12, "%f", RightDriveSmart.velocity(percent));
      thisReading += output;
      thisReading += " ";
      // output[12] = "";
      snprintf(output, 12, "%f", RightDriveSmart.rotation(degrees));
      thisReading += output;
      thisReading += " ";

      // char output[12];
      snprintf(output, 12, "%f", IntakeL.velocity(percent));
      thisReading += output;
      thisReading += " ";

      // output[12] = "";
      snprintf(output, 12, "%f", LiftMotor.velocity(percent));
      thisReading += output;
      thisReading += " ";
      // output[12] = "";
      snprintf(output, 12, "%f", LiftMotor.rotation(degrees));
      thisReading += output;
      thisReading += " ";

      // output[12] = "";
      snprintf(output, 12, "%f", TiltMotor.velocity(percent));
      thisReading += output;
      thisReading += " ";
      // output[12] = "";
      snprintf(output, 12, "%f", TiltMotor.rotation(degrees));
      thisReading += output;
      thisReading += " \n";
      ofs << thisReading;

      // data = (uint8_t*)realloc(data, sizeof(uint8_t) * 5 * count + 5);
      // uint8_t* thisReading;
      // thisReading = new uint8_t[5];
      /*thisReading = {(uint8_t) (LeftDriveSmart.velocity(percent)+100),
                                (uint8_t)
      (RightDriveSmart.velocity(percent)+100), (uint8_t)
      (IntakeL.velocity(percent)+100), (uint8_t)
      (LiftMotor.velocity(percent)+100), (uint8_t)
      (TiltMotor.velocity(percent)+100)
      };*/
      //*(thisReading) = (uint8_t) (LeftDriveSmart.velocity(percent)+100);
      //*(thisReading+1) = (uint8_t) (RightDriveSmart.velocity(percent)+100);
      //*(thisReading+2) = (uint8_t) (IntakeL.velocity(percent)+100);
      //*(thisReading+3) = (uint8_t) (LiftMotor.velocity(percent)+100);
      //*(thisReading+4) = (uint8_t) (TiltMotor.velocity(percent)+100);
      // int startIndex = count * 5;
      // for(int i = 0; i < 5; i++) {
      //  data[startIndex + i] = thisReading[i];
      //}
      if (donerecording) {
        // Brain.SDcard.savefile("recording.bin", data, sizeof(data));
        recording = false;
        ofs.close();
      }
    } else {
      wait(20, msec); // Sleep the task for a short amount of time to
                      // prevent wasted resources.
    }
    // controllerBaseInput();
  }
}

//
// Main will set up the competition functions and callbacks.
//
int main() {
  // Set up callbacks for autonomous and driver control periods.
  Competition.autonomous(autonomous);
  Competition.drivercontrol(usercontrol);

  // Run the pre-autonomous function.
  pre_auton();

  // Prevent main from exiting with an infinite loop.
  while (true) {
    wait(100, msec);
  }
}
void STOPCODE_DEBUG() {
  Drivetrain.stop();
  LiftMotor.stop();
  LiftMotor2.stop();
  IntakeR.stop();
  IntakeL.stop();
  TiltMotor.stop();
  TiltMotor2.stop();
  while (true)
    wait(20, msec);
}

/*void goToPos(motor m, double vel, double pos) {
  m.setVelocity(vel, percent);
  if (true || pos > m.rotation(degrees)) {
    if (vel > 0 && !m.isSpinning()) {
      m.spin(forward);
    }
  } else {
    m.stop();
  }

  if (true || pos < m.rotation(degrees)) {
    if (vel < 0 && !m.isSpinning()) {
      m.spin(forward);
    }
  } else {
    m.stop();
  }
}*/
void goToPos(motor m, double vel, double pos) {
  m.setVelocity(vel, percent);
  if (vel != 0 && !m.isSpinning()) {
    m.spin(forward);
  }
}
void controllerBaseInput() {

  // int drivetrainLeftSideSpeed = Controller1.Axis3.position();
  // int drivetrainRightSideSpeed = Controller1.Axis2.position();
  double axisAngle =
      atan2(Controller1.Axis3.position(), Controller1.Axis4.position());
  axisAngle *= 180 / 3.1415926;

  // if(Controller1.Axis4.position() < 0) axisAngle += 180;
  // Controller1.Screen.setCursor(0, 50);
  // Controller1.Screen.clearLine();
  // Controller1.Screen.print(axisAngle);
  int drivetrainLeftSideSpeed = Controller1.Axis3.position();
  int drivetrainRightSideSpeed = Controller1.Axis4.position();
  int powerLevel =
      sqrt(Controller1.Axis3.position() * Controller1.Axis3.position() +
           Controller1.Axis4.position() * Controller1.Axis4.position());
  // left side
  if (axisAngle < 90 && axisAngle >= 0)
    drivetrainLeftSideSpeed = powerLevel;
  else if (axisAngle <= 181 && axisAngle >= 90)
    drivetrainLeftSideSpeed = int(powerLevel * (135 - axisAngle) / 45);
  else if (axisAngle >= -181 && axisAngle <= -90)
    drivetrainLeftSideSpeed = -powerLevel;
  else if (axisAngle > -90 && axisAngle < 0)
    drivetrainLeftSideSpeed = int(powerLevel * (45 + axisAngle) / 45);

  if (axisAngle <= 181 && axisAngle >= 90)
    drivetrainRightSideSpeed = powerLevel;
  else if (axisAngle < 90 && axisAngle >= 0)
    drivetrainRightSideSpeed = -int(powerLevel * (45 - axisAngle) / 45);
  else if (axisAngle > -90 && axisAngle < 0)
    drivetrainRightSideSpeed = -powerLevel;
  else if (axisAngle >= -181 && axisAngle <= -90)
    drivetrainRightSideSpeed = -int(powerLevel * (135 + axisAngle) / 45);

  if (Controller1.Axis4.position() == 0 && Controller1.Axis3.position() > 0 &&
      (int(axisAngle) == 90 || int(axisAngle) == 0)) {
    drivetrainLeftSideSpeed = powerLevel;
    drivetrainRightSideSpeed = powerLevel;
  }
  if (Controller1.Axis4.position() == 0 && Controller1.Axis3.position() < 0 &&
      (int(axisAngle) == -90 || int(axisAngle) == 0)) {
    drivetrainLeftSideSpeed = -powerLevel;
    drivetrainRightSideSpeed = -powerLevel;
  }
  if (Controller1.Axis4.position() < 0 &&
      (int(axisAngle) == 180 || int(axisAngle) == -180 ||
       int(axisAngle) == 0)) {
    drivetrainLeftSideSpeed = -powerLevel;
    drivetrainRightSideSpeed = powerLevel;
  }
  int IntakeSpeed = Controller1.Axis2.position();
  if (IntakeSpeed < 5 && IntakeSpeed > -5) {
    // check if the left motor has already been stopped
    if (IntakeStop) {
      // stop the left drive motor
      IntakeL.stop();
      IntakeR.stop();
      // tell the code that the left motor has been stopped
      IntakeStop = false;
    }
  } else {
    // reset the toggle so that the deadband code knows to stop the left motor
    // next time the input is in the deadband range
    IntakeStop = true;
  }
  // only tell the left drive motor to spin if the values are not in the
  // deadband range
  if (speedin) {
    Controller1.Screen.setCursor(2, 0);
    // Controller1.Screen.clearLine();
    Controller1.Screen.print("Fast intake");
    Controller1.Screen.setCursor(4, 0);
    Controller1.Screen.print(filenum);
  } else {
    Controller1.Screen.setCursor(2, 0);
    // Controller1.Screen.clearLine();
    Controller1.Screen.print("Slow intake");
    Controller1.Screen.setCursor(4, 0);
    Controller1.Screen.print(filenum);
    IntakeSpeed /= 4;
  }

  /*if (Controller1.ButtonDown.pressing()) {
    drivetrainLeftSideSpeed = -20;
    drivetrainRightSideSpeed = -20;
    IntakeStop = true;
    IntakeSpeed = -20;
  } else if (Controller1.ButtonUp.pressing()) {
    drivetrainLeftSideSpeed = 20;
    drivetrainRightSideSpeed = 20;
    IntakeStop = true;
    IntakeSpeed = 20;
  } else {*/
    drivetrainLeftSideSpeed *= driveMultiplier;
    drivetrainRightSideSpeed *= driveMultiplier;
  //}
  if (IntakeStop) {
    IntakeL.setVelocity(IntakeSpeed, percent);
    IntakeR.setVelocity(IntakeSpeed, percent);
    IntakeL.spin(forward);
    IntakeR.spin(forward);
  }
  // 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
    // next time 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);
  }
  // check the ButtonL1/ButtonL2 status to control LiftMotor
  if (Controller1.ButtonL1.pressing()) {
    LiftMotor.spin(forward);
    LiftMotor2.spin(forward);
    Controller1LeftShoulderControlMotorsStopped = false;
  } else if (Controller1.ButtonL2.pressing()) {
    LiftMotor.spin(reverse);
    LiftMotor2.spin(reverse);
    Controller1LeftShoulderControlMotorsStopped = false;
  } else if (!Controller1LeftShoulderControlMotorsStopped) {
    LiftMotor.stop(hold);
    LiftMotor2.stop(hold);
    // set the toggle so that we don't constantly tell the motor to stop when
    // the buttons are released
    Controller1LeftShoulderControlMotorsStopped = true;
  }

  // only tell the right drive motor to spin if the values are not in the
  // deadband range

  // check the ButtonR1/ButtonR2 status to control Intake
  /*if (Controller1.ButtonR1.pressing()) {
    IntakeR.spin(forward);
    IntakeL.spin(forward);
    Controller1RightShoulderControlMotorsStopped = false;
  } else if (Controller1.ButtonR2.pressing()) {
    IntakeR.spin(reverse);
    IntakeL.spin(reverse);
    Controller1RightShoulderControlMotorsStopped = false;
  } else if (!Controller1RightShoulderControlMotorsStopped) {
    IntakeR.stop(hold);
    IntakeL.stop(hold);
    // set the toggle so that we don't constantly tell the motor to stop when
  the buttons are released Controller1RightShoulderControlMotorsStopped = true;
  }*/
  // check the Right Shoulder Buttons status to control TiltMotor
  if (Controller1.ButtonR1.pressing()) {
    TiltMotor.spin(forward);
    TiltMotor2.spin(forward);
    Controller1UpDownButtonsControlMotorsStopped = false;
  } else if (Controller1.ButtonR2.pressing()) {
    TiltMotor.spin(reverse);
    TiltMotor2.spin(reverse);
    Controller1UpDownButtonsControlMotorsStopped = false;
  } else if (!Controller1UpDownButtonsControlMotorsStopped) {
    TiltMotor.stop(hold);
    TiltMotor.stop(hold);
    // set the toggle so that we don't constantly tell the motor to stop when
    // the buttons are released
    Controller1UpDownButtonsControlMotorsStopped = true;
  }
}