/*
 * Master Firmware: Slave Sensor Data Acquisition
 *
 * Wireless Observational Modular Aerial Network
 * 
 * Ethan Zonca
 * Matthew Kanning
 * Kyle Ripperger
 * Matthew Kroening
 *
 */

#include "../config.h"
#include <avr/io.h>
#include <stdbool.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <util/delay.h>
#include <avr/wdt.h>
#include <avr/pgmspace.h>
#include "serial.h"
#include "serparser.h"
#include "slavesensors.h"
#include "sensordata.h"
#include "led.h"
#include "looptime.h"

// Label lookup table
// Make sure there are never more labels than there are MAX_NUM_SENSORS! 
const char label_0[] PROGMEM = "BoardTemp";
const char label_1[] PROGMEM = "HeaterStatus";
const char label_2[] PROGMEM = "BatteryLevel";
const char label_3[] PROGMEM = "AirTemp";
const char label_4[] PROGMEM = "AmbientLight";
const char label_5[] PROGMEM = "Humidity";
const char label_6[] PROGMEM = "Pressure";
const char label_7[] PROGMEM = "Altitude";
const char label_8[] PROGMEM = "CPM-Radiation";

const char *const labelLookup[] PROGMEM =
{
	label_0,
	label_1,
	label_2,
	label_3,
	label_4,
	label_5,
	label_6,
	label_7,
	label_8,
};

char labelBuffer[15]; // Size to length of label
char* slavesensors_getLabel(uint8_t sensorID) 
{
	if(sensorID < 9)
	{
		strncpy_P(labelBuffer,(char*)pgm_read_word(&(labelLookup[sensorID])),15);
		
		return labelBuffer;
	}
	else 
	{
		return NULL;
	}
}

uint8_t currentSlave = 0;
uint8_t currentSlaveSensor = 0;

bool requesting = false;

void slavesensors_setup() 
{
	
}

//#define DEBUG_NETWORKSCAN
//#define DEBUG_GETSLAVEDATA

char* bufPtr = 0x00;

static char slaveAddressLow[MAX_NUM_SLAVES][9];
static char slaveAddressHigh[MAX_NUM_SLAVES][9];
static char slaveNames[MAX_NUM_SLAVES][15];

uint8_t loggerIndex = 255;
uint8_t nodeCount = 0;
bool dataReady = false;

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
	_delay_ms(200); // 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, 15);
			}
			
			// 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++) 
	{
		snprintf(debugBuf, 64, "  %s - %s%s (%u)\r\n", slaveNames[i],slaveAddressHigh,slaveAddressLow[i], 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();
}

//#define DEBUG_CONTEXTSWITCH
//#define DEBUG_SELECTNODE

uint8_t selectedNode = 0;
uint8_t slavesensors_getselectednode() 
{
	return selectedNode;
}

void slavesensors_selectnode(uint8_t nodeIndex)
{
	if(selectedNode == nodeIndex) 
	{
		return;
	}
	serial0_ioff();
	
	#ifdef DEBUG_CONTEXTSWITCH
	uint32_t startTime = time_millis();
	#endif
	
	#ifdef DEBUG_SELECTNODE
	serial0_sendString("Switch to node ");
	serial0_sendChar(nodeIndex + 0x30);
	serial0_sendString("\r\n");
	#endif
	
	_delay_ms(20);
	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();
		selectedNode = nodeIndex;
	}
	_delay_ms(2);
	
	#ifdef DEBUG_SELECTNODE
	serial0_sendString("Selected ");
	serial0_sendChar(nodeIndex + 0x30);
	serial0_sendString("\r\n");
	#endif
	
	#ifdef DEBUG_CONTEXTSWITCH
	uint32_t switchTime = time_millis() - startTime;
	char tmpB[32];
	snprintf(tmpB, 32, "CTXSW: %lu ms\r\n", switchTime);
	serial0_sendString(tmpB);
	#endif
	
	serial0_ion();
	return;
}

void slavesensors_selectlogger() 
{
	if(loggerIndex != 255) 
	{
		slavesensors_selectnode(loggerIndex);
	}	
}

void slavesensors_exitAT() 
{
	// Exit AT
	serial0_sendString("ATCN");
	serial0_sendChar(0x0D);
	_delay_ms(2);
	//// Wait until we get "OK"
	//int atStart = time_millis();
	//while(!serial0_hasChar()) {
		//if(time_millis() - atStart > 500) {
			//led_errorcode(ERROR_EXITAT);
			//wdt_reset();
			//return 1;
		//}
	//};
	
	xbeeIsOk();
}

// Enter AT mode. Leaves "OK" on the buffer.
int slavesensors_enterAT() 
{
	// Delay guard time
	_delay_ms(2);


	serial0_ioff(); // interrupts MUST be off
	
	// Enter AT mode
	serial0_sendChar('+'); // Enter AT mode
	serial0_sendChar('+');
	serial0_sendChar('+');
	_delay_ms(2);
	// Wait 1ms until we get "OK"
	//int atStart = time_millis();
	//while(!serial0_hasChar()) {
		//if(time_millis() - atStart > 500) {
			//led_errorcode(ERROR_ATFAIL);
			//wdt_reset();
			//return 1;
		//}
	//};	
	
	return xbeeIsOk();

}

int xbeeIsOk() 
{
	char* tmppntr = serial0_readLine();
	if(strcmp(tmppntr, "OK") == 0)
	{
		return 0;
	}
	else
	{
		led_errorcode(ERROR_NOXBEE);
		return 1;
	}
}

bool slavesensors_dataReady() 
{
	return dataReady;
}

bool slavesensors_isrequesting() 
{
	return requesting;	
}

void slavesensors_startprocess() 
{
	requesting = true;
	slavesensors_request();		
}

// TODO: inline. static.
void slavesensors_request() 
{
	if(currentSlave == loggerIndex) 
	{
		currentSlave++;
		if(currentSlave >= (nodeCount)) 
		{
			slavesensors_selectlogger();
			return;
		}
	}
	slavesensors_selectnode(currentSlave);
	serial_sendCommand("@"); // Request data!
}


uint8_t numReadingsToExpect = 0; // number of values that the slave is about to send (testing)


// TODO: needs to skip logger!
void slavesensors_process(uint8_t parseResult) 
{
	if(!requesting) 
	{
		// we got a command when we didn't request anything. probably skip it.
		return;
	}
	
	// TODO: timeout. If we're at NODATA for a long time and we are requesting, that's an issue.
	// TODO: If we time out, WE NEED TO RESET THE PARSER. It could be in a bad state.
	else if(parseResult == PARSERESULT_NODATA) 
	{
		// Wait for data
	}
	
	// Finished reception of a message (one sensor data value). If not finished, send out command to get the next one
	else if(parseResult == PARSERESULT_PARSEOK)
	{
		
		#ifdef DEBUG_GETSLAVEDATA
		char debug[50];
		snprintf(debug, 50, "Slave %u sensor %u of total nodes %u\r\n", currentSlave, currentSlaveSensor,nodeCount);
		serial0_sendString(debug);
		#endif
		
		// We got some data, let's handle it
		// ASCII payload
		uint8_t len = getPayloadLength();
		char* load = getPayload();
		uint8_t type = getPayloadType();
		int32_t parsedVal = strtol(load, NULL, 10);//atoi(load);

		// Special case for slave telling us how many things we're about to get		
		if(type + 0x30 == '@')
		{
			
			#ifdef DEBUG_GETSLAVEDATA
			serial0_sendString("Got an awesome count!\r\n");
			serial0_sendChar(parsedVal + 0x30);
			serial0_sendString("\r\n");
			#endif
			
			numReadingsToExpect = parsedVal;
			currentSlaveSensor = 0;
			requesting = true;
		}
		else 
		{
		
			// Store data in structure
			sensordata_set(currentSlave,type,parsedVal);
			
			#ifdef DEBUG_GETSLAVEDATA
			serial0_sendString("Stored some sexy data!\r\n");
			#endif 
			
			// If we finished all sensors for all slaves
			
			if(currentSlave >= (nodeCount-1) && currentSlaveSensor >= (numReadingsToExpect-1))
			{
				#ifdef DEBUG_GETSLAVEDATA
				serial0_sendString("We got all data for all slaves!\r\n");
				#endif
				
				dataReady = true;
				currentSlave = 0;
				currentSlaveSensor = 0;
				requesting = false;
				led_alert();
			}
			// If we finished up one slave, go to the next
			else if(currentSlaveSensor >= (numReadingsToExpect-1)) 
			{
				#ifdef DEBUG_GETSLAVEDATA
				serial0_sendString("Finished up one slave, go to another.\r\n");
				#endif
				
				currentSlave++;
				currentSlaveSensor = 0;
				requesting = true;
				
				if(currentSlave == loggerIndex) 
				{
					if(currentSlave >= (nodeCount-1)) 
					{
						// We hit the last one, we're done.
						dataReady = true;
						currentSlave = 0;
						currentSlaveSensor = 0;
						requesting = false;
						led_alert();
						return;
					}
					else 
					{
						currentSlave++; // increment to the next slave after the logger
					}
				}
				
				slavesensors_request();
			}
			// If we haven't finished a slave (or all of them), just get the next sensor of the current slave
			else
			{
				#ifdef DEBUG_GETSLAVEDATA
				serial0_sendString("Give me another sensor value...");
				#endif
				
				// request data for the current sensor of the current slave
				currentSlaveSensor++;
				requesting = true;
				//slavesensors_request();	 slaves now send all values at once, we don't need to keep requesting
			}
		}
	}
	
	// If fail, try retransmit. Or we could skip and hit it next time.
	// TODO: Maximum number of retransmissions
	else if(parseResult == PARSERESULT_FAIL) 
	{
		if(requesting) 
		{
			slavesensors_request();	// re-request
		}			
	}
	
	
	else if(parseResult == PARSERESULT_STILLPARSING)
	{
		return; // do nothing
	}
	else 
	{
		// something is terribly wrong!
		return;
	}
}