/*
 * Example 2 demonstrates:
 *  -- returning values from functions
 *  -- pointers, including the & and * operators
 *  -- reading from the console
 *  -- while loops
*/

#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>

#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

/*
 * 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);

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

/*
 * Beep the given number of times.
 */
void beep_repeatedly(int numberOfBeeps);
 
/******************************************************************************************************
 * MAIN starts here.
 ******************************************************************************************************
 */
 
/*
 * Connects to the Create and goes forward for the given number of seconds at the given speed.
 * Then goes forward until bumped.
*/
int main() {
	int actualDistance;
	float targetSeconds;
	float actualSeconds;
	int targetSpeed;

	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 seconds do you want the robot to go? ");
	scanf("%f", &targetSeconds);
	printf("What speed do you want the robot to go, in mm/sec? ");
	scanf("%d", &targetSpeed);
	
	actualDistance = forward_seconds(targetSeconds, &actualSeconds, targetSpeed);
	
	printf("\n");
	printf("Attempted to go %5.2f seconds at speed %d mm/second.\n", targetSeconds, targetSpeed);
	printf("Thus attempted a distance of %6.1f millimeters.\n\n", targetSeconds * targetSpeed);
	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_repeatedly(3);
}

/*
 * Beep one time.
 */
void beep_oncence() {
	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;
}

// TODO: Define the forward_until_bumped function specified below
// and call it at the end of main (after the beeps).
// In main, print the distance travelled, elapsed time, and which bumper was bumped.

/*
 * 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.
 */