void drive_ticks_without (int forward, int ticks)
{
	int left, right;

	reset_encoder(LEFT_MOTOR_ENCODER);
	reset_encoder(RIGHT_MOTOR_ENCODER);
	center_servos();

	while (1) {
		if (forward) {
			fd(LEFT_MOTOR);
			fd(RIGHT_MOTOR);
		} else {
			bk(LEFT_MOTOR);
			bk(RIGHT_MOTOR);
		}

		left = read_encoder(LEFT_MOTOR_ENCODER);
		right = read_encoder(RIGHT_MOTOR_ENCODER);
		if (left > ticks || right > ticks) {
			break;
		}
	}

	off(LEFT_MOTOR);
	off(RIGHT_MOTOR);
	sleep(0.1);
}

void drive (int forward)
{
	int left, right;

	reset_encoder(LEFT_MOTOR_ENCODER);
	reset_encoder(RIGHT_MOTOR_ENCODER);

	while (1) {
		left = read_encoder(LEFT_MOTOR_ENCODER);
		right = read_encoder(RIGHT_MOTOR_ENCODER);

		if (left - right > 100) {
			if (forward) {
				servo(LEFT_SERVO, LEFT_SERVO_90);
				servo(RIGHT_SERVO, RIGHT_SERVO_45);
				sleep(0.1);
				off(LEFT_MOTOR);
				fd(RIGHT_MOTOR);
				sleep(0.1);
			} else {
				servo(LEFT_SERVO, LEFT_SERVO_90);
				servo(RIGHT_SERVO, RIGHT_SERVO_45);
				sleep(0.1);
				off(LEFT_MOTOR);
				bk(RIGHT_MOTOR);
				sleep(0.1);
			}
		} else if (right - left > 100) {
			if (forward) {
				servo(LEFT_SERVO, LEFT_SERVO_45);
				servo(RIGHT_SERVO, RIGHT_SERVO_90);
				sleep(0.1);
				fd(LEFT_MOTOR);
				off(RIGHT_MOTOR);
				sleep(0.1);
			} else {
				servo(LEFT_SERVO, LEFT_SERVO_45);
				servo(RIGHT_SERVO, RIGHT_SERVO_90);
				sleep(0.1);
				bk(LEFT_MOTOR);
				off(RIGHT_MOTOR);
				sleep(0.1);
			}
		} else {
			center_servos();
			if (forward) {
				fd(LEFT_MOTOR);
				fd(RIGHT_MOTOR);
			} else {
				bk(LEFT_MOTOR);
				bk(RIGHT_MOTOR);
			}
		}
	}
}

void drive_ticks (int forward, int ticks)
{
	int pid;

	reset_encoder(LEFT_MOTOR_ENCODER);
	reset_encoder(RIGHT_MOTOR_ENCODER);
	pid = start_process(drive(forward));
	while (1) {
		if (read_encoder(LEFT_MOTOR_ENCODER) >= ticks && read_encoder(RIGHT_MOTOR_ENCODER) >= ticks) {
			kill_process(pid);
			break;
		}
	}
	off(LEFT_MOTOR);
	off(RIGHT_MOTOR);

	sleep(0.1);
}

void drive_into_wall (int forward, float timeout)
{
	int attempt = 0;
	long start_wait;
	int hit;
	int left = 0, right = 0;
	int driver_pid;
	int active_process = 0;
	int a, b;
	float timeouttime = seconds() + timeout;

	driver_pid = start_process(drive(forward));
	active_process = 1;

	while (!(left && right) && (attempt <= 1)) {
		if (forward) {
			a = get_digital(LEFT_FRONT_BUMPER);
			b = get_digital(RIGHT_FRONT_BUMPER);
		} else {
			a = get_digital(LEFT_REAR_BUMPER);
			b = get_digital(RIGHT_REAR_BUMPER);
		}

		if (seconds() >= timeouttime) {
			kill_process(driver_pid);
			active_process = 0;

			attempt++;
			drive_ticks(!forward, 50);
			driver_pid = start_process(drive(forward));
			active_process = 1;
			left = 0;
			right = 0;
			timeouttime = seconds() + 1.5;
		}

		if (a) {
			left = 1;
		}
		if (b) {
			right = 1;
		}

		if (left && right) {
			kill_process(driver_pid);
			active_process = 0;
			break;
		}

		if (left) {
			kill_process(driver_pid);
			active_process = 0;

			servo(LEFT_SERVO, LEFT_SERVO_90);
			servo(RIGHT_SERVO, RIGHT_SERVO_45);
			sleep(0.1);
			if (forward) {
				off(LEFT_MOTOR);
				fd(RIGHT_MOTOR);
			} else {
				off(LEFT_MOTOR);
				bk(RIGHT_MOTOR);
			}
		}

		if (right) {
			kill_process(driver_pid);
			active_process = 0;

			servo(LEFT_SERVO, LEFT_SERVO_45);
			servo(RIGHT_SERVO, RIGHT_SERVO_90);
			sleep(0.1);
			if (forward) {
				fd(LEFT_MOTOR);
				off(RIGHT_MOTOR);
			} else {
				bk(LEFT_MOTOR);
				off(RIGHT_MOTOR);
			}
		}
	}

	if (active_process) {
		kill_process(driver_pid);
	}
	center_servos();
	off(LEFT_MOTOR);
	off(RIGHT_MOTOR);
}

void drive_into_line (int forward, float timeout)
{
	int attempt = 0;
	long start_wait;
	int hit;
	int left = 0, right = 0;
	int driver_pid;
	int active_process = 0;
	int a, b;
	float timeouttime = seconds() + timeout;

	driver_pid = start_process(drive(forward));
	active_process = 1;

	while (!(left && right) && (attempt <= 1)) {
		if (forward) {
			a = (get_analog(LEFT_FRONT_LIGHT, 3) > THRESHOLD_1);
			b = (get_analog(RIGHT_FRONT_LIGHT, 3) > THRESHOLD_2);
		} else {
			a = (get_analog(LEFT_REAR_LIGHT, 3) > THRESHOLD_3);
			b = (get_analog(RIGHT_REAR_LIGHT, 3) > THRESHOLD_4);
		}

		if (seconds() >= timeouttime) {
			kill_process(driver_pid);
			active_process = 0;

			attempt++;
			drive_ticks(!forward, 50);
			driver_pid = start_process(drive(forward));
			active_process = 1;
			left = 0;
			right = 0;
			timeouttime = seconds() + 1.5;
		}

		if (a) {
			left = 1;
		}
		if (b) {
			right = 1;
		}

		if (left && right) {
			kill_process(driver_pid);
			active_process = 0;
			break;
		}

		if (left) {
			kill_process(driver_pid);
			active_process = 0;

			servo(LEFT_SERVO, LEFT_SERVO_90);
			servo(RIGHT_SERVO, RIGHT_SERVO_45);
			sleep(0.1);
			if (forward) {
				off(LEFT_MOTOR);
				fd(RIGHT_MOTOR);
			} else {
				off(LEFT_MOTOR);
				bk(RIGHT_MOTOR);
			}
		}

		if (right) {
			kill_process(driver_pid);
			active_process = 0;

			servo(LEFT_SERVO, LEFT_SERVO_45);
			servo(RIGHT_SERVO, RIGHT_SERVO_90);
			sleep(0.1);
			if (forward) {
				fd(LEFT_MOTOR);
				off(RIGHT_MOTOR);
			} else {
				bk(LEFT_MOTOR);
				off(RIGHT_MOTOR);
			}
		}
	}

	if (active_process) {
		kill_process(driver_pid);
	}
	center_servos();
	off(LEFT_MOTOR);
	off(RIGHT_MOTOR);
}

void drive_into_ball (float timeout)
{
	int driver_pid;
	float timeouttime = seconds() + timeout;

	driver_pid = start_process(drive(1));

	while (1) {
		if (seconds() >= timeouttime) {
			kill_process(driver_pid);
			break;
		}

		if (GRABBING_BALL) {
			kill_process(driver_pid);

			driver_pid = start_process(drive(0));
			sleep(0.7);
			kill_process(driver_pid);

			off(LEFT_MOTOR);
			off(RIGHT_MOTOR);
			while (GRABBING_BALL) {
			}
			break;
		}
	}

 	center_servos();
	off(LEFT_MOTOR);
	off(RIGHT_MOTOR);
}

void inch_to_ball ()
{
	float starttime = seconds();

	while (1) {
		drive_ticks(1, 10);

		sleep(0.1);

		if (GRABBING_BALL) {
			break;
		}
	}
}

void shimmy_onto_line (int forward)
{
	while (1) {
		if (forward) {
			if (get_analog(LEFT_REAR_LIGHT, 3) < THRESHOLD_3 && get_analog(RIGHT_REAR_LIGHT, 3) < THRESHOLD_4) {
				return;
			} else if (get_analog(LEFT_REAR_LIGHT, 3) > THRESHOLD_3) {
				backup_right(15);
				sleep(0.1);
				drive_ticks_without(0,30);
				sleep(0.1);
				backup_left(15);
				sleep(0.1);
				drive_into_wall(1, 5.0);
			} else {
				backup_left(15);
				sleep(0.1);
				drive_ticks_without(0,30);
				sleep(0.1);
				backup_right(15);
				sleep(0.1);
				drive_into_wall(1, 5.0);
			}
		} else {
			if (get_analog(LEFT_REAR_LIGHT, 3) < THRESHOLD_3 && get_analog(RIGHT_REAR_LIGHT, 3) < THRESHOLD_4) {
				return;
			} else if (get_analog(LEFT_REAR_LIGHT, 3) > THRESHOLD_3) {
				turn_left(15);
				sleep(0.1);
				drive_ticks_without(1,30);
				sleep(0.1);
				turn_right(15);
				sleep(0.1);
				drive_into_wall(0, 5.0);
			} else {
				turn_right(15);
				sleep(0.1);
				drive_ticks_without(1,30);
				sleep(0.1);
				turn_left(15);
				sleep(0.1);
				drive_into_wall(0, 5.0);
			}
		}
	}
}