/*
* Company: Botronix
* Author: AJC
* Target Device: ATMEGA164P running on the Ranger Robot
* Description: The robot will drive in a forward direction until an obsacle
* is detected with the front bumper.  After a collision has been detected, the
* robot will stop, reverse direction, spin to a new direction, and drive 
* forward.
*/
#include <avr/io.h>
#include <avr/interrupt.h>
#include <stdio.h>

//Macros
#define bit_set(x,y) (x |= (1 << y))
#define bit_clr(x,y) (x &= ~(1 << y))

//Constants
#define TEST_SERVOS 1
#define TEST_LIGHT_SENSORS 0
#define BUMPER_2_LEDS 1

//Variables
const uint16_t LEFT_STOP = 1500;
const uint16_t RIGHT_STOP = 1500;

static int uart_putchar(char c, FILE *stream);
static FILE mystdout = FDEV_SETUP_STREAM(uart_putchar, NULL,
                                             _FDEV_SETUP_WRITE);

volatile uint8_t timer0_ovf_count, buzzer_tone;
uint8_t cnt_dir = 1;
volatile uint16_t adc_val, adc_val_0, adc_val_1, adc_val_2, adc_val_3, adc_val_4;
volatile uint8_t adc_int_flag = 0, adc_chan, blink_flag;

void ms_sleep(uint8_t ms)
{
  // Use 10000 = 1 second  Max 65534 or 6.5534 seconds
  //TCNT2  = 0;
  timer0_ovf_count = 0;
  while (timer0_ovf_count != ms);
}

/*###############################################
			Timer/Counter0 Overflow ISR
  ###############################################*/
  
ISR(TIMER0_OVF_vect)
{
  timer0_ovf_count++;
}

/*###############################################
			Timer/Counter0 Output Compare Match A ISR
  ###############################################*/
  
ISR(TIMER0_COMPA_vect)
{
  /*
  PB3 = Buzzer
  */
  
  //Toggle buzzer output pin
  if(bit_is_set(PINB,PB3))
    bit_clr(PORTB,PB3);
  else
    bit_set(PORTB,PB3);
    
  //setup next output compare register interrupt
  OCR0A += buzzer_tone;
}

/*###############################################
			Timer/Counter1 Output Compare Match A ISR
  ###############################################*/
  
ISR(TIMER1_COMPA_vect)
{
}

/*###############################################
			Timer/Counter1 Output Compare Match B ISR
  ###############################################*/
  
ISR(TIMER1_COMPB_vect)
{
}

/*###############################################
			ADC Conversion Complete ISR
  ###############################################*/
  
ISR(ADC_vect)
{
  adc_val = ADCW;
  ADCSRA &= ~_BV(ADIE);		// disable ADC interrupt
  adc_int_flag = 1;
}

/*===============================================
    InitPorts
    
		Initialize I/O Ports
  ===============================================*/

void InitPorts(void)
{

  /*********************************************
  					PORTA
  
   PA0 = Microphone
   PA1 = ADC1 (Light Sensor Front Left)
   PA2 = ADC2 (Light Sensor Rear Left)
   PAA = ADC3 (Light Sensor Front Right)
   PA4 = ADC4 (Light Sensor Rear Right)
   PA5 = Dip switch bit0
   PA6 = Dip switch bit1
   PA7 = Program
  **********************************************/
  
  PORTA = 0b00011110; // enable input pin pullups (1) 
  
  DDRA 	= 0b00000000;	//0 is input, 1 is output
  
  
  /*********************************************
  					PORTB
  
   PB0 = Left side odometry direction
   PB1 = Right side odometry direction
   PB2 = Right side front bumper signal
   PB3 = Buzzer
   PB4 = Left side front bumper signal
   PB5 = SPI MOSI
   PB6 = SPI MISO
   PB7 = SPI SCK
  **********************************************/
  
  //Enable all input pin pullups (1) and set all outputs to zero
  PORTB = 0b01010111;
  
  DDRB 	= 0b10101000;	//0 is input, 1 is output
  
  
  /*********************************************
  					PORTC
  
   PC0 = GPIO/I2C clock
   PC1 = GPIO/I2C data
   PC2 = LED4/Line detector signal 0
   PC3 = LED3/Line detector signal 1
   PC4 = LED2/Line detector signal 2
   PC5 = LED1/Line detector signal 3
   PC6 = Line detector signal 4
   PC7 = Line detector signal 5
  **********************************************/
  
  //Enable all input pin pullups (1) and set all outputs to zero
  PORTC = 0b11000011;
  
  DDRC 	= 0b00111100;	//0 is input, 1 is output
  
  
  /*********************************************
  					PORTD
  
   PD0 = Serial Comms RX
   PD1 = Serial Comms TX
   PD2 = Left side odometry trigger
   PD3 = Right side odometry trigger
   PD4 = Right side propulsion servo control signal
   PD5 = Left side propulsion servo control signal
   PD6 = Servo 0 control signal
   PD7 = Servo 1 control signal
  **********************************************/
  
  //Enable input pin pullups (1) and set all outputs to zero
  PORTD = 0b00001101;
    
  DDRD 	= 0b11110010;	//0 is input, 1 is output
  
  
}

/*===============================================
		InitUART()
    
    Initialize UART
  ===============================================*/
void InitUART(void)
{

  //UCSR0A: leave default values
  
  UCSR0B = _BV(RXEN0) | _BV(TXEN0);
  
  //UCSR0C: leave default values
  
  //38.4K baud
  UBRR0H = 0b00000000;
  UBRR0L = 0b00011001;
  
  //115.2K baud
  //UBRR0H = 0b00000000;
  //UBRR0L = 0b00001000;

}

/*===============================================
    InitADC
    
		Initialize Analog to Digital Converter
  ===============================================*/
void InitADC(void)
{
  /*
  Reference Selection = AVcc
    REFS1 = 0
    REFS0 = 1
  
  ADC Left Adjust Result = Disabled
    ADLAR = 0
  
  Analog Channel and Gain Selection = Single-ended ADC1 (Light Sensor Front
                                      Left), 1x Gain
    MUX4 = 0
    MUX3 = 0
    MUX2 = 0
    MUX1 = 0
    MUX0 = 1
   
  ADC Enable = Enable
    ADEN = 1
  
  ADC Start Conversion = No not start
    ADSC = 0
  
  ADC Auto Trigger Enable = Disable
    ADATE = 0
  
  ADC Interrupt Enable = Enable
    ADIE = 1
  
  ADC Prescaler Select = 128
    ADPS2 = 1
    ADPS1 = 1
    ADPS0 = 1
  
  ADC Auto Trigger Source = Free Running Mode
    ADTS2 = 0
    ADTS1 = 0
    ADTS0 = 0
  
  Digital Input Disable
    ADC7D = 0
    ADC6D = 0
    ADC5D = 0
    ADC4D = 0
    ADC3D = 1
    ADC2D = 1
    ADC1D = 1
    ADC0D = 1
  */
  
  ADMUX = _BV(REFS0);
  ADCSRA = _BV(ADEN) | _BV(ADIE) | _BV(ADPS2) | _BV(ADPS1) | _BV(ADPS0);
  DIDR0 = _BV(ADC3D) | _BV(ADC2D) |_BV(ADC1D) | _BV(ADC0D);
  
  adc_chan = 1;  

}

/*===============================================
    InitTimer0()
 
    Initialize Timer/Counter0
    Timer/Counter0 is used to generate blinking
    LED and buzzer waveforms 
  ===============================================*/
void InitTimer0(void)
{
  /*
  Compare Match Output A Mode = Normal port operation, OC0A disconnected
    COM0A1 = 0
    COM0A0 = 0
      
  Compare Match Output B Mode = Normal port operation, OC0B disconnected
    COM0B1 = 0
    COM0B0 = 0
      
  Force Output Compare A = off
    FOC0A = 0
  
  Force Output Compare B = off
    FOC0B = 0
    
  Wave Form Generation Mode = Normal
    WGM02 = 0
    WGM01 = 0
    WGM00 = 0
  
  Clock source = clki/o   
  Prescaler = 1024
    CS02 = 1
    CS01 = 0
    CS00 = 0
  
  Timer/Counter0 Output Compare Match A Interrupt = Disable
    OCIE0A = 0
  
  Timer/Counter0 Output Compare Match B Interrupt = Disable
    OCIE0B = 0
        
  Timer/Counter0 Overflow Interrupt = Enable
    TOIE0 = 1
    
  Drive buzzer with 4KHz square wave from 
  Timer/Counter0
  
  TimerCounter0 overflow interrupt timing:
  16Mhz/1024 = 15625Hz
  15625Hz/256 = 61Hz
  
  61Hz is well below the needed 4KHz.  The 
  output compare match A will be enabled.  
  Set OCR0A = 2
  */
  
  TCCR0B = _BV(CS02) | _BV(CS00);
  TIMSK0 = _BV(OCIE0A) | _BV(TOIE0);				// Overflow Interrupt Enable
  OCR0A = 0x02;
}

/*===============================================
    InitTimer1
 
    Initialize Timer/Counter1
    Timer/Counter1 is used to generate waveforms
    for the propulsion servos 
  ===============================================*/
void InitTimer1(void)
{
  /*
  Compare Match Output A Mode = Clear OC1A on Compare Match when up-counting
                                Set OC1A on Compare Match when down-counting
    COM1A1 = 1
    COM1A0 = 0
      
  Compare Match Output B Mode = Normal port operation, OC0B disconnected
    COM1B1 = 1
    COM1B0 = 0
  
  Force Output Compare A = off
    FOC1A = 0
  
  Force Output Compare B = off
    FOC1B = 0
    
  Wave Form Generation Mode = PWM, Phase and Frequency Correct, TOP = ICR1
    WGM13 = 1
    WGM12 = 0
    WGM11 = 0
    WGM10 = 0
  
  Input Noise Canceler = Disable
    ICNC1 = 0
  
  Input Capture Edge Select = Falling
    ICNS1 = 0
    
  Clock source = clki/o   
  Prescaler = 8
    CS12 = 0
    CS11 = 1
    CS10 = 0
  
  Timer/Counter1 Output Compare Match A Interrupt = Enable
    OCIE1A = 1
    
  Timer/Counter1 Output Compare Match B Interrupt = Disable
    OCIE1B = 1
        
  Timer/Counter0 Overflow Interrupt = Disable
    TOIE1 = 0
  */
  
  //Set ICR1 to 20,000 to start pulse at a rate of 50Hz
  ICR1 = 20000;
  
  //OCR range ~= 1000 to 2000
  //Default OCRn to all stop
  OCR1A = LEFT_STOP;
  OCR1B = RIGHT_STOP;
  
  TCCR1A = _BV(COM1A1) | _BV(COM1B1);
  TCCR1B = _BV(WGM13) | _BV(CS11);
  TIMSK1 = _BV(OCIE1B) | _BV(OCIE1A);				// Overflow Interrupt Enable
  
}


/*===============================================
    TestPCComms()
 
    Send serial data to PC
  ===============================================*/
void TestPCComms(void)
{
	uint8_t char_array[10] = {10, 10, 10, 10, 13, 'H','e','l','l','o'};
	uint8_t index;

	for(index = 0; index < 10; index = index + 1)
	{
	
	// Wait for empty transmit buffer
	while ( !( UCSR0A & (1<<UDRE0)) );
	// Put data into buffer, sends the data 
	UDR0 = char_array[index];

	}

	printf("\n\rBegin Ranger Test...");
	
	printf("\n\rPress an Alphanumeric key to continue");
	/* Wait for data to be received */
  while ( !(UCSR0A & (1<<RXC0)) );
  
  printf("\n\r  You pressed: %c", UDR0);
  printf("\n\r");

}

/*
 * Send character c down the UART Tx, wait until tx holding register
 * is empty.
 */
int uart_putchar(char c, FILE *stream)
{

  if (c == '\a')
    {
      fputs("*ring*\n", stderr);
      return 0;
    }

  if (c == '\n')
    uart_putchar('\r', stream);
  loop_until_bit_is_set(UCSR0A, UDRE0);
  UDR0 = c;

  return 0;
}

/*===============================================
    Bumper2LEDs

    Illuminate LED1 and LED4 based on bumper
  ===============================================*/
void Bumper2LEDs(void)
{
  /*
	PD6 = Right side front bumper signal
	PD7 = Left side front bumper signal
	
	PC2 = LED4/Line detector signal 0
	PC5 = LED1/Line detector signal 3
	
	LED1(PC5) = Right bumper switch (PD6)
	LED4(PC2) = Left bumper switch (PD7)	
  */

#if 0
  //METHOD 1: Non-macro
  {	
  	if(PINB & 0b00010000)
  	{
  		//Turn off LED1
  		PORTC = PORTC | 0b00100000;			
  	}
  	else
  	{
  		//Turn on LED1
  		PORTC = PORTC & 0b11011111;
  	}
    
  	if(PINB & 0b00001000)
  	{
  		//Turn off LED4
  		PORTC = PORTC | 0b00000100;
  	}
  	else
  	{
  		//Turn on LED4
  		PORTC = PORTC & 0b11111011;
    }
  }
#else
  //METHOD2: Using macros	
  {	
    if(bit_is_set(PINB, 4))
  		bit_set(PORTC, PC5);
  	else
  	  bit_clr(PORTC, PC5);
    
    if(bit_is_set(PINB, 2))
  		bit_set(PORTC,PC2);
  	else
  		bit_clr(PORTC,PC2);
  }
#endif
      
}

/*===============================================
    DIP2LEDs

    Illuminate LED1 and LED4 based on DIP switch
  ===============================================*/
void DIP2LEDs(void)
{
  /*
  PA5 = Dip switch 1
  PA6 = Dip switch 2
  
	PC2 = LED4/Line detector signal 0
	PC5 = LED1/Line detector signal 3
	
	LED1(PC5) = DIP switch 1 (PA6)
	LED4(PC2) = DIP switch 2 (PA5)
  */ 
  
  if(bit_is_set(PINA,PINA5))
    bit_set(PORTC,PC2);
  else
    bit_clr(PORTC,PC2);
    
  if(bit_is_set(PINA,PINA6))
    bit_set(PORTC,PC5);
  else
    bit_clr(PORTC,PC5);
    
}
/*===============================================
    BlinkLED3

    blink LED3
  ===============================================*/
void BlinkLED3(void)
{
  /*
  PC3 = LED3/Line detector signal 1
  */
  
  if(timer0_ovf_count == 8)
  { 
    blink_flag = 1;
       
    if(bit_is_clear(PINC, PINC3))
			//PORTC = pctemp | 0b00000100;
			bit_set(PORTC,PC3);
		else
			//PORTC = pctemp & 0b11111011;
			bit_clr(PORTC,PC3);
		
    //Clear Timer0 Overflow interrupt counter	
    timer0_ovf_count = 0;
  }

}

/*===============================================
    TestBuzzer

    Turn on buzzer when DIP switch 1 is on
  ===============================================*/

void TestBuzzer(void)
{
  /*
  PA5 = Dip switch 1
  PA6 = Dip switch 2
  PB3 = Buzzer
  */
  
  //Turn ON/OFF Buzzer
  if(bit_is_set(PINA, PINA5))
  {
    cli();
    //Enable Counter/Timer0 Output Compare A
    bit_set(TIMSK0,OCIE0A);
    sei();
  }
  else
  {
    cli();
    //Enable Counter/Timer0 Output Compare A
    bit_clr(TIMSK0,OCIE0A);
    sei();
  }

  //Set Buzzer's Tone
  if(bit_is_set(PINA, PINA6))
    buzzer_tone = 2;
  else
    buzzer_tone = 3;
  
}

/*===============================================
    TestServos

    Run propulsion servos
  ===============================================*/
void TestServos(void)
{
  uint16_t ocr1a_temp;
  
  cli();
  ocr1a_temp = OCR1A;
  sei();
  
  if(ocr1a_temp <= 1000)
    //start up-count
    cnt_dir = 1;
  
  if(ocr1a_temp >= 2000)
    //start down-count
    cnt_dir = 0;
  
//  if(timer0_ovf_count == 8)
  if(blink_flag)
  {
    //printf("\n\r%i",ocr1a_temp);
    
    if(cnt_dir)
    {
      cli();
      OCR1A += 100;
      OCR1B -= 100;
      sei();
    }
    else
    {
      cli();
      OCR1A -= 100;
      OCR1B += 100;
      sei();
    }
    
    blink_flag = 0;
  }
}

/*===============================================
    TestLightSensors

    Test ADC by acquiring light sensor data
  ===============================================*/
void TestLightSensors(void)
{
  /*
  PA1 = ADC1 (Light Sensor Front Left)
  PA2 = ADC2 (Light Sensor Front Right)
  PAA = ADC3 (Light Sensor Rear Left)
  PA4 = ADC4 (Light Sensor Rear Right)
   
  ADC Start Conversion by writing 1 to ADSC in ADCSRA register
  */
  
  uint8_t i;
  
  if(adc_int_flag)
  {
    switch(adc_chan)
    {
      case 1:
        adc_val_1 = adc_val;
        //Setup for ADC2
        ADMUX &= ~_BV(MUX0);
        ADMUX |= _BV(MUX1);
        adc_chan = 2;
        break;
      case 2:
        adc_val_2 = adc_val;
        //Setup for ADC3
        ADMUX |= _BV(MUX0);
        adc_chan = 3;
        break;
      case 3:
        adc_val_3 = adc_val;
        //Setup for ADC4
        ADMUX &= ~_BV(MUX1) & ~_BV(MUX0);
        ADMUX |= _BV(MUX2);
        adc_chan = 4;
        break;
      case 4:
        adc_val_4 = adc_val;
        //Setup for ADC1
        ADMUX &= ~_BV(MUX2);
        ADMUX |= _BV(MUX0);
        adc_chan = 1;
        
        //print results
        printf("%i\t\t%i\t\t%i\t\t%i", adc_val_1, adc_val_2, adc_val_3,
                                       adc_val_4 );
        for(i = 0; i < 60; i++)
          printf("\b");

        break;
      default:
        //Setup for ADC1
        ADMUX &= ~_BV(MUX2);
        ADMUX |= _BV(MUX0);
        adc_chan = 1;
        break;
    }

    
    adc_int_flag = 0;
      
    //Start one conversion
    ADCSRA |= _BV(ADIE);
    ADCSRA |= _BV(ADSC);
  }


}

/*===============================================
    TestMic

    Test Microphone by acquiring sensor data
  ===============================================*/
void TestMic(void)
{
  /*
  PA0 = ADC0 (Microphone)
   
  ADC Start Conversion by writing 1 to ADSC in ADCSRA register
  */
  
  uint8_t i;
  
  ADMUX &= ~_BV(MUX4) & ~_BV(MUX3) & ~_BV(MUX2) & ~_BV(MUX1) & ~_BV(MUX0);
  
  if(adc_int_flag)
  {
    
    adc_val_0 = adc_val/16;
    printf(" %i", adc_val);
    for(i = 0; i < 5; i++)
      printf("\b");  //backspace

    
    //printf("\n\r");
//    for(i = 0; i < adc_val_0; i++)
//      printf(" ");
//    printf("|");
    
        
    /*for(i = 0; i < (adc_val + 1); i++)
      printf("\b");*/
        
    adc_int_flag = 0;
      
    //Start one conversion
    ADCSRA |= _BV(ADIE);
    ADCSRA |= _BV(ADSC);
  }


}

/*===============================================
    Main
  ===============================================*/
int main(void)
{
  InitPorts();
  InitADC();
	InitTimer0();
  InitTimer1();
  
  InitUART();
  stdout = &mystdout;
  TestPCComms();
  /* enable interrupts */
  sei();	
  
  printf("\n\rLED3 is blinking");
  printf("\n\rSet DIP switch 1 down to turn on Buzzer");
  printf("\n\rSet DIP switch 2 to change Buzzer's tone");
  
  if (BUMPER_2_LEDS)
  {
		printf("\n\rTrigger right bumper to illuminate LED1");
	  printf("\n\rTrigger left bumper to illuminate LED4");
	}
  
  if (TEST_SERVOS)
  {
  	printf("\n\rWheels should alternately moving CW and CCW");
  }
  
  if (TEST_LIGHT_SENSORS)
  {
		printf("\n\n\rLight Sensor inputs:");
	  printf("\n\rFront Left\tRear Left\tFront Right\tRear Right");
	}
	
  printf("\n\r");
  
  //Clear LEDs (LEDs are active low!!)
	PORTC = PORTC | 0b00111100;

	ADMUX |= _BV(MUX0);
	
	//Start one ADC conversion
	ADCSRA |= _BV(ADIE);
  ADCSRA |= _BV(ADSC);
	
	while(1)
  {
  
  
    //Call either Bumper2LEDs or DIP2LEDs, but not both simultaneously
    if (BUMPER_2_LEDS)
		{
			Bumper2LEDs();
		}
		else
		{
    	DIP2LEDs();
    }
    
    BlinkLED3();
    TestBuzzer();
    
    if (TEST_SERVOS)
  	{
    	TestServos();
    }
    
    if (TEST_LIGHT_SENSORS)
  	{
    	TestLightSensors();
		}
		  
    TestMic();

	}

  return 0;

}