// USART Ring Buffer, by Gierig 2013

#include "include.h"											//Basic / Config Include
#include <avr/io.h>
#include <avr/interrupt.h>
#include "usart.h"											
#include "motor.h"	


//Define some Static  Variables  & function (only reachable within this file)
static const __flash command_s s_cmd[] = {				//Command Struct is Const so we move it into the flash
		//CMD,	ptr to function,  how many Parameter +1
		{ 'r',  &MOTOR_Right, 2 },
		{ 'l',  &MOTOR_Left,  2 },
		{ 's',  &MOTOR_Stop,  0 },
		{ '1',  &Toggle_LED,  0 },		
		{ 'w',  &MOTOR_Forwrd, 1 }

	};


//RX
static uint8_t usart_rx_buffer[USART_RX_BUFFER_MAX];			//RX Ring Buffer
static uint8_t *usart_rx_read = usart_rx_buffer;				//read from buffer to function -> USART_RX_Byte 
static uint8_t *usart_rx_write= usart_rx_buffer;				//write to Buffer from USRAT-> ISR USART_RX
//TX
static uint8_t usart_tx_buffer[USART_TX_BUFFER_MAX];			//TX Ring Buffer
static uint8_t *usart_tx_write = usart_tx_buffer;				//write into buffer from function -> USART_TX_Byte
static uint8_t *usart_tx_read = usart_tx_buffer;				//read from buffer and send it via USRAT -> ISR USART_UDERE)
//Menu
static uint8_t usart_cmd_count = 0x00;							//Counts the incoming Bytes from RX Buffer.								
static pFunc8_t p_usart_cmd_fptr;								//function Pointer, store the command to execute
static uint8_t usart_cmd_buffer[USART_CMD_BUFFER_MAX];			//Buffer to store [0]->Command length and [1..MAX]->commandData

//Static Function, also only available within this file
static uint8_t USART_RX_Byte(void);								//get Byte and set Pointer to next Byte


//USART_INIT, setup Baud rate, RX IRQ ON)
void USART_Init(){
	UBRRH = UBRRH_VALUE;										// Baud rate with values define <setbaud.h>
	UBRRL = UBRRL_VALUE;	
	#if USE_2X													// U2X-Mode if need also from setbaud.h
	 UCSRA |= (1 << U2X);
	#else
	 UCSRA &= ~(1 << U2X);
	#endif
	UCSRB |= (1<<RXEN) | (1<<TXEN) | (1<<RXCIE);				// RX & TX engine & RX ISR to ON
	}


//Check USRAT Data and collect & executes Parameter and commands
void USART_Menu(void){
	uint8_t temp;
	temp=USART_RX_Byte();										//get actual byte content from RX buffer + Maint. buffer index
	switch (usart_cmd_count++){									//count incoming Byte
	 case 0x00:													//first incoming Byte contain the command
		for (uint8_t x = 0; x != SIZEOF_FP(s_cmd); x++){		//search command struct for a command	(SIZEOF_FP define in Globals.H)
			if (temp == s_cmd[x].Command){						//find the command....
				p_usart_cmd_fptr = s_cmd[x].p_ToFunction;		//..set command from struct data
				usart_cmd_buffer[0] = s_cmd[x].ParameterCount;	//..set parameter size from struct data
				usart_cmd_buffer[0]++;							//..add correction as we need a +1 logic (for Parinor :-))
				break;											//break the FOR loop
				}
			}
		if ( usart_cmd_buffer[0]  == 0 ){						//error handling if there is no command found
			usart_cmd_count = 0x00;								//reset incoming Byte Counter, so next time a new Command start
			USART_TX_Byte('#');
			break;
			}
		if (usart_cmd_buffer[0] == 1){							//Commands without extra Data are execute direct
			p_usart_cmd_fptr( usart_cmd_buffer);				//Execute Command (always a PTR with the command buffer as parameter)
			usart_cmd_count = 0x00;								//reset incoming Byte Counter, so next time a new Command start
			usart_cmd_buffer[0] = 0x00;							//Reset Command Length!
			//USART_TX_Byte('O');		
			break;
			}
	break; //end case 0x00
	default:													//Next incoming bytes...
		if ( usart_cmd_count == usart_cmd_buffer[0] ){			//Incoming Byte Counter, check if we have enough Bytes for actual Command
			usart_cmd_buffer[(usart_cmd_count-1)] = temp;		//Store last byte to command_buffer
			p_usart_cmd_fptr(usart_cmd_buffer);
			usart_cmd_count = 0x00;
			usart_cmd_buffer[0] = 0x00;
			//USART_TX_Byte('O');									//Send "O" for OK
			}
		  else {
			usart_cmd_buffer[(usart_cmd_count-1)] = temp;		//Fill the command buffer array with next byte
			}
	break;	//end default case			
  }//end Switch usart_cmd_count

}//END USART_Menu								


//read from buffer and return to function 
static uint8_t USART_RX_Byte(void){
	uint8_t temp;
// 	if(usart_rx_read == usart_rx_write){					//security checks if there is a byte on read buffer DISABLE save 20Byte flash
// 		USART_TX_Byte('F');	
// 		return 0;
// 		}		
	temp =*usart_rx_read;									//read from ring buffer
	usart_rx_read++;										//point to next byte in buffer
 	if (usart_rx_read == usart_rx_buffer + USART_RX_BUFFER_MAX){ //check if Buffer full now	
			usart_rx_read=usart_rx_buffer;					//reset to buffer start address (load Pointer with buffer start address)
		}
	return temp;	
	}//end RX_BYTE

	
//write into buffer from function & start USART TX UDR Empty ISR
void USART_TX_Byte(uint8_t x){
		*usart_tx_write = x;									//read from ring buffer
		usart_tx_write++;										//point to next byte in buffer
		if (usart_tx_write == usart_tx_buffer + USART_TX_BUFFER_MAX){ //check if Buffer full now
			usart_tx_write = usart_tx_buffer;					//reset to buffer start address.
		}
		 UCSRB |= (1<<UDRIE);									// turn on  the UDRE to start send out 		
	}//end


// Interrupts

//USART RX Interrupt, store a new incoming byte into Buffer & maintain RX_write pointer & activate CommandParser
ISR(USART_RX_vect){
			*usart_rx_write=UDR;								//store Data to write pointer address	
			usart_rx_write++;		
			if (usart_rx_write == usart_rx_buffer + USART_RX_BUFFER_MAX){ //check if Buffer Overflow  now				
					usart_rx_write = usart_rx_buffer;			//reset to buffer start Address.							
				}											
			GPIOR0  |= (1<<USART_NEW_BYTE);						//Set FLAG for action in Mainloop			
	}//end WS2812_LEDCount_RX_Vect


//USART TX UDR empty, write bytes from buffer till buffer is empty and disable itself, also maintain the TX_read pointer
ISR(USART_UDRE_vect){
	if( usart_tx_read == usart_tx_write ){						// if Buffer empty
		UCSRB &= ~(1<<UDRIE);									// lock the the UDRE  ISR
		return;
		}
	UDR=*usart_tx_read;											//Push out actual Data
	usart_tx_read++;											
	if (usart_tx_read == usart_tx_buffer + USART_TX_BUFFER_MAX){//check if Buffer full now	
		usart_tx_read = usart_tx_buffer;						//reset to buffer start Address.
		}
	}//end USART_TX_Vect