/*
 * Master Firmware: Sensor Data
 *
 * 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 <stdio.h>
#include <stdbool.h>
#include <string.h>
#include "sensordata.h"
#include "slavesensors.h"
#include "sensors.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;
	}
}

bool isEven = true;

// Generate APRS comment
// TODO: Can we move this buffer to a local scope of this function?

char* slavesensors_getAPRScomment(char* commentBuffer, uint16_t bufferSize) 
{
	snprintf(commentBuffer,bufferSize, "~v%s~_%s~|%s", get_speedKnots(), get_latitudeLSBs(), get_longitudeLSBs());
	
	if(isEven) 
	{
		// Master details
		uint16_t len = strlen(commentBuffer);
		snprintf(commentBuffer + len, bufferSize-len, "~t9%d~b9%d~s%s~h%s", sensors_getBoardTemp(), sensors_getBatt(), get_sv(), get_hdop());
		
		// 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, bufferSize-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, bufferSize-len, "~b%u%li",i,val);
			}
		
			// Pressure
			val = sensordata_get(i, SENSOR_PRESSURE);
			if(val != -2111111111) {
				uint16_t len = strlen(commentBuffer);
				snprintf(commentBuffer + len, bufferSize-len, "~P%li",val);
			}
		
			// Air Temperature
			val = sensordata_get(i, SENSOR_AIRTEMP);
			if(val != -2111111111) {
				uint16_t len = strlen(commentBuffer);
				snprintf(commentBuffer + len, bufferSize-len, "~C%li",val);
			}
			
			// Humidity
			val = sensordata_get(i, SENSOR_HUMIDITY);
			if(val != -2111111111) {
				uint16_t len = strlen(commentBuffer);
				snprintf(commentBuffer + len, bufferSize-len, "~H%li",val);
			}
		
			// Altitude
			val = sensordata_get(i, SENSOR_ALTITUDE);
			if(val != -2111111111) {
				uint16_t len = strlen(commentBuffer);
				snprintf(commentBuffer + len, bufferSize-len, "~A%li",val);
			}
		
			// Radiation
			val = sensordata_get(i, SENSOR_CPM_RADIATION);
			if(val != -2111111111) {
				uint16_t len = strlen(commentBuffer);
				snprintf(commentBuffer + len, bufferSize-len, "~R%li",val);
			}
		
		}
		
		if(logger_aprsInfoTextAvailable())
		{
			uint16_t len = strlen(commentBuffer);
			snprintf(commentBuffer + len, bufferSize-len, "~%s",logger_getAprsInfoText());
			logger_aprsInfoTextConsumed();
		}
		
		isEven = false;
	}
	else {
		// odd does nothing
		isEven = true;
	}	
	

	
	
	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,VBatt,GPSTime,GPSLat,GPSLon,GPSAlt,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,%u,%s,%s%s,%s%s,%s,%s,%s,%s,%s,", time_millis(), sensors_getBoardTemp(), sensors_getBatt(), get_timestamp(), get_latitudeTrimmed(), get_latitudeLSBs(), get_longitudeTrimmed(), get_longitudeLSBs(),get_gpsaltitude(), 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);
	}
}

bool isTouchdown = false;
bool sensordata_isTouchdown()
{
	return isTouchdown;
}

int32_t sensordata_getSensorValue(uint8_t sensorID) 
{
	// Loop through all slaves
	for(int i=0; i<MAX_NUM_SLAVES; i++)
	{
		uint32_t val = sensordata_get(i, sensorID);
		if(val != -2111111111) {
			return val;
		}
	}		
	return -2111111111;
}

void sensordata_checkTouchdown()
{
	// If we have reached touchdown, never get out of this state
	if(isTouchdown)
	{
		return;
	}
	
	if(time_millis() > sysconfig->buzzer_failsafe_duration)
	{
		isTouchdown = true;
	}
	else
	{
		int32_t altitude = sensordata_getSensorValue(SENSOR_ALTITUDE);
		if(altitude != -2111111111 && altitude < sysconfig->buzzer_trigger_maxaltitude && time_millis() > sysconfig->buzzer_trigger_minduration)
		{
			isTouchdown = true;
		}			
	}
}