void wall_follow (int forward, int left_wall)
{
	int left, right;

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

	while (1) {
		if (left_wall) {
			if (forward) {
				if (get_digital(LEFT_FRONT_SIDE)) {
					shift_servos(80);
					motor(LEFT_MOTOR, 100);
					motor(RIGHT_MOTOR, 50);
				} else {
					shift_servos(-40);
					motor(LEFT_MOTOR, 90);
					motor(RIGHT_MOTOR, 100);
				}
			} else {
				if (get_digital(LEFT_REAR_SIDE)) {
					shift_servos(80);
					motor(LEFT_MOTOR, -100);
					motor(RIGHT_MOTOR, -50);
				} else {
					shift_servos(-40);
					motor(LEFT_MOTOR, -90);
					motor(RIGHT_MOTOR, -100);
				}
			}
		} else {
			if (forward) {
				if (get_digital(RIGHT_FRONT_SIDE)) {
					shift_servos(-80);
					motor(LEFT_MOTOR, 50);
					motor(RIGHT_MOTOR, 100);
				} else {
					shift_servos(40);
					motor(LEFT_MOTOR, 100);
					motor(RIGHT_MOTOR, 90);
				}
			} else {
				if (get_digital(RIGHT_REAR_SIDE)) {
					shift_servos(-80);
					motor(LEFT_MOTOR, -50);
					motor(RIGHT_MOTOR, -100);
				} else {
					shift_servos(40);
					motor(LEFT_MOTOR, -100);
					motor(RIGHT_MOTOR, -90);
				}
			}
		}
	}
}

void wall_follow_ticks (int forward, int left_side, int ticks)
{
	int pid;

	pid = start_process(wall_follow(forward, left_side));
	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);
}

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

	driver_pid = start_process(wall_follow(forward, left_side));

	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);

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

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

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

		if (left) {
			kill_process(driver_pid);

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

		if (right) {
			kill_process(driver_pid);

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

	kill_process(driver_pid);
	center_servos();
	off(LEFT_MOTOR);
	off(RIGHT_MOTOR);

	sleep(0.1);
}

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

	driver_pid = start_process(wall_follow(forward, left_side));

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

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

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

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

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

		if (left) {
			kill_process(driver_pid);

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

		if (right) {
			kill_process(driver_pid);

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

	kill_process(driver_pid);
	center_servos();
	off(LEFT_MOTOR);
	off(RIGHT_MOTOR);

	sleep(0.1);
}