/*

 * Master Firmware: Status and Error LED Handler

 *

 * Wireless Observational Modular Aerial Network

 * 

 * Ethan Zonca

 * Matthew Kanning

 * Kyle Ripperger

 * Matthew Kroening

 *

 */

#ifndef LED_H_

#define LED_H_

#include <avr/io.h>

enum leds {

	LED_ACT0 = 0,

	LED_ACT1,

	LED_ACT2,

	LED_ACT3,

	LED_POWER,

	LED_STATUS,

	LED_ERROR,

	LED_SIDEBOARD,

	LED_ACTIVITY,

	LED_CYCLE,

};

typedef struct {uint8_t* direction; uint8_t* port; uint8_t pin;} led_t;

// Match order of leds enum

static led_t ledList[] = {

	{&DDRA, &PORTA, PA1}, // ACT0

	{&DDRA, &PORTA, PA2}, // ACT1

	{&DDRA, &PORTA, PA3}, // ACT2

	{&DDRA, &PORTA, PA4}, // ACT3

//pcb:

	{&DDRB, &PORTB, PB4}, // POWER

	{&DDRB, &PORTB, PB3}, // STATUS

	{&DDRB, &PORTB, PB2}, // ERROR

//breadboard:

//	{&DDRA, &PORTA, PA2}, // POWER

//	{&DDRA, &PORTA, PA0}, // STATUS

//	{&DDRA, &PORTA, PA1}, // ERROR

	{&DDRD, &PORTD, PD6}, // SIDEBOARD

	{&DDRD, &PORTD, PD5}, // ACTIVITY

	{&DDRD, &PORTD, PD4}, // CYCLE

};

void led_setup();

void led_on(uint8_t led);

void led_off(uint8_t led);

void led_toggle(uint8_t led);

void led_errorcode(uint8_t code);

void led_spin();

#endif /* LED_H_ */

char* slavesensors_slavename(uint8_t id) {

	return slaveNames[id];

}

void slavesensors_network_scan() {

	serial0_ioff();

	int atOK;

	#ifdef DEBUG_OUTPUT

	serial0_sendString("Beginning network scan...\r\n\r\n");

	#endif

	_delay_ms(500); // xbee warmup

	wdt_reset();

	led_on(LED_ACTIVITY);

	atOK = slavesensors_enterAT();

	// wait for OK

	if(atOK == 0)

	{

		led_on(LED_CYCLE);

		serial0_sendString("ATND");

		serial0_sendChar(0x0D);

		// wait for scan to complete

		uint16_t scanStart = time_millis(); 		

		while(!serial0_hasChar()) {

			if(time_millis() - scanStart > 7000) {

				led_errorcode(ERROR_XBEETIMEOUT);

				return;

			}

			wdt_reset();

		}

		// Scan data end when newline by itself ("")	

		int lineCount = 0;	

		while(1) {

			bufPtr = serial0_readLine();

			// If we're starting a new block but got a newline instead, we're done!

			if(lineCount == 0 && strcmp(bufPtr, "") == 0) {

				break;			

			}

			if(lineCount == 1) {

				strncpy(slaveAddressHigh[nodeCount],bufPtr, 9);

			}

			else if(lineCount == 2) {

				strncpy(slaveAddressLow[nodeCount],bufPtr, 9);

			}

			else if(lineCount == 3) {

				strncpy(slaveNames[nodeCount], bufPtr, 20);

			}

			// If we've finished one chunk (including the newline after it). Can't be else if because it controls increment.

			if(lineCount == 9) {

				// bufPtr should be null at this point, because we read in a newline after one chunk

				nodeCount++;

				lineCount = 0;

			}

			else {

				lineCount++;

			}

		}		

		slavesensors_exitAT();

	}

	// Display number of found nodes on spinning indicator

	switch(nodeCount) {

		case 0:

			break;

		case 3:

			led_on(LED_ACT2);

			_delay_ms(100);

		case 2:

			led_on(LED_ACT1);

			_delay_ms(100);	

		case 1:

			led_on(LED_ACT0);

			_delay_ms(100);

	}

	led_on(LED_SIDEBOARD);

	_delay_ms(200);

	led_off(LED_SIDEBOARD);

	#ifdef DEBUG_OUTPUT

	char debugBuf[64];

	serial0_sendString("Discovered: \r\n");

	for(int i=0; i<nodeCount; i++) {

		serial0_sendString(debugBuf);

	}

	serial0_sendString("\r\n");

	if(atOK != 0) {

		serial0_sendString("AT mode failed \r\n");

	}

	#endif

	for(int i=0; i<nodeCount; i++) 

	{

		if(strcmp(slaveNames[i], XBEE_LOGDEST_NAME) == 0) 

		{

			loggerIndex = i;

		}

	}

	_delay_ms(100);

	slavesensors_selectlogger();

	serial0_ion();

}

void slavesensors_selectnode(uint8_t nodeIndex)

{

	char tmpBuf[23];

	// If we can get into AT mode

	if(slavesensors_enterAT() == 0) {

		snprintf(tmpBuf, 23, "ATDH %s%c",slaveAddressHigh[nodeIndex], 0x0D);

		serial0_sendString(tmpBuf);

		if(xbeeIsOk() != 0) {

			led_errorcode(ERROR_NOXBEE);

			return;

		}

		snprintf(tmpBuf, 23, "ATDL %s%c",slaveAddressLow[nodeIndex], 0x0D);

		serial0_sendString(tmpBuf);

		if(xbeeIsOk() != 0) {

			led_errorcode(ERROR_NOXBEE);

			return;

		}

		slavesensors_exitAT();

	}

	return;

}

void slavesensors_selectlogger() 

{

	if(loggerIndex != 255) {

		slavesensors_selectnode(loggerIndex);

	}	

}

void slavesensors_exitAT() 

{

	// Exit AT

	serial0_sendString("ATCN");

	serial0_sendChar(0x0D);

	xbeeIsOk(); // wait for OK

}

// Enter AT mode. Leaves "OK" on the buffer.

int slavesensors_enterAT() 

{

	// Delay guard time

	_delay_ms(1);

	// Enter AT mode

	serial0_sendChar('+'); // Enter AT mode

	serial0_sendChar('+');

	serial0_sendChar('+');

	// Wait 1ms until we get "OK"

	int atStart = time_millis();

	while(!serial0_hasChar()) {

		if(time_millis() - atStart > 500) {

			led_errorcode(ERROR_XBEETIMEOUT);

			wdt_reset();

			return 1;

		}

	};	

	return xbeeIsOk();

}

int xbeeIsOk() 

{

	char* tmppntr = serial0_readLine();

	if(strcmp(tmppntr, "OK") == 0)

	{

		return 0;

/*

 * Master Firmware

 *

 * Wireless Observational Modular Aerial Network

 * 

 * Ethan Zonca

 * Matthew Kanning

 * Kyle Ripperger

 * Matthew Kroening

 *

 */

#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 <string.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/gps.h"

#include "lib/i2c.h"

#include "lib/boardtemp.h"

#include "lib/heater.h"

#include "lib/looptime.h"

#include "lib/slavesensors.h"

#include "lib/serparser.h"

#include "lib/sensordata.h"

int main(void)

{

	// Initialize libraries

	time_setup();

	watchdog_setup(); // enables interrupts

	gps_setup();

	serial0_setup();

	serial1_setup();

	i2c_init();

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

	slavesensors_setup();

	logger_setup();

	afsk_setup();

	//\f

	#ifdef DEBUG_OUTPUT

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

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

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

	#endif

	serial0_sendString("\r\nHello.\r\n\r\n");

	slavesensors_network_scan(); // This can take a little while

	// Buffer for string operations

	char logbuf[128];

	// Software timers	

	uint32_t lastAprsBroadcast = 0;

	uint32_t lastLog = 0;

	uint32_t lastLedCycle = 0;

	bool dataWasReady = false;

	// Result of last parser run

	int parseResult = PARSERESULT_NODATA;

	serial1_ioff();

	while(1)

    {

		// Periodic: LED execution indicator

		if(time_millis() - lastLedCycle > LEDCYCLE_RATE) {

			led_spin();

			if(!afsk_busy())

				serial1_ion();

			lastLedCycle = time_millis();	

		}

		// Periodic: Logging

		if(time_millis() - lastLog > LOGGER_RATE) 

		{

			led_on(LED_CYCLE);

			heater_regulateTemp();

			// Print out GPS debug

			//snprintf(debugBuf, 128, "GPS> time: %s lat: %s lon: %s speed: %s hdop: %s course: %s\r\n",

			//get_timestamp(),get_latitude(),get_longitude(),get_speedKnots(),get_hdop(), get_course());

			//serial0_sendString(debugBuf);

			// Turn on sideboard LED if we have a fix

			if(strcmp("99.99", get_hdop()) == 0) {

				led_off(LED_SIDEBOARD);

			}

			else {

				led_on(LED_SIDEBOARD);

			}

			sensors_readBoardTemp(); // i2c read, 400k

			// If we've gotten data from all slaves once, we're ready to make a CSV header and start logging

			if(slavesensors_dataReady()) {

				if(!dataWasReady) {

					char csvHeader[128];

					csvHeader[0] = 0x00;

					for(uint8_t i=0; i<MAX_NUM_SLAVES; i++) {

						for(uint8_t j=0; j<MAX_NUM_SENSORS; j++) {

							int16_t tmp = sensordata_get(i, j);

							if(tmp != -32768) {

								// FIXME: will the 128 here really provide safety? might want to subtract the strlen

								snprintf(csvHeader + strlen(csvHeader), 128,"%s-Sensor%u,", slavesensors_slavename(i), j);	

							}

						}

					}

					snprintf(csvHeader + strlen(csvHeader), 128,"\r\n");	

					serial0_sendString(csvHeader);

					logger_log(csvHeader);

					dataWasReady = true;

				}

				logbuf[0] = 0x00;

				for(int i=0; i<MAX_NUM_SLAVES; i++) {

					for(int j=0; j<MAX_NUM_SENSORS; j++) {

						int16_t tmp = sensordata_get(i, j);

						if(tmp != -32768) {

							snprintf(logbuf + strlen(logbuf),128," %d,", tmp);

						}

					}

				}

				snprintf(logbuf + strlen(logbuf),128,"\r\n");

				serial0_sendString(logbuf);			

				logger_log(logbuf);

			}			

			// Print out logger debug

			#ifdef DEBUG_OUTPUT

			//serial0_sendString("LOG> ");

			//serial0_sendString(logbuf);

			#endif

			led_off(LED_CYCLE);

			lastLog = time_millis();

		}		

		// Periodic: APRS transmission

		if(time_millis() - lastAprsBroadcast > APRS_TRANSMIT_PERIOD) 

		{

			while(afsk_busy());

			serial1_ioff();

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

		parse_gps_transmission();

		wdt_reset();

    }

}