#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;
boolean robot_alive = true;
//=================================================






// initialize all of the motors and servos with thir PWM ports on the arduino
void setup(){
  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(){
  if (robot_alive){
    autonomous(20);
    teleop(150);
    exit(0);
    robot_alive = false;
  }
}



// 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 = 180;
  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;
  boolean operations_commencing = true;
  while (millis()-startTime<time) {
    if (operations_commencing){
      operation_deqube();
      operation_pick_me_up();
      stop_driving();
      drive_backwards(3600);


      operations_commencing = false;
    }
  }
}


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


//drives back and forth with the treads running attempting to pick up cubes
void operation_pick_me_up(){
  LIFT_MOTOR_TWO.write(0);
  LIFT_MOTOR_ONE.write(180);
  drive_forwards(600);
  drive_backwards(600);
  LEFT_MOTOR_ONE.write(180);
  LEFT_MOTOR_TWO.write(180);
  delay(100);
  stop_driving();
  drive_forwards(500);
  delay(6000);
  LIFT_MOTOR_TWO.write(90);
  LIFT_MOTOR_ONE.write(90);
}

//drives back at full speed
//for use in auto
void drive_backwards(int time){
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 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();
}

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

  }
}