/* uC: ATMEGA88 */

#include <atmel_start.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>
#include <NRC_UDS_protocol.h>

#define F_CPU 8000000   // Clock Speed
#define BAUD 9600
#define MYUBRR ((F_CPU/(16*BAUD))-1)
#define RX_BUFFER_SIZE 50

unsigned char receivebuffer[RX_BUFFER_SIZE];
int indexBuffer;
bool dataReady = false;

void USART_Init( unsigned int ubrr)
{
    /*Set baud rate */
    UBRR0H = (ubrr>>8);
    UBRR0L = ubrr;
    
    /*Enable receiver and transmitter no interrupt enabled */
    UCSR0B |= (1<<RXEN0)|(1<<TXEN0);

    /* Set frame format: 8data, 1stop bit */
    /* config of BT module is (8 bits data, no parity bit, 1 stopbit) */
    
    UCSR0C |= (1<<UCSZ00) | (1<<UCSZ01);    
}

void fillReceiveBuffer(void)
{
    char data;

    if (dataReady) return;

    if ((UCSR0A & (1<<RXC0)))
    {
        data = UDR0;
        if (data = '\r') 
        {
            receivebuffer[indexBuffer] = 0;
            dataReady = true;
        } else if (indexBuffer < (RX_BUFFER_SIZE-1))
        {
            receivebuffer[indexBuffer++] = data;
        }   
    }       
}

void UART_Transmit( unsigned char data )
{
    /* Wait for empty transmit buffer */
    while ( !( UCSR0A & (1<<UDRE0)) );
    /* Put data into buffer, sends the data */
    UDR0 = data;
}

void stateMachine(void)
{
    if (!dataReady) return;

    switch (receivebuffer[0])
    {
        case UDS_WRITE_DATA:
            PORTC = 0xFF;
            UART_Transmit(0x44);
            if(receivebuffer[1]== 0x33)
            {
                UART_Transmit(0x55);
            }
            break;
            
        case UDS_READ_DATA:     
            PORTC = 0x00;
            UART_Transmit(0x77);
            if(receivebuffer[1]== 0x33)
            {
                UART_Transmit(0x55);                
            }
            break;
    }
    dataReady = false;      
    indexBuffer = 0;
}

int main( void )
{
    USART_Init(51); 
    DDRC |= (1 << DDC0) |(1 << DDC1)|(1 << DDC2) |(1 << DDC4)|(1 << DDC5);
    PORTC = 0x10;

    while(1)
    {
        fillReceiveBuffer();
        stateMachine();
    }
    return 1;
}