/*******************************************************************************
* Program 14.1
* This program controls a left and right motor using left and right IR sensors
* that track a black line.
* The motor should stay off for the first 3 seconds of the program
* The objective of the program is to have the motors track forward
*******************************************************************************/

//Constants for digital I/O motor output pins
int RIGHT_MOTOR = 0;
int LEFT_MOTOR = 1;
int BALL_COLLECTION_MOTOR = 2;
int BALL_DEPOSIT_MOTOR = 3;

//Constants for digital I/O sensor input pins
int RIGHT_LIGHT_SENSOR = 4;
int LEFT_LIGHT_SENSOR = 5;
int BALL_SENSOR = 6;

setup(){
  pinMode(RIGHT_MOTOR, OUTPUT);
  pinMode(LEFT_MOTOR, OUTPUT);
  pinMode(BALL_COLLECTOR_MOTOR, OUTPUT);
  pinMode(BALL_DEPOSIT_MOTOR, OUTPUT);
  pinMode(RIGHT_LIGHT_SENSOR, INPUT);
  pinMode(LEFT_LIGHT_SENSOR, INPUT);

  digitalWrite(RIGHT_MOTOR, HIGH);  //Make sure the right motor is off
  digitalWrite(LEFT_MOTOR, HIGH);  //Make sure the left motor is off
  delay(300);  //Wait for 3 seconds
}

loop(){
  if(digitalRead(RIGHT_LIGHT_SENSOR))  //If the right sensor sees white...
    digitalWrite(RIGHT_MOTOR, HIGH);     //Run the right motor forward
  if(digitalRead(LEFT_LIGHT_SENSOR))   //If the left sensor sees white...
    digitalWrite(LEFT_MOTOR, HIGH);      //Run the left motor forward
  
  if(!digitalRead(LEFT_LIGHT_SENSOR)   //If the left sensor sees black...
    digitalWrite(LEFT_MOTOR, LOW);       //Stop the left motor
  if(!digitalRead(RIGHT_LIGHT_SENSOR)  //If the right sensor sees black...
    digitalWrite(RIGHT_MOTOR, LOW);      //Stop the right motor
    
  //These could be a lot more simple :)
  //digitalWrite(RIGHT_MOTOR, digitalRead(RIGHT_LIGHT_SENSOR));
  //digitalWrite(LEFT_MOTOR, digitalRead(LEFT_LIGHT_SENSOR));
  
  if(digitalRead(BALL_SENSOR))
    getBall();
  
  if(!digitalRead(LEFT_LIGHT_SENSOR) && !digitalRead(RIGHT_LIGHT_SENSOR))
    shootBalls(2);  //Shoot two balls
}

getBall(){
  //Stop both drive motors
  digitalWrite(RIGHT_MOTOR, LOW);
  digitalWrite(LEFT_MOTOR, LOW);

  digitalWrite(BALL_COLLECTION_MOTOR, HIGH);
  delay(200);
  digitalWrite(BALL_COLLECTION_MOTOR, LOW);
  
  //The motors should start up when we go back to the loop...
}

shootBalls(int count){
  //Drive forward for a bit o compensate for the offset sensors...
  digitalWrite(RIGHT_MOTOR, HIGH);
  digitalWrite(LEFT_MOTOR, HIGH);
  delay(100);    //1 second delay

  //Turn right until we cross the line
  while(digitalRead(LEFT_LIGHT_SENSOR)){
    digitalWrite(RIGHT_MOTOR, HIGH);
    digitalWrite(LEFT_MOTOR, LOW);  //How do we reverse?
  }
  
  /*
  //Turn left until we cross the line
  while(digitalRead(RIGHT_LIGHT_SENSOR)){
    digitalWrite(RIGHT_MOTOR, LOW);  //How do we reverse?
    digitalWrite(LEFT_MOTOR, HIGH);
  }
  */

  //Stop both motors
  digitalWrite(RIGHT_MOTOR, LOW);
  digitalWrite(LEFT_MOTOR, LOW);

  //Shoot balls
  digitalWrite(BALL_DEPOSIT_MOTOR, HIGH);
  delay(50*count);   //Shoots the specified number of balls
  digitalWrite(BALL_DEPOSIT_MOTOR, LOW);
  
}