/*
 * 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)
{
	led_on(LED_POWER);
	
	// Initialize libraries
	time_setup();
	watchdog_setup(); // enables interrupts
	led_setup();	
	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;
				//snprintf(logbuf, 128, "%lu,%d,%u,%s,%s,%s,%s,%s\r\n", time_millis(), sensors_getBoardTemp(),get_timestamp(),get_latitude(),get_longitude(),get_speedKnots(),get_hdop(), get_course());
				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();
    }
}