/*
 * IRobot Create
 * "Oh No, Our Robot Isn't Done!"
 * 2009
 * Coded by: Stephen Carlson
 * Modified: 3/14/09
 * Purpose: Obtain Botguy.
 *
 */

// Servo positions and ports.
#define CLOSED 100
#define OPEN 1000
#define CLAW 1
#define ARM 2
#define UP 800
#define MID 400
#define DOWN 100

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);
}

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);
}

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);
}

void arm_down() {
	int i = get_servo_position(ARM);
	// If it is below...
	if (i < DOWN)
		// Iterate up!
		for (; i < DOWN; i += 100) {
			set_servo_position(ARM, i);
			msleep(50L);
		}
	else
		// Iterate down!
		for (; i > DOWN; i -= 100) {
			set_servo_position(ARM, i);
			msleep(50L);
		}
	// Ensure that it has reached its position.
	set_servo_position(ARM, DOWN);
	msleep(50L);
}

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) {
    create_drive_straight(speed);
    if (speed < 0) dist = -dist;
    // Time = Distance/Speed!!!
    msleep(dist * 1000L / (long)speed);
    create_stop();
    msleep(50L);
}

// Ramps the speed of the robot without stopping!
void create_ramp(int speed1, long dist1, int speed2, long dist2) {
    create_drive_straight(speed1);
    if (speed1 < 0) dist1 = -dist1;
    msleep(dist1 * 1000L / (long)speed1);
    create_drive_straight(speed2);
    if (speed2 < 0) dist2 = -dist2;
    msleep(dist2 * 1000L / (long)speed2);
    create_stop();
    msleep(50L);
}

// Waits until either line sensor hits a line.
void until_line_either() {
	while (analog10(10) < 300 && analog10(9) <300) msleep(10L);
}

// Waits until the left line sensor hits a line.
void until_line_left() {
	while (analog10(10) < 300) msleep(10L);
}

// Waits until the right line sensor hits a line.
void until_line_right() {
	while (analog10(9) < 300) msleep(10L);
}

// Waits until both 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(10) >= 300;
		right = analog10(9) >= 300;
		if (left) had_left = 1;
		if (right) had_right = 1;
		msleep(10L);
	}
}

// Aligns the robot to a line.
void align_to_line() {
	int had_left = 0, left = 0, had_right = 0, right = 0;
	create_drive_direct(100, 100);
	while (!had_left || !had_right) {
		left = analog10(10) >= 300;
		right = analog10(9) >= 300;
		if (left) had_left = 1;
		if (right) had_right = 1;
		if (had_left && !had_right)
			create_drive_direct(100, 50);
		if (!had_left && had_right)
			create_drive_direct(50, 100);
		msleep(10L);
	}
	create_stop();
}

int main() {
	int iro;
	// sensor testing
	disable_servos();
	while (1) {
		cbc_display_clear();
		printf("IRL is %d\nIRR is %d\nIRET is %d\nSonar is %d\n", analog10(10), analog10(9), analog10(8), sonar(15));
		if (black_button()) break;
		msleep(50L);
	}
	// initialize
	create_connect();
	create_full();
	set_servo_position(ARM, UP);
	set_servo_position(CLAW, CLOSED);
	enable_servos();
	msleep(1000L);
	// go, go, go!!!
	// get initial IR position
	iro = analog10(8);
	// arc to Botguy
	create_drive_direct(-450, -500);
	arm_mid();
	msleep(1000L);
	// arc from Botguy
	create_drive_direct(-500, -420);
	// until we reach the drop-off
	while (analog10(8) > iro - 200) msleep(20L);
	// steal Botguy
	create_drive_straight(-400);
	mrp(3, 1000L, 300L);
	// get out of the way
	create_ramp(-400, 500, -200, 100);
	// secure Botguy
	mav(3, 800L);
	msleep(300L);
	freeze(3);
	// turn towards the slope
	create_spin_block(200, -30);
	create_mrp(300, 100);
	create_spin_block(200, -30);
	create_mrp(-400, 300);
	// and charge!!!!
	arm_up();
	create_drive_straight(-300);
	msleep(100L);
	// first line
	until_line_both();
	msleep(500L);
	// second line
	until_line_either();
	create_stop();
	// retract the arm
	mrp(3, 1000L, -200L);
	bmd(3);
	// push Botguy out of the way
	create_spin_block(200, -16);
	create_spin_block(200, 14);
	// drop the windmill
	create_mrp(-200, 150);
	claw_open();
	// all done!
	ao();
	create_disconnect();
	disable_servos();
	return 0;
}