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

void TurnAngle(int goalangle);
void DriveDistance(int goaldistance);

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 < 3)
  {
  DriveDistance(500);
  	TurnAngle(120);
	
	// play "Beep-boop" and wait while it plays	
	byteTx(CmdPlay);
    byteTx(BUMP_SONG);
	delayAndUpdateSensors(750);
	
	
	loopcounter = loopcounter + 1;
  }
  
  // Spin around
  TurnAngle(360);
  TurnAngle(-360);

  // Stop driving

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

}

/* drive frontwards or backwards the requested number of millimeters */

void DriveDistance(int goaldistance)
{

  delayAndUpdateSensors(10);
  distance = 0;
	
  if (goaldistance < 0) {
  
    while(distance > goaldistance)
    {
	  drive(-300, RadStraight);
      delayAndUpdateSensors(20);
    }
  }
  else {
    while(distance < goaldistance)
    {
	  drive(300, RadStraight);
      delayAndUpdateSensors(20);
    }
  }
  drive(0, RadStraight);
}

/* turn the requested number of degrees.  positive degrees mean clockwise,
** negative degrees mean counter-clockwise.
*/

void TurnAngle(int goalangle)
{
  delayAndUpdateSensors(10);
  angle = 0;
  goalangle = -goalangle;	// Interface seems to have CCW and CW reversed .. ?
  
  if (goalangle > 0) {
     while(angle < goalangle)
     {
		drive(300, RadCCW);
        delayAndUpdateSensors(20);
     }
   }
   else {
        while(angle > goalangle)
     {
		drive(300, RadCW);
        delayAndUpdateSensors(20);
     }
  }
  drive(0, RadStraight);
}