/*----------------------------------------------------------------------------*/
/*                                                                            */
/*    Module:       main.cpp                                                  */
/*    Author:       samantha                                                  */
/*    Created:      Tue Sep 14 2021                                           */
/*    Description:  V5 project                                                */
/*                                                                            */
/*----------------------------------------------------------------------------*/

// ---- START VEXCODE CONFIGURED DEVICES ----
// Robot Configuration:
// [Name]               [Type]        [Port(s)]
// leftDrives           motor_group   1, 7
// rightDrives          motor_group   2, 8
// Controller1          controller
// MOGO                 motor         6
// IntakeFront          motor         5
// IntakeTower          motor         4
// Lift                 motor         3
// ---- END VEXCODE CONFIGURED DEVICES ----

#include "vex.h"

using namespace vex;

competition Competition;

//VARIABLES
bool driveSlowToggled;
bool intakeDirToggled;
bool intakeToggled;
int dir;

void pre_auton(void) {
  // Initializing Robot Configuration. DO NOT REMOVE!
  vexcodeInit();
  MOGO.setStopping(hold);
  Lift.setStopping(hold);
  leftDrives.setStopping(coast);
  rightDrives.setStopping(coast);
  Lift.setPosition(0, degrees);
}

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

void autonomous(void) {

}

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

//////////////TOGGLERS////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

//TOGGLE DRIVE SLOW
void bPressed(){
  driveSlowToggled = !(driveSlowToggled);
}
//TOGGLE DRIVE REVERSE
void xPressed(){
  dir *= -1;
}
//TOGGLE INTAKES
void aPressed(){
  intakeToggled = !(intakeToggled);
}
void yPressed(){
  intakeDirToggled = !(intakeDirToggled);
}

/////////USER CONTROL/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

void usercontrol(void) {
  // User control code here, inside the loop
  driveSlowToggled = false;
  intakeDirToggled = false;
  intakeToggled = false;
  dir = 1;
  IntakeFront.setVelocity(50, percent);
  IntakeTower.setVelocity(100, percent);
  MOGO.setVelocity(100, percent);
  Lift.setVelocity(100,percent);
  while (1) {
    //lift Up/down r1/r2
    //Intakes - a toggle on/off
    //MOGO - l1/l2
    //A - Intakes toggle
    //B - drive speed toggle
    //Y - reverse intake
    //X - reverse drive

    //lift 360 to -10

    ///////DRIVING///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    //DRIVE SPEED TOGGLE
    Controller1.ButtonB.pressed(bPressed);
    //DRIVE DIR TOGGLE
    Controller1.ButtonX.pressed(xPressed);
    //DRIVING
    if (Controller1.Axis2.position(percent) > 15 || Controller1.Axis2.position(percent) < -15) {
      if (driveSlowToggled == true){
        rightDrives.setVelocity((Controller1.Axis2.position(percent)/2*dir), percent);
      }
      else{
        rightDrives.setVelocity((Controller1.Axis2.position(percent)*dir), percent);
      }
      rightDrives.spin(forward);
    }
    else {
      rightDrives.stop();
    }
    if (Controller1.Axis3.position(percent) > 15 || Controller1.Axis3.position(percent) < -15) {
      if (driveSlowToggled == true){
        leftDrives.setVelocity((Controller1.Axis3.position(percent)/2*dir), percent);
      }
      else {
        leftDrives.setVelocity((Controller1.Axis3.position(percent)*dir), percent);
      }
      leftDrives.spin(forward);
    }
    else {
      leftDrives.stop();
    }


    ////////LIFTS//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    //Front Lift
    if (Controller1.ButtonR1.pressing()){
      Lift.spin(reverse);
    }
    else if (Controller1.ButtonR2.pressing()){
      Lift.spin(forward);
    }
    else {
      Lift.stop();
    }

    //BACK LIFT
    if (Controller1.ButtonL1.pressing()){
      MOGO.spin(reverse);
    }
    else if (Controller1.ButtonL2.pressing()){
      MOGO.spin(forward);
    }
    else {
      MOGO.stop();
    }

    /////////INTAKES////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    //INTAKES
    Controller1.ButtonA.pressed(aPressed);
    Controller1.ButtonY.pressed(yPressed);
    if (intakeToggled){
      if (intakeDirToggled){
        IntakeFront.spin(reverse);
        IntakeTower.spin(reverse);
      }
      else {
        IntakeFront.spin(forward);
        IntakeTower.spin(forward);
      }
    }
    else {
      IntakeFront.stop();
      IntakeTower.stop();
    }

    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

    wait(20, msec); // Sleep the task for a short amount of time to
                    // prevent wasted resources.
  }
}

//
// 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);
  }
}