// 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 bumpcounter = 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);
  
	while (bumpcounter < 10) {
		DriveDistance(1000);
		if( (sensors[SenBumpDrop] & BumpLeft) && (sensors[SenBumpDrop] & BumpRight) ) {
			byteTx(CmdPlay);
			byteTx(RESET_SONG);
			delayAndUpdateSensors(750);
			DriveDistance(-100);
			TurnAngle(180);
			bumpcounter++;
		}
		else if(sensors[SenBumpDrop] & BumpLeft) {
			byteTx(CmdPlay);
			byteTx(BUMP_SONG);
			delayAndUpdateSensors(750);
			TurnAngle(30);
			bumpcounter++;
		}
		else if(sensors[SenBumpDrop] & BumpRight) {
			byteTx(CmdPlay);
			byteTx(BUMP_SONG);
			delayAndUpdateSensors(750);
			TurnAngle(-30);
			bumpcounter++;
		}		
	}


  // 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) && !(sensors[SenBumpDrop] & BumpEither) )
    {
	  drive(-300, RadStraight);
      delayAndUpdateSensors(20);
    }
  }
  else {
    while( (distance < goaldistance) && !(sensors[SenBumpDrop] & BumpEither) )
    {
	  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);
}