#define FORCE		7

#define DELAY		60

#define DRIVE		OUT_A
#define STEER		OUT_C

#define LEFT		IN_3
#define RIGHT		IN_1
#define LIGHT		IN_2

#define DIR_LEFT	1
#define DIR_STRAIGHT	2
#define DIR_RIGHT	3

int dir;

sub left
{
	if (dir == DIR_LEFT) {
		return;
	}
	Rev(STEER, FORCE);
	Sleep(DELAY);
	if (dir == DIR_RIGHT) {
		Sleep(DELAY);
	}
	Off(STEER);
	dir = DIR_LEFT;
}

sub right
{
	if (dir == DIR_RIGHT) {
		return;
	}
	Fwd(STEER, FORCE);
	Sleep(DELAY);
	if (dir == DIR_LEFT) {
		Sleep(DELAY);
	}
	Off(STEER);
	dir = DIR_RIGHT;
}

sub straight
{
	if (dir == DIR_STRAIGHT) {
		return;
	}
	if (dir == DIR_LEFT) {
		Fwd(STEER, FORCE);
	} else {
		Rev(STEER, FORCE);
	}
	Sleep(DELAY);
	Off(STEER);
	dir = DIR_STRAIGHT;
}

task beeping
{
	while (1 == 1) {
		PlayNote(2000, 50);
		Sleep(50);
		PlayNote(0, 50);
		Sleep(50);
	}
}

task main
{
	Sensor(LIGHT, IN_LIGHT);
	Sensor(LEFT, IN_SWITCH);
	Sensor(RIGHT, IN_SWITCH);

	dir = DIR_RIGHT;
	left();
	straight();

	Sleep(100);
	start beeping;
	Rev(DRIVE, 7);

	while (1 == 1) {
		if (LEFT == 1) {
			left();
		} else if (RIGHT == 1) {
			right();
		} else {
			straight();
		}
	}
}