/*

 * 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"

#include "logger.h"

// !!! Remember to update the ENUM in slavesensors.h when changing things here

// 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[32]; // 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;

//#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];

static char loggerAddressLow[9];

static char loggerAddressHigh[9];

uint8_t nodeCount = 0;

bool dataReady = false;

void slavesensors_setup()

{

	loggerAddressLow[0] = 0x00;

	loggerAddressHigh[0] = 0x00;

}

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

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

		int lineCount = 0;	

		while(1) 

		{

			// Wait for scan to complete. If we timeout, return.

			if(waitTimeout(TIMEOUT_NETWORKSCAN)) 

			{

				return;

			}

			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 finished one chunk (including the newline after it). Can't be else if because it controls increment.

			if(lineCount == 9) 

			{

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

				{

					// Save logger address in the loggerAddressXXXX variables

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

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

					lineCount = 0;

					// don't increment, just overwrite this next time

				}

				else {

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

	led_off(LED_ACT0);

	led_off(LED_ACT1);

	led_off(LED_ACT2);

	led_off(LED_ACT3);

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

	}

	_delay_ms(500);

	led_on(LED_SIDEBOARD);

	_delay_ms(500);

	led_off(LED_SIDEBOARD);

	#ifdef DEBUG_NETWORKSCAN

	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

	char infobuf[25];

	snprintf(infobuf, 25, "discovered %u nodes", nodeCount);

	info_log_msg(infobuf);

	_delay_ms(100);

	slavesensors_selectlogger();

	// If we don't have slaves to worry about, we're good to go

	if(nodeCount == 0)

	{

		dataReady = true;

	}		

	serial0_ion();

}

//#define DEBUG_CONTEXTSWITCH

//#define DEBUG_SELECTNODE

uint8_t selectedNode = 255;

uint8_t slavesensors_getselectednode() 

{

	return selectedNode;

}

void slavesensors_selectnode(uint8_t nodeIndex)

{

	if(selectedNode == nodeIndex)

	{

		return;

	}

	if(slavesensors_selectaddress(slaveAddressHigh[nodeIndex],slaveAddressLow[nodeIndex]) == true) 

	{

		selectedNode = nodeIndex;

	}	

}

bool slavesensors_selectaddress(char* addrHigh, char* addrLow) 

{

	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",addrHigh, 0x0D);

		serial0_sendString(tmpBuf);

		if(xbeeIsOk() != 0) 

		{

			error_log(ERROR_XBEETIMEOUT, true);

			return false;

		}

		snprintf(tmpBuf, 23, "ATDL %s%c",addrLow, 0x0D);

		serial0_sendString(tmpBuf);

		if(xbeeIsOk() != 0) 

		{

			error_log(ERROR_XBEETIMEOUT, true);

			return false;

		}

		slavesensors_exitAT();

	}

	_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 true;

}

void slavesensors_selectlogger() 

{

	if(loggerAddressLow[0] != 0x00) 

	{

		slavesensors_selectaddress(loggerAddressHigh,loggerAddressLow);

	}	

}

void slavesensors_exitAT() 

{

	// Exit AT

	serial0_sendString("ATCN");

	serial0_sendChar(0x0D);

	if(waitTimeout(TIMEOUT_EXITAT)) 

	{

		return;

	}

	xbeeIsOk();

}

bool waitTimeout(uint32_t timeout) {

	uint32_t scanStart = time_millis();

	uint32_t lastBlink = 0;

	while(!serial0_hasChar())

	{

		if(time_millis() - scanStart > timeout)

		{

			error_log(ERROR_XBEETIMEOUT, true);

			return true;

		}

		if(time_millis() - lastBlink > 50)

		{

			led_spin();

			lastBlink = time_millis();

		}

		wdt_reset();

	}

	return false;

}

// 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('+');

	return xbeeIsOk();

}

int xbeeIsOk() 

{

	if(waitTimeout(TIMEOUT_XBEERESPONSE)) {

		error_log(ERROR_XBEETIMEOUT, true);

		return 1;

	}

	char* tmppntr = serial0_readLine();

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

	{

		return 0;

	}

	else

	{

		error_log(ERROR_SLAVETIMEOUT, true);

		return 1;

	}

}

bool slavesensors_dataReady() 

{

	return dataReady;

}

bool slavesensors_isrequesting() 

{

	return requesting;	

}

void slavesensors_startprocess() 

{

	if(nodeCount == 0)

	{

		return;

	}		

	requesting = true;

	slavesensors_request();		

}

uint32_t beginRequest = 0;

void slavesensors_request() 

{

	slavesensors_selectnode(currentSlave);

	beginRequest = time_millis();

	serial_sendCommand("@"); // Request data!

}

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

uint8_t numRetries = 0;

void gotoNextSlaveOrSensor(bool fail) {

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

		if(!fail) 

		{

			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;

		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;

	}

}

// Request data from slave modules

void slavesensors_process(uint8_t parseResult) 

{

	if(!requesting) 

	{

		// we got a command when we didn't request anything. skip it.

		return;

	}

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

		if(requesting && ( (time_millis() - beginRequest) > TIMEOUT_SLAVEREQUEST)) {

			// if we're requesting, we have no data, and we're over the timeout, this is bad!

			// setParserState(STATE_RESET); - meh, can't do this because it freaking increments the cirbufptr

			char msg[128];

			snprintf(msg, 128, "Slave %u (%s) timeout",currentSlave,slaveNames[currentSlave]);

			error_log_msg(ERROR_SLAVETIMEOUT, false, msg); // log error, don't blink LED

			gotoNextSlaveOrSensor(true);

		}

	}

	// 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 data, reset retries

		numRetries = 0;

		// 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 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 data!\r\n");

			#endif 

			gotoNextSlaveOrSensor(false);

		}

	}

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

		{

			if(numRetries < MAX_SLAVEREQUEST_RETRIES) 

			{

				slavesensors_request();	// re-request

			}

			else {

				numRetries = 0;

				gotoNextSlaveOrSensor(true);

			}

		}			

	}

	else if(parseResult == PARSERESULT_STILLPARSING)

	{

		return; // do nothing

	}

	else 

	{

		error_log_msg(ERROR_FATAL, true, "parseResult invalid!");

		return;

	}

}