#define LIGHT	IN_1
#define LIMIT	IN_2
#define BUMPER	IN_3

#define GRABBER	OUT_A
#define LEFT	OUT_C
#define RIGHT	OUT_B

#define SPEED	7

#define L_FWD	Fwd(LEFT, SPEED)
#define L_REV	Rev(LEFT, SPEED)
/* connection to right motor is reversed: */
#define R_FWD	Rev(RIGHT, SPEED)
#define R_REV	Fwd(RIGHT, SPEED)

#define STOP	Off(LEFT);	Off(RIGHT)

#define BLACK	(LIGHT < 40)
#define WHITE	(LIGHT >= 40)

int count;

sub grab
{
	Fwd(GRABBER, 7);
	Sleep(100);
	if (LIMIT == 1) {
		/* uh oh - we were already fully closed - back off */
		Rev(GRABBER, 7);
		Sleep(100);
		Off(GRABBER);
		return;
	}
	wait(LIMIT == 1);
	Off(GRABBER);
}

sub drop
{
	Rev(GRABBER, 7);
	Sleep(100);	
	if (LIMIT == 1) {
		/* uh oh - we were already fully open - back off */
		Fwd(GRABBER, 7);
		Sleep(100);
		Off(GRABBER);
		return;
	}
	wait(LIMIT == 1);
	Fwd(GRABBER, 7);
	Sleep(130);
	Off(GRABBER);
}

#define SCAN(t)		repeat (t) { if (BLACK) return; Sleep(1); }

sub search
{
	L_FWD;	R_REV;
	SCAN(50);
	L_FWD;	R_FWD;
	SCAN(20);
	while (1==1) {
		L_REV;	R_FWD;
		SCAN(100);
		L_FWD;	R_FWD;
		SCAN(20);
		L_FWD;	R_REV;
		SCAN(100);
		L_FWD;	R_FWD;
		SCAN(20);
	}
}	

sub centre
{
	/* center back-forth */
	L_FWD;	R_FWD;
	wait (WHITE);
	L_REV;	R_REV;
	wait (BLACK);
	count = 0;
	while (BLACK) {
		Sleep(2);
		count += 1;
	}
	L_FWD;	R_FWD;
	wait (BLACK);
	Sleep(count);
	STOP;

	/* center left-right */
	L_REV;	R_FWD;
	wait (WHITE);
	L_FWD;	R_REV;
	wait (BLACK);
	count = 0;
	while (BLACK) {
		Sleep(2);
		count += 1;
	}
	L_REV;	R_FWD;
	wait (BLACK);
	Sleep(count);
	STOP;
}

task main
{
	Sensor(LIMIT,  IN_SWITCH);
	Sensor(BUMPER, IN_SWITCH);
	Sensor(LIGHT,  IN_LIGHT);

	drop();
	while (1==1) {
		search();
		centre();
		grab();
		L_FWD;	R_REV;
		Sleep(150);
		L_FWD;	R_FWD;
		Sleep(50);
		STOP;
		drop();
		L_REV;	R_REV;
		Sleep(50);
		L_REV;	R_FWD;
		Sleep(150);
		STOP;
	}
}