#include "robot-config.h"
/*---------------------------------------------------------------------------*/
/*                                                                           */
/*        Description: Competition template for VCS VEX V5                   */
/*                                                                           */
/*---------------------------------------------------------------------------*/

//Creates a competition object that allows access to Competition methods.
vex::competition    Competition;
void drivef(int distance, int speed){//Drive Foward
    Right.rotateFor(distance, rotationUnits::deg,speed,velocityUnits::pct,false);
    RightB.rotateFor(distance, rotationUnits::deg,speed,velocityUnits::pct,false);
    Left.rotateFor(distance, rotationUnits::deg,speed,velocityUnits::pct,false);
    LeftB.rotateFor(distance, rotationUnits::deg,speed,velocityUnits::pct);
}
void driver(int distance, int speed){//Drive Reverse
    Right.rotateFor(-distance, rotationUnits::deg,speed,velocityUnits::pct,false);
    RightB.rotateFor(-distance, rotationUnits::deg,speed,velocityUnits::pct,false);
    Left.rotateFor(-distance, rotationUnits::deg,speed,velocityUnits::pct,false);
    LeftB.rotateFor(-distance, rotationUnits::deg,speed,velocityUnits::pct);
}
void right(int distance, int speed){//Drive Right
    Right . rotateFor(distance, rotationUnits::deg,speed,velocityUnits::pct,false);
    RightB . rotateFor(distance, rotationUnits::deg,speed,velocityUnits::pct,false);
    Left . rotateFor(-distance, rotationUnits::deg,speed,velocityUnits::pct,false);
    LeftB . rotateFor(-distance, rotationUnits::deg,speed,velocityUnits::pct);
}
void left(int distance, int speed){//Drive Left
    Right . rotateFor(-distance, rotationUnits::deg,speed,velocityUnits::pct,false);
    RightB . rotateFor(-distance, rotationUnits::deg,speed,velocityUnits::pct,false);
    Left . rotateFor(distance, rotationUnits::deg,speed,velocityUnits::pct,false);
    LeftB . rotateFor(distance, rotationUnits::deg,speed,velocityUnits::pct);
}
void armu(int position, int speed){//Arm Control
    Arm.rotateTo(position,rotationUnits::deg,speed,velocityUnits::pct);
}


//Autonomous Idea!
//Grab the two closest orange blocks to the wall, push them both, make at least one block in,
//and use the claw to stack the other orange claw on top of each other.
/*---------------------------------------------------------------------------*/
/*                          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 cortex has been powered on and    */
/*  not every time that the robot is disabled.                               */
/*---------------------------------------------------------------------------*/

void pre_auton( void ) {
  // 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.   */
/*---------------------------------------------------------------------------*/

void autonomous( void ) {
  // ..........................................................................
  // Insert autonomous user code here.

 // ..........................................................................

}

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

void usercontrol( void ) {
  // 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.
    // ........................................................................

      int ArmSpeedPCT = 75;
      int ClawSpeedPCT = 75;

      Left.spin(vex::directionType::fwd, Controller1.Axis3.value() + Controller1.Axis1.value(), vex::velocityUnits::pct);
      LeftB.spin(vex::directionType::fwd, Controller1.Axis3.value() + Controller1.Axis1.value(), vex::velocityUnits::pct);
      Right.spin(vex::directionType::fwd, Controller1.Axis2.value() - Controller1.Axis1.value(), vex::velocityUnits::pct);
      RightB.spin(vex::directionType::fwd, Controller1.Axis2.value() - Controller1.Axis1.value(), vex::velocityUnits::pct);


      { if(Controller1.ButtonR1.pressing()) {
          Arm.spin(vex::directionType::fwd, ArmSpeedPCT, vex::velocityUnits::pct);
      }
      else if(Controller1.ButtonR2.pressing()) {
          Arm.spin(vex::directionType::rev, ArmSpeedPCT, vex::velocityUnits::pct);
      }
      else {
          Arm.stop(vex::brakeType::brake);
      }


      if(Controller1.ButtonL1.pressing()) {
          Claw.spin(vex::directionType::fwd, ClawSpeedPCT, vex::velocityUnits::pct);
      }
      else if(Controller1.ButtonL2.pressing()) {
          Claw.spin(vex::directionType::rev, ClawSpeedPCT, vex::velocityUnits::pct);
      }
      else {
          Claw.stop(vex::brakeType::brake);
      }


      }



  }


    vex::task::sleep(20); //Sleep the task for a short amount of time to prevent wasted resources.
  }


//
// Main will set up the competition functions and callbacks.
//
int main() {

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

    //Set up callbacks for autonomous and driver control periods.
    Competition.autonomous( autonomous );
    Competition.drivercontrol( usercontrol );

    //Prevent main from exiting with an infinite loop.
    while(1) {
      vex::task::sleep(100);//Sleep the task for a short amount of time to prevent wasted resources.
    }
      //Our first program :-D
}