/*
 * Master Firmware: Slave Sensor Data Acquisition
 *
 * This file is part of OpenTrack.
 *
 * OpenTrack is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Affero General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * OpenTrack is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Affero General Public License for more details.
 *
 * You should have received a copy of the GNU Affero General Public License
 * along with OpenTrack. If not, see <http://www.gnu.org/licenses/>.
 * 
 * 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 = "Heater";
const char label_2[] PROGMEM = "VBatt";
const char label_3[] PROGMEM = "AirTemp";
const char label_4[] PROGMEM = "Light";
const char label_5[] PROGMEM = "Humidity";
const char label_6[] PROGMEM = "Pressure";
const char label_7[] PROGMEM = "Altitude";
const char label_8[] PROGMEM = "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 
	{
		// Print out the sensor ID if there is no label in the lookup table
		snprintf(labelBuffer, 15,"%u",sensorID);
		return labelBuffer;
	}
}

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();
			led_power_toggle();
			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;
				
				char msg[128];
				snprintf(msg, 128, "Slave %u (%s) failed max retries",currentSlave,slaveNames[currentSlave]);
				error_log_msg(ERROR_SLAVETIMEOUT, false, msg); // log error, don't blink LED
				
				gotoNextSlaveOrSensor(true);
			}
		}			
	}
	
	
	else if(parseResult == PARSERESULT_STILLPARSING)
	{
		return; // do nothing
	}
	else 
	{
		error_log_msg(ERROR_FATAL, true, "parseResult invalid!");
		return;
	}
}