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

#include "../config.h"
#include <stdio.h>
#include <stdbool.h>
#include <string.h>
#include "sensordata.h"
#include "slavesensors.h"
#include "boardtemp.h"
#include "looptime.h"
#include "gps.h"
#include "logger.h"

// Slave sensor reading storage
int32_t slaves[MAX_NUM_SLAVES][MAX_NUM_SENSORS];

void sensordata_setup() 
{
	for(int i=0; i<MAX_NUM_SLAVES; i++) 
	{
		for(int j=0; j<MAX_NUM_SENSORS; j++) 
		{
			slaves[i][j] = -2111111111; // minimum value of 16 bit integer
		}
	}
}

// Store a sensor value in memory
void sensordata_set(uint8_t nodeID, uint8_t type, int32_t value)
{
	if(nodeID < MAX_NUM_SLAVES) 
	{
		slaves[nodeID][type] = value;
	}	
}

// Retrieve a sensor value from memory
int32_t sensordata_get(uint8_t nodeID, uint8_t type) 
{
	// Avoid reading out of bad places!
	if(nodeID < MAX_NUM_SLAVES) 
	{
		return slaves[nodeID][type];
	}
	else 
	{
		return 0;
	}
}


// Generate APRS comment
// TODO: Can we move this buffer to a local scope of this function?
#define COMMENTBUFFER_SIZE 128
char commentBuffer[COMMENTBUFFER_SIZE];
char* slavesensors_getAPRScomment() 
{
	snprintf(commentBuffer,COMMENTBUFFER_SIZE, "t9%d s%s v%s h%s _%s |%s ", sensors_getBoardTemp(), get_sv(), get_speedKnots(), get_hdop(), get_latitudeLSBs(), get_longitudeLSBs());
	
	// Find slave sensors to include in this log
	for(int i=0; i<MAX_NUM_SLAVES; i++)
	{
		// Board temperature sensors (all slaves)
		uint32_t val = sensordata_get(i, SENSOR_BOARDTEMP);
		if(val != -2111111111) {
			uint16_t len = strlen(commentBuffer);
			snprintf(commentBuffer + len, COMMENTBUFFER_SIZE-len, " t%u%li",i,val);
		}
		
		// Battery voltages (all slaves)
		val = sensordata_get(i, SENSOR_BATTERYLEVEL);
		if(val != -2111111111) {
			uint16_t len = strlen(commentBuffer);
			snprintf(commentBuffer + len, COMMENTBUFFER_SIZE-len, " l%u%li",i,val);
		}
		
		// Pressure
		val = sensordata_get(i, SENSOR_PRESSURE);
		if(val != -2111111111) {
			uint16_t len = strlen(commentBuffer);
			snprintf(commentBuffer + len, COMMENTBUFFER_SIZE-len, " P%li",val);
		}
		
		// Air Temperature
		val = sensordata_get(i, SENSOR_AIRTEMP);
		if(val != -2111111111) {
			uint16_t len = strlen(commentBuffer);
			snprintf(commentBuffer + len, COMMENTBUFFER_SIZE-len, " C%li",val);
		}
		
		// Altitude
		val = sensordata_get(i, SENSOR_ALTITUDE);
		if(val != -2111111111) {
			uint16_t len = strlen(commentBuffer);
			snprintf(commentBuffer + len, COMMENTBUFFER_SIZE-len, " A%li",val);
		}
		
		// Radiation
		val = sensordata_get(i, SENSOR_CPM_RADIATION);
		if(val != -2111111111) {
			uint16_t len = strlen(commentBuffer);
			snprintf(commentBuffer + len, COMMENTBUFFER_SIZE-len, " R%li",val);
		}
		
	}
	
	if(logger_aprsInfoTextAvailable())
	{
		uint16_t len = strlen(commentBuffer);
		snprintf(commentBuffer + len, COMMENTBUFFER_SIZE-len, " %s",logger_getAprsInfoText());
		logger_aprsInfoTextConsumed();
	}
	
	
	return commentBuffer;
}


// Generates CSV headers on first run and logs values to the SD card (if data available)
bool dataWasReady = false;
void sensordata_logvalues() 
{
	// Generate CSV header after we have queried all slaves once
	if(slavesensors_dataReady()) 
	{
	
		// Only generate/write header the first time data is ready
		if(!dataWasReady) 
		{
			#define CSV_BUFFER_SIZE 64
			char csvHeader[CSV_BUFFER_SIZE];
			csvHeader[0] = 0x00;
			
			// Add master data headers
			logger_log("Time,BoardTemp,GPSTime,GPSLat,GPSLon,GPSSpeed,GPSHDOP,GPSCourse,GPSSV,");
			
			// Add slave data headers
			for(uint8_t i=0; i<MAX_NUM_SLAVES; i++) 
			{
				for(uint8_t j=0; j<MAX_NUM_SENSORS; j++) 
				{
					int32_t tmp = sensordata_get(i, j);
					
					// If a sensor value exists, write a header for it
					if(tmp != -2111111111) 
					{
						snprintf(csvHeader, CSV_BUFFER_SIZE,"%s-%s,", slavesensors_slavename(i), slavesensors_getLabel(j));
						logger_log(csvHeader);
					}
				}
			}
		
			// End line and write to SD card
			snprintf(csvHeader, CSV_BUFFER_SIZE,"\r\n");
			logger_log(csvHeader);
			
			dataWasReady = true;
		}
	
		// Write CSV sensor values to SD card
		#define CSV_LOGLINE_SIZE 512
		char logbuf[CSV_LOGLINE_SIZE];
		logbuf[0] = 0x00;
		
		// Write master sensor values
		snprintf(logbuf, CSV_LOGLINE_SIZE, "%lu,%d,%s,%s,%s,%s,%s,%s,%s,", time_millis(), sensors_getBoardTemp(),get_timestamp(),get_latitudeTrimmed(),get_longitudeTrimmed(),get_speedKnots(),get_hdop(), get_course(), get_sv());
		
		// Write slave sensor values
		for(int i=0; i<MAX_NUM_SLAVES; i++) 
		{
			for(int j=0; j<MAX_NUM_SENSORS; j++) 
			{
				int32_t tmp = sensordata_get(i, j);
				
				// If a sensor value exists, log the data
				if(tmp != -2111111111) 
				{
					snprintf(logbuf + strlen(logbuf),CSV_LOGLINE_SIZE-strlen(logbuf)," %ld,", tmp);
				}
			
			}
		}
		
		// End line and write to log
		snprintf(logbuf + strlen(logbuf),CSV_LOGLINE_SIZE-strlen(logbuf),"\r\n");
		logger_log(logbuf);
	}
}