/*
 * IRobot Create
 * "Oh No, Our Robot Isn't Done!"
 * 2009
 * Revision Date: 4/12
 * 
 * Code by: Stephen Carlson, Terry Sifrit
 * Purpose: Gets Botguy and the orange/green cups. Also deploys one windmill correctly.
 * 
 * Revision History:
 *  3/25: Successfully gets Botguy!!!
 *  3/28: Stephen: Took out second windmill grab (not working)
 *  4/1:  Stephen: Tried spin follow of black line (failed)
 *  4/4:  Stephen: Successfully reached the peak on D/R!
 *  4/6:  Terry: Tuned the first turn to more reliably reach the peak
 *  4/8:  Terry: Power level problems and arcing issues. Tried removing first direct drive, but no go.
 *  4/10: Stephen: Moved arm to separate process to allow simultaneous freeze and hold. Added light start.
 *  4/12: Terry: Test test test... (and small changes here and there!)
 */

// Defined constants allow easy modification of the code.

// Sensor Ports:
// Left IR (down facing)
#define IRL 10
// Right IR (down facing)
#define IRR 9
// IR Top Hat (right facing)
#define IRTH 8
// Light Sensor
#define LIGHT 11

// Servo Positions:
// Servo position for claw closed.
#define CLOSED 100
// Servo position for claw open.
#define OPEN 1000
// Servo position for arm up.
#define UP 800
// Servo position for arm middle (to drop umbrella/guide cups)
#define MID 400

// Servo Ports:
// The claw port (note ports are 1-4)
#define CLAW 1
// The arm port.
#define ARM 2

// Miscellaneous:
// The threshold for black line/white surface on IR down facing sensors.
#define THRS 700
// The amount to deploy the front arm for full expulsion of cups.
#define FULL 200
// The amount to deploy the front arm for partial expulsion of cups.
#define PART 160


// Deploys the arm to grab Botguy and then holds it. This is meant to be run as a separate process.
void arm() {
	mrp(3, 800, 400L);
	bmd(3);
	off(3);
	// This is the key to the process - regular set-and-forget can't freeze at the end.
	freeze(3);
	// The process ends at this point. The next off command will override the freeze.
}

// Opens the claw incrementally to avoid throwing the turbine.
void claw_open() {
	int i = get_servo_position(CLAW);
	// If it is below...
	if (i < OPEN)
		// Iterate up!
		for (; i < OPEN; i += 100) {
			set_servo_position(CLAW, i);
			msleep(50L);
		}
	else
		// Iterate down!
		for (; i > OPEN; i -= 100) {
			set_servo_position(CLAW, i);
			msleep(50L);
		}
	// Ensure that it has reached its position.
	set_servo_position(CLAW, OPEN);
	msleep(50L);
}

// Closes the claw incrementally to avoid smashing the turbine.
void claw_close() {
	int i = get_servo_position(CLAW);
	// If it is below...
	if (i < CLOSED)
		// Iterate up!
		for (; i < CLOSED; i += 100) {
			set_servo_position(CLAW, i);
			msleep(50L);
		}
	else
		// Iterate down!
		for (; i > CLOSED; i -= 100) {
			set_servo_position(CLAW, i);
			msleep(50L);
		}
	// Ensure that it has reached its position.
	set_servo_position(CLAW, CLOSED);
	msleep(50L);
}

// Raises the arm slowly to avoid stressing the servo.
void arm_up() {
	int i = get_servo_position(ARM);
	// If it is below...
	if (i < CLOSED)
		// Iterate up!
		for (; i < UP; i += 100) {
			set_servo_position(ARM, i);
			msleep(50L);
		}
	else
		// Iterate down!
		for (; i > UP; i -= 100) {
			set_servo_position(ARM, i);
			msleep(50L);
		}
	// Ensure that it has reached its position.
	set_servo_position(ARM, UP);
	msleep(50L);
}

// Moves the arm to the middle position slowly to avoid trashing the cups/dropping the turbine.
void arm_mid() {
	int i = get_servo_position(ARM);
	// If it is below...
	if (i < MID)
		// Iterate up!
		for (; i < MID; i += 100) {
			set_servo_position(ARM, i);
			msleep(50L);
		}
	else
		// Iterate down!
		for (; i > MID; i -= 100) {
			set_servo_position(ARM, i);
			msleep(50L);
		}
	// Ensure that it has reached its position.
	set_servo_position(ARM, MID);
	msleep(50L);
}

// Moves the create drive base the specified number of mm at the given speed in mm/sec.
void create_mrp(int speed, long dist) {
	// Goes straight:
	create_drive_straight(speed);
	if (speed < 0) dist = -dist;
	// Time = Distance/Speed!!!
	msleep(dist * 1000L / (long)speed);
	// Stop and spin-down.
	create_stop();
	msleep(50L);
}

// Waits until either IR down-facing line sensor hits a line.
void until_line_either() {
	while (analog10(IRL) < THRS && analog10(IRR) < THRS) msleep(10L);
}

// Waits until the left IR down-facing line sensor hits a line.
void until_line_left() {
	while (analog10(IRL) < THRS) msleep(10L);
}

// Waits until the right IR down-facing line sensor hits a line.
void until_line_right() {
	while (analog10(IRR) < THRS) msleep(10L);
}

// Waits until both IR down-facing line sensors hit a line.
void until_line_both() {
	int had_left = 0, left = 0, had_right = 0, right = 0;
	while (!had_left || !had_right) {
		left = analog10(IRL) >= THRS;
		right = analog10(IRR) >= THRS;
		// Avoids a problem that could leave the robot stuck in a loop forever.
		// If the robot ever had seen a line, set an accumulator variable that will
		//  hold the status even if the robot passes the line on one side.
		if (left) had_left = 1;
		if (right) had_right = 1;
		msleep(10L);
	}
}

// The mseconds() function is used for timeouts to determine how many milliseconds
//  have elapsed since some relative time in the past.
long mseconds() {
	return (long)(seconds() * 1000.);
}

// The
int main() {
	// Variable used to store the auto-align value and for iteration.
	int iro, what;
	// Used to store time from mseconds() for relative timeouts.
	long time;
	// Turn off servos to save power.
	disable_servos();
	cbc_display_clear();
	msleep(500L);
	// Display alignment prompt. The user should place the Create at this time.
	printf("Align...\nBLACK to auto-align\nA to continue\n");
	while (1) {
		// A is to exit.
		if (a_button()) break;
		if (black_button()) {
			// Black button displays auto-align. 860 on the top-hat is perfect.
			iro = analog10(IRTH) - 860;
			if (iro > -25 && iro < 25)
				// A little hystersis to account for sensor variations.
				printf("Align? Yes\n");
			else
				printf("Align? No\n");
		}
		msleep(10L);
	}
	printf("Align done\n");
	msleep(1000L);
	// Initialize Create connection and servos.
	create_connect();
	create_full();
	// No arm_up here - could thrash the servo. If you set before enabling, no thrash.
	set_servo_position(ARM, UP);
	set_servo_position(CLAW, CLOSED);
	enable_servos();
	// Prompt for light start.
	printf("Ready...\nLight Start? (A=yes, B=no)\n");
	while (!a_button() && !b_button());
	if (a_button()) {
		// Use KIPR library function to wait for light (soon to be replaced with faster version)
		printf("Calibrate\n");
		msleep(1000L);
		wait_for_light(LIGHT);
	} else if (b_button()) {
		// 2 second wait to allow the tester to run, hide, and start the camera.
		printf("Go in 2!\n");
		msleep(2000L);
	}
	// Stop the robot in 119 seconds if it doesn't reach the end of code.
	shut_down_in(119.);
	// Drive straight at Botguy. Max speed - opponent wants him too!
	create_drive_straight(-500);
	msleep(1600L);
	// Steal Botguy using the arm process to hold him.
	start_process(arm);
	// Get out of the way by arcing towards our hill.
	create_drive_direct(-500, -360);
	msleep(1400L);
	create_mrp(-400, 400L);
	// Turn towards the slope. This needs tuning (78 too low; 86 too high; still working on it!)
	create_spin_block(250, -82);
	// Move up a little to push stuff up the hill.
	create_mrp(-250, 260);
	// Release iron-clad hold to push stuff in front.
	mrp(3, 500, -100);
	bmd(3);
	// Back off a little to free stuck objects.
	create_mrp(250, 110);
	for (what = 0; what < 3; what++) {
		// 3 times: whack objects with the arm (fd-rev-fd-rev...) to move them in front.
		// This seems to be more effective then just powering away with one movement.
		// Back
		motor(3, -50);
		msleep(100L);
		// Stop and allow spin-down
		off(3);
		msleep(50L);
		// Power forward!
		motor(3, 80);
		msleep(200L);
		// Stop and allow spin-down
		off(3);
		msleep(50L);
	}
	// Hold everything you've got - we're off for a ride!
	freeze(3);
	// Charge!!!
	create_drive_straight(-250);
	// Cross the first line. The middle line could clash one sensor, so wait for both.
	until_line_both();
	msleep(500L);
	// Almost there! Wait for the second line (the peak delimiter).
	until_line_either();
	beep();
	create_stop();
	// Retract the arm and release the stuff.
	mrp(3, 500, -200L);
	bmd(3);
	// Eject anything blocking turbine deployment.
	mrp(0, 700, -300L);
	bmd(0);
	// Retract arm to allow parking in front of turbines.
	mrp(0, 500, 320L);
	bmd(0);
	// Shove Botguy and cups off to the left.
	create_spin_block(200, 11);
	create_mrp(-200, 200);
	create_mrp(200, 200);
	create_spin_block(200, -15);
	// Drive up and drop the windmill. I'll take a burger to go.
	create_mrp(-200, 220);
	arm_mid();
	claw_open();
	// Wait for it to fall... Add a twitch/shake to ensure that it drops?
	msleep(500L);
	// Clear the turbine...
	create_mrp(200, 70);
	arm_up();
	// Get out of the way of the CBC!
	create_mrp(400, 1300);
	// Game over. We are the champions.
	ao();
	create_disconnect();
	disable_servos();
	return 0;
}