/* Ken Dye
 * mouser.c
 * NQC code for controlling a mapping bumper robot.
 */

#pragma noinit
#define LEFT		OUT_A
#define RIGHT		OUT_C

#define BUMPER		SENSOR_1
#define RIGHTROT	SENSOR_2
#define LEFTROT		SENSOR_3

#define EVENT_COLLIDE	0

#define MAX_POINTS	16

#define BACKOFF_DISTANCE	20

task main()
{
	int point_count = 0;
	int bias = 0;
	int state = 0; // 0 for moving forward, 1 for reverse, 
		           // 2 for turning
	int newheading = 0;
	int leftpower = 1;
	int rightpower = 5;
	int turn = 1;
	CreateDatalog(6*MAX_POINTS);
	
	SetSensor(LEFTROT, SENSOR_ROTATION);
	ClearSensor(LEFTROT);
	SetSensor(RIGHTROT, SENSOR_ROTATION);
	ClearSensor(RIGHTROT);
	SetSensorMode(BUMPER, SENSOR_MODE_RAW);

	SetPower(LEFT, 3); 
	SetPower(RIGHT,2);

	Fwd(RIGHT);
	Fwd(LEFT);
	On(RIGHT+LEFT);
	SelectDisplay(DISPLAY_SENSOR_3);

	while(point_count < MAX_POINTS)
	{
		

		switch(state)
		{
		case 0:
			if(BUMPER < 950) {
				Off(LEFT+RIGHT);

				AddToDatalog(LEFTROT);
				AddToDatalog(RIGHTROT);
				point_count++;					

				newheading = (Random(62)+28);

				if(Random(1) == 0)			// randomize left or right turn
					newheading = newheading * -1;
				ClearSensor(LEFTROT);
				ClearSensor(RIGHTROT);
				state = 1; // first back off the wall a little
				Rev(LEFT+RIGHT);
				On(LEFT+RIGHT);
			}
			break;
		case 1:
			if(RIGHTROT >= BACKOFF_DISTANCE)
			{
				Off(LEFT+RIGHT);
				AddToDatalog(LEFTROT);
				AddToDatalog(RIGHTROT); // Log how much we actually backed up
				state = 2; // now we turn!
				ClearSensor(LEFTROT);
				ClearSensor(RIGHTROT);

				if(newheading < 0) {
					Fwd(RIGHT);
					Rev(LEFT);
				} else {
					Fwd(LEFT);
					Rev(RIGHT);
				}
				SetPower(LEFT+RIGHT,1);
				On(RIGHT+LEFT);
			}
			break;
		case 2:
			if(abs(RIGHTROT) >= abs(newheading))  // LEFTROT should == RIGHTROT while
												 // turning
			{
				Off(LEFT+RIGHT);
				AddToDatalog(LEFTROT);  // log how much we actually turned
				AddToDatalog(RIGHTROT);
				state = 0;              // move forward again
				ClearSensor(LEFTROT);
				ClearSensor(RIGHTROT);
				SetPower(LEFT, 3); 
				SetPower(RIGHT,2);
				Fwd(LEFT+RIGHT);
				On(LEFT+RIGHT);

			}
			break;
		default:
			break;
		}

	}
	Off(LEFT+RIGHT);

}