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

  loopcounter = 0;
  while (loopcounter < 4)
  {

    angle = 0;
	
 // spin around 360 degrees
 
    while(angle < 360)
    {
	  drive(250, RadCCW);
      delayAndUpdateSensors(20);
    }
	
// Stop spinning	
	drive(0, RadStraight);

// play "Beep-boop" and wait while it plays	
	byteTx(CmdPlay);
    byteTx(BUMP_SONG);
	delayAndUpdateSensors(750);

	loopcounter = loopcounter + 1;
  }

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

}