task main() {
	RobotState state;
	initialize(&state);
	short leftSpeed, rightSpeed;
	short prevState = INITIALSTATE;
	bool distLess50;
	bool irDetected = false;
	bool sawRedBlue = false;
	bool goBackward = false;
	float startAngle = 0;

	//waitForStart();
	wait1Msec(state.delayTime*1000);
	INITIALDRIVE();
	while(true){
		//if state changes: stop motors, play tone, reset timers, gyro and lights
		if (prevState != state.currentState){
			if (prevState != PARK_DRIVE2 && prevState != CHECKEND){
				stopMotors();
				leftSpeed = 0;
				rightSpeed = 0;
			}
			AUDIO_DEBUG(500, 10);
			time1[T1] = 0;
			resetGyroAccel(&state);
			LEDController(0x00);
			distLess50 = false;
			startAngle = state.degrees;
		}

		getSensors(&state);
		prevState = state.currentState;

		if(state.currentState == FINDLINE_TURN){
			drive(0, TURNSPEED);
			if(state.color2 == RED || state.color2 == BLUE){
				sawRedBlue = true;
			}
			if (sawRedBlue && state.color2 == BLACK){
				state.currentState = LINEFOLLOW;
			}
			if(abs(state.degrees) > 10){
				state.currentState = LINEFOLLOW;
			}
		/* state FINDLINE_DRIVE seems unnecessary
		} else if (state.currentState == FINDLINE_DRIVE) {
			drive(DRIVESPEED, DRIVESPEED*.95);
			if (state.irDir == 5 && irDetected == false) {
				state.currentState = SCOREBLOCK;
			} else if(state.color2 == RED || state.color2 == BLUE){
				state.currentState = LINEFOLLOW;
			} */
		} else if (state.currentState == LINEFOLLOW) {
			if (state.dist < 50) {
				distLess50 = true;
			}
			if (state.irDir == 5 && irDetected == false) {
				state.currentState = SCOREBLOCK;
			} else if (state.dist > 50 && distLess50 && irDetected == true) {
				state.currentState = GOTOEND;
			} else if (goBackward){
				if (state.color == RED || state.color == BLUE) {
					leftSpeed = -DRIVESPEED*LINEFOLLOWRATIO;
					rightSpeed = -DRIVESPEED;
				}else {
					leftSpeed = -DRIVESPEED;
					rightSpeed = -DRIVESPEED*LINEFOLLOWRATIO;
				}
			} else {
				if (state.color2 == RED || state.color2 == BLUE) {
					leftSpeed = DRIVESPEED*LINEFOLLOWRATIO;
					rightSpeed = DRIVESPEED;
				} else {
					leftSpeed = DRIVESPEED;
					rightSpeed = DRIVESPEED*LINEFOLLOWRATIO;
				}
			}
			drive(leftSpeed, rightSpeed);
		} else if (state.currentState == SCOREBLOCK) {
			irDetected = true;
			goBackward = true;
			LEDController(B2);
			blockScorer();
			state.currentState = LINEFOLLOW;
		} else if (state.currentState == GOTOEND) {
			drive(-DRIVESPEED, -DRIVESPEED);
			if(time1[T1] >= 300) {
				state.currentState = PARK_TURN1;
			}
		} else if (state.currentState == PARK_TURN1) {
			drive(-TURNSPEED, TURNSPEED);
			if(abs(state.degrees) >= 15){
				state.currentState = PARK_DRIVE1;
			}
		} else if (state.currentState == PARK_DRIVE1) {
			drive(-DRIVESPEED, -DRIVESPEED);
			if(state.color == RED || state.color == BLUE){
				state.currentState = PARK_TURN2;
			}
		} else if (state.currentState == PARK_TURN2) {
			DRIVESPECIAL(-TURNSPEED, TURNSPEED);
			if(abs(state.degrees) >= 22.5){
				state.currentState = PARK_DRIVE2; //skip state HARVEST
			}
		} else if (state.currentState == HARVEST) {
			motor[harvester] = 100;
			DRIVESPECIAL(2*DRIVESPEED, 2*DRIVESPEED);
			if (time1[T1] >= 500){
				leftSpeed = startAngle/abs(startAngle)*TURNSPEED;
				rightSpeed = -startAngle/abs(startAngle)*TURNSPEED;
				drive(leftSpeed, rightSpeed);
				if (abs(startAngle-state.degrees) <= 0.5){
					state.currentState = PARK_DRIVE2;
				}
			}
		} else if (state.currentState == PARK_DRIVE2) {
			DRIVESPECIAL(-2*DRIVESPEED, -2*DRIVESPEED);
			if(abs(state.x_accel) > 35 && state.x_accel < 100){
				wait1Msec(20);
				state.currentState = CHECKEND;
			}
		} else if (state.currentState == CHECKEND) {
			if(abs(state.x_accel) > 35 && abs(state.x_accel) < 100){
				state.currentState = END; //if it's > 30 after 1 sec of pushing, you're done
			} else {
				state.currentState = PARK_DRIVE2; //else you need to do more pushing
			}
		} else if (state.currentState == END){
			DRIVESPECIAL(-2*DRIVESPEED, -2*DRIVESPEED);
			if(time1[T1] >= 1000){
				break;
			}
		} else {
			break;
		}
	}
}
task main() {
	RobotState state;
	initialize(&state);
	short leftSpeed, rightSpeed;
	short prevState = INITIALSTATE;
	bool distLess50;
	bool irDetected = false;
	bool goBackward = false;
	float startAngle = 0;
	int numTimeAccelTriggered = 0;

	waitForStart();
	wait1Msec(state.delayTime*1000);
	INITIALDRIVE();
	while(true){
		//if state changes: stop motors, play tone, reset timers, gyro and lights
		if (prevState != state.currentState){
			if (prevState != PARK_DRIVE2 && prevState != CHECKEND){
				stopMotors();
				leftSpeed = 0;
				rightSpeed = 0;
			}
			AUDIO_DEBUG(500, 10);
			time1[T1] = 0;
			resetGyroAccel(&state);
			LEDController(0x00);
			distLess50 = false;
			startAngle = state.degrees;
		}

		getSensors(&state);
		prevState = state.currentState;

		if(state.currentState == FINDLINE_TURN){
			drive(0, TURNSPEED);
			if(abs(state.degrees) > 10){
				state.currentState = LINEFOLLOW;
			}
		} else if (state.currentState == LINEFOLLOW) {
			if (state.dist < 50) {
				distLess50 = true;
			}
			if (state.irDir == 5 && irDetected == false) {
				state.currentState = SCOREBLOCK;
			} else if (state.dist > 50 && distLess50 && irDetected == true){
				state.currentState = GOTOEND;
			} else if (goBackward){
				leftSpeed = -DRIVESPEED;
				rightSpeed = -DRIVESPEED;
			} else {
				leftSpeed = DRIVESPEED;
				rightSpeed = DRIVESPEED;
			}
			drive(leftSpeed, rightSpeed);
		} else if (state.currentState == SCOREBLOCK) {
			irDetected = true;
			goBackward = true;
			LEDController(B2);
			blockScorer();
			state.currentState = LINEFOLLOW;
		} else if (state.currentState == GOTOEND) {
			drive(-DRIVESPEED, -DRIVESPEED);
			if(time1[T1] >= 300) {
				state.currentState = PARK_TURN1;
			}
		} else if (state.currentState == PARK_TURN1) {
			drive(-TURNSPEED, TURNSPEED);
			if(abs(state.degrees) >= 15){
				state.currentState = PARK_DRIVE1;
			}
		} else if (state.currentState == PARK_DRIVE1) {
			drive(-DRIVESPEED, -DRIVESPEED);
			if((state.color == RED || state.color == BLUE) && time1[T1] > 500){
				state.currentState = PARK_TURN2;
			}
		} else if (state.currentState == PARK_TURN2) {
			DRIVESPECIAL(-TURNSPEED, TURNSPEED);
			if(abs(state.degrees) >= 27){
				state.currentState = PARK_DRIVE2; //skip state HARVEST
			}
		} else if (state.currentState == HARVEST) {
			motor[harvester] = 100;
			DRIVESPECIAL(2*DRIVESPEED, 2*DRIVESPEED);
			if (time1[T1] >= 500){
				leftSpeed = startAngle/abs(startAngle)*TURNSPEED;
				rightSpeed = -startAngle/abs(startAngle)*TURNSPEED;
				drive(leftSpeed, rightSpeed);
				if (abs(startAngle-state.degrees) <= 0.5){
					state.currentState = PARK_DRIVE2;
				}
			}
		} else if (state.currentState == PARK_DRIVE2) {
			DRIVESPECIAL(-2*DRIVESPEED, -2*DRIVESPEED);
			if(abs(state.x_accel) > 35 && time1[T1] >= 300){
				numTimeAccelTriggered++;
			} else {
				numTimeAccelTriggered = 0;
			}
			if (numTimeAccelTriggered >= 3) {
				state.currentState = END;
			}
			wait1Msec(20);
		} else if (state.currentState == END){
			DRIVESPECIAL(-2*DRIVESPEED, -2*DRIVESPEED);
			if(time1[T1] >= 300){
				break;
			}
		} else {
			break;
		}
	}
}