/* sensordemo.c
 *
 * A demonstration of how to read the Create's sensors.
 * Runs in three different modes (and demonstrates reading button presses as we change modes)
 * Mode 1:  Nose bump sensors.  Beeps high tone for left sensor, low tone for right sensor.
 * Mode 2:  Lookdown sensors.  Beeps four different tones for the four lookdown sensors.
 * Mode 3:  Lookright sensor.  Beeps tone changes as you move your hand towards the sensor.
 *
 */



// 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
#define	BEEP	4
#define BOOP	5
#define	TONE_1	6
#define	TONE_2	7
#define	TONE_3	8
#define	TONE_4	9



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




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

int main (void) 
{
	
	int mode = 0;	/* 1 = bump sensors, 2 = cliff sensors, 3 = wall sensor, 4 = top IR sensor*/
	int playpressed = 0, advancepressed = 0;
	int whichcliff;
	int i;

  // Set up Create and module
  initialize();
  LEDBothOff;
  powerOnRobot();
  byteTx(CmdStart);
  baud(Baud28800);
  defineSongs();
  byteTx(CmdControl);
  byteTx(CmdSafe);		// Make this "CmdFull" if you want to get out of safe mode

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

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

// This first loop lets us set the demonstration mode
	
 while (!playpressed) {
	delayAndUpdateSensors(20);
// Check for button press
	playpressed = sensors[SenButton] & ButtonPlay;
	advancepressed = sensors[SenButton] & ButtonAdvance;

// Has advance (>>|) button been pressed?	
	if (advancepressed) {
// turn advance led on
		byteTx(CmdLeds);
		byteTx(LEDAdvance);
		byteTx(0);
		byteTx(128);
		
// Increase mode counter, and boop this many times,		
		mode++;
		for (i = 0; i < mode; i++) {
			byteTx(CmdPlay);
			byteTx(BOOP);
			delayAndUpdateSensors(250);
		}
		
// turn advance led off
		byteTx(CmdLeds);
		byteTx(LEDAdvance);
		byteTx(0);
		byteTx(0);
	}	
  }
  
// end of setting up mode.  If mode is zero, don't do anything.
  if (mode == 0) {
	byteTx(CmdPlay);
	byteTx(BUMP_SONG);
	delayAndUpdateSensors(750);
	exit(0);
  }

// play start song
  byteTx(CmdPlay);
  byteTx(START_SONG);
  delayAndUpdateSensors(750);
  
 // main loop begins
 for(;;)
  {
    delayAndUpdateSensors(20);
	
// are we looking for the front bumpers?	
	if (mode == 1) {
		if(sensors[SenBumpDrop] & BumpLeft) {
			byteTx(CmdPlay);
			byteTx(BEEP);
			delayAndUpdateSensors(500);
		}
	 
		if(sensors[SenBumpDrop] & BumpRight) {
			byteTx(CmdPlay);
			byteTx(BOOP);
			delayAndUpdateSensors(500);
		}
	 }
	 
// are we checking for the look-down sensors?
	if (mode == 2) {
	
// For demo purposes, we assume the robot is upside down.  Thus, we only care about the
// sensors NOT showing a cliff.  That's why the "!" symbol.
	
		whichcliff = 0;
	
		if(!sensors[SenCliffL])
			whichcliff = TONE_1;
			
		if(!sensors[SenCliffFL])
			whichcliff = TONE_2;
			
		if(!sensors[SenCliffFR])
			whichcliff = TONE_3;
			
		if(!sensors[SenCliffR])
			whichcliff = TONE_4;
			
		if (whichcliff != 0) {
			byteTx(CmdPlay);
			byteTx(whichcliff);
			delayAndUpdateSensors(250);
		}
	 }
	 
// are we checking for the look-right wall sensor?

	if (mode == 3) {
		if(sensors[SenWall]) {
			byteTx(CmdPlay);
			byteTx(BEEP);
			delayAndUpdateSensors(500);
		}
		else {
			byteTx(CmdPlay);
			byteTx(BOOP);
			delayAndUpdateSensors(500);
		}
	 }
	 
// are we checking for the top IR sensor?

	 if (mode == 4) {
		if(sensors[SenVWall]) {
			byteTx(CmdPlay);
			byteTx(BEEP);
			delayAndUpdateSensors(250);
		}
		else {
			byteTx(CmdPlay);
			byteTx(BOOP);
			delayAndUpdateSensors(500);
		}
	}
	}
}
  


// 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 velocity, int16_t radius)
{
  byteTx(CmdDrive);
  byteTx((uint8_t)((velocity >> 8) & 0x00FF));
  byteTx((uint8_t)(velocity & 0x00FF));
  byteTx((uint8_t)((radius >> 8) & 0x00FF));
  byteTx((uint8_t)(radius & 0x00FF));
}




// Return an angle value in the range 53 to 180 (degrees)
uint16_t randomAngle(void)
{
    return (53 + ((uint16_t)(random() & 0xFF) >> 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
  byteTx(CmdSong);
  byteTx(BUMP_SONG);
  byteTx(2);
  byteTx(74);
  byteTx(12);
  byteTx(59);
  byteTx(24);

  // Hi beep 
  byteTx(CmdSong);
  byteTx(BEEP);
  byteTx(1);
  byteTx(96);
  byteTx(6);

  
    // Low boop 
  byteTx(CmdSong);
  byteTx(BOOP);
  byteTx(1);
  byteTx(72);
  byteTx(6);

// Tone 1 for lookdown sensors 
  byteTx(CmdSong);
  byteTx(TONE_1);
  byteTx(1);
  byteTx(100);
  byteTx(6);

  // Tone 2 for lookdown sensors 
  byteTx(CmdSong);
  byteTx(TONE_2);
  byteTx(1);
  byteTx(90);
  byteTx(6);
  
  // Tone 3 for lookdown sensors 
  byteTx(CmdSong);
  byteTx(TONE_3);
  byteTx(1);
  byteTx(80);
  byteTx(6);
    
  // Tone 4 for lookdown sensors 
  byteTx(CmdSong);
  byteTx(TONE_4);
  byteTx(1);
  byteTx(70);
  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);
}