/*
 * 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 "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/trackuinoGPS/gpsMKa.h"

#include "lib/i2c.h"
#include "lib/boardtemp.h"

#include "lib/looptime.h"
#include "lib/slavesensors.h"
#include "lib/serparser.h"
#include "lib/sensordata.h"

int main(void)
{
    _delay_ms(1500); // warmup
	
	// Initialize libraries
	time_setup();
	watchdog_setup(); // enables interrupts
	led_setup();	
	serial0_setup();
	serial1_setup();
	i2c_init();
	sensordata_setup(); // must happen before sensors/logger/afsk
	slavesensors_setup();
	logger_setup();
	afsk_setup();
	//\f
	serial0_sendString("\r\n---------------------------------\r\n");
	serial0_sendString("HAB Controller 1.0 - Initialized!\r\n");
	serial0_sendString("---------------------------------\r\n");
	serial0_sendString("\r\nHello.\r\n\r\n");
	
	slavesensors_network_scan();
	
	led_on(LED_POWER);
	led_off(LED_SIDEBOARD);
	
	// Buffer for string operations
	char logbuf[128];
	char debugBuf[128];
	
	// Software timers	
	uint32_t lastAprsBroadcast = 0;
	uint32_t lastLog = 0;
	uint32_t lastLedCycle = 0;
	
	// Result of last parser run
	int parseResult = PARSERESULT_NODATA;
	
	// Write CSV header to SD card
	//logger_log("ProgramTime,LastAprsBroadcast,LastLog\n");
	
	uint8_t ctr = 0;
	void spin() {
		
		if(ctr == 0) {
			led_on(LED_ACT0);
			led_off(LED_ACT1);
			led_off(LED_ACT2);
			led_off(LED_ACT3);
		}			
		else if (ctr == 1) {
			led_on(LED_ACT1);
			led_off(LED_ACT0);
			led_off(LED_ACT2);
			led_off(LED_ACT3);
		}			
		else if (ctr == 2) {
			led_on(LED_ACT2);
			led_off(LED_ACT1);
			led_off(LED_ACT0);
			led_off(LED_ACT3);
		}			
		else if (ctr == 3) {
			led_on(LED_ACT3);
			led_off(LED_ACT1);
			led_off(LED_ACT2);
			led_off(LED_ACT0);
		}			
		ctr = (ctr + 1) % 4;
	}
	
	serial1_ioff();
	
	while(1)
    {
		
		// Periodic: LED execution indicator
		if(time_millis() - lastLedCycle > LEDCYCLE_RATE) {
			spin();
			
			if(!afsk_busy())
				serial1_ion();
			lastLedCycle = time_millis();	
		}
		
		// Periodic: Logging
		if(time_millis() - lastLog > LOGGER_RATE) 
		{
			led_on(LED_CYCLE);
			
			
			// 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);
			
			
			sensors_readBoardTemp(); // i2c read, 400k
			snprintf(logbuf, 128, "%lu,%d,%uF,%s,%s,%s,%s,%s\r\n", time_millis(), sensors_getBoardTemp(),get_timestamp(),get_latitude(),get_longitude(),get_speedKnots(),get_hdop(), get_course());
			logger_log(logbuf);
			
			// Print out logger debug
			//serial0_sendString("LOG> ");
			//serial0_sendString(logbuf);
			
			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();
    }
}