#include <Servo.h>
#include <PPM.h>

//Set up a connection to the vex controller
//=========================================
PPM ppm(2);
//=========================================



//Set up the servos and motors to interface with the arduino
//==========================================================
Servo LEFT_MOTOR_ONE;
Servo LEFT_MOTOR_TWO;
//=====================
Servo RIGHT_MOTOR_ONE;
Servo RIGHT_MOTOR_TWO;
//=====================
Servo LIFT_MOTOR_ONE;
Servo LIFT_MOTOR_TWO;
//=====================
Servo SWEEPER_SERVO;
//==========================================================



//set up the global integer values used between funtions
//======================================================
int sweeper_position = 0;
int requested_sweeper_position = 0;
//=======================
int LEFT_INPUT;
int RIGHT_INPUT;
//======================================================



//set up the global booleans used between functions
//=================================================
boolean LIFT_INPUT;
boolean arm_toggled = false;
//=================================================


// Wait until controller is turned on.  Do not edit!!!
void waitForController () {
  while (ppm.getChannel(1) == 0) { } // do nothing until controller is switched on
}




// initialize all of the motors and servos with thir PWM ports on the arduino
void setup(){
  
  Serial.begin(9600);
  
  LEFT_MOTOR_ONE.attach(8);
  LEFT_MOTOR_TWO.attach(9);

  RIGHT_MOTOR_ONE.attach(6);
  RIGHT_MOTOR_TWO.attach(7);

  LIFT_MOTOR_ONE.attach(10);
  LIFT_MOTOR_TWO.attach(11);

  SWEEPER_SERVO.attach(12);
}


//the loop function does the following:
//  runs the autonymous code for 20s
//  runs the teleoperated code for 150s (2.5 minutes)
//  exits the program
void loop(){
  arm_in();
  waitForController();
  autonomous(20);
  teleop(150);
  finish();
}

void finish () {
  Serial.println("Done");
  delay(50);     // With no delay the program could exit before the serial has completed printing
  exit(0);       // Exit
}



// the poll function returns no values, but servs to store all of the inputs or their derrived values
// as global variables for use in other functions
void poll(){
  //the joysticks
  //used for driving
  LEFT_INPUT = ppm.getChannel(3);
  RIGHT_INPUT = ppm.getChannel(2);

  //the channel 6 buttons (right hand operated)
  //used for turning the lifting motors on and off
  if (ppm.getChannel(6) > 150) //down button
    LIFT_INPUT = false;
  if (ppm.getChannel(6) < 30) //up button
    LIFT_INPUT = true;

  //the channel 5 buttons (left hand operated)
  //used for opening and closing the arm
  if (ppm.getChannel(5) > 150) //down button
    requested_sweeper_position = 0;
  else if (ppm.getChannel(5) < 30) //up button
    requested_sweeper_position =120;
  else //no button
    arm_toggled = true;

}


//runs the autonomous section of the code for the set time
void autonomous(unsigned long time) {
  unsigned long startTime=millis();
  time=time * 1000;
  while (millis()-startTime<time) {
    operation_deqube();  
    operation_pick_me_up();
    
    while (millis()-startTime<time) {Serial.println("autonomous has time left");}
    Serial.println("Autonomous Over!");
    delay(200);
  }
}


//pulls the green cubes towards the conveyor belts
void operation_deqube(){
  arm_in();
  drive_backwards(1000);
  delay(1000);
  arm_out();
  drive_forwards(1000);
  move_arm(0,180,1000);
}


//drives back and forth with the treads running attempting to pick up cubes
void operation_pick_me_up(){
  //turn on the lift
  LIFT_MOTOR_TWO.write(0);
  LIFT_MOTOR_ONE.write(180);
  
  //turn a little on 
  drive_forwards(600);
  drive_backwards(600);
  
  delay(100);
  stop_driving();
  
  drive_forwards(1500);
  drive_backwards_turn(1000);
  
  drive_forwards(2000);
  drive_backwards_turn(1000);
  
  drive_forwards(2000);
  drive_backwards_turn(1000);
  
  drive_forwards(2000);
  drive_backwards_turn(1000);
  
  drive_forwards(2000);
  drive_backwards_turn(1000);
  
  
}

//drives back at full speed
//for use in auto
void drive_backwards(int time){
  Serial.println("driving backwards");
  LEFT_MOTOR_ONE.write(0);
  LEFT_MOTOR_TWO.write(0);
  RIGHT_MOTOR_ONE.write(180);
  RIGHT_MOTOR_TWO.write(180);
  delay(time);
  stop_driving();
}

//drives back at turning speed
//for use in auto
void drive_backwards_turn(int time){
  Serial.println("driving backwards and turning");
  LEFT_MOTOR_ONE.write(80);
  LEFT_MOTOR_TWO.write(80);
  RIGHT_MOTOR_ONE.write(180);
  RIGHT_MOTOR_TWO.write(180);
  delay(time);
  stop_driving();
}

//drives forward at full speed
//for use in auto
void drive_forwards(int time){
  LEFT_MOTOR_ONE.write(180);
  LEFT_MOTOR_TWO.write(180);
  RIGHT_MOTOR_ONE.write(0);
  RIGHT_MOTOR_TWO.write(0);
  delay(time);
  stop_driving(); 
}

//turns right at full speed
//for use in auto
void turn_right(int time){
  LEFT_MOTOR_ONE.write(180);
  LEFT_MOTOR_TWO.write(180);
  RIGHT_MOTOR_ONE.write(180);
  RIGHT_MOTOR_TWO.write(180);
  delay(time);
  stop_driving();
}

//turns left at full speed
//for use in auto
void turn_left(int time){
  LEFT_MOTOR_ONE.write(0);
  LEFT_MOTOR_TWO.write(0);
  RIGHT_MOTOR_ONE.write(0);
  RIGHT_MOTOR_TWO.write(0);
  delay(time);
  stop_driving();
}

//stops driving
//for use in auto and teleop
void stop_driving(){
  LEFT_MOTOR_ONE.write(90);
  LEFT_MOTOR_TWO.write(90);
  RIGHT_MOTOR_ONE.write(90);
  RIGHT_MOTOR_TWO.write(90);
}



// a tricky recursive function to move the servo more slowly
void move_arm(double toPosition, double fromPosition, double time){
  double dpms = (toPosition-fromPosition) / (time * 1.0);
  if (time == 0)
    return;
  else{
    delay(1);
    SWEEPER_SERVO.write(fromPosition+dpms);
    move_arm(toPosition, fromPosition+dpms, time-1);
  }
}


//move the arm in super fast
void arm_in(){
  SWEEPER_SERVO.write(0);
}

//move the arm out super fast
void arm_out(){
  SWEEPER_SERVO.write(180);
}







// the function to allow operator control for the input number of seconds
void teleop(unsigned long time) {
  unsigned long startTime=millis();
  time=time * 1000;
  while (millis()-startTime<time) {
    poll();

    LEFT_MOTOR_ONE.write(180-LEFT_INPUT);
    LEFT_MOTOR_TWO.write(180-LEFT_INPUT);

    RIGHT_MOTOR_ONE.write(RIGHT_INPUT);
    RIGHT_MOTOR_TWO.write(RIGHT_INPUT);

    LIFT_MOTOR_TWO.write(LIFT_INPUT?0:90);
    LIFT_MOTOR_ONE.write(LIFT_INPUT?180:90);

    if (arm_toggled){
      arm_toggled = false;
      SWEEPER_SERVO.write(requested_sweeper_position);
    }

  }
}