#define START_LIGHT_PORT 31
#define FRONT_SERVO 	  3
#define BACK_SERVO 	  1
#define ARM_SERVO         2
#define LEFT_MOTOR_1 	  0
#define LEFT_MOTOR_2 	  1
#define RIGHT_MOTOR_1 	  2
#define RIGHT_MOTOR_2 	  3
#define ENCODER_1	  7
#define ENCODER_2	  8

#define SERVO_TURNING		950
#define FRONT_SERVO_STRAIGHT 	1100
#define BACK_SERVO_STRAIGHT 	3200
#define FRONT_SERVO_SIDEWAYS 	3700
#define BACK_SERVO_SIDEWAYS 	400
#define ARM_SERVO_DOWN    	1000
#define ARM_SERVO_UP            4000
#define BLUE		  0
#define WHITE		  1

#define FRONT_IR 15
#define LEFT_IR 12
#define RIGHT_IR 14
#define BACK_IR 13

#define START_LIGHT_THRESHOLD	70
#define FL_LIGHT	 29
#define FL_THRESHOLD	 25

#define FR_LIGHT	 28
#define FR_THRESHOLD	 22

#define BL_LIGHT	 26
#define BL_THRESHOLD	 50

#define BR_LIGHT	 25
#define BR_THRESHOLD	 30

int lastServoPos1 = -1;
int lastServoPos2 = -1;
int ticksPer90 =  18;

void setup() 
{
  ao();
  enable_encoder(ENCODER_1);
  enable_encoder(ENCODER_2);
  move_servo(FRONT_SERVO,FRONT_SERVO_STRAIGHT);
  move_servo(BACK_SERVO,BACK_SERVO_STRAIGHT);
  move_servo(ARM_SERVO,ARM_SERVO_UP);
  enable_servos();
 
}


void drive(int speed)
{
	motor(LEFT_MOTOR_1,speed);
	motor(LEFT_MOTOR_2,speed);
	motor(RIGHT_MOTOR_1,speed);
	motor(RIGHT_MOTOR_2,speed);
}

void front_drive(int speed)
{
	motor(LEFT_MOTOR_1,speed);
	motor(LEFT_MOTOR_2,speed);
}

void back_drive(int speed)
{
	motor(RIGHT_MOTOR_1,speed);
	motor(RIGHT_MOTOR_2,speed);
}
void move_servo(int port, int pos) 
{
	servo(port, pos);
	if (port==FRONT_SERVO) {
		sleep((float)(abs(lastServoPos1 - pos)) / 7500.0);
		lastServoPos1 = pos;
	}
	if (port==BACK_SERVO) {
		sleep((float)(abs(lastServoPos2 - pos)) / 7500.0);
  		lastServoPos2 = pos;
  	}
  	
}

void right(int angle) {
  	int dest = (angle * ticksPer90)/90;
  	move_servo(BACK_SERVO,BACK_SERVO_STRAIGHT);
  	move_servo(FRONT_SERVO,FRONT_SERVO_STRAIGHT+SERVO_TURNING);
  	reset_encoder(ENCODER_1);
	reset_encoder(ENCODER_2);
  	drive(80);
  	while ((read_encoder(ENCODER_1) < dest) ||
	       (read_encoder(ENCODER_2) < dest)) {
    	sleep(0.02);
  }
  drive(0);
}


void left(int angle) {
  	int dest = (angle * ticksPer90)/90;
  	move_servo(BACK_SERVO,BACK_SERVO_STRAIGHT);
  	move_servo(FRONT_SERVO,FRONT_SERVO_STRAIGHT-SERVO_TURNING);
  	reset_encoder(ENCODER_1);
	reset_encoder(ENCODER_2);
  	drive(80);
    	while ((read_encoder(ENCODER_1) < dest) ||
	       (read_encoder(ENCODER_2) < dest)) {
   	 sleep(0.02);
  	}
  drive(0);
}

void back_right(int angle) {
  	int dest = (angle * ticksPer90)/90;
  	move_servo(FRONT_SERVO,FRONT_SERVO_STRAIGHT);
  	move_servo(BACK_SERVO,BACK_SERVO_STRAIGHT-SERVO_TURNING);
  	reset_encoder(ENCODER_1);
	reset_encoder(ENCODER_2);
  	drive(-80);
  	while ((read_encoder(ENCODER_1) < dest) ||
	       (read_encoder(ENCODER_2) < dest)) {
    	sleep(0.02);
  }
  drive(0);
}


void back_left(int angle) {
  	int dest = (angle * ticksPer90)/90 - 3;
  	move_servo(FRONT_SERVO,FRONT_SERVO_STRAIGHT);
  	move_servo(BACK_SERVO,BACK_SERVO_STRAIGHT+SERVO_TURNING);
  	reset_encoder(ENCODER_1);
	reset_encoder(ENCODER_2);
  	drive(-80);
    	while ((read_encoder(ENCODER_1) < dest) || 
	       (read_encoder(ENCODER_2) < dest)) {
   	 sleep(0.02);
  	}
  drive(0);
}


void orient() {
  int not_oriented;
  not_oriented=1;
  while(not_oriented) {
	if (((analog(FL_LIGHT) > FL_THRESHOLD) && 
	     (analog(FR_LIGHT) < FR_THRESHOLD) && 
	     (analog(BL_LIGHT) > BL_THRESHOLD) && 
	     (analog(BR_LIGHT) > BR_THRESHOLD)) ||
	    ((analog(FL_LIGHT) < FL_THRESHOLD) && 
	     (analog(FR_LIGHT) > FR_THRESHOLD) && 
	     (analog(BL_LIGHT) < BL_THRESHOLD) && 
	     (analog(BR_LIGHT) < BR_THRESHOLD)))
	{
		forward_start();
		not_oriented=0;
		
	} 
	if (
		((analog(FL_LIGHT) > FL_THRESHOLD) && 
		(analog(FR_LIGHT) > FR_THRESHOLD) && 
		(analog(BL_LIGHT) < BL_THRESHOLD) && 
		(analog(BR_LIGHT) > BR_THRESHOLD)) ||
		((analog(FL_LIGHT) < FL_THRESHOLD) && 
		(analog(FR_LIGHT) < FR_THRESHOLD) && 
		(analog(BL_LIGHT) > BL_THRESHOLD) && 
		(analog(BR_LIGHT) < BR_THRESHOLD))
	    )
	{
		backward_start();
		not_oriented=0;
	} 
	if (((analog(FL_LIGHT) > FL_THRESHOLD) && 
		(analog(FR_LIGHT) > FR_THRESHOLD) && 
		(analog(BL_LIGHT) > BL_THRESHOLD) && 
		(analog(BR_LIGHT) < BR_THRESHOLD) ) ||
		((analog(FL_LIGHT) < FL_THRESHOLD) && 
		(analog(FR_LIGHT) < FR_THRESHOLD) && 
		(analog(BL_LIGHT) < BL_THRESHOLD) && 
		(analog(BR_LIGHT) > BR_THRESHOLD) ))
	{
		left_start();
		not_oriented=0;
	}
	if (((analog(FL_LIGHT) < FL_THRESHOLD) && 
		(analog(FR_LIGHT) > FR_THRESHOLD) && 
		(analog(BL_LIGHT) > BL_THRESHOLD) && 
		(analog(BR_LIGHT) > BR_THRESHOLD) ) ||
		((analog(FL_LIGHT) > FL_THRESHOLD) && 
		(analog(FR_LIGHT) < FR_THRESHOLD) && 
		(analog(BL_LIGHT) < BL_THRESHOLD) && 
		(analog(BR_LIGHT) < BR_THRESHOLD) ))
	{
		right_start();
		not_oriented=0;
	} 
  }
}

void forward_start()
{
  right_sideways(20);
  sleep(.25);
  forward(0);
  sleep(.25);
  forward(27);
  sleep(.25);
  back_right(100);
  get_ball();
}

void backward_start()
{
	left_sideways(20);
	sleep(.25);
	backward(0);
	sleep(.25);
	backward(2);
	sleep(.25);
	back_left(100);
        get_ball();
}

void right_start()
{
  float t;
  move_servo(FRONT_SERVO, (FRONT_SERVO_STRAIGHT + 115));
  drive(100);
  while( !digital(9) && (analog(20) > 100) ) { }
  ao();
  move_servo(ARM_SERVO,ARM_SERVO_DOWN);
  forward(1);
  sleep(2.0);
  move_servo(BACK_SERVO, (BACK_SERVO_STRAIGHT - 200));
  drive(-100);
  t=seconds();
  while((seconds() - t) < 10.0) { }
  ao();
}

void left_start()
{
        backward(0);
	sleep(.25);
	backward(20);
	sleep(.25);
	right_sideways(0);
	sleep(.25);
	right_sideways(12);
	get_ball();
}


int read_color()
{	int color;
	if ((analog(FL_LIGHT) > FL_THRESHOLD) && 
		(analog(FR_LIGHT) > FR_THRESHOLD) &&
		(analog(BL_LIGHT) > BL_THRESHOLD) &&
		(analog(BR_LIGHT) > BR_THRESHOLD))
		color=BLUE;
	if ((analog(FL_LIGHT) < FL_THRESHOLD) && 
		(analog(FR_LIGHT) < FR_THRESHOLD) &&
		(analog(BL_LIGHT) < BL_THRESHOLD) &&
		(analog(BR_LIGHT) < BR_THRESHOLD))
		color=WHITE;	
	return color;
}

void right_sideways(int dist)
{
	move_servo(FRONT_SERVO,FRONT_SERVO_SIDEWAYS);
	move_servo(BACK_SERVO,BACK_SERVO_SIDEWAYS);
	reset_encoder(ENCODER_1);
	reset_encoder(ENCODER_2);
	sleep(.5);
	front_drive(100);
	back_drive(-100);
	while ((read_encoder(ENCODER_1) < dist) ||
	       (read_encoder(ENCODER_2) < dist))
	{
		sleep(0.02);
	}
	drive(0);
}

void left_sideways(int dist)
{
	move_servo(FRONT_SERVO,FRONT_SERVO_SIDEWAYS);
	move_servo(BACK_SERVO,BACK_SERVO_SIDEWAYS);
	reset_encoder(ENCODER_1);
	reset_encoder(ENCODER_2);
	sleep(.5);
	front_drive(-100);
	back_drive(100);
	while ((read_encoder(ENCODER_1) < dist) ||
	       (read_encoder(ENCODER_2) < dist))
	{
		sleep(0.02);
	}
	drive(0);
}

	
void backward(int dist) 
{
	move_servo(FRONT_SERVO,FRONT_SERVO_STRAIGHT);
	move_servo(BACK_SERVO,BACK_SERVO_STRAIGHT);
  	reset_encoder(ENCODER_1);
        reset_encoder(ENCODER_2);
	sleep(.5);
  	drive(-100);
  	while ((read_encoder(ENCODER_1) < dist) || 
	       (read_encoder(ENCODER_2) < dist))
  	{
    		sleep(0.02);
	}
    	drive(0);
}

void forward(int dist) 
{
  	move_servo(FRONT_SERVO,FRONT_SERVO_STRAIGHT);
  	move_servo(BACK_SERVO,BACK_SERVO_STRAIGHT);
  	reset_encoder(ENCODER_1);
	reset_encoder(ENCODER_2);
	sleep(.5);
  	drive(100);
  	while ((read_encoder(ENCODER_1) < dist) || 
	       (read_encoder(ENCODER_2) < dist))  
  	{
    		sleep(0.02);
  	}
  	drive(0);
}

void get_ball() 
{
  float t;

        backward(0);
	sleep(.25);
	drive(-80);
	t=seconds();
	while ((analog(18) > 100) && ((seconds()-t) < 5.0)) { }
	read_color();
	right(4);
	forward(0);
	left_sideways(0);
	sleep(.25);
	left_sideways(1);
	front_drive(-80);
	back_drive(80);
	t=seconds();
	while ((analog(16) > 100 ) && ((seconds()-t < 5.0))) { }
	right_sideways(2);
	forward(0);
	sleep(.25);
	forward(1);
	drive(100);
	t=seconds();
	while((!digital(9) || (analog(20) > 100) ) && ((seconds()-t) < 5.0)) { }
	ao();
	move_servo(ARM_SERVO,ARM_SERVO_DOWN);
	t=seconds();
        forward(50);
}


void main() {
	/* don't transmit yet! */
	ir_transmit_off();
	
	/* align with start light and go! */
	start_machine(START_LIGHT_PORT);	
	printf("GO!!!\n");
	
	/* Start of code */
	setup();
	/*while(!start_button()) {} */
	
	orient();
	/*	get_ball(); */
	/*patrol();*/
	/*make_last_point();*/
	/*	forward(35);
		move_servo(2,3990);*/

}


void left_right()
{
  float t;
  int diff,a,b;
  front_drive(-80);
  back_drive(80);
  reset_encoder(ENCODER_1);
  diff=1;
  while(1)
  {
    a=read_encoder(ENCODER_1);
    if (analog(16) < 100)
    {
      front_drive(80);
      back_drive(-80);
    }
    if (analog(18) < 100)
    { 
      drive(-80);
      front_drive(-80);
      back_drive(80);
    }
    /*sleep(1.0);*/
    b=read_encoder(ENCODER_1);
    diff=b-a;
  }
}


void patrol( )
{ 
  int pid;
  float t;
  left_sideways(0);
  move_servo(FRONT_SERVO, (FRONT_SERVO_SIDEWAYS + 25));
  move_servo(BACK_SERVO, (BACK_SERVO_SIDEWAYS - 150));
  pid=start_process(left_right());
  while (analog(distance_sensor) < 75) { }
  kill_process(pid);
  ao();
  forward(1);
  motor(4,100);
  motor(5,100);
  drive(-80);
  t=seconds();
  while (((analog(22) > 50) || (analog (23) > 50)) && ( (seconds()-t) < 10.0)) { }
  forward(1);
  drive(50);
  if (read_color()==BLUE) 
  {		  
     while (analog(START_LIGHT_PORT) > START_LIGHT_THRESHOLD)
     {
        sleep(0.02);
      }      
   }
   if (read_color()==WHITE)
   {
      while (analog(START_LIGHT_PORT) < START_LIGHT_THRESHOLD)
      {
	 sleep(0.02);
       }
   }
   forward(10);
   patrol();

}		

#define left_wall_sensor 16
#define right_wall_sensor 18
#define distance_sensor 4
#define back_tiny_left 22
#define back_tiny_right 23