#define WATCHDOG_TIME_LIMIT_CS 1000
#define PEN_PERIOD 30
#define BLACK_THRESH 40
#define DRIVE_SPEED 1
#define DRIVE_MM_PER_CS 2
#define TURN_SPEED 1
#define TURN_CDEG_PER_CS 15

task dropPen()
{
	SetDirection(OUT_A, OUT_REV);
	SetPower(OUT_A, 7);	
	OnFor(OUT_A,PEN_PERIOD);
}

task raisePen()
{
	SetDirection(OUT_A, OUT_FWD);
	SetPower(OUT_A, 7);	
	OnFor(OUT_A,PEN_PERIOD);
}

// Positive drive will move so as to drag pen
void drive(int dist_mm) 
{
	if (dist_mm > 0) {
		SetDirection(OUT_B + OUT_C, OUT_FWD);
	} else {
		SetDirection(OUT_B + OUT_C, OUT_REV);
		dist_mm *= -1;
	}
	SetPower(OUT_B + OUT_C, DRIVE_SPEED);
	OnFor(OUT_B + OUT_C, dist_mm / DRIVE_MM_PER_CS);
}

// Positive turn will rotate clockwise
void turn(int angle_cdeg) 
{
	if (angle_cdeg > 0) {
		SetDirection(OUT_B, OUT_FWD);
		SetDirection(OUT_C, OUT_REV);
	} else {
		SetDirection(OUT_B, OUT_REV);
		SetDirection(OUT_C, OUT_FWD);
		angle_cdeg *= -1;
	}
	SetPower(OUT_B + OUT_C, TURN_SPEED);
	OnFor(OUT_B + OUT_C, angle_cdeg / TURN_CDEG_PER_CS);
}

// Drive in a straight line until the sensor threshold is triggered
void driveUntil()
{
	SetSensorType(SENSOR_1, SENSOR_TYPE_LIGHT);
	SetDirection(OUT_B + OUT_C, OUT_FWD);
	SetPower(OUT_B + OUT_C, DRIVE_SPEED);
	SetOutput(OUT_B + OUT_C, OUT_ON);
	while (SENSOR_1 < 53);
	while (SENSOR_1 > 52);
	SetOutput(OUT_B + OUT_C, OUT_FLOAT);
}

// Turn in place until the sensor threshold is triggered
void turnUntil()
{
	SetSensorType(SENSOR_1, SENSOR_TYPE_LIGHT);
	SetDirection(OUT_B, OUT_REV);
	SetDirection(OUT_C, OUT_FWD);
	SetPower(OUT_B + OUT_C, TURN_SPEED);
	SetOutput(OUT_B + OUT_C, OUT_ON);
	while (SENSOR_1 < 53);
	while (SENSOR_1 > 52);
	SetOutput(OUT_B + OUT_C, OUT_FLOAT);
}

// Rattle around inside a polygon (tested with a box)
void stayInBox()
{
	while (1) {
		driveUntil();
		Wait(10); // Allow inertia to get you over the line
		turnUntil();
	}
}

// Stops the robot after a period of time has elapsed
task watchdog()
{
	Wait(WATCHDOG_TIME_LIMIT_CS);
	SetOutput(OUT_A + OUT_B + OUT_C, OUT_FLOAT);
	StopAllTasks();
}

// Should stop the fool thing when it runs off the board
task carpetminder()
{
	while (SENSOR_1 > 48);
	SetOutput(OUT_A + OUT_B + OUT_C, OUT_FLOAT);
	StopAllTasks();
}


task main()
{
	start watchdog;
	//start dropPen;
	//Wait(100);
	//start raisePen;	
	//drive(-200);
	//turn(1800);
	stayInBox();
	SetOutput(OUT_A + OUT_B + OUT_C, OUT_FLOAT);
}