//----------------------------------------------------------------------------------
// <copyright file="TurnerC3_Orig.c" company="LiquidRobotics">
//	Copyright (c) Liquid Robotics Corporation.  All rights reserved.
// </copyright>
//
// <summary>
// 	This is the source file for managing the TurnerC3 Fluorometer in Data Log Mode
// </summary>
//
// <owner>Jim Kirklin</owner>
//----------------------------------------------------------------------------------



#include "includes.h"
#include <avr/io.h>
#include <avr/wdt.h>
#include <avr/power.h>
#include "uart.h"
#include "pib.h"
#include "timer.h"
#include "lrstat.h"
#include "spidebug.h"
#include "wdtrig.h"
#include "pib_apps.h"
#include "User_app_init.h"
#include "fltPyldComms.h"
#include "UserData.h"
#include "Turner_C3_Orig.h"

#include "SerialDebug.h"



#define FLUOROMETER_CLASS	0x000B
#define TURNER_C3			0x0001

//Serial Number stored at offset 0x0010 from PM base -- 16 bytes
#define TURNER_C3_SERNUM_OFFSET			0x0010
//Sample Rate stored at offset 0x0040 from PM base -- 4 bytes
#define TURNER_C3_SAMPLE_OFFSET			0x0040
//Autorun State stored at offset 0x0044 from PM base -- 1 byte
#define TURNER_C3_AUTORUN_STATE_OFFSET	0x0044


typedef struct _TURNER_C3_REPORT_XMIT_
{
	PIB_IRIDIUM_DATA_XMIT	comm;
	uint32_t				payloadID;
	char 		fluorometerSample[120];
}TURNER_C3_REPORT_XMIT, *PTURNER_C3_REPORT_XMIT;

typedef struct _TURNER_C3_ORIG_DATA_REPORT_XMIT_
{
	PIB_IRIDIUM_DATA_XMIT	comm;
	C3_ORIG_DATA_REPORT		reportData;
}TURNER_C3_ORIG_DATA_REPORT_XMIT, *PTURNER_C3_ORIG_DATA_REPORT_XMIT;

typedef struct _TURNER_C3_ORIG_ASCII_REPORT_XMIT_
{
	PIB_IRIDIUM_DATA_XMIT	comm;
	C3_ORIG_ASCII_REPORT	reportASCII;
}TURNER_C3_ORIG_ASCII_REPORT_XMIT, *PTURNER_C3_ORIG_ASCII_REPORT_XMIT;


typedef struct _TURNER_C3_ORIG_BUFFER_
{
	char 		bufferTx[256];
	char 		bufferRx[1024];
	uint32_t 	sampleRate;
	BOOL		sampleC3;
	uint8_t		initState;
	char 		fluorometerSample[120];

	uint8_t					sampleCount;
	uint16_t				totalSamples;
	C3_ORIG_DATA_REPORT		reportData;
	C3_ORIG_ASCII_REPORT	reportASCII;

	char					tokenSeparatorList[MAX_TOKEN_SEPARATORS];
	CONSOLE_RESPONSE_INFO	responseInfo;
	char					consoleResponse[256];

}TURNER_C3_ORIG_BUFFER, *PTURNER_C3_ORIG_BUFFER;

typedef struct _TURNER_C3_ORIG_TASK_
{
	uint8_t	taskID;
	uint8_t portNum;
	uint16_t eepromAddress;
	PTURNER_C3_ORIG_BUFFER 	pBuf;
}TURNER_C3_ORIG_TASK, *PTURNER_C3_ORIG_TASK;

uint8_t TurnerC3_Orig_Init(PTURNER_C3_ORIG_TASK taskInfo);
uint8_t TurnerC3_Orig_Rx(PTURNER_C3_ORIG_TASK pTaskInfo, char* pBuffer, uint16_t* pLen, char* eol, int eolLen);
void 	TurnerC3_Orig_GetDataSample(PTURNER_C3_ORIG_TASK pTtaskInfo);
uint8_t TurnerC3_Orig_DataReportXmit(PTURNER_C3_ORIG_TASK pTaskInfo);
uint8_t TurnerC3_Orig_DataReportXmitType91(PTURNER_C3_ORIG_TASK pTaskInfo);
uint8_t TurnerC3_Orig_ASCIIReportXmit(PTURNER_C3_ORIG_TASK pTaskInfo, uint16_t len);
uint8_t TurnerC3_Orig_ParseFile(PTURNER_C3_ORIG_TASK pTaskInfo, uint32_t fileID);
uint8_t TurnerC3_Orig_QueueFile(PTURNER_C3_ORIG_TASK pTaskInfo, uint32_t fileID);

void SerialPortPassThroughC3(uint8_t port);

static char* ThreadSafeStrTok(char** context, char* sep);

extern char fileIn[NUM_TASK_FILES][MAX_FILE_SIZE_IN] __attribute__((section(".xdata")));
extern char fileOut[NUM_TASK_FILES][MAX_FILE_SIZE_OUT] __attribute__((section(".xdata")));
extern uint16_t fileOutSize[NUM_TASK_FILES];
extern uint16_t fileInSize[NUM_TASK_FILES];
extern BOOL fileAvailable[NUM_TASK_FILES];
extern uint32_t fileAvailableID[NUM_TASK_FILES];
extern BOOL transmitPending[NUM_TASK_FILES];

extern OS_EVENT* pMutexFloatQueue;
extern FLOAT_TX_BUFFER txFloatData[MAX_FLOAT_BUFFERS] __attribute__((section(".xdata")));
extern uint8_t	headFloatTx;
extern uint8_t tailFloatTx;

extern uint8_t	DotToPad(char* pad, char* dot);
extern uint16_t mainTaskAddress;

extern FLOAT_TX_BUFFER txFloatAck[MAX_ACK_BUFFERS] __attribute__((section(".xdata")));
extern uint8_t	headFloatAckTx;
extern uint8_t tailFloatAckTx;

extern int32_t latitude;
extern int32_t longitude;

extern BOOL serialPortPassThrough[NUM_TASK_FILES];
extern BOOL serialPortPassThroughOK;
extern BOOL readyForSerialPassThru;

prog_char separator1[] = {'%', ' ', '.', ':', '/', '\t', '\r', '\n' ,'\0'};
prog_char separator2[] = {' ', '\t', '\r', '\n', '\0'};

void TurnerC3_Orig_ParseConsole(PTURNER_C3_ORIG_TASK pTaskInfo);
uint8_t TurnerC3_Orig_ParseCommand(PTURNER_C3_ORIG_TASK pTaskInfo, char command, char* cmdParams);
void TurnerC3_Orig_QueueConsoleResponse(PTURNER_C3_ORIG_TASK pTaskInfo);


extern CONSOLE_CMD_FORMAT consoleIn[NUM_TASK_FILES] __attribute__((section(".xdata")));
extern BOOL consoleAvailable[NUM_TASK_FILES];

extern BOOL kill[NUM_TASK_FILES];
extern BOOL powerDown[NUM_TASK_FILES];

#define NUM_C3_ORIG_COMMANDS		7
#define LEN_C3_COMMANDS				8

extern const prog_char tokenListConsole[];
prog_char c3OrigCommands[NUM_C3_ORIG_COMMANDS][LEN_C3_COMMANDS] = {"on", "off", "autorun", "run", "stop", "id", "status"};
prog_char c3OrigParserID[] = "C3";
prog_char c3OrigCmdList[] = "ofarxisOFARXIS";


uint8_t TurnerC3_Orig_Init(PTURNER_C3_ORIG_TASK pTaskInfo)
{
	uint8_t retval = SUCCESS;

	if(pTaskInfo->portNum == USER_PORT_1)
		pTaskInfo->eepromAddress = EEPROM_PM1_START;
	if(pTaskInfo->portNum == USER_PORT_2)
		pTaskInfo->eepromAddress = EEPROM_PM2_START;
	if(pTaskInfo->portNum == USER_PORT_3)
		pTaskInfo->eepromAddress = EEPROM_PM3_START;

	if(pTaskInfo->pBuf != NULL)
		memset(pTaskInfo->pBuf, 0, TASK_BUF_SIZE);

	OSuartInitPort(pTaskInfo->portNum, 9600, UART_MODE_EIGHT_DATA_BITS, UART_MODE_ONE_STOP_BIT, UART_MODE_NO_PARITY);

	pTaskInfo->pBuf->sampleC3 = FALSE;

	if(pTaskInfo->eepromAddress != 0)
	{
		uint32_t temp;
		EepromReadBlock((void*)(pTaskInfo->eepromAddress + TURNER_C3_SAMPLE_OFFSET), (void*)&temp, 4);

		if((temp == 0xFFFFFFFF) || (temp == 0x00000000))
		{
			temp = 60;
			EepromWriteBlock((void*)(pTaskInfo->eepromAddress + TURNER_C3_SAMPLE_OFFSET), (void*)&temp, 4);
		}
		pTaskInfo->pBuf->sampleRate = temp;

		pTaskInfo->pBuf->initState = 0xFF;
		EepromReadByte((void*)(pTaskInfo->eepromAddress + TURNER_C3_AUTORUN_STATE_OFFSET), (void*)&pTaskInfo->pBuf->initState);
		if((pTaskInfo->pBuf->initState != AUTORUN_ON) && (pTaskInfo->pBuf->initState != AUTORUN_OFF))
		{
			pTaskInfo->pBuf->initState = AUTORUN_OFF;	//Default to 1 sample per minute if not defined
			EepromWriteByteUnSigned((void*)(pTaskInfo->eepromAddress + TURNER_C3_AUTORUN_STATE_OFFSET), pTaskInfo->pBuf->initState);
		}

		if(pTaskInfo->pBuf->initState == AUTORUN_ON)
		{
			char tempBuf[2] = " ";
			TurnerC3_Orig_ParseCommand(pTaskInfo, 'r', tempBuf);
		}

	}

	OSTimeDlyHMSM(0,0,1,0);



	return retval;
}


void TurnerC3_Orig(void* pParam)
{
	TURNER_C3_ORIG_TASK taskInfo;
	PTURNER_C3_ORIG_TASK pTaskInit = (PTURNER_C3_ORIG_TASK)pParam;
	uint8_t retval = FAIL;

	taskInfo.taskID = pTaskInit->taskID;
	taskInfo.portNum = pTaskInit->portNum;
	taskInfo.pBuf = pTaskInit->pBuf;

	retval = TurnerC3_Orig_Init(&taskInfo);

	if(retval == SUCCESS)
	{
		while(1)
		{
			OSTimeDlyHMSM(0,0,0,500);

			sprintf_P(dbgMsg, PSTR("C3 Orig Loop for PM%d"), taskInfo.portNum);
			SEND_MSG;

			if(taskInfo.pBuf->sampleC3 == TRUE)
			{
				TurnerC3_Orig_GetDataSample(&taskInfo);
			}

			if(fileAvailable[taskInfo.portNum] == TRUE)
			{
				TurnerC3_Orig_ParseFile(&taskInfo, fileAvailableID[pTaskInit->portNum]);
				fileAvailableID[taskInfo.portNum] = 0;
				fileAvailable[taskInfo.portNum] = FALSE;
			}

			if (powerDown[taskInfo.portNum] == TRUE)
			{
				sprintf_P(dbgMsg, PSTR("Power down...")); SEND_MSG;
			}
			
			if(serialPortPassThrough[taskInfo.portNum] == TRUE)
			{
				//Set Flag so the float COMM task will proceed
				serialPortPassThroughOK = TRUE;

				//Reset to new Comm Params on Serial Port
				OSuartUpdatePort(taskInfo.portNum, 57600, UART_MODE_EIGHT_DATA_BITS, UART_MODE_ONE_STOP_BIT, UART_MODE_NO_PARITY);

				//Delay 15 seconds to spawn CSoft App on PC
				OSTimeDlyHMSM(0,0,15,0);

				//Power On C3 Personality
				//Toggle DTR Line to put C3 in proper mode

				switch(taskInfo.portNum)
				{
					case 1:
						BSET(DDRK, _BV(PK2));
						BSET(PORTK, _BV(PK2));
						break;
					case 2:
						BSET(DDRH, _BV(PH6));
						BSET(PORTH, _BV(PH6));
						break;
					case 3:
						BSET(DDRF, _BV(PF2));
						BSET(PORTF, _BV(PF2));
						break;
				}

				EnablePIBModule(taskInfo.portNum, TRUE, FALSE);
				OSTimeDlyHMSM(0,0,0,500);

				switch(taskInfo.portNum)
				{
					case 1:
						BCLR(PORTK, _BV(PK2));
						break;
					case 2:
						BCLR(PORTH, _BV(PH6));
						break;
					case 3:
						BCLR(PORTF, _BV(PF2));
						break;
				}

				SerialPortPassThroughC3(taskInfo.portNum);
			}


			if(consoleAvailable[taskInfo.portNum] == TRUE)
			{
				taskInfo.pBuf->responseInfo.source = consoleIn[taskInfo.taskID].cmdSrcAddress;
				taskInfo.pBuf->responseInfo.status = 0;
				taskInfo.pBuf->responseInfo.length = consoleIn[taskInfo.taskID].cmdSize;
				taskInfo.pBuf->responseInfo.id = consoleIn[taskInfo.taskID].id;

				TurnerC3_Orig_ParseConsole(&taskInfo);
				consoleAvailable[taskInfo.portNum] = FALSE;
			}

			// Check to see if we need to end this task and release resources
			if (kill[taskInfo.portNum] == TRUE) 
			{
				sprintf_P(dbgMsg, PSTR("Ending task...."));
				SEND_MSG;

				uint8_t val = 0xFF;
				uint16_t startAddr = 0;

				if(taskInfo.portNum == USER_PORT_1)
					startAddr = EEPROM_PM1_START;
				if(taskInfo.portNum == USER_PORT_2)
					startAddr = EEPROM_PM2_START;
				if(taskInfo.portNum == USER_PORT_3)
					startAddr = EEPROM_PM3_START;

				if (startAddr == 0)
				{
					sprintf_P(dbgMsg, PSTR("No start address for EEPROM"));
					SEND_MSG;
					kill[taskInfo.portNum] = FALSE;
				}
				else
				{
					// Write all F's to EEPROM
					for (uint16_t addr = startAddr; addr < startAddr + EEPROM_PM_SIZE; addr++)
						EepromWriteBlock((void*)addr, (void*)&val, 1);

					memset(taskInfo.pBuf, 0, TASK_BUF_SIZE);

					// Now end task (ie, just sleep for a really long time)
					while (1) OSTimeDlyHMSM(1,0,0,0);
				}
            }
		}
	}
	return;
}


uint8_t TurnerC3_Orig_Rx(PTURNER_C3_ORIG_TASK pTaskInfo, char* pBuffer, uint16_t* pLen, char* eol, int eolLen)
{
	uint8_t retval = FAIL;
	char* pChar = pBuffer;
	uint16_t count = 0;
	BOOL done = FALSE;
	int compare = 0;
	uint8_t err = 0;
	char c = 0;


	c = (char) OSuartGetCharWait(pTaskInfo->portNum, 256, &err);	//Wait 2 seconds for first character
			
	if(err != OS_ERR_NONE)
		{
			*pLen = 0;
			return retval;
		}

	*pChar = c;
	pChar++;
	count++;

	while((done == FALSE) && (count<110))
	{
		c = (char) OSuartGetCharWait(pTaskInfo->portNum, 256, &err);
			
		if(err != OS_ERR_NONE)
		{
			*pLen = count;
			return retval;
		}

		*pChar = c;
		pChar++;
		count++;

		if(count >= 1)
		{
			compare = strncmp((char *)(pChar - 1), eol, eolLen);
			if(compare == 0)
			{
				retval = SUCCESS;
				done = TRUE;
			}
		}
	}
	*pLen = count;

	return retval;
}


void TurnerC3_Orig_GetDataSample(PTURNER_C3_ORIG_TASK pTaskInfo)
{
	uint16_t len = 0;
	char eol = 0x0A;
//	uint8_t boardID = (uint8_t)(mainTaskAddress>>8);
	uint8_t retval = FAIL;




	memset(pTaskInfo->pBuf->bufferRx, 0, 1024);
	retval = TurnerC3_Orig_Rx(pTaskInfo, pTaskInfo->pBuf->bufferRx, &len, &eol, 1);

	if(retval == SUCCESS)
	{
		tm time;
						
		uint8_t count;
		BOOL done = FALSE;
	
		char* tokens[16];
		char sep[9];	// = {'%', ' ', '.', ':', '/', '\t', '\r', '\n' ,'\0'};
//		char sep[] = {'%', ' ', '.', ':', '/', '\t', '\r', '\n' ,'\0'};
		char* pSample = (char*)pTaskInfo->pBuf->bufferRx;

		memcpy_P(sep, separator1, 9);
//		memset(pTaskInfo->pBuf->fluorometerSample, ' ', 120);
//		sprintf_P(pTaskInfo->pBuf->fluorometerSample, PSTR("Bd: 0x%02x  Tsk: 0x%02x  Lat: %+09ld  Lon: %+010ld  "),
//					boardID, pTaskInfo->taskID, latitude, longitude); 
//		memcpy(&pTaskInfo->pBuf->fluorometerSample[54], pTaskInfo->pBuf->bufferRx, len - 2);
//		TurnerC3_Orig_DataReportXmit(pTaskInfo);


		for(count=0; count<10; count++)
		{
			if(*pSample == 'D')
				done = TRUE;
			pSample++;
		}

		count = 0;
		pSample = (char*)pTaskInfo->pBuf->bufferRx;

		while((count < 16) && (done == FALSE)) 
		{
			tokens[count] = ThreadSafeStrTok(&pSample, sep);
			if(tokens[count] == NULL)
				done = TRUE;
			count++;
		}
		if(count >= 16)	
		{
#define X_DATA_COUNT pTaskInfo->pBuf->reportData.dataBlock[pTaskInfo->pBuf->sampleCount]
			time.tm_sec = (uint8_t)atoi(tokens[5]);
			time.tm_min = (uint8_t)atoi(tokens[4]);
			time.tm_hour = (uint8_t)atoi(tokens[3]);
			time.tm_mday = (uint8_t)atoi(tokens[1]);
			time.tm_mon = (uint8_t)atoi(tokens[0]);
			time.tm_year = (uint8_t)atoi(tokens[2]) + 2000;

			X_DATA_COUNT.headerData.latitude = latitude;
			X_DATA_COUNT.headerData.longitude = longitude;
			X_DATA_COUNT.headerData.timestamp = (uint32_t)mktime(time);

			X_DATA_COUNT.dataC1.integer = (uint16_t)atol(tokens[6]);
			X_DATA_COUNT.dataC1.decimal = (uint8_t)atoi(tokens[7]);
			X_DATA_COUNT.dataC2.integer = (uint16_t)atol(tokens[8]);
			X_DATA_COUNT.dataC2.decimal = (uint8_t)atoi(tokens[9]);
			X_DATA_COUNT.dataC3.integer = (uint16_t)atol(tokens[10]);
			X_DATA_COUNT.dataC3.decimal = (uint8_t)atoi(tokens[11]);
			X_DATA_COUNT.pressure.integer = (uint16_t)atol(tokens[12]);
			X_DATA_COUNT.pressure.decimal = (uint8_t)atoi(tokens[13]);
			X_DATA_COUNT.temperature = (uint16_t)((uint32_t)atol(tokens[14]) * (uint32_t)100 + (uint32_t)atoi(tokens[15]));
			pTaskInfo->pBuf->sampleCount++;
#undef X_DATA_COUNT
			if(pTaskInfo->pBuf->sampleCount == MAX_C3_ORIG_SAMPLES)
			{
				pTaskInfo->pBuf->reportData.headerReport.payloadType = 0x00000060;
				pTaskInfo->pBuf->reportData.headerReport.boardID = mainBoardID;
				pTaskInfo->pBuf->reportData.headerReport.taskID = pTaskInfo->taskID;
				pTaskInfo->pBuf->reportData.headerReport.dataFormat = 9;
				pTaskInfo->pBuf->reportData.headerReport.parser = 0;
				pTaskInfo->pBuf->reportData.headerReport.priority = 2;
				pTaskInfo->pBuf->reportData.headerReport.repeatCount = pTaskInfo->pBuf->sampleCount;

				//transmit samples
				TurnerC3_Orig_DataReportXmitType91(pTaskInfo);

				memset(&(pTaskInfo->pBuf->reportData), 0, sizeof(C3_ORIG_DATA_REPORT));
				pTaskInfo->pBuf->sampleCount = 0;
			}
		}

		if(done == TRUE)
		{
			uint16_t length = 0;

#define X_HDR pTaskInfo->pBuf->reportASCII
			X_HDR.headerReport.payloadType = 0x00000060;
			X_HDR.headerReport.boardID = mainBoardID;
			X_HDR.headerReport.taskID = pTaskInfo->taskID;
			X_HDR.headerReport.dataFormat = 0;
			X_HDR.headerReport.parser = 1;
			X_HDR.headerReport.priority = 2;
			X_HDR.headerReport.repeatCount = 1;

			X_HDR.headerData.latitude = latitude;
			X_HDR.headerData.longitude = longitude;
			X_HDR.headerData.timestamp = gettime();

			memset(X_HDR.data, 0, 165);
			memcpy(X_HDR.data, pTaskInfo->pBuf->bufferRx, len - 2);

			//transmit samples
			length = sizeof(TYPE_91_REPORT_HEADER) + sizeof(TYPE_90_91_DATA_HEADER) + strlen(X_HDR.data);
			TurnerC3_Orig_ASCIIReportXmit(pTaskInfo, length);

			memset(&X_HDR, 0, sizeof(C3_ORIG_ASCII_REPORT));
#undef X_HDR
		}
	}
}

uint8_t TurnerC3_Orig_DataReportXmit(PTURNER_C3_ORIG_TASK pTaskInfo)
{
	uint8_t retval = 0;
	uint8_t err = 0;
	PTURNER_C3_REPORT_XMIT pReport = NULL;

	OSMutexPend(pMutexFloatQueue, INFINITE, &err);

	pReport = (PTURNER_C3_REPORT_XMIT)&(txFloatData[headFloatTx]);

	pReport->comm.headers.length  = sizeof(TURNER_C3_REPORT_XMIT) + CRC16_SIZE;
	pReport->comm.headers.destination = FLOAT_IRIDIUM_INTERFACE;

	pReport->comm.iridiumInfo.structureType = 0x80;
	pReport->comm.iridiumInfo.size = pReport->comm.headers.length - sizeof(PIB_IRIDIUM_DATA_XMIT) - CRC16_SIZE;

	pReport->payloadID = 0x00000050;

	memcpy(&(pReport->fluorometerSample), &(pTaskInfo->pBuf->fluorometerSample), 120);


	(headFloatTx < MAX_FLOAT_BUFFERS - 1) ? (headFloatTx++) : (headFloatTx = 0);

	if(headFloatTx == tailFloatTx)
	{
		(tailFloatTx < MAX_FLOAT_BUFFERS - 1) ? (tailFloatTx++) : (tailFloatTx = 0);
	}

	OSMutexPost(pMutexFloatQueue);

	return retval;
}


uint8_t TurnerC3_Orig_DataReportXmitType91(PTURNER_C3_ORIG_TASK pTaskInfo)
{
	uint8_t retval = 0;
	uint8_t err = 0;
	PTURNER_C3_ORIG_DATA_REPORT_XMIT pReport = NULL;

	OSMutexPend(pMutexFloatQueue, INFINITE, &err);

	pReport = (PTURNER_C3_ORIG_DATA_REPORT_XMIT)&(txFloatData[headFloatTx]);
	memset(pReport, 0, sizeof(FLOAT_TX_BUFFER));

	pReport->comm.headers.length  = sizeof(TURNER_C3_ORIG_DATA_REPORT_XMIT) + CRC16_SIZE;
	pReport->comm.headers.destination = FLOAT_IRIDIUM_INTERFACE;

	pReport->comm.iridiumInfo.structureType = 0x91;
	pReport->comm.iridiumInfo.size = pReport->comm.headers.length - sizeof(PIB_IRIDIUM_DATA_XMIT) - CRC16_SIZE;


	memcpy(&(pReport->reportData), &(pTaskInfo->pBuf->reportData), sizeof(C3_ORIG_DATA_REPORT));


	(headFloatTx < MAX_FLOAT_BUFFERS - 1) ? (headFloatTx++) : (headFloatTx = 0);

	if(headFloatTx == tailFloatTx)
	{
		(tailFloatTx < MAX_FLOAT_BUFFERS - 1) ? (tailFloatTx++) : (tailFloatTx = 0);
	}

	OSMutexPost(pMutexFloatQueue);

	return retval;
}


uint8_t TurnerC3_Orig_ASCIIReportXmit(PTURNER_C3_ORIG_TASK pTaskInfo, uint16_t len)
{
	uint8_t retval = SUCCESS;
	uint8_t err = 0;
	PTURNER_C3_ORIG_ASCII_REPORT_XMIT pReport = NULL;

	OSMutexPend(pMutexFloatQueue, INFINITE, &err);

	pReport = (PTURNER_C3_ORIG_ASCII_REPORT_XMIT)&(txFloatData[headFloatTx]);

//	pReport->comm.headers.length  = sizeof(TURNER_C3_ASCII_REPORT_XMIT) + CRC16_SIZE;
	pReport->comm.headers.length  = sizeof(PIB_IRIDIUM_DATA_XMIT) + len + CRC16_SIZE;
	pReport->comm.headers.destination = FLOAT_IRIDIUM_INTERFACE;

	pReport->comm.iridiumInfo.structureType = 0x91;
	pReport->comm.iridiumInfo.size = pReport->comm.headers.length - sizeof(PIB_IRIDIUM_DATA_XMIT) - CRC16_SIZE;


	memcpy(&(pReport->reportASCII), &(pTaskInfo->pBuf->reportASCII), sizeof(C3_ORIG_ASCII_REPORT));


	(headFloatTx < MAX_FLOAT_BUFFERS - 1) ? (headFloatTx++) : (headFloatTx = 0);

	if(headFloatTx == tailFloatTx)
	{
		(tailFloatTx < MAX_FLOAT_BUFFERS - 1) ? (tailFloatTx++) : (tailFloatTx = 0);
	}

	OSMutexPost(pMutexFloatQueue);

	return retval;
}

static char* ThreadSafeStrTok(char** context, char* sep)
{
	if (!context || !*context) return "";

	char* retval = &(*context)[strspn(*context, sep)];
	char* eos = &retval[strcspn(retval, sep)];

	if (*eos) *eos++ = '\0';

	*context = eos;
	return retval;
}

void TurnerC3_Orig_ParseConsole(PTURNER_C3_ORIG_TASK pTaskInfo)
{
	char* myCommand = consoleIn[pTaskInfo->taskID].cmd; 
	char* result = NULL;
	char command = 0;
	BOOL goodCommand = FALSE;
	uint8_t resultCode = 0;

	sprintf_P(dbgMsg, PSTR("Parsing console: <%s>"), myCommand); SEND_MSG;

	myCommand[MAX_FILE_SIZE_IN - 1] = 0;			//Make sure command string is NULL terminated

	memset(pTaskInfo->pBuf->tokenSeparatorList, 0, MAX_TOKEN_SEPARATORS);
	strcat_P(pTaskInfo->pBuf->tokenSeparatorList, tokenListConsole);

	result = strtok_r(myCommand, pTaskInfo->pBuf->tokenSeparatorList, &myCommand);
	resultCode = 1;				//No Command Received

	if(result != NULL)
	{
		resultCode = 2;			//Unknown Parser Code
		if(strncasecmp_P(result, c3OrigParserID, strlen_P(c3OrigParserID)) == 0)
		{
			resultCode = 3;		//Unknown command

			command = getCommand(&myCommand, c3OrigCommands[0], LEN_C3_COMMANDS, NUM_C3_ORIG_COMMANDS, c3OrigCmdList, pTaskInfo->pBuf->tokenSeparatorList);

			if(command != 0)
				goodCommand = TRUE;
		}
	}

	if(goodCommand == TRUE)
	{
		sprintf_P(dbgMsg, PSTR("good message")); SEND_MSG;
		resultCode = TurnerC3_Orig_ParseCommand(pTaskInfo, command, myCommand);
	}
	else
	{
		sprintf_P(dbgMsg, PSTR("bad .. code=%d"), resultCode); SEND_MSG;
		sprintf_P(dbgMsg, PSTR("cmd = <%s><%s>"), myCommand, result); SEND_MSG;
	}

	if(resultCode != SUCCESS)
	{
		switch(resultCode)
		{
			case 1:
				pTaskInfo->pBuf->responseInfo.status = CONSOLE_RESPONSE_EMPTY_CMD;
				break;
			case 2:
				pTaskInfo->pBuf->responseInfo.status = CONSOLE_RESPONSE_WRONG_DEVICE;
				break;
			case 3:
				pTaskInfo->pBuf->responseInfo.status = CONSOLE_RESPONSE_UNKNOWN_CMD;
				break;
			default:
				pTaskInfo->pBuf->responseInfo.status = CONSOLE_RESPONSE_CMD_FAILED;
				break;
		}
	}

	TurnerC3_Orig_QueueConsoleResponse(pTaskInfo);

	memset(consoleIn[pTaskInfo->taskID].cmd, 0, MAX_FILE_SIZE_IN);
	consoleIn[pTaskInfo->taskID].cmdSrcAddress = 0;
	consoleIn[pTaskInfo->taskID].functionCode = 0;
	consoleIn[pTaskInfo->taskID].cmdSize = 0;
	consoleIn[pTaskInfo->taskID].id = 0;

	return;

}

void TurnerC3_Orig_QueueConsoleResponse(PTURNER_C3_ORIG_TASK pTaskInfo)
{
	PCONSOLE_ACK_XMIT_2 pReport;
	uint8_t err = 0;
	char* pData;

	OSMutexPend(pMutexFloatQueue, INFINITE, &err);

	pReport = (PCONSOLE_ACK_XMIT_2)&(txFloatAck[headFloatAckTx]);
	pData = (char*)pReport;

	pReport->comm.headers.destination = FLOAT_TASK_FORWARD_MESSAGE;		//pTaskInfo->pBuf->responseInfo.source;

	pReport->comm.fwdInfo.msgFormat = MSG_FORMAT_SHIP_TO_SHORE;
	pReport->comm.fwdInfo.destination = pTaskInfo->pBuf->responseInfo.source;
	pReport->comm.fwdInfo.source = mainTaskAddress | pTaskInfo->taskID;

	if(pTaskInfo->pBuf->responseInfo.status < CONSOLE_RESPONSE_UNKNOWN_CMD)
	{
		pReport->comm.headers.length = sizeof(PIB_DATA_FORWARD_MESSAGE) + sizeof(CONSOLE_ACK) + pTaskInfo->pBuf->responseInfo.length + CRC16_SIZE;

		pReport->comm.iridiumInfo.headerICD.type = ACK_CONSOLE_DATA;

		pReport->consoleAck.len = pTaskInfo->pBuf->responseInfo.length;
		pReport->consoleAck.disposition = pTaskInfo->pBuf->responseInfo.status;
		pReport->consoleAck.format = pTaskInfo->pBuf->responseInfo.format;
		pData = pData + sizeof(PIB_DATA_FORWARD_MESSAGE) + sizeof(CONSOLE_ACK);
		memcpy(pData, pTaskInfo->pBuf->consoleResponse, pTaskInfo->pBuf->responseInfo.length);

		memcpy(dbgMsg, pTaskInfo->pBuf->consoleResponse, pTaskInfo->pBuf->responseInfo.length+1); SEND_MSG;
	}
	else
	{
		pReport->comm.headers.length = sizeof(PIB_DATA_FORWARD_MESSAGE) + sizeof(CONSOLE_NACK) + CRC16_SIZE;
		pReport->comm.iridiumInfo.headerICD.type = NACK_CONSOLE_DATA;
		pReport->consoleNack.numDispositions = 1;
		pReport->consoleNack.disposition[0] = pTaskInfo->pBuf->responseInfo.status;
	}

	pReport->comm.iridiumInfo.headerICD.packetID = pTaskInfo->pBuf->responseInfo.id;
	pReport->comm.iridiumInfo.headerICD.size = pReport->comm.headers.length - sizeof(PIB_DATA_FORWARD_MESSAGE) - CRC16_SIZE;


	(headFloatAckTx < MAX_ACK_BUFFERS - 1) ? (headFloatAckTx++) : (headFloatAckTx = 0);

	if(headFloatAckTx == tailFloatAckTx)
	{
		(tailFloatAckTx < MAX_ACK_BUFFERS - 1) ? (tailFloatAckTx++) : (tailFloatAckTx = 0);
	}

	OSMutexPost(pMutexFloatQueue);

	return;
}

uint8_t TurnerC3_Orig_ParseCommand(PTURNER_C3_ORIG_TASK pTaskInfo, char command, char* cmdParams)
{
	uint8_t retval = FAIL;

	switch(command)
	{
		case 'x':
		case 'X':
		case 'f':
		case 'F':
		{
			pTaskInfo->pBuf->sampleC3 = FALSE;
			EnablePIBModule(pTaskInfo->portNum, FALSE, FALSE);
			retval = SUCCESS;
			if(pTaskInfo->pBuf->sampleCount > 0)
			{
				pTaskInfo->pBuf->reportData.headerReport.payloadType = 0x00000060;
				pTaskInfo->pBuf->reportData.headerReport.boardID = mainBoardID;
				pTaskInfo->pBuf->reportData.headerReport.taskID = pTaskInfo->taskID;
				pTaskInfo->pBuf->reportData.headerReport.dataFormat = 9;
				pTaskInfo->pBuf->reportData.headerReport.parser = 0;
				pTaskInfo->pBuf->reportData.headerReport.priority = 2;
				pTaskInfo->pBuf->reportData.headerReport.repeatCount = MAX_C3_ORIG_SAMPLES;

				//transmit samples
				TurnerC3_Orig_DataReportXmitType91(pTaskInfo);

				memset(&(pTaskInfo->pBuf->reportData), 0, sizeof(C3_ORIG_DATA_REPORT));
				pTaskInfo->pBuf->sampleCount = 0;
			}

			break;
		}
		case 'r':
		case 'R':
		case 'o':
		case 'O':
		{
			pTaskInfo->pBuf->sampleC3 = TRUE;
			EnablePIBModule(pTaskInfo->portNum, TRUE, FALSE);
			OSTimeDlyHMSM(0,0,0,100);

			memset(pTaskInfo->pBuf->bufferRx, 0, 1024);

			uint8_t err = OS_ERR_NONE;		
			while(err == OS_ERR_NONE)
			{
				OSuartGetCharWait(pTaskInfo->portNum, 64, &err);		//wait .05 seconds -- basically clearing buffer
			}

			retval = SUCCESS;

			break;
		}
		case 'i':
		case 'I':
		{
			retval = SUCCESS;
			break;
		}
		case 's':
		case 'S':
		{
			retval = SUCCESS;
			break;
		}
		case 'a':
		case 'A':
		{
			char* result = NULL;

			memset(pTaskInfo->pBuf->tokenSeparatorList, 0, MAX_TOKEN_SEPARATORS);
			strcat_P(pTaskInfo->pBuf->tokenSeparatorList, tokenListConsole);

			result = strtok_r(cmdParams, pTaskInfo->pBuf->tokenSeparatorList, &cmdParams);

			if(result != NULL)
			{
				char* pTest = result;
				if((*pTest == 'o') || (*pTest == 'O'))
				{
					pTest++;
				
					if((*pTest == 'f') || (*pTest == 'F'))
						*result = *pTest;
				}

				retval = SUCCESS;
				switch(*result)
				{
					case 'y':
					case 'Y':
					case 'o':
					case 'O':
					case '1':
						pTaskInfo->pBuf->initState = AUTORUN_ON;
						break;
					case 'n':
					case 'N':
					case 'f':
					case 'F':
					case '0':
						pTaskInfo->pBuf->initState = AUTORUN_OFF;
						break;
					default:
						retval = FAIL;
				}

				if(retval == SUCCESS)
					EepromWriteByteUnSigned((void*)(pTaskInfo->eepromAddress + TURNER_C3_AUTORUN_STATE_OFFSET), pTaskInfo->pBuf->initState);
			}
			else
			{
				retval = FAIL;
			}
			break;
		}
	}

	if(retval == SUCCESS)
	{
		sprintf_P(pTaskInfo->pBuf->consoleResponse, PSTR("Turner C3:  Sampling is %s:  Autorun is %s"), pTaskInfo->pBuf->sampleC3 ? "ON":"OFF", (pTaskInfo->pBuf->initState==AUTORUN_ON) ? "ON":"OFF");
		pTaskInfo->pBuf->responseInfo.status = CONSOLE_RESPONSE_OK;
		pTaskInfo->pBuf->responseInfo.length = strlen(pTaskInfo->pBuf->consoleResponse);			
	}

	return retval;
}


uint8_t TurnerC3_Orig_ParseFile(PTURNER_C3_ORIG_TASK pTaskInfo, uint32_t fileID)
{
	uint8_t retval = FAIL;
	uint16_t len = 0;
	char* pBuf;
	uint8_t err = 0;
	char c;

	memset(&(fileOut[pTaskInfo->portNum][0]), 0, MAX_FILE_SIZE_OUT);

	strupr(&fileIn[pTaskInfo->portNum][0]);
	len = strlen(&fileIn[pTaskInfo->portNum][0]);

	pBuf = &fileIn[pTaskInfo->portNum][0];
	if(len > 4)
	{
		char* context = pBuf;
		char sep[5];	// = { ' ', '\t', '\r', '\n', '\0' };
//		char sep[] = { ' ', '\t', '\r', '\n', '\0' };
		char* cmdTok = ThreadSafeStrTok(&context, sep);
		
		memcpy_P(sep, separator2, 5);

		if (strncmp(cmdTok, "CMD", 3) == 0)
		{
			char* subCommand = ThreadSafeStrTok(&context, sep);

			//We have a Start/Stop command
			if(strncmp(subCommand, "STO", 3) == 0)
			{
				pTaskInfo->pBuf->sampleC3 = FALSE;
				EnablePIBModule(pTaskInfo->portNum, FALSE, FALSE);
				retval = SUCCESS;
				if(pTaskInfo->pBuf->sampleCount > 0)
				{
					pTaskInfo->pBuf->reportData.headerReport.payloadType = 0x00000060;
					pTaskInfo->pBuf->reportData.headerReport.boardID = mainBoardID;
					pTaskInfo->pBuf->reportData.headerReport.taskID = pTaskInfo->taskID;
					pTaskInfo->pBuf->reportData.headerReport.dataFormat = 9;
					pTaskInfo->pBuf->reportData.headerReport.parser = 0;
					pTaskInfo->pBuf->reportData.headerReport.priority = 2;
					pTaskInfo->pBuf->reportData.headerReport.repeatCount = MAX_C3_ORIG_SAMPLES;

					//transmit samples
					TurnerC3_Orig_DataReportXmitType91(pTaskInfo);

					memset(&(pTaskInfo->pBuf->reportData), 0, sizeof(C3_ORIG_DATA_REPORT));
					pTaskInfo->pBuf->sampleCount = 0;
				}

			}

			else if(strncmp(subCommand, "STA", 3) == 0)
			{
				pTaskInfo->pBuf->sampleC3 = TRUE;
				EnablePIBModule(pTaskInfo->portNum, TRUE, FALSE);
				OSTimeDlyHMSM(0,0,0,100);

				memset(pTaskInfo->pBuf->bufferRx, 0, 1024);

				err = OS_ERR_NONE;		
				while(err == OS_ERR_NONE)
				{
					c = (char) OSuartGetCharWait(pTaskInfo->portNum, 64, &err);		//wait .05 seconds -- basically clearing buffer
				}

				retval = SUCCESS;
			}
			if(retval == SUCCESS)
			{
				memcpy_P(&fileOut[pTaskInfo->portNum][0], PSTR("OK"), 2);
				fileOutSize[pTaskInfo->portNum] = 2;
			}
		}
	}

	if(retval == FAIL)
	{
		memcpy_P(&fileOut[pTaskInfo->portNum][0], PSTR("Unrecognized Command"), 20);
		fileOutSize[pTaskInfo->portNum] = 20;
		retval = SUCCESS;
	}

	TurnerC3_Orig_QueueFile(pTaskInfo, fileID);
	
	return retval;
}


uint8_t TurnerC3_Orig_QueueFile(PTURNER_C3_ORIG_TASK pTaskInfo, uint32_t fileID)
{
	uint8_t retval = FAIL;
	PFS_REPORT pReport;
	uint8_t err = 0;
	tm* dateTime;
	time_t time;

	OSMutexPend(pMutexFloatQueue, INFINITE, &err);

	pReport = (PFS_REPORT)&(txFloatData[headFloatTx]);

	memset(pReport, 0, sizeof(FS_REPORT));

	pReport->headers.length  = sizeof(FS_REPORT) + CRC16_SIZE;
	pReport->headers.destination = FLOAT_FS_INTERFACE;
	pReport->type = NEW_FILE_AVAILABLE;

	time=gettime();

	//Need to fill in these values appropriately
	pReport->newFile.fileSize = fileOutSize[pTaskInfo->portNum];
	dateTime = gmtime(&time);
	pReport->newFile.creationDate.day = dateTime->tm_mday;
	pReport->newFile.creationDate.hour = dateTime->tm_hour;
	pReport->newFile.creationDate.minute = dateTime->tm_min;
	pReport->newFile.creationDate.month = dateTime->tm_mon + 1;
	pReport->newFile.creationDate.second = dateTime->tm_sec;
	pReport->newFile.creationDate.year = (uint8_t)(dateTime->tm_year - 100);
	dateTime = gmtime(&time);
	pReport->newFile.modifiedDate.day = dateTime->tm_mday;
	pReport->newFile.modifiedDate.hour = dateTime->tm_hour;
	pReport->newFile.modifiedDate.minute = dateTime->tm_min;
	pReport->newFile.modifiedDate.month = dateTime->tm_mon + 1;
	pReport->newFile.modifiedDate.second = dateTime->tm_sec;
	pReport->newFile.modifiedDate.year = (uint8_t)(dateTime->tm_year - 100);

	pReport->newFile.fileAttributes = 0;
	memset(&pReport->newFile.calculatedMD5, 0, sizeof(MD5));
	memset(&pReport->newFile.providedMD5, 0, sizeof(MD5));
	pReport->newFile.calculatedCRC = 0;
	pReport->newFile.providedCRC = 0;
	pReport->newFile.storageControllerAddress = mainTaskAddress | pTaskInfo->portNum;

	char fileName[13];
	sprintf(fileName, "%.8lX.txt", fileID);

	DotToPad(pReport->newFile.fileName, fileName);

	(headFloatTx < (MAX_FLOAT_BUFFERS - 1)) ? (headFloatTx++) : (headFloatTx = 0);
	
	if(headFloatTx == tailFloatTx)
	{
		(tailFloatTx < (MAX_FLOAT_BUFFERS - 1)) ? (tailFloatTx++) : (tailFloatTx = 0);
	}

	OSMutexPost(pMutexFloatQueue);

	return retval;
}

void SerialPortPassThroughC3(uint8_t port)
{
	readyForSerialPassThru = TRUE;

	while(1)
	{
		OSTimeDlyHMSM(1,0,0,0);
	}
}