/* motor ports */
#define LEFT_MOTOR_1		0
#define LEFT_MOTOR_2		1
#define RIGHT_MOTOR_1 		2
#define RIGHT_MOTOR_2 		3
#define LIFTER_MOTOR_1 		4
#define LIFTER_MOTOR_2 		5

/* for timeouts - the maximum amount of time a 90 degree should take. actually
 * well over the expected time */
#define MAX_TURN_T			1.00	

/* servo port and info */
#define SERVO				0
#define SERVO_LEFT			3900
#define SERVO_STRAIGHT		2010
#define SERVO_RIGHT			100

/* ARM STUFF */
#define ARM_SERVO			5
#define ARM_EXTENDED			1750
#define ARM_RETRACTED			3850

/* SENSOR PORTS */

/* shaft encoder ports */
#define LEFT_ENCODER		7
#define RIGHT_ENCODER		8

/* bump sensor ports */
#define BACK_LEFT_BUMP		9
#define BACK_RIGHT_BUMP		10
#define LEFT_FRONT_BUMP		17
#define RIGHT_FRONT_BUMP	12
#define FRONT_LEFT_BUMP		13
#define FRONT_RIGHT_BUMP	14
#define LEFT_BACK_BUMP		15
#define RIGHT_BACK_BUMP		16

/* light sensor ports */
#define STARTING_LIGHT		31
#define FRONT_LEFT_LIGHT	2
#define FRONT_RIGHT_LIGHT	3
#define FRONT_MID_LIGHT		4
#define BACK_LEFT_LIGHT		5
#define BACK_RIGHT_LIGHT	6
#define MIDDLE_LIGHT		16

/* STARTING ORIENTATIONS W.R.T. OTHER ROBOT */
#define TOWARD				0
#define AWAY				1
#define LEFT				2
#define RIGHT				3

/* what side is ours */
#define BLUE_SIDE			0
#define WHITE_SIDE			1

#define LEFT_FLAG			0
#define RIGHT_FLAG			1

#define LEFT_WALL			0
#define RIGHT_WALL			1

/* the readings we should find from the inner and outer wheels of a turn */
#define ENCODER_RT_90		12
#define ENCODER_RT_90_ST		13
#define ENCODER_LT_90		13
#define ENCODER_LT_90_ST		11
#define ENCODER_180			26
#define ENCODER_180_ST		26
#define ENCODER_270			38
#define ENCODER_360			50

#define true				1
#define false				0


/* global variables */
int which_side; /* which side is ours, i.e. are we white or blue */
int servo_pos = SERVO_STRAIGHT;
int threshold = 170;

void extend_arm() {
	servo(ARM_SERVO, ARM_EXTENDED);
}

void retract_arm() {
	servo(ARM_SERVO, ARM_RETRACTED);
}

int is_blue(int sensor_port) {
	if (analog(sensor_port) > threshold) {
		return true;
	}
	return false;
}

int find_orientation() {
	int front_left, front_right, back_left, back_right, i;
	int temp1, temp2, temp3, temp4;
	int diff1, diff2, diff3, diff4;
	which_side = -1;
	i = 0;
	while (which_side == -1) {
		front_left = is_blue(FRONT_LEFT_LIGHT);
		front_right = is_blue(FRONT_RIGHT_LIGHT);
		back_left = is_blue(BACK_LEFT_LIGHT);
		back_right = is_blue(BACK_RIGHT_LIGHT);
		if ((front_left + front_right + back_left + back_right) == 3) {
			which_side = BLUE_SIDE;
			printf("Our side is blue.\n");
		} else if ((front_left + front_right + back_left + back_right) == 1) {
			which_side = WHITE_SIDE;
			printf("Our side is white.\n");
		}
		else {
			printf("I'm confused.\n");
			if (i >= 5000) {
				printf("De-confusing self.\n");
				temp1 = analog(FRONT_LEFT_LIGHT);
				temp2 = analog(FRONT_RIGHT_LIGHT);
				temp3 = analog(BACK_LEFT_LIGHT);
				temp4 = analog(BACK_RIGHT_LIGHT);
				diff1 = max(abs(temp1 - temp2), max(abs(temp2 - temp3), 
								abs(temp1 - temp3)));
				diff2 = max(abs(temp1 - temp2), max(abs(temp2 - temp4), 
								abs(temp1 - temp4)));
				diff3 = max(abs(temp1 - temp4), max(abs(temp4 - temp3), 
								abs(temp1 - temp3)));
				diff4 = max(abs(temp4 - temp2), max(abs(temp2 - temp3), 
								abs(temp4 - temp3)));
				if ((diff1 < diff2) && (diff1 < diff3) && (diff1 < diff4)) {
					if (temp4 > max(temp1, max(temp2, temp3))) {
						which_side = WHITE_SIDE;
						threshold = (temp4 + max(temp1, max(temp2, temp3)))/2;
					} else {
						which_side = BLUE_SIDE;
						threshold = (temp4 + min(temp1, min(temp2, temp3)))/2;
					}
					return LEFT;
				} else if ((diff2 < diff1) && (diff2 < diff3) && 
								(diff2 < diff4)) {
					if (temp3 > max(temp1, max(temp2, temp4))) {
						which_side = WHITE_SIDE;
						threshold = (temp3 + max(temp1, max(temp2, temp4)))/2;
					} else {
						which_side = BLUE_SIDE;
						threshold = (temp3 + min(temp1, min(temp2, temp4)))/2;
					}
					return AWAY;
				} else if ((diff3 < diff1) && (diff3 < diff2) && 
								(diff3 < diff4)) {
					if (temp2 > max(temp1, max(temp4, temp3))) {
						which_side = WHITE_SIDE;
						threshold = (temp2 + max(temp1, max(temp4, temp3)))/2;
					} else {
						which_side = BLUE_SIDE;
						threshold = (temp2 + min(temp1, min(temp4, temp3)))/2;
					}
					return TOWARD;
				} else {
					if (temp1 > max(temp2, max(temp3, temp4))) {
						which_side = WHITE_SIDE;
						threshold = (temp1 + max(temp4, max(temp2, temp3)))/2;
					} else {
						which_side = BLUE_SIDE;
						threshold = (temp1 + min(temp4, min(temp2, temp3)))/2;
					}
					return RIGHT;
				}
			}
			i++;
		}
	}
	if ((front_left == front_right) && (front_right  == back_left)) {
		return LEFT;
	} else if ((front_left == front_right) && (front_right == back_right)) {
		return AWAY;
	} else if ((back_left == back_right) && (back_left == front_right)) {
		return RIGHT;
	} else {
		return TOWARD;
	}
}

void fix_orientation(int orientation) {
	if (orientation == LEFT) {
		turn_around_st();
	} else if (orientation == AWAY) {
		turn_left_90_st();
	} else if (orientation == TOWARD) {
		turn_right_90_st();
	} else { /* orientation == RIGHT */
		move(-100);
		sleep(.2);
		stop();
		move(100);
		sleep(.2);
		stop();
	}
}

void reset_encoders() {
	reset_encoder(LEFT_ENCODER);
	reset_encoder(RIGHT_ENCODER);
}

void enable_encoders() {
	enable_encoder(LEFT_ENCODER);
	enable_encoder(RIGHT_ENCODER);
}

void read_encoders(int* left, int* right) {
	*left = read_encoder(LEFT_ENCODER);
	*right = read_encoder(RIGHT_ENCODER);
}

int roller_bumper(int which_wall) {
	if (which_wall == LEFT_WALL) {
		return LEFT_FRONT_BUMP;
	}
	return RIGHT_FRONT_BUMP;
}

 
void ram() {
	stop();
	servo_center();
	move(-100);
	sleep(.5);
	stop();
	move(100);
	sleep(.5);
	stop();
}

void wall_follow(int which_wall) {
	long last_stall_check;
	int first_time_thru = true;
	/*
	int left_enc = 0;
	int right_enc = 0;
	int curr_left;
	int curr_right;
	*/
	reset_encoders();
	move(100);
	last_stall_check = mseconds();
	while (!at_wall()) {
		if (((mseconds() - last_stall_check) >= 20L) && !first_time_thru) {
			/*
			read_encoders(&curr_left, &curr_right);
			*/
			if ((left_stalled && (which_wall == LEFT_WALL) && !right_stalled && !stopped) ||
				(right_stalled && (which_wall == RIGHT_WALL) && !left_stalled && !stopped)) {
				flush(which_wall);
			} else if ((left_stalled || right_stalled) && !stopped) {
				ram();
			}
			/*
			left_enc = curr_left;
			right_enc = curr_right;
			*/
			move(100);
			last_stall_check = mseconds();
		}
		first_time_thru = false;
		if (!digital(roller_bumper(which_wall))) {
			printf("I\'m off the wall\n");
			if (which_wall == LEFT_WALL) {
				servo_left(3);
			} else {
				servo_right(4);
			}
		} else {
			printf("I hit the wall\n");
			if (which_wall == LEFT_WALL) {
				servo_center();
			} else {
				servo_center();
			}
		}
	}
	stop();
}

void turn_left() {
	servo_left(90);
	drive_left_half(-100);
	drive_right_half(100);
}

void turn_right() {
	servo_right(90);
	drive_right_half(-100);
	drive_left_half(100);
}

void turn_right_90() {
	float t;
	reset_encoders();
	turn_right();
	t = seconds();
	while ((read_encoder(RIGHT_ENCODER) < ENCODER_RT_90) &&
			(read_encoder(LEFT_ENCODER) < ENCODER_RT_90) && 
			(seconds() - t <= MAX_TURN_T)) {
	}
	stop();
}

void turn_right_90_st() {
	float t;
	reset_encoders();
	turn_right();
	t = seconds();
	while ((read_encoder(RIGHT_ENCODER) < ENCODER_RT_90_ST) &&
			(read_encoder(LEFT_ENCODER) < ENCODER_RT_90_ST) && 
			(seconds() - t <= MAX_TURN_T)) {
	}
	stop();
}

void turn_left_90_st() {
	float t;
	reset_encoders();
	turn_left();
	t = seconds();
	while ((read_encoder(RIGHT_ENCODER) < ENCODER_LT_90_ST) &&
			(read_encoder(LEFT_ENCODER) < ENCODER_LT_90_ST) &&
			(seconds() - t <= MAX_TURN_T)) {
	}
	stop();
}

void turn_left_90() {
	float t;
	reset_encoders();
	turn_left();
	t = seconds();
	while ((read_encoder(RIGHT_ENCODER) < ENCODER_LT_90) &&
			(read_encoder(LEFT_ENCODER) < ENCODER_LT_90) &&
			(seconds() - t <= MAX_TURN_T)) {
	}
	stop();
}

void turn_left_360() {
	float t;
	reset_encoders();
	turn_left();
	t = seconds();
	while ((read_encoder(RIGHT_ENCODER) < ENCODER_360) &&
			(read_encoder(LEFT_ENCODER) < ENCODER_360) &&
			(seconds() - t <= 4.0*MAX_TURN_T)) {
	}
	stop();
}

void turn_around_st() {
	float t;
	reset_encoders();
	turn_right();
	t = seconds();
	while ((read_encoder(RIGHT_ENCODER) < ENCODER_180_ST) &&
			(read_encoder(LEFT_ENCODER) < ENCODER_180_ST) &&
			(seconds() - t <= 2.0*MAX_TURN_T)) {
	}
	stop();
}

void turn_around() {
	float t;
	reset_encoders();
	turn_right();
	t = seconds();
	while ((read_encoder(RIGHT_ENCODER) < ENCODER_180) &&
			(read_encoder(LEFT_ENCODER) < ENCODER_180) &&
			(seconds() - t <= 2.0*MAX_TURN_T)) {
	}
	stop();
}

void turn_around_left() {
	float t;
	reset_encoders();
	turn_left();
	t = seconds();
	while ((read_encoder(RIGHT_ENCODER) < ENCODER_180) &&
			(read_encoder(LEFT_ENCODER) < ENCODER_180) &&
			(seconds() - t <= 2.0*MAX_TURN_T)) {
	}
	stop();
}

void move(int speed) {
	drive_left_half(speed);
	drive_right_half(speed);
}

void flush(int which_wall) {
	servo_center();
	move(-100);
	sleep(.3);
	stop();
	if (which_wall == LEFT_WALL) {
		servo_right(15);
	} else {
		servo_left(15);
	}
	move(50);
	sleep(.6);
	stop();
	servo_center();
}

void drive_left_half(int speed) {
	if (!speed) { 
		left_half_off();
	}
	speed = max(speed, -100);
	speed = min(speed, 100);
	motor(LEFT_MOTOR_1, speed);
	motor(LEFT_MOTOR_2, speed);
	stopped = false;
}

void left_half_off() {
	off(LEFT_MOTOR_1);
	off(LEFT_MOTOR_2);
}

void drive_right_half(int speed) {
	if (!speed) { 
		right_half_off();
	}
	speed = max(speed, -100);
	speed = min(speed, 100);
	motor(RIGHT_MOTOR_1, speed);
	motor(RIGHT_MOTOR_2, speed);
	stopped = false;
}

void right_half_off() {
	off(RIGHT_MOTOR_1);
	off(RIGHT_MOTOR_2);
}

int stopped = true;
void stop() {
	left_half_off();
	right_half_off();
	stopped = true;
	servo_center();
}

void start_lifter() {
	motor(LIFTER_MOTOR_1, 100);
	motor(LIFTER_MOTOR_2, 100);
}

void stop_lifter() {
	off(LIFTER_MOTOR_1);
	off(LIFTER_MOTOR_2);
}

int at_line(int port) {
	if (which_side == BLUE_SIDE) {
		return (is_blue(port));
	}
	return !is_blue(port);
}

int at_our_line(int port) {
	return !at_line(port);
}

/* whether we've just run forward into the wall */
int at_wall() {
	return (digital(FRONT_LEFT_BUMP) && digital(FRONT_RIGHT_BUMP));
/* && (left_stalled && right_stalled && !stopped);*/
}

/* turns the servo to the specified angle, leaving the motors as they are */
void servo_right(int angle) {
	int p = (int)(((float)angle/90.0)*(float)(SERVO_RIGHT-SERVO_STRAIGHT) +(float)SERVO_STRAIGHT);
	servo(SERVO, p);
	sleep( (float)abs(p-servo_pos) / 6000.0);
	servo_pos = p;
}

void servo_left(int angle) {
	int p = (int)(((float)angle/90.0)*(float)(SERVO_LEFT-SERVO_STRAIGHT) +(float)SERVO_STRAIGHT);
	servo(SERVO, p);
	sleep( (float)abs(p-servo_pos) / 6000.0);
	servo_pos = p;
}

void servo_center() {
	servo_right(0);
}

void swivel() {
	move(-100);
	sleep(.2);
	stop();
	turn_left_90();
	turn_around();
	turn_left_90();
}

void line_follow() {
	int speed, flag;
	int front_left, front_mid, front_right, back_left, back_right;
	long t, t1;
	int swiveled = false;
	long begin_time = mseconds();
	speed = 83;
	t1 = 0L;
	move(speed);
	printf("Line following...\n");
	if (at_line(FRONT_LEFT_LIGHT) && !at_line(FRONT_RIGHT_LIGHT)) {
		flag = LEFT_FLAG;
	} else if (at_line(FRONT_RIGHT_LIGHT) && !at_line(FRONT_LEFT_LIGHT)) {
		flag = RIGHT_FLAG;
	}
	while (!at_wall() || (mseconds() - begin_time <= 500L)) {
		/* refresh our variables */
		front_left = at_line(FRONT_LEFT_LIGHT);
		front_mid = at_line(FRONT_MID_LIGHT);
		front_right = at_line(FRONT_RIGHT_LIGHT);
		back_left = at_line(BACK_LEFT_LIGHT);
		back_right = at_line(BACK_RIGHT_LIGHT);
		t = mseconds();
		
		/* timeout in case we get stuck */
		if ((t - begin_time >= 7000L) && !swiveled){
			printf("Swiveling.\n");
			swiveled = true;
			swivel();
			servo_center();
			move(100);
			sleep(1.0);
			break;
		} else if (left_stalled && digital(FRONT_LEFT_BUMP)) {
			t1 = 0L;
			printf("Left Stall\n"); 
			left_half_off();
			servo_left(20);
			drive_right_half(100);
		} else if (right_stalled && digital(FRONT_RIGHT_BUMP)) {
			t1 = 0L;
			printf("Right Stall\n"); 
			right_half_off();
			servo_right(20);
			drive_left_half(100);
		} else if (front_mid && back_left && back_right) {
			t1 = 0L;
			flag = -1;
			servo_center();
			move(speed+33);
		} else if (front_mid && back_left) {
			t1 = 0L;
			flag = -1;
			servo_right(5);
			move(speed+16);
		} else if (front_mid && back_right) {
			t1 = 0L;
			flag = -1;
			servo_left(5);
			move(speed+16);
		} else if (front_mid && (flag != -1)) {
			t1 = 0L;
			if (flag == LEFT_FLAG) {
				right_half_off();
				drive_left_half(speed+16);
				servo_right(30);
			} else if (flag == RIGHT_FLAG) {
				drive_right_half(speed+16);
				left_half_off();
				servo_left(30);
			}
		} else if (front_left && (back_left || back_right)) {
			t1 = 0L;
			flag = -1; 
			servo_left(5);
			move(speed);
		} else if (front_right && (back_right || back_left)) {
			t1 = 0L;
			flag = -1;
			servo_right(5);
			move(speed);
		} else if (front_left && !front_right) {
			t1 = 0L;
			if (flag != RIGHT_FLAG) {
				flag = LEFT_FLAG;
				servo_left(20);
				move(speed);
			}
		} else if (front_right && !front_left) {
			t1 = 0L;
			if (flag != LEFT_FLAG) {
				flag = RIGHT_FLAG;
				servo_right(20);
				move(speed);
			}
		} else if (t1 == 0L) {
			t1 = mseconds();
		} else if ( (mseconds() - t1 >= 1000L) && (servo_pos == SERVO_STRAIGHT)) {
			acquire_line(2000L);
			t1 = 0L;
		}
	}
	stop();
}

int odd(int a) {
	return (a % 2);
}

/* just moves around forward and backward and turns until it gets the middle
 * sensor on the line */
void acquire_line(long max_t) {
	long t;
	float t0;
	int speed = 66;
	int i = 0, j = 0;
	printf("Acquiring Line...\n");
	t0 = seconds();
	while (!at_line(MIDDLE_LIGHT)) {
		while (!at_line(MIDDLE_LIGHT) && (i < 2)) {
			move(-1*speed);
			t = mseconds();
			while (!at_line(MIDDLE_LIGHT) && (mseconds() - t <= max_t)) { }
			stop();
			if (!at_line(MIDDLE_LIGHT)) {
				move(speed);
				t = mseconds();
				while (!at_line(MIDDLE_LIGHT) && (mseconds() - t <= 2L*max_t)) { }
				stop();
				if (!at_line(MIDDLE_LIGHT)) {
					move(-1 * speed);
					msleep(max_t);
					stop();
					if (odd(j)) {
						turn_right();
					} else {
						turn_left();
					}
					sleep(.3);
					stop();
				}
			}
			stop();
			i++;
		}
		i = 0;
		j++;
		max_t = max_t + 500L; 
	}
}

void position_on_line(int right_first) {
	long t, max_t;
	int keep_trying = true;
	max_t = 300L;
	printf("Positioning self on line.\n");
	while (keep_trying) {
		if (!at_line(MIDDLE_LIGHT)) {
			acquire_line(500L);
		}
		if (right_first) {
			turn_right();
		} else {
			turn_left();
		}
		t = mseconds();
		while (!(((at_line(FRONT_LEFT_LIGHT) || at_line(FRONT_MID_LIGHT)) || 
			at_line(FRONT_RIGHT_LIGHT)) && (at_line(BACK_LEFT_LIGHT) || 
			at_line(BACK_RIGHT_LIGHT))) && (mseconds() - t <= max_t)) { }
		stop();
		if (!(((at_line(FRONT_LEFT_LIGHT) || at_line(FRONT_MID_LIGHT)) || 
			at_line(FRONT_RIGHT_LIGHT)) && (at_line(BACK_LEFT_LIGHT) || 
			at_line(BACK_RIGHT_LIGHT)))) { 
			if (right_first) {
				turn_left();
			} else {
				turn_right();
			}
			t = mseconds();
			while (!(((at_line(FRONT_LEFT_LIGHT) || at_line(FRONT_MID_LIGHT)) || 
				at_line(FRONT_RIGHT_LIGHT)) && (at_line(BACK_LEFT_LIGHT) || 
				at_line(BACK_RIGHT_LIGHT))) && (mseconds() - t <= 2L*max_t)) { }
			stop();
			if (!(((at_line(FRONT_LEFT_LIGHT) || at_line(FRONT_MID_LIGHT)) || 
				at_line(FRONT_RIGHT_LIGHT)) && (at_line(BACK_LEFT_LIGHT) || 
				at_line(BACK_RIGHT_LIGHT)))) { 
				if (right_first) {
					turn_right();
				} else {
					turn_left();
				}
				msleep(max_t);
				stop();
			}
		}
		if (((at_line(FRONT_LEFT_LIGHT) || at_line(FRONT_MID_LIGHT)) || 
			at_line(FRONT_RIGHT_LIGHT)) && (at_line(BACK_LEFT_LIGHT) || 
			at_line(BACK_RIGHT_LIGHT))) { 
			if (!at_line(FRONT_MID_LIGHT)) {
				if (at_line(FRONT_LEFT_LIGHT)) {
					servo_left(90);
					drive_left_half(-50);
					drive_right_half(50);
				} else {
					servo_right(90);
					drive_left_half(50);
					drive_right_half(-50);
				}
				t = mseconds();
				while (!at_line(FRONT_MID_LIGHT) && (mseconds() - t <= 50L) &&
					(at_line(BACK_LEFT_LIGHT) || at_line(BACK_RIGHT_LIGHT))) { }
				stop();
			}
			keep_trying = false;
		}
		if (keep_trying) { max_t = max_t + 200L; }
	}
	stop();
}

void turn_around_on_line() {
	long t;
	int flag, change;
	printf("Turning around on the line.\n");
	if (!at_line(FRONT_MID_LIGHT) && !at_line(FRONT_LEFT_LIGHT) && 
		!at_line(FRONT_RIGHT_LIGHT) && !at_line(MIDDLE_LIGHT)) {
		turn_around();
		acquire_line(1000L);
	} else {
		turn_right();
		t = mseconds();
		sleep(.5);
		while (!at_line(FRONT_MID_LIGHT) && (mseconds() - t <= 1200L)) { }
		acquire_line(500L);
	}
	/* MIDDLE_LIGHT should be on the line now. */
	position_on_line(true);
}

void timebomb() {
	sleep(60.0);
	ir_transmit_off();
	ao();
	disable_servos();
	while (true) {
		hog_processor();
	}
}

void sentry() {
	line_follow();
	move(-100);
	sleep(.2);
	stop();
}

void dodge() {
	if (stopped) {
		return;
	}
	if (left_stalled && right_stalled) {
		ram();
	} else if (left_stalled) {
		move(-100);
		sleep(.4);
		stop();
		servo_right(10);
		move(100);
		sleep(.4);
		stop();
	} else { /* right_stalled */
		move(-100);
		sleep(.4);
		stop();
		servo_right(10);
		move(100);
		sleep(.4);
		stop();
	}
}

void go_home(int on_our_wall) {
	long t;
	if (!on_our_wall) {
		move(100);
		servo_right(3);
		while (!at_wall()) { }
		stop();
		move(-100);
		sleep(.2);
		stop();
		turn_right_90();
	}
	servo_left(2);
	move(100);
	t = mseconds();
	while (!(at_line(BACK_LEFT_LIGHT) && at_line(FRONT_LEFT_LIGHT)) && 
		(mseconds() - t <= 5000L)) { }
	stop();
	servo_right(30);
	move(100);
	sleep(.4);
	stop();
}
 
void setup() {
	enable_servos();
	enable_encoders();
	servo_center();
	retract_arm();
}

void calibrate() {
	int dark = 255, light = 0, i, retry;
	retry = true;
	while (retry) {
		printf("Put on dark and hit START\n");
		start_press();
		printf("Move back.\n");
		sleep(1.0);
		for (i=1; i<=10; i++) {
			dark = min(dark, analog(FRONT_LEFT_LIGHT));
			dark = min(dark, analog(FRONT_MID_LIGHT));
			dark = min(dark, analog(FRONT_RIGHT_LIGHT));
			dark = min(dark, analog(BACK_LEFT_LIGHT));
			dark = min(dark, analog(BACK_RIGHT_LIGHT));
			dark = min(dark, analog(MIDDLE_LIGHT));
		}
		printf("Lowest dark reading = %d\n", dark);
		sleep(.5);
		printf("Put on light and hit START\n");
		start_press();
		printf("Move back.\n");
		sleep(1.0);
		for (i=1; i<=10; i++) {
			light = max(light, analog(FRONT_LEFT_LIGHT));
			light = max(light, analog(FRONT_MID_LIGHT));
			light = max(light, analog(FRONT_RIGHT_LIGHT));
			light = max(light, analog(BACK_LEFT_LIGHT));
			light = max(light, analog(BACK_RIGHT_LIGHT));
			light = max(light, analog(MIDDLE_LIGHT));
		}
		printf("Highest light reading = %d\n", light);
		sleep(.5);
		if (light > dark) {
			printf("Light readings > dark readings. Try again.\n");
			sleep(.5);
		} else {
			printf("Set threshold to %d?  START = yes.\n", (light+dark)/2);
			while (true) {
				if (start_button()) {
					retry = false;
					threshold = (light+dark)/2;
					break;
				} else if (stop_button()) {
					break;
				}
			}
			while (start_button()) { }
			while (stop_button()) { }
		}
	}
}
	

void show_readings() {
	int recalibrate = false;
	while (true) {
		printf("%d, %d, %d, %d\n", analog(FRONT_LEFT_LIGHT), 
			analog(FRONT_RIGHT_LIGHT), analog(BACK_LEFT_LIGHT), 
			analog(BACK_RIGHT_LIGHT));
		sleep(.1);
		if (start_button()) {
			recalibrate = true;
			break;
		} else if (stop_button()) {
			recalibrate = false;
			break;
		}
	}
	while (stop_button()) { }
	while (start_button()) { }
	if (recalibrate) {
		calibrate();
	}
}

int left_stalled = 0;
int right_stalled = 0;
void stall_checker() {
	int left, right, left_old, right_old;
	read_encoders(&left, &right);
	while (true) {
		sleep(.3);
		left_old = left;
		right_old = right;
		read_encoders(&left, &right);
		left_stalled = (left == left_old);
		right_stalled = (right == right_old);
	}
}

void reverse_to_their_line() {
	float t;
	move(-66);
	t = seconds();
	while(!at_line(MIDDLE_LIGHT)  && (seconds() - t <= 3.0)) { }
	stop();
}

void reverse_to_our_line() {
	float t;
	move(-66);
	t = seconds();
	while(!at_our_line(MIDDLE_LIGHT)  && (seconds() - t <= 3.0)) { }
	stop();
}

void show_time() {
	float t0 = seconds();
	while (true) {
		printf("%d\n", (int)(seconds() - t0));
		sleep(1.0);
	}
}

void turn_right_270() {
	float t;
	reset_encoders();
	turn_right();
	t = seconds();
	while ((read_encoder(RIGHT_ENCODER) < ENCODER_270) &&
			(read_encoder(LEFT_ENCODER) < ENCODER_270) &&
			(seconds() - t <= 3.0*MAX_TURN_T)) {
	}
	stop();
}

void knock_ball_off() {
	extend_arm();
	sleep(.3);
	turn_around_left();
	retract_arm();
	turn_right_90();
	move(-100);
	sleep(.25);
	stop();
	turn_right_270();
}

void home_to_line(int speed) {
	float t;
	move(speed);
	t = seconds();
	sleep(150.0/((float)speed));
	while (!at_line(MIDDLE_LIGHT) && (seconds() - t <= 8.0)) { }
	if (seconds() - t > 8.0) {
		acquire_line(1500L);
	}
	stop();
}

void main() {
	int stall_pid, time_pid, on_our_wall, orientation;
	float start_time, t;
	int i = 0;

	ir_transmit_off();
	enable_servos();
	retract_arm();
	disable_servos();
	show_readings();
	printf("Turn batteries on!!!\n");
	stop_press();

	start_machine(STARTING_LIGHT);

	ir_transmit_on();
	start_time = seconds();
	stall_pid  = start_process(stall_checker());
	time_pid = start_process(show_time());

	setup();
	fix_orientation(find_orientation());
	start_lifter();

	servo_right(1);
	home_to_line(100);	
	/* angle away and become flush to back wall */
	servo_left(10);
	move(83);
	t = seconds();
	while (!at_wall() && (seconds() - t <= 3.0)) {
		if (digital(FRONT_LEFT_BUMP)) {
			left_half_off();
			drive_right_half(100);
			servo_left(20);
		} else if (digital(FRONT_RIGHT_BUMP)) {
			right_half_off();
			drive_left_half(100);
			servo_right(20);
		}
	}
	stop();
	/* Reverse and acquire line */
	reverse_to_their_line();
	turn_left();
	t = seconds();
	sleep(.3);
	while (!at_line(BACK_LEFT_LIGHT) && !at_line(BACK_RIGHT_LIGHT) &&
		(seconds() - t <= .8)) { }
	stop();
	/* sentry */
	while (true) {
		on_our_wall = true;
		sentry();
		turn_around_on_line();
		sentry();
		turn_right_90();
		go_home(on_our_wall);
		t = seconds() - start_time;
		while (((i == 0) && (t < 24.0)) || ((i == 1) && (t < 48.0))) {
			t = seconds() - start_time;
		}
		knock_ball_off();
		servo_left(4);
		home_to_line(100);
		acquire_line(400L);
		turn_left();
		t = seconds();
		while (!(((at_line(FRONT_LEFT_LIGHT) || at_line(FRONT_MID_LIGHT)) || 
			at_line(FRONT_RIGHT_LIGHT)) && (at_line(BACK_LEFT_LIGHT) || 
			at_line(BACK_RIGHT_LIGHT))) && (seconds() - t <= 1.0)) { }
		stop();
		i++;
	}

	ir_transmit_off();
	printf("%d\n", (int)(seconds() - start_time));
	kill_process(stall_pid);
	kill_process(time_pid);
	stop();
	stop_lifter();
}