/*
 * Example 3 demonstrates:
 *  -- static and dynamic arrays
*/

#include <stdio.h>
#include <kiss-compat.h>
#include <kiss-graphics.h>
#include <kiss-serial.h>
#include <kiss-create.h>
#include <kiss-input.h>

#include <time.h>

#include <time.h>

#define BEEP_SONG     15	// Song 15 is reserved for beeping
#define BEEP_NOTE     50	// Note to use for a beep
#define BEEP_DURATION 16	// Number of 1/64th's of a second

#define SECONDS_PER_TEST 2.5

/*
 * forward_until_bumped()
 * Drives forward at the given speed until the left or right front bumper bumps into something.
 * Returns the distance travelled, in millimeters.
 * Sets the second parameter to the actual number of seconds travelled.
 * Sets the third parameter to which bumper was bumped:
 *   0 for left bumper, 1 for right bumper, 2 for both, -1 for neither.
 */
int forward_until_bumped(int speed, float* actualSeconds, int* bumperBumped);

/*
 * Runs the robot, which repeatedly:
 *   -- Goes forward until bumped (left or right front bumper).
 *   -- Beeps 3 times.
 *   -- Turns right 90 degrees.
 *   -- Beeps 5 times.
 */
void do_bumps(int n, int* distances);

/*
 * Runs the robot backwards along the path it took via do_bumps(), stopping at its original starting point.
 */
void repeat_bumps(int numberOfBumps, int* distances);

/*
 * forward()
 * Drives forward at the given speed (negative if you want to go backwards) for the given distance.
 * Returns the actual distance travelled.
 */
 int forward(int speed, int millimeters);

/*
 * backward()
 * Drives backward at the given speed for the given distance.
 * Returns the actual distance travelled.
 */
int backward(int speed, int millimeters);

/*
 * Asks the user for the speeds and stores them in the array.
 */
void input_speeds(int* speeds, int numberOfSpeeds);

/*
 * Tests the given speeds by running the robot at those speeds.
 */
void test_speeds(int* speeds, int numberOfSpeeds);

/*
 * Prints the contents of the given array, one entry per line.
 */
void print_array(int* array, int arraySize);

/*
 * Asks the user how many bumps she wants and returns that number.
 */
int how_many_bumps();

/*
 * Beep one time.
 */
void beep_once();

/*
 * Beep the given number of times.
 */
void beep_repeatedly(int numberOfBeeps);

/*
 * Goes forward the given number of seconds.  Returns the distance travelled in mm.
 * Sets the second parameter to the actual number of seconds travelled.
 */
int forward_seconds(float targetSeconds, float* actualSeconds, int speed);

/*
 * Connects to the Create and runs the requested number of speed tests.
 * Then goes backwards through the speed tests.
*/
int main() {
	int* speeds;
	int numberOfSpeeds;
	int numberOfBumps;
	int* distances;

	if (create_connect() == 0) {
		printf("Connected to the Create robot.\n");
	} else {
		printf("Could not connect to the Create robot.\n");
		exit(1);
	}
	
	create_safe();

	printf("How many speeds do you want to test? ");
	scanf("%d", &numberOfSpeeds);
	
	speeds = (int*) malloc(numberOfSpeeds * sizeof(int));

	input_speeds(speeds, numberOfSpeeds);
	print_array(speeds, numberOfSpeeds);
	
	printf("Now go to the robot.\n");
	printf("Every time you hear 3 beeps, press the Play button (the middle one) to run the next speed test.\n");
	printf("The tests conclude with a long series of beeps.\n");
	
	test_speeds(speeds, numberOfSpeeds);
	beep_repeatedly(10);
	
	numberOfBumps = how_many_bumps();
	printf("You asked for %d bumps.\n", numberOfBumps);
	
	distances = (int*) malloc(numberOfBumps * sizeof(int));

	do_bumps(numberOfBumps, distances);
	print_array(distances, numberOfBumps);
	
	repeat_bumps(numberOfBumps, distances);
}

/*
 * Asks the user for the speeds and stores them in the array.
 */
void input_speeds(int* speeds, int numberOfSpeeds) {
	int k;
	
	for (k = 0; k < numberOfSpeeds; ++k) {
		printf("Enter a speed to test (mm / second, an integer in the range of -500 to 500): ");
		scanf("%d", &(speeds[k]));
	}
}

/*
 * Tests the given speeds by running the robot at those speeds.
 */
void test_speeds(int* speeds, int numberOfSpeeds) {
	int k;
	
	float targetSeconds;
	float actualSeconds;
	int actualDistance;
	
	targetSeconds = SECONDS_PER_TEST;
	for (k = 0; k < numberOfSpeeds; ++k) {
		beep_repeatedly(3);
		create_buttons();
		while (gc_play_button == 0) {
			create_buttons();	// Wait for the Play button to be pressed.
		}
		
		actualDistance = forward_seconds(targetSeconds, &actualSeconds, speeds[k]);
		
		printf("\n");
		printf("Attempted to go %5.2f seconds at speed %d mm/second.\n", targetSeconds, speeds[k]);
		printf("Thus attempted a distance of %6.1f millimeters.\n\n", targetSeconds * speeds[k]);
		printf("Actual distance was %d millimeters.\n", actualDistance);
		printf("Actual time was %5.2f seconds.\n", actualSeconds);
		printf("Hence actual speed was %6.1f millimeters/second.\n", (float) (actualDistance) / actualSeconds);
	}
}

/*
 * Beep one time.
 */
void beep_once() {
	int song = BEEP_SONG;
	
	gc_song_array[song][0] = 1;
	gc_song_array[song][1] = BEEP_NOTE;			// Note (must be between 31 and xxx)
	gc_song_array[song][2] = BEEP_DURATION;		// Duration (number of 1/64th's of a second)
	
	create_load_song(song);
	create_play_song(song);
	sleep(BEEP_DURATION / 64.0);	// Wait for the note-playing to finish.
}

/*
 * Beep the given number of times.
 */
void beep_repeatedly(int numberOfBeeps) {
	int k;
	
	for (k = 0; k < numberOfBeeps; ++k) {
		beep_once();
		sleep(0.5 * BEEP_DURATION / 64.0);
	}
}

/*
 * Goes forward the given number of seconds.  Returns the distance travelled in mm.
 * Sets the second parameter to the actual number of seconds travelled.
 */
int forward_seconds(float targetSeconds, float* actualSeconds, int speed) {
	clock_t start, finish;
	double elapsed;
	
	gc_distance = 0;
	elapsed = 0.0;
	
	start = clock();
	create_drive_straight(speed);
	
	// The following loop could be replaced by sleep(targetSeconds).  It is a loop here as an example of "wait".
	while (elapsed < targetSeconds) {
		finish = clock();
		elapsed = ((double) (finish - start)) / CLOCKS_PER_SEC;
	}
	create_stop();
	
	create_distance();
	
	*actualSeconds = elapsed;
	return gc_distance;
}

/*
 * forward_until_bumped()
 * Drives forward at the given speed until the left or right front bumper bumps into something.
 * Returns the distance travelled, in millimeters.
 * Sets the second parameter to the actual number of seconds travelled.
 * Sets the third parameter to which bumper was bumped:
 *   0 for left bumper, 1 for right bumper, 2 for both, -1 for neither.
 */
int forward_until_bumped(int speed, float* actualSeconds, int* bumperBumped) {
	clock_t start, finish;
	double elapsed;

	gc_distance = 0;
	elapsed = 0.0;
	
	start = clock();
	create_drive_straight(speed);
	
	create_bumpdrop();
	while (gc_lbump == 0 && gc_rbump == 0) {
		create_bumpdrop();
	}
	create_stop();
	
	finish = clock();
	elapsed = ((double) (finish - start)) / CLOCKS_PER_SEC;
	
	if (gc_lbump == 1 && gc_rbump == 1) {
		*bumperBumped = 2;
	} else if (gc_lbump == 1) {
		*bumperBumped = 0;
	} else if (gc_rbump == 1) {
		*bumperBumped = 1;
	} else {
		*bumperBumped = 0;
	}
	create_distance();
	
	*actualSeconds = elapsed;
	
	return gc_distance;
}

// TODO 1: Write a function called print_array() that:
//   -- takes an array of integers and the size of the array
//   -- prints the values in the array, one value per line.
// Test your function by printing the speeds array in main, as entered by the user.

/*
 * Prints the contents of the given array, one entry per line.
 */
void print_array(int* array, int arraySize) {
	int k;
	
	for (k = 0; k < arraySize; ++k) {
		printf("%d\n", array[k]);
	}
}

// TODO 2: Write a function called how_many_bumps() that:
//   -- Asks the user how many bumps she wants and returns that number.
// Test your function by calling it from main.

/*
 * Asks the user how many bumps she wants and returns that number.
 */
int how_many_bumps() {
	int numberOfBumps;
	
	printf("How many bumps do you want? ");
	scanf("%d", &numberOfBumps);
	
	return numberOfBumps;
}

// TODO 3: Write a function called do_bumps(int n) that:
// Runs the robot, repeating the following n times:
//   -- Go forward until bumped (left or right front bumper).
//   -- Beep 3 times.
//   -- Turn right 90 degrees.
//   -- Beep 5 times.
// Test your function by calling it from main.

/*
 * Runs the robot, which repeatedly:
 *   -- Goes forward until bumped (left or right front bumper).
 *   -- Beeps 3 times.
 *   -- Turns right 90 degrees.
 *   -- Beeps 5 times.
 */
void do_bumps(int numberOfBumps, int* distances) {
	int k;
	int speed;
	float actualSeconds;
	int bumperBumped;
	
	speed = 100;
	for (k = 0; k < numberOfBumps; ++k) {
		distances[k] = forward_until_bumped(speed, &actualSeconds, &bumperBumped);
		beep_repeatedly(3);
		gc_total_angle = 0;
		create_angle();
		create_spin_CW(speed);
		while (gc_total_angle > -90) {
			create_angle();	// Wait until the turn is 90 degrees.
		}
		create_stop();
		beep_repeatedly(5);
	}
}

// TODO 4: Modify your do_bumps() function so that it has a second parameter that is an array of integers.
// Each time that the robot goes forward, store the distance travelled (in millimeters) in the array.
// Thus, at the conclusion of do_bumps(), the array has stored all the distances travelled.
// Test your do_bumps() function by printing the values stored in the array.

// TODO 5: Write a repeat_bumps() function that has appropriate parameters to do the following:
//   -- After the robot concludes its requested number of bumps (and associated 90 degree right turns),
//   -- the robot traverses the path it took, but backwards, stopping at its original starting point.
// Test your function by calling it from main.

/*
 * Runs the robot backwards along the path it took via do_bumps(), stopping at its original starting point.
 */
void repeat_bumps(int numberOfBumps, int* distances) {
	int k;
	int speed;
	float actualSeconds;
	int bumperBumped;
	
	speed = 100;
	for (k = numberOfBumps - 1; k >= 0; --k) {
		gc_total_angle = 0;
		create_angle();
		create_spin_CCW(speed);
		
		while (gc_total_angle < 90) {
			create_angle();	// Wait until the turn is 90 degrees.
		}
		create_stop();
		beep_repeatedly(1);
		
		backward(speed, distances[k]);
		beep_repeatedly(2);
	}
}

/*
 * forward()
 * Drives forward at the given speed for the given distance.
 * Returns the actual distance travelled.
 */
int forward(int speed, int millimeters) {
	gc_distance = 0;
	
	create_distance();
	create_drive_straight(speed);
	
	while (gc_distance < millimeters) {
		create_distance();
	}
	create_stop();
	
	return gc_distance;
}
	
/*
 * backward()
 * Drives backward at the given speed for the given distance.
 * Returns the actual distance travelled.
 */
int backward(int speed, int millimeters) {
	gc_distance = 0;
	
	create_distance();
	create_drive_straight(-speed);
	
	while (gc_distance > -millimeters) {
		create_distance();
	}
	create_stop();
	
	return -gc_distance;
}