#include <Servo.h>
int lightPin = A0;  
int check = 0;
int pushButton1 = 3;
int pushButton2 = 2;
int checkButton = 0;
int pos=0;
int x=0;
int y=0;
//Create Servo Objects
Servo servo90_1;
Servo servo90_2;
Servo servogate;
int servo90_1Pin = 13;
int servo90_2Pin = 1;
int servogatePin = 0;

//Standard PWM DC control
int E1 = 5;     //M1 Speed Control
int E2 = 6;     //M2 Speed Control
int M1 = 4;    //M1 Direction Control
int M2 = 7;    //M1 Direction Control

//Define char variable to use for given code
char val = 0; 

//Channel for PWM Signal
int ch1; //Left/Right, Pin 8
int ch2; //Forward, Pin 9
int ch3; //Servo 1/Ramp, Pin 10
int ch4; //Servo 2/Door, Pin 11
 
int movement = 0;
int turn = 0;

void stop(void)                    //Stop
{
  digitalWrite(E1,LOW);
  digitalWrite(E2,LOW);   
}   
void advance(char a,char b)          //Move forward
{
  analogWrite (E1,a);      //PWM Speed Control
  digitalWrite(M1,HIGH); 
  analogWrite (E2,b);   
  digitalWrite(M2,LOW); 
}                       
void back_off (char a,char b)          //Move backward
{
  analogWrite (E1,a);
  digitalWrite(M1,LOW);// LOW   
  analogWrite (E2,b);    
  digitalWrite(M2,HIGH);//LOW
}
void turn_L (char a,char b)             //Turn Left
{
  analogWrite (E1,a);
  digitalWrite(M1,LOW);    
  analogWrite (E2,b);    
  digitalWrite(M2,LOW);
}
void turn_R (char a,char b)             //Turn Right
{
  analogWrite (E1,a);
  digitalWrite(M1,HIGH);    
  analogWrite (E2,b);    
  digitalWrite(M2,HIGH);
}
void setup(void) 
{ 
  int i;
  for(i=4;i<=7;i++)
    pinMode(i, OUTPUT);  
  Serial.begin(19200);      //Set Baud Rate
  Serial.println("Run keyboard control");
 //Setup Servos
  servo90_1.attach(servo90Pin);
  servo90_2.attach(servo90Pin);
  servogate.attach(servogatePin);

  //Setup Pins to receive PWM Signal from RC Receiver
  pinMode(8, INPUT); // Set our input pins as such
  pinMode(9, INPUT);
  pinMode(10, INPUT);
  pinMode(11, INPUT);
  
  //PWM or Analogue output for Servo
  //pinMode(12, OUTPUT);
  //pinMode(13, OUTPUT);  
  
  // make the pushbutton's pin an input:
  pinMode(pushButton1, INPUT);
  pinMode(pushButton2, INPUT);


} 
void loop(void) 
{//Buttons
  int buttonState1 = digitalRead(pushButton1);
  Serial.println(buttonState1);
  int buttonState2 = digitalRead(pushButton2);
  Serial.println(buttonState2);
  
  //Servo control for ramp
  int servoAngle90 = map(ch3, 1000, 1890, 10, 89);
  if (ch3 == 0)
  {servo90_1.detach();
  servo90_2.detach();}
  
  if (servoAngle90 != 45)
  {servo90_1.attach(13);
  servo90_2.attach(12);
  servo90_1.write(servoAngle90);
  servo90_2.write(90 - servoAngle90);
  }
  else
  {servo90_1.detach();
  servo90_2.detach();
  }
  //Servo control for gate
  servogate.attach(servogatePin);                                             
  int servoAngle = map(ch3, 1000, 1890, 10, 175);
  Serial.println(servoAngle);
  if (servoAngle > 160)
  { servogate.write(servoAngle);
  }
  else
  {servogate.detach();
  }
  //delay(100);

     
//Serial.print("servoAngle90:"); //Serial debugging stuff
//Serial.println(servoAngle90);
//Serial.print("servoAngle30:"); //Serial debugging stuff
//Serial.println(servoAngle30);

//Movement/Turning for Robot

  ch1 = pulseIn(8, HIGH, 25000); // Read the pulse width of  
  ch2 = pulseIn(9, HIGH, 25000); // each channel
  ch3 = pulseIn(10, HIGH, 25000);
  ch4 = pulseIn(11, HIGH, 25000);

  Serial.println(ch1);
  Serial.println(ch2);
  Serial.println(ch3);
  Serial.println(ch4);
 
  //Set Mapping for Ch2 (Move)
  movement = map(ch2, 1000, 2000, -500, 500); //center over zero
  movement = constrain(movement, -255, 255); //only pass values whose absolutes are
                                   //valid pwm values
  
  /*Here we're determining whether a left or a right turn is being 
  executed*/
  //Mapping for Turning
  turn = map(ch1,1000,2000,-500,500);//Change Mapping for PWM
  turn = constrain(turn, -255, 255);//turn = constrain(turn, -255, 255);
  

      //Code to control old code for Movement/Turning for Robot
      if (movement > 70)//Move Forward, Note, actual movement when centered <70, therefore, re-center with code
      {
        val = 'w';//advance (movement,movement);   //move forward in max speed
      }
      else if (movement < 10)//Move Backward
      {
        val = 's'; //back_off (-movement,-movement);   //move back in max speed
      }
      else if (turn < -80)//Turn Left
      {
        val = 'a'; //turn_L (100,100);
      }
      else if (turn > 0)//Turn Right 
      {
        val = 'd'; //turn_R (100,100);
      }
      else if (ch1 < 5 || ch2 <5 || ch4 < 5)
      {
        val = 'x';
        
       
      }
      else //if (movement == 0)
      {
        val = 'x';
      }
      /*else
      {
        val = 'x';
      }*/
      
  
      //Old Code to Control the Movement    
      switch(val)
      {
      case 'w'://Move Forward
        advance (movement,movement);   //move forward in max speed
        break;
      case 's'://Move Backward
        back_off (-movement,-movement);   //move back in max speed
        break;                                    //With abs(movement), Robot goes backward when RC controller is off
      case 'a'://Turn Left
        turn_L (abs(turn),abs(turn)); // abs function does somehow make the movement more smooth
        break;       
      case 'd'://Turn Right
        turn_R (turn,turn);
        break;
      case 'z':
        Serial.println("Hello");
        break;
      case 'x':
        stop();
        break;
      }
    
    //else stop(); Not needed, if val == -1, go to else, which stops it 
  
   //while loop for Automation
 while (check == 0) //Big while Loop checking automation and manual
{
  ch1 = pulseIn(8, HIGH, 25000);
  ch2 = pulseIn(9, HIGH, 25000);
if (ch1 >= 1800 && ch2 >=1800) //RC is activated
{check = 1; // while loop exits
}
else 
{analogRead(lightPin);
}
Serial.print("channel 1 value:");
Serial.println(ch1);
int sensorValue = analogRead(lightPin);
Serial.print("sensorValue:");
Serial.println(sensorValue);
if (sensorValue >= 850)
{x=1;}
else
{sensorValue=analogRead(lightPin);
}

//Automation
while (x==1) //Medium while loop for automation actions
{ checkButton =0;
//servo90 to collect ping pong balls 
delay(200);
while(checkButton ==0)
{    ch1 = pulseIn(8, HIGH, 25000);
    if (ch1>1800)
    {break;}
  Serial.print("Raising ramp");
  for(pos = 0; pos < 90; pos += 1)  
  {                                  
    servo90_1.write(pos);              
servo90_2.write(90-pos); 
    delay(20);                        
  } 
Serial.print("Raised ramp");
checkButton=1;
y=1;}
    ch1 = pulseIn(8, HIGH, 25000);
    if (ch1>1800)
    {break;}

//movement of robot
while (y==1)
{ ch1 = pulseIn(8, HIGH, 25000);
if (ch1>1800)
{break;}
delay(200);
int buttonState1 = digitalRead(pushButton1);
int buttonState2 = digitalRead(pushButton2);
//Forward towards the top
  if(buttonState1 == 0 && checkButton==1)
  {Serial.print("Advancing to top");
  advance(255,255);
  buttonState1 = digitalRead(pushButton1);
  }
  
    ch1 = pulseIn(8, HIGH, 25000);
    if (ch1>1800)
    {break;}

  if (buttonState1 == 1 && checkButton==1)// hits ledge, reverses and turns RIGHT (gold)
  {Serial.print("Hit ledge!");
  back_off(255,255);
  delay (1000);
  Serial.print("Reversed");
  turn_R(255,255);
  delay (400);
  Serial.print("Adjusted to the RIGHT");
  checkButton = 2;
  buttonState1=digitalRead(pushButton1);
  } 
    ch1 = pulseIn(8, HIGH, 25000);
    if (ch1>1800)
    {break;}  
 

  if (buttonState2 == 1 && checkButton==1)// hits ledge, reverses and turns RIGHT (gold)
  {Serial.print("Hit ledge!");
  back_off(255,255);
  delay (1000);
  Serial.print("Reversed");
  turn_R(255,255);
  delay (400);
  Serial.print("Adjusted to the RIGHT");
  checkButton = 2;
  buttonState1=digitalRead(pushButton1);
  }
 
     ch1 = pulseIn(8, HIGH, 25000);
    if (ch1>1800)
    {break;}
    
 //Forward at an angle
  if(buttonState2 == 0 && checkButton==2)
  {Serial.print("Moving forward");
  advance(255,255);
  buttonState1=digitalRead(pushButton1);
  }
  else if (buttonState2 == 1 && checkButton==2) // hits ledge, reverses and turns RIGHT (gold) towards Tennis Balls
 {Serial.print("Hit ledge!");
   back_off(255,255);
  delay (1000);
  Serial.print("Reversed");
  turn_R(255,255);
  delay (300);
  Serial.print("Adjusted to the RIGHT to face tennis balls");
  checkButton = 3;
  buttonState1=digitalRead(pushButton1) ;
 }
 
     ch1 = pulseIn(8, HIGH, 25000);
    if (ch1>1800)
    {break;}
    
 //Forward to Tennis Balls
 if(checkButton == 3 && buttonState1==0)
  {Serial.print("Moving forward to tennis balls");
  advance(255,255);
  buttonState1=digitalRead(pushButton1);
  buttonState2=digitalRead(pushButton2);
  }
  
      ch1 = pulseIn(8, HIGH, 25000);
    if (ch1>1800)
    {break;}
    
  if (checkButton == 3 && buttonState1 == 1) //hits Tennis Ball
  {Serial.print("Hit the balls!");
    back_off(255,255);
  delay (800);
  Serial.print("Reversed");
  turn_L(255,255);
  delay (400);
  Serial.print("Adjusted to the LEFT to face death star");
  checkButton = 4;
  buttonState2=digitalRead(pushButton2); // faces Death Star
  }
  
      ch1 = pulseIn(8, HIGH, 25000);
    if (ch1>1800)
    {break;}
    
   if (checkButton == 3 && buttonState2 == 1) //hits Tennis Ball
  {Serial.print("Hit the balls!");
    back_off(255,255);
  delay (800);
  Serial.print("Reversed");
  turn_L(255,255);
  delay (400);
  Serial.print("Adjusted to the LEFT to face death star");
  checkButton = 4;
  buttonState2=digitalRead(pushButton2); // faces Death Star
  }
  
      ch1 = pulseIn(8, HIGH, 25000);
    if (ch1>1800)
    {break;}
 
 //INSERT CODE FOR servoGate
 while (checkButton == 4)
 {
  servogate.write(20);
  delay(5000);
  servogate.detach();
  checkButton = 5;
  buttonState2=digitalRead(pushButton2);
  Serial.print("Shot the Torpedoes!");
  
 }
 
     ch1 = pulseIn(8, HIGH, 25000);
    if (ch1>1800)
    {break;}
    
 //Escape from exploding star
 if (checkButton == 5)
 {turn_L(255,255); 
 delay(400);
  Serial.print("Turned LEFT to escape");
 checkButton = 6;
 buttonState2=digitalRead(pushButton2);}
 
     ch1 = pulseIn(8, HIGH, 25000);
    if (ch1>1800)
    {break;}
    
if (checkButton == 6 && buttonState2==0)
{Serial.print("Moving to escape");
  advance (255,255);
buttonState2=digitalRead(pushButton2);
buttonState1=digitalRead(pushButton1);
}

    ch1 = pulseIn(8, HIGH, 25000);
    if (ch1>1800)
    {break;}

if (checkButton == 6 && buttonState1 == 1)
{Serial.print("Hit ledge!");
back_off(255,255);
delay(300);
Serial.print("Reversed");
turn_L(255,255);
delay (400);
Serial.print("Turned LEFT again to escape");
advance(255,255);
delay (1000);
Serial.print("Escaped. Mission complete");
stop();
delay (20000);
}

    ch1 = pulseIn(8, HIGH, 25000);
    if (ch1>1800)
    {break;}

if (checkButton == 6 && buttonState2 == 1)
{Serial.print("Hit ledge!");
back_off(255,255);
delay(300);
Serial.print("Reversed");
turn_L(255,255);
delay (400);
Serial.print("Turned LEFT again to escape");
advance(255,255);
delay (1000);
Serial.print("Escaped. Mission complete");
stop();
delay (20000);
}

    ch1 = pulseIn(8, HIGH, 25000);
    if (ch1>1800)
    {break;}
  
check=1;// while loop exists
}
}
}


  
Serial.print("movement:"); //Serial debugging stuff
Serial.println(movement);
//delay(500);

Serial.print("turn:"); //Serial debugging stuff
Serial.println(turn);
//delay(500);


Serial.println(); //Serial debugging stuff
Serial.println();
Serial.println(); 
  

  
}