#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.
motor Left1MotorA = motor(PORT4, ratio6_1, false);
motor Left1MotorB = motor(PORT5, ratio6_1, false);
motor_group Left1 = motor_group(Left1MotorA, Left1MotorB);

motor Left2 = motor(PORT21, ratio6_1, false);

motor Right1MotorA = motor(PORT11, ratio6_1, true);
motor Right1MotorB = motor(PORT14, ratio6_1, true);
motor_group Right1 = motor_group(Right1MotorA, Right1MotorB);

motor Right2 = motor(PORT15, ratio6_1, true);

motor Intake = motor(PORT10, ratio6_1, false);

digital_out Wings = digital_out(Brain.ThreeWirePort.G);
digital_out Intakepnumatic = digital_out(Brain.ThreeWirePort.F);
controller Controller1 = controller(primary);
motor Cata = motor(PORT17, ratio36_1, true);

pot Pot = pot(Brain.ThreeWirePort.B);
distance Distance1 = distance(PORT19);



// 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

// Include the V5 Library
#include "vex.h"

// Allows for easier use of the VEX Library
using namespace vex;

competition Competition;

float myVariable;

// "when started" hat block
int whenStarted1() {
  Left1.setMaxTorque(100.0, percent);
  Left2.setMaxTorque(100.0, percent);
  Right1.setMaxTorque(100.0, percent);
  Right2.setMaxTorque(100.0, percent);
  Intake.setVelocity(100.0, percent);
  Intake.spin(forward);
  Cata.setStopping(hold);
  Cata.setVelocity(100.0, percent);
  Cata.setMaxTorque(100.0, percent);
  return 0;
}

// "when Controller1 ButtonR1 pressed" hat block
void onevent_Controller1ButtonR1_pressed_0() {
  Wings.set(true);
}

// "when Controller1 ButtonR1 released" hat block
void onevent_Controller1ButtonR1_released_0() {
  Wings.set(false);
}

// "when driver control" hat block
int ondriver_drivercontrol_0() {
  while (true) {
    Left1.spin(forward);
    Left2.spin(forward);
    Right1.spin(forward);
    Right2.spin(forward);
    if ((5.0 > Controller1.Axis3.position() && Controller1.Axis3.position() > -5.0) && (5.0 > Controller1.Axis1.position() && Controller1.Axis1.position() > -5.0)) {
      Left1.setVelocity(0.0, percent);
      Left2.setVelocity(0.0, percent);
      Right1.setVelocity(0.0, percent);
      Right2.setVelocity(0.0, percent);
    }
    else {
      Left1.setVelocity((Controller1.Axis3.position() + Controller1.Axis1.position() * 0.25), percent);
      Left2.setVelocity((Controller1.Axis3.position() + Controller1.Axis1.position() * 0.25), percent);
      Right1.setVelocity(((Controller1.Axis3.position() - Controller1.Axis1.position() * 0.25) - 5.0), percent);
      Right2.setVelocity(((Controller1.Axis3.position() - Controller1.Axis1.position() * 0.25) - 5.0), percent);
    }
    if (Controller1.ButtonR1.pressing()) {
      Intake.setVelocity(100.0, percent);
      Intake.spin(forward);
      wait(100, msec);
    } else if(Controller1.ButtonR2.pressing()){
      Intake.setVelocity(-100.0, percent);
      Intake.spin(forward);
      wait(100, msec);
    } else {
      Intake.stop();
      wait(100, msec);
    }
    if(Controller1.ButtonL1.pressing()){
      Wings.set(true);
    } else {
      Wings.set(false);
    }
    //if(Controller1.ButtonB.pressing()){
    if(Pot.angle(percent) > 64 && Pot.angle(percent) < 68){
      if(Controller1.ButtonA.pressing() || Distance1.objectDistance(inches) < 6){
        Cata.setVelocity(100, percent);
        Cata.spinFor(forward, 60, degrees);
        Controller1.rumble(rumbleShort);
      } else {
        Cata.stop();
      }
    } else {
      Cata.spin(forward);
      if(Pot.angle(percent) > 55 && Pot.angle(percent) < 60){
        Cata.setVelocity(50, percent);
      } else if(Pot.angle(percent) > 60 && Pot.angle(percent) < 64){
        Cata.setVelocity(25, percent);
      } else {
        Cata.setVelocity(100, percent);
      }
    }
    //}
  wait(5, msec);
  }
  return 0;
}

// "when autonomous" hat block
int onauton_autonomous_0() {
  return 0;
}

void VEXcode_driver_task() {
  // Start the driver control tasks....
  vex::task drive0(ondriver_drivercontrol_0);
  while(Competition.isDriverControl() && Competition.isEnabled()) {this_thread::sleep_for(10);}
  drive0.stop();
  return;
}

void VEXcode_auton_task() {
  // Start the auton control tasks....
  vex::task auto0(onauton_autonomous_0);
  while(Competition.isAutonomous() && Competition.isEnabled()) {this_thread::sleep_for(10);}
  auto0.stop();
  return;
}



int main() {
  vex::competition::bStopTasksBetweenModes = false;
  Competition.autonomous(VEXcode_auton_task);
  Competition.drivercontrol(VEXcode_driver_task);

  // register event handlers
  Controller1.ButtonR1.pressed(onevent_Controller1ButtonR1_pressed_0);
  Controller1.ButtonR1.released(onevent_Controller1ButtonR1_released_0);

  wait(15, msec);
  // post event registration

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

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

  whenStarted1();
}