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

			}

			else // Not start of frame, reset

			{

				#ifdef DEBUG

				#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 destination module ID

		else if(parserState == STATE_GETID)

		{

			if(byte == MODULE_ID) // Message intended for this module; keep parsing

			{

				#ifdef DEBUG

				serial0_sendString("dest\r\n");

				#endif

				checksumCalc += byte;

				setParserState(STATE_GETDATATYPE);

			}

			else // Transmission is intended for another module; reset

			{

				#ifdef DEBUG

				#endif

				setParserState(STATE_RESET);

			}

		}

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

			{

				serial0_sendString("eof\r\n");

				sprintf(debugBuff, "%d B, sum=%d\r\n", payloadLength, checksumCalc);

				serial0_sendString((debugBuff));

				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) {

				serial0_sendString("check\r\n");

				setParserState(STATE_RESET);

				return PARSERESULT_PARSEOK;

			}

			else {

				serial0_sendString("bcheck\r\n");

				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;

}

#include "config.h"

#include <avr/io.h>

#include <util/delay.h>

#include <avr/wdt.h>

#include <avr/interrupt.h>

#include <stdio.h>

#include <stdbool.h>

#include "lib/serial.h"

#include "lib/aprs.h"

#include "lib/afsk.h"

#include "lib/led.h"

#include "lib/logger.h"

#include "lib/watchdog.h"

#include "lib/sd/sd_raw_config.h"

#include "lib/looptime.h"

#include "lib/slavesensors.h"

#include "lib/serparser.h"

#include "lib/sensordata.h"

void micro_setup() 

{

}

int main(void)

{

	// Initialize libraries

	time_setup();

	micro_setup();

	watchdog_setup();

	led_setup();

	serial0_setup();

	serial1_setup();

	sensordata_setup(); // must happen before sensors/logger/afsk

	slavesensors_setup();

	logger_setup();

	afsk_setup();

	//serial0_sendString("\r\n\r\n---------------------------------\r\n");

	//serial0_sendString("HAB Controller 1.0 - Initialized!\r\n");

	//serial0_sendString("---------------------------------\r\n\r\n");

	led_on(POWER);

	// Buffer for string operations

	// Software timers	

	uint32_t lastAprsBroadcast = 0;

	uint32_t lastLog = 0;

	// Result of last parser run

	int parseResult = PARSERESULT_NODATA;

	// Write CSV header to SD card

	while(1)

    {

		// Periodic: Logging

		if(time_millis() - lastLog > LOGGER_RATE) 

		{

			led_on(STAT);

			led_off(STAT);

			lastLog = time_millis();

		}		

		// Periodic: APRS transmission

		if(time_millis() - lastAprsBroadcast > APRS_TRANSMIT_PERIOD) 

		{

			while(afsk_busy());

			aprs_send(); // non-blocking

			//serial0_sendString("Initiating APRS transmission...\r\n");

			// Start getting values for next transmission

			if(slavesensors_isrequesting())

			{

				// TODO: something is terribly wrong

			}

			else 

			{				

				slavesensors_startprocess();

			}

			lastAprsBroadcast = time_millis();

		}			

		parseResult = serparser_parse();

		slavesensors_process(parseResult);

		wdt_reset();

    }

}