/*
 * Master Firmware: Serial Parser
 *
 * Wireless Observational Modular Aerial Network
 * 
 * Ethan Zonca
 * Matthew Kanning
 * Kyle Ripperger
 * Matthew Kroening
 *
 */


#include <avr/io.h>
#include <avr/interrupt.h>
#include "../config.h"
#include "serial.h"
#include "serparser.h"
#include "led.h"

// Circular buffer for incoming data
uint8_t buffer[BUFFER_SIZE];

// Location of parser in the buffer
uint8_t bufferParsePosition = 0;

// Location of receive byte interrupt in the buffer
volatile uint16_t bufferDataPosition = 0;

// Parser state
uint8_t parserState = STATE_RESET;
uint8_t lastParserState = STATE_RESET;

// Length of current payload data (and checksum)
uint8_t payloadLength = 0;

// Data and checksum of most recent transmission
char receivedPayload[MAX_PAYLOAD_LEN];

// Payload type ID of the sensor of most recent transmission
char receivedPayloadType = 0;

// Checksum to be calculated and then compared to the received checksum
char checksumCalc = 0;

// Accessors
uint8_t getPayloadLength() 
{
	return payloadLength;
}
uint8_t* getPayload() 
{
	return receivedPayload;
}
uint8_t getPayloadType() {
	return receivedPayloadType;
}


// Could inline if program space available
static void setParserState(uint8_t state)
{
	lastParserState = parserState;
	parserState = state;
	
	// If resetting, clear vars
	if(state == STATE_RESET) 
	{
		payloadLength = 0;
		checksumCalc = 0;
	}
	
	// Every time we change state, we have parsed a byte
	bufferParsePosition = (bufferParsePosition + 1) % BUFFER_SIZE;
}

// Receive data on USART

char debugBuff[16];


ISR(USART0_RX_vect)
{
	buffer[bufferDataPosition % BUFFER_SIZE] = UDR0;
	bufferDataPosition = (bufferDataPosition + 1) % BUFFER_SIZE;
	//sprintf(debugBuff, "bdp: %d, bpp: %d \r\n", bufferDataPosition, bufferParsePosition);
	//serial0_sendString((debugBuff)); 
}




#define DEBUG

// Parse data from circular buffer
int serparser_parse(void)
{
	
	char byte;

	// Process first command (if any) on the circular buffer
	while(bufferDataPosition != bufferParsePosition)
	{
		byte = buffer[bufferParsePosition];
		
		// Reset 
		if(parserState == STATE_RESET)
		{
			if(byte == '[') // Start of frame; keep parsing
			{
				#ifdef DEBUG
				serial0_sendString("start\r\n");
				#endif
				setParserState(STATE_GETDATATYPE);
			}
			else // Not start of frame, reset
			{
				#ifdef DEBUG
				serial0_sendString("invalid\r\n");
				#endif
				setParserState(STATE_RESET);
				return PARSERESULT_NODATA; // no valid start bit; better luck next time. run the function the next time around the main loop.
			}
		}
		
		// Get payload type ID
		else if(parserState == STATE_GETDATATYPE)
		{
			#ifdef DEBUG
			serial0_sendString("type\r\n");
			#endif
			receivedPayloadType = byte; // Store the type of data receiving
			checksumCalc += byte;
			setParserState(STATE_GETDATA);
		}
		
		// Get payload data
		else if(parserState == STATE_GETDATA)
		{		
			if (byte == ']') // End of frame
			{
				#ifdef DEBUG
				serial0_sendString("eof\r\n");
				sprintf(debugBuff, "%d B, sum=%d\r\n", payloadLength, checksumCalc);
				serial0_sendString((debugBuff));
				#endif
				
				receivedPayload[payloadLength] = 0; // null-terminate string for atoi
				
				setParserState(STATE_GETCHECKSUM);
				// Checksum is now after the close bracket to solve bug FS#29		
				

			}
			else // Still receiving data
			{
				#ifdef DEBUG
				serial0_sendString("data\r\n");
				#endif
				receivedPayload[payloadLength] = byte;
				payloadLength++;
				checksumCalc += byte;
				
				// Data buffer overrun protection
				if(payloadLength > MAX_PAYLOAD_LEN) {
					#ifdef DEBUG
					serial0_sendString("ovf\r\n");
					#endif
					setParserState(STATE_RESET);
					return PARSERESULT_FAIL;
				}
				else {
					// Set state. MUST call even though state is maintained to update parse position
					setParserState(STATE_GETDATA);
					return PARSERESULT_STILLPARSING;
				}
				
			}

		}
		else if(STATE_GETCHECKSUM)
		{
			// TODO: Compare checksums
			if(byte == checksumCalc) {
				#ifdef DEBUG
				serial0_sendString("check\r\n");
				#endif
				setParserState(STATE_RESET);
				return PARSERESULT_PARSEOK;
			}
			else {
				#ifdef DEBUG
				serial0_sendString("bcheck\r\n");
				#endif
				setParserState(STATE_RESET);
				
				return PARSERESULT_FAIL;
			}
			
			/*
			if(bufferParsePosition == bufferDataPosition)
			{
				// We are at the end of the line. No more data to parse.
				setParserState(STATE_RESET);
				return PARSERESULT_PARSEOK;
			}
			else
			{
				setParserState(STATE_RESET);
				// we could choose to keep parsing now, or parse the next message next loop around (better idea).
				// return now so we hit it the next time around
				return PARSERESULT_PARSEOK;
			}
			*/
		}			
	}
	return PARSERESULT_NODATA;
	
}