// Includes
#include <avr/interrupt.h>
#include <avr/io.h>
#include <stdlib.h>
#include "oi.h"

// Constants
#define RESET_SONG 0
#define START_SONG 1
#define BUMP_SONG  2
#define END_SONG   3

// Functions in util.c
void byteTx(uint8_t value);
void delayMs(uint16_t time_ms);
void delayAndUpdateSensors(unsigned int time_ms);
void initialize(void);
void powerOnRobot(void);
void baud(uint8_t baud_code);
void drive(int16_t velocity, int16_t radius);
void defineSongs(void);

extern uint8_t sensors[Sen6Size];
extern int16_t distance;
extern int16_t angle;


int main (void) 
{

  int loopcounter = 0;


  // Set up Create and module
  initialize();
  LEDBothOff;
  powerOnRobot();
  byteTx(CmdStart);
  baud(Baud28800);
  defineSongs();
  byteTx(CmdControl);
  byteTx(CmdSafe);

  // Stop just as a precaution
  drive(0, RadStraight);

  // Play the reset song and wait while it plays
  byteTx(CmdPlay);
  byteTx(RESET_SONG);
  delayAndUpdateSensors(750);

  
  // go 1 meter (1000 mm) straight, medium speed

	distance = 0;
	while(distance < 1000)
	{
		drive(250, RadStraight);
		delayAndUpdateSensors(20);
	}
	drive(0, RadStraight);

    angle = 0;
	
 // Turn 90 degrees right, slowly
 
	while(angle > -90)
	{	
		drive(50, RadCW);
		delayAndUpdateSensors(20);
	}
	drive(0, RadStraight);

  // go 0.5 meter (500 mm) straight, medium speed

	distance = 0;
	while(distance < 500)
	{
		drive(250, RadStraight);
		delayAndUpdateSensors(20);
	}
	drive(0, RadStraight);

 // Turn 90 degrees left, medium speed
 
	while(angle < 90)
	{	
		drive(250, RadCCW);
		delayAndUpdateSensors(20);
	}
	drive(0, RadStraight);
  
    // go 866 mm straight, full speed

	distance = 0;
	while(distance < 866)
	{
		drive(500, RadStraight);
		delayAndUpdateSensors(20);
	}
	drive(0, RadStraight);

// Play end song and wait
  delayAndUpdateSensors(500);
  byteTx(CmdPlay);
  byteTx(END_SONG);
  delayAndUpdateSensors(2500);

}