/* drive.c
 * Designed to run on Create Command Module
 *
 * The basic architecture of this program can be re-used to easily 
 * write a wide variety of Create control programs.  All sensor values
 * are polled in the background (using the serial rx interrupt) and 
 * stored in the sensors array as long as the function 
 * delayAndUpdateSensors() is called periodically.  Users can send commands
 * directly a byte at a time using byteTx() or they can use the 
 * provided functions, such as baud() and drive().
 */



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


// Constants
#define RESET_SONG 0
#define START_SONG 1
#define BUMP_SONG_LEFT  2
#define BUMP_SONG_RIGHT 3
#define BUMP_SONG_BOTH 4
#define END_SONG   5

#define BEHAVIOR_STOP 0
#define BEHAVIOR_ESCAPE 1
#define BEHAVIOR_AVOID 2
#define BEHAVIOR_CRUISE 3
#define BEHAVIOR_MAX_ID 4

#define ESCAPE_NO_BUMP 0
#define ESCAPE_BACK_UP 1
#define ESCAPE_TURN 2

// Global variables
volatile uint16_t timer_cnt = 0;
volatile uint8_t timer_on = 0;
volatile uint8_t sensors_flag = 0;
volatile uint8_t sensors_index = 0;
volatile uint8_t sensors_in[Sen6Size];
volatile uint8_t sensors[Sen6Size];
volatile int16_t distance = 0;
volatile int16_t angle = 0;

//for behavior control
unsigned char behavior_control[BEHAVIOR_MAX_ID]; //takes control requests from behaviors
unsigned char winner; //for winner of behavior arbitration
volatile int16_t commands[BEHAVIOR_MAX_ID][2]; /* for actions from behaviors
	commands[BEHAVIOR_ID][0] = right wheel v
	commands[BEHAVIOR_ID][1] = left wheel v */


// Functions
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);
uint16_t randomAngle(void);
void defineSongs(void);

//functions for behaviors
unsigned char arbitrate(void);

void cruise(void);
void avoid(void);
void escape(void);

int main (void) 
{
  // Set up Create and module
  initialize();
  LEDBothOff;
  powerOnRobot();
  byteTx(CmdStart);
  baud(Baud28800);
  defineSongs();
  byteTx(CmdControl);
  byteTx(CmdFull);

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

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

  //set power led on and green (and not too bright)
  byteTx(CmdLeds);
  byteTx(0x00);
  byteTx(0);
  byteTx(32);

  while(1)
  {
    if(UserButtonPressed)
    {
      // Play start song and wait
      byteTx(CmdPlay);
      byteTx(START_SONG);
      delayAndUpdateSensors(2813);

      // Drive around until a button or unsafe condition is detected
      while(!(UserButtonPressed)
            && (!sensors[SenCliffL])
            && (!sensors[SenCliffFL])
            && (!sensors[SenCliffFR])
            && (!sensors[SenCliffR])
            && (!sensors[SenChAvailable])
        )
      {

        delayAndUpdateSensors(10);
		
        //behaviors do their thing
		cruise();
		escape();
		avoid();
		
		//then decide what to do
		winner = arbitrate();
		
		if(!winner){
		  //no winner, stop
		  drive(0,0);
		}
		else{
		  drive(commands[winner][0],commands[winner][1]);
		}
		
		//handle LEDs & beeps
        if(sensors[SenBumpDrop] & BumpEither)  // Check for a bump
        {
          // Set the turn parameters and reset the angle
          if(sensors[SenBumpDrop] & BumpLeft)
            LED2On;
          if(sensors[SenBumpDrop] & BumpRight)
            LED1On;

          // Play the bump song
          byteTx(CmdPlay);
		  if(!(sensors[SenBumpDrop] ^ BumpBoth)){
			byteTx(BUMP_SONG_BOTH);
		  }
		  else if(sensors[SenBumpDrop] & BumpLeft){
			byteTx(BUMP_SONG_LEFT);
		  }
		  else{
			byteTx(BUMP_SONG_RIGHT);
		  }
        }		
        else
        {
          // Otherwise,stop
		  LEDBothOff;
        }

        // wait a little more than one robot tick for sensors to update
        delayAndUpdateSensors(20);
      }

      // Stop driving
      drive(0, 0);

	  //set power led on and green (and not too bright)
	  byteTx(CmdLeds);
	  byteTx(0x00);
	  byteTx(0);
	  byteTx(32);
	  
	  //reset CM leds
	  LEDBothOff;

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

    }
  }
}

// behavior functons
unsigned char arbitrate(void){
  extern unsigned char behavior_control[];
  unsigned char winner;
  
  for(winner = 0; winner < BEHAVIOR_MAX_ID; winner++){
    if(behavior_control[winner]){
	  return winner;
	}
  }
  
  //nobody wants control
  return BEHAVIOR_STOP;
}

void cruise(void){

  //always wants control
  behavior_control[BEHAVIOR_CRUISE] = BEHAVIOR_CRUISE;
  
  //go straight, slowly
  commands[BEHAVIOR_CRUISE][0] = 40;
  commands[BEHAVIOR_CRUISE][1] = 40;
  
}

void avoid(void){
  behavior_control[BEHAVIOR_AVOID] = 0;
}

void escape(void){
   static signed char escape_angle;
   static unsigned char state = ESCAPE_NO_BUMP;

	//punt control - and grab it later if wanted
	behavior_control[BEHAVIOR_ESCAPE] = 0;
   
   switch(state)
   {
		case ESCAPE_NO_BUMP:
			if(sensors[SenBumpDrop] & BumpEither){  // Check for a bump
				//some kind of bump occured, i want control
				behavior_control[BEHAVIOR_ESCAPE] = BEHAVIOR_ESCAPE;
				
				//first step - backup
				distance = 0;
				state = ESCAPE_BACK_UP;
				
				//then turn to this angle:
				escape_angle = 15 + randomAngle();
				angle = 0;
				
				if(sensors[SenBumpDrop] & BumpRight){
				  //bump on the right, want a negative turn
				  escape_angle = -1 * escape_angle;
				}
			}
			break;
		
		case ESCAPE_BACK_UP:
			behavior_control[BEHAVIOR_ESCAPE] = BEHAVIOR_ESCAPE;

			if(distance > -50){
				commands[BEHAVIOR_ESCAPE][0] = -250;
				commands[BEHAVIOR_ESCAPE][1] = -250;
			}
			else{
				//all the way bakced up, time to turn
				state = ESCAPE_TURN;
				distance = 0;
			}
			break;
		
		case ESCAPE_TURN:
			if(abs(angle) < abs(escape_angle)){
				//still turning
				behavior_control[BEHAVIOR_ESCAPE] = BEHAVIOR_ESCAPE;
				
				if(escape_angle >= 0)
				{
					//right turn
					commands[BEHAVIOR_ESCAPE][0] = -300;
					commands[BEHAVIOR_ESCAPE][1] = 300;
				}
				else{
					//left turn
					commands[BEHAVIOR_ESCAPE][0] = 300;
					commands[BEHAVIOR_ESCAPE][1] = -300;
				}
			}
			else{
				//turned all the way
				state = ESCAPE_NO_BUMP;
			}
			break;
	}
   
}
// end behavior functions


// Serial receive interrupt to store sensor values
SIGNAL(SIG_USART_RECV)
{
  uint8_t temp;


  temp = UDR0;

  if(sensors_flag)
  {
    sensors_in[sensors_index++] = temp;
    if(sensors_index >= Sen6Size)
      sensors_flag = 0;
  }
}




// Timer 1 interrupt to time delays in ms
SIGNAL(SIG_OUTPUT_COMPARE1A)
{
  if(timer_cnt)
    timer_cnt--;
  else
    timer_on = 0;
}




// Transmit a byte over the serial port
void byteTx(uint8_t value)
{
  while(!(UCSR0A & _BV(UDRE0))) ;
  UDR0 = value;
}




// Delay for the specified time in ms without updating sensor values
void delayMs(uint16_t time_ms)
{
  timer_on = 1;
  timer_cnt = time_ms;
  while(timer_on) ;
}




// Delay for the specified time in ms and update sensor values
void delayAndUpdateSensors(uint16_t time_ms)
{
  uint8_t temp;

  timer_on = 1;
  timer_cnt = time_ms;
  while(timer_on)
  {
    if(!sensors_flag)
    {
      for(temp = 0; temp < Sen6Size; temp++)
        sensors[temp] = sensors_in[temp];

      // Update running totals of distance and angle
      distance += (int)((sensors[SenDist1] << 8) | sensors[SenDist0]);
      angle += (int)((sensors[SenAng1] << 8) | sensors[SenAng0]);

      byteTx(CmdSensors);
      byteTx(6);
      sensors_index = 0;
      sensors_flag = 1;
    }
  }
}




// Initialize the Mind Control's ATmega168 microcontroller
void initialize(void)
{
  cli();

  // Set I/O pins
  DDRB = 0x10;
  PORTB = 0xCF;
  DDRC = 0x00;
  PORTC = 0xFF;
  DDRD = 0xE6;
  PORTD = 0x7D;

  // Set up timer 1 to generate an interrupt every 1 ms
  TCCR1A = 0x00;
  TCCR1B = (_BV(WGM12) | _BV(CS12));
  OCR1A = 71;
  TIMSK1 = _BV(OCIE1A);

  // Set up the serial port with rx interrupt
  UBRR0 = 19;
  UCSR0B = (_BV(RXCIE0) | _BV(TXEN0) | _BV(RXEN0));
  UCSR0C = (_BV(UCSZ00) | _BV(UCSZ01));

  // Turn on interrupts
  sei();
}




void powerOnRobot(void)
{
  // If Create's power is off, turn it on
  if(!RobotIsOn)
  {
      while(!RobotIsOn)
      {
          RobotPwrToggleLow;
          delayMs(500);  // Delay in this state
          RobotPwrToggleHigh;  // Low to high transition to toggle power
          delayMs(100);  // Delay in this state
          RobotPwrToggleLow;
      }
      delayMs(3500);  // Delay for startup
  }

}




// Switch the baud rate on both Create and module
void baud(uint8_t baud_code)
{
  if(baud_code <= 11)
  {
    byteTx(CmdBaud);
    UCSR0A |= _BV(TXC0);
    byteTx(baud_code);
    // Wait until transmit is complete
    while(!(UCSR0A & _BV(TXC0))) ;

    cli();

    // Switch the baud rate register
    if(baud_code == Baud115200)
      UBRR0 = Ubrr115200;
    else if(baud_code == Baud57600)
      UBRR0 = Ubrr57600;
    else if(baud_code == Baud38400)
      UBRR0 = Ubrr38400;
    else if(baud_code == Baud28800)
      UBRR0 = Ubrr28800;
    else if(baud_code == Baud19200)
      UBRR0 = Ubrr19200;
    else if(baud_code == Baud14400)
      UBRR0 = Ubrr14400;
    else if(baud_code == Baud9600)
      UBRR0 = Ubrr9600;
    else if(baud_code == Baud4800)
      UBRR0 = Ubrr4800;
    else if(baud_code == Baud2400)
      UBRR0 = Ubrr2400;
    else if(baud_code == Baud1200)
      UBRR0 = Ubrr1200;
    else if(baud_code == Baud600)
      UBRR0 = Ubrr600;
    else if(baud_code == Baud300)
      UBRR0 = Ubrr300;

    sei();

    delayMs(100);
  }
}




// Send Create drive commands in terms of velocity and radius
void drive(int16_t left_velocity, int16_t right_velocity)
{
	//nb - this is changed from the default drive command to control each wheel independantly

  //check for valid input - mins and maxes defined by iRobot
  if(left_velocity < -500){
    return error(ERROR_LEFT_VELOCITY_TOO_LOW);
  }else if(left_velocity > 500){
    return error(ERROR_LEFT_VELOCITY_TOO_HIGH);
  }else if(right_velocity < -500){
    return error(ERROR_RIGHT_VELOCITY_TOO_LOW);
  }else if(right_velocity > 500){
    return error(ERROR_RIGHT_VELOCITY_TOO_HIGH);
  }
  
  byteTx(CmdDriveWheels);
  byteTx((uint8_t)((left_velocity >> 8) & 0x00FF));
  byteTx((uint8_t)(left_velocity & 0x00FF));
  byteTx((uint8_t)((right_velocity >> 8) & 0x00FF));
  byteTx((uint8_t)(right_velocity & 0x00FF));

}




// Return an angle value in the range 0 to 45 (degrees)
uint16_t randomAngle(void)
{
    return (0 + ((uint16_t)(random() & 0x2D) >> 1));
}



// Define songs to be played later
void defineSongs(void)
{
  // Reset song
  byteTx(CmdSong);
  byteTx(RESET_SONG);
  byteTx(4);
  byteTx(60);
  byteTx(6);
  byteTx(72);
  byteTx(6);
  byteTx(84);
  byteTx(6);
  byteTx(96);
  byteTx(6);

  // Start song
  byteTx(CmdSong);
  byteTx(START_SONG);
  byteTx(6);
  byteTx(69);
  byteTx(18);
  byteTx(72);
  byteTx(12);
  byteTx(74);
  byteTx(12);
  byteTx(72);
  byteTx(12);
  byteTx(69);
  byteTx(12);
  byteTx(77);
  byteTx(24);

  // Bump song left
  byteTx(CmdSong);
  byteTx(BUMP_SONG_LEFT);
  byteTx(2);
  byteTx(74);
  byteTx(12);
  byteTx(59);
  byteTx(24);

  // Bump song right
  byteTx(CmdSong);
  byteTx(BUMP_SONG_RIGHT);
  byteTx(2);
  byteTx(59);
  byteTx(12);
  byteTx(74);
  byteTx(24);

  // bump song both
  byteTx(CmdSong);
  byteTx(BUMP_SONG_BOTH);
  byteTx(4);
  byteTx(74);
  byteTx(6);
  byteTx(74);
  byteTx(6);
  byteTx(59);
  byteTx(6);
  byteTx(59);
  byteTx(6);

  // End song
  byteTx(CmdSong);
  byteTx(END_SONG);
  byteTx(5);
  byteTx(77);
  byteTx(18);
  byteTx(74);
  byteTx(12);
  byteTx(72);
  byteTx(12);
  byteTx(69);
  byteTx(12);
  byteTx(65);
  byteTx(24);
}