//----------------------------------------------------------------------------------
// <copyright file="SeabirdCTD_37SI.c" company="LiquidRobotics">
//	Copyright (c) Liquid Robotics Corporation.  All rights reserved.
// </copyright>
//
// <summary>
// 	This is the source for managing Seabird 37SI CTD
// </summary>
//
// <owner>Jim Kirklin</owner>
//----------------------------------------------------------------------------------

#include "includes.h"
#include <avr/io.h>
#include <avr/pgmspace.h>
#include <string.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 "SeabirdCTD_37SI.h"

#include "SerialDebug.h"

#define CTD_CLASS			0x000A
#define CTD_SEABIRD_37_SI	0x0001

//Serial Number stored at offset 0x0010 from PM base -- 16 bytes
#define SEABIRD_37_SI_SERNUM_OFFSET		0x0010
//Sample Rate stored at offset 0x0040 from PM base -- 4 bytes
#define SEABIRD_37_SI_SAMPLE_OFFSET		0x0040

typedef struct _CTD_SAMPLE_
{
	int32_t		temp;
	int32_t		cond;
	uint8_t		day;
	uint8_t		month;
	uint8_t		year;		//Year - 2000
	uint8_t		hour;
	uint8_t		minute;
	uint8_t		second;
}CTD_SAMPLE, *PCTD_SAMPLE;

typedef struct _CTD_REPORT_
{
	uint16_t	deviceClass;	//CTD/ADCP/etc.
	uint16_t	deviceType;		//Seabird 32SI/Seabird 52/Teledyne Citadel/etc.
	char		sernum[16];
	uint8_t		numSamples;
	CTD_SAMPLE	samples[8];
}CTD_REPORT, *PCTD_REPORT;

typedef struct _CTD_DATA_XMIT_
{
	PIB_IRIDIUM_DATA_XMIT	comm;
	uint32_t				payloadID;
	uint8_t					version;	
	CTD_REPORT				ctdReport;
}CTD_DATA_XMIT, *PCTD_DATA_XMIT;

typedef struct _SEABIRD_CTD_37_SI_BUFFER_
{	
	char 					bufferTx[256];
	char 					bufferRx[1024];
	uint32_t 				sampleRate;
	char 					serialNumber[16];
	BOOL					sampleCTD;
	uint32_t 				nextSample;
	uint8_t					reportSampleCount;
	CTD_REPORT				reportData;
	CONSOLE_RESPONSE_INFO	responseInfo;
	char					tokenSeparatorList[MAX_TOKEN_SEPARATORS];
	char					consoleResponse[256];
}SEABIRD_CTD_37_SI_BUFFER, *PSEABIRD_CTD_37_SI_BUFFER;

typedef struct _SEABIRD_CTD_37_SI_TASK_
{
	uint8_t	taskID;
	uint8_t portNum;
	uint16_t	eepromAddress;
	PSEABIRD_CTD_37_SI_BUFFER 	pBuf;
}SEABIRD_CTD_37_SI_TASK, *PSEABIRD_CTD_37_SI_TASK;


uint8_t SeabirdCTD_37SI_Init(PSEABIRD_CTD_37_SI_TASK taskInfo);
uint8_t SB37SI_CTD_Tx(PSEABIRD_CTD_37_SI_TASK pTaskInfo, char* pBuffer, uint16_t len);
uint8_t SB37SI_CTD_Rx(PSEABIRD_CTD_37_SI_TASK pTaskInfo, char* pBuffer, uint16_t* pLen);
void 	SB37SI_GetCTDSample(PSEABIRD_CTD_37_SI_TASK pTtaskInfo);
uint8_t SB37SI_ReportCTDTelemetry(PSEABIRD_CTD_37_SI_TASK pTaskInfo);
uint8_t SB37SI_AlphatoMonth(char* pChar);
uint8_t SB37SI_CTDReportXmit(PSEABIRD_CTD_37_SI_TASK pTaskInfo);
uint8_t SB37SI_GetSerNum(PSEABIRD_CTD_37_SI_TASK pTaskInfo);
uint8_t SB37SI_ParseFile(PSEABIRD_CTD_37_SI_TASK pTaskInfo, uint32_t fileID);
uint8_t SB37SI_QueueFile(PSEABIRD_CTD_37_SI_TASK pTaskInfo, uint32_t fileID);

void SerialPortPassThrough37SI(uint8_t port);

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 FLOAT_TX_BUFFER txFloatAck[MAX_ACK_BUFFERS] __attribute__((section(".xdata")));
extern uint8_t	headFloatAckTx;
extern uint8_t tailFloatAckTx;

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


void SB37SI_ParseConsole(PSEABIRD_CTD_37_SI_TASK pTaskInfo);
uint8_t SB37SI_ParseCommand(PSEABIRD_CTD_37_SI_TASK pTaskInfo, char command, char* cmdParams);
void SB37SI_QueueConsoleResponse(PSEABIRD_CTD_37_SI_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];


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


#define NUM_CTD37SI_COMMANDS	5
#define CTD37SI_CMD_LEN			7


extern const prog_char tokenListConsole[];
prog_char ctd37SICommands[NUM_CTD37SI_COMMANDS][CTD37SI_CMD_LEN] = {"run", "stop", "cmd", "id", "status"};
prog_char ctd37SIParserID[] = "37SI";
prog_char ctd37SICmdList[] = "rxcisRXCIS";


uint8_t SeabirdCTD_37SI_Init(PSEABIRD_CTD_37_SI_TASK pTaskInfo)
{
	uint8_t retval = SUCCESS;
	uint16_t baseAddr = 0;
	uint16_t len = 0;

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

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

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

	EnablePIBModule(pTaskInfo->portNum, TRUE, FALSE);

	if(baseAddr != 0)
	{
		uint32_t temp;
		EepromReadBlock((void*)(baseAddr + SEABIRD_37_SI_SAMPLE_OFFSET), (void*)&temp, 4);

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

	OSTimeDlyHMSM(0,0,1,0);
	OSuartPutCharWait(pTaskInfo->portNum, 0x0D, 10);
	SB37SI_CTD_Rx(pTaskInfo, pTaskInfo->pBuf->bufferRx, &len);

	SB37SI_GetSerNum(pTaskInfo);
	memcpy(pTaskInfo->pBuf->reportData.sernum, pTaskInfo->pBuf->serialNumber, 16);

	OSTimeDlyHMSM(0,0,0,100);

	pTaskInfo->pBuf->reportSampleCount = 8;
	pTaskInfo->pBuf->sampleCTD = FALSE;
	pTaskInfo->pBuf->nextSample = 0;

	return retval;
}


void Sbrd37SI(void* pParam)
{
	SEABIRD_CTD_37_SI_TASK taskInfo;
	PSEABIRD_CTD_37_SI_TASK pTaskInit = (PSEABIRD_CTD_37_SI_TASK)pParam;
	uint8_t retval = FAIL;
	uint32_t myTick = 0;


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

	retval = SeabirdCTD_37SI_Init(&taskInfo);

	if(retval == SUCCESS)
	{
		while(1)
		{
			OSTimeDlyHMSM(0,0,0,500);
			sprintf_P(dbgMsg, PSTR("CTD37SI Loop for PM%d"), taskInfo.portNum);
			SEND_MSG;

			if(taskInfo.pBuf->sampleCTD == TRUE)
			{
				myTick = getsecondtick();
				if(myTick >= taskInfo.pBuf->nextSample)
				{
					SB37SI_GetCTDSample(&taskInfo);
					taskInfo.pBuf->nextSample = myTick + taskInfo.pBuf->sampleRate;
				}
			}

			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;
				
				EnablePIBModule(taskInfo.portNum, TRUE, FALSE);
				OSTimeDlyHMSM(0,0,0,500);

				SerialPortPassThrough37SI(taskInfo.portNum);
			}

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

			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;

				SB37SI_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);
					
					// Turn off PIB
					EnablePIBModule(taskInfo.portNum, FALSE, FALSE);
					
					memset(taskInfo.pBuf, 0, sizeof(SEABIRD_CTD_37_SI_BUFFER));

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


uint8_t SB37SI_CTD_Tx(PSEABIRD_CTD_37_SI_TASK pTaskInfo, char* pBuffer, uint16_t len)
{
	uint8_t retval = FAIL;
	uint8_t firstCharTries = 3;
	char c = 0;
	uint8_t err = 0;

	if((len > 0) && (len <= 255))
	{
		while(len > 0)
		{
			err = OSuartPutCharWait(pTaskInfo->portNum, *pBuffer, 10);
			if(err != OS_ERR_NONE)
				return retval;

			c = 0;
			c = (char) OSuartGetCharWait(pTaskInfo->portNum, 128, &err);
		
			if(err != OS_ERR_NONE)
			{
				if(firstCharTries > 0)
				{
					firstCharTries--;
					OSTimeDlyHMSM(0,0,0,100);
				}
				else
					return retval;
			}
			if(c == *pBuffer)
			{
				pBuffer++;
				firstCharTries = 0;
				len--;
			}
			else
			{
				if(firstCharTries == 0)
					return retval;
			}
		}

		err = OSuartPutCharWait(pTaskInfo->portNum, 0x0D, 10);		//Terminate command with carriage return		
		if(err != OS_ERR_NONE)
			return retval;

		c = 0;
		c = (char) OSuartGetCharWait(pTaskInfo->portNum, 10, &err);	//Echos is Carriage Return
		c = (char) OSuartGetCharWait(pTaskInfo->portNum, 10, &err);	//And Line Feed
		if(c == 0x0A)
			retval = SUCCESS;
	}


	return retval;
}

uint8_t SB37SI_CTD_Rx(PSEABIRD_CTD_37_SI_TASK pTaskInfo, char* pBuffer, uint16_t* pLen)
{
	uint8_t retval = FAIL;
	char* pChar = pBuffer;
	uint16_t count = 0;
	const char eol[2] = {0x53, 0x3E};
	BOOL done = FALSE;
	int compare = 0;
	uint8_t err = 0;
	char c = 0;


	c = (char) OSuartGetCharWait(pTaskInfo->portNum, 1280, &err);	//Wait 10 seconds for first character
			
	if(err != OS_ERR_NONE)
		return retval;

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

	while((done == FALSE) && (count<1024))
	{
		c = (char) OSuartGetCharWait(pTaskInfo->portNum, 100, &err);	//Wait 10 seconds for first character
			
		if(err != OS_ERR_NONE)
			return retval;

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

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

	return retval;
}


void SB37SI_GetCTDSample(PSEABIRD_CTD_37_SI_TASK pTaskInfo)
{
	char cmdTs[2] = {0x54, 0x53};
	char cmdDs[2] = {0x44, 0x53};

	char* last;
	char* first = pTaskInfo->pBuf->bufferRx;
	uint8_t ntok;
	char tokenDelimiters[] = " :,\r\n";
	char* tokens[11];


	uint16_t len = 0;

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

	SB37SI_CTD_Tx(pTaskInfo, cmdTs, 2);

	SB37SI_CTD_Rx(pTaskInfo, pTaskInfo->pBuf->bufferRx, &len);

	if(len == 11)
	{
		uint8_t index = pTaskInfo->pBuf->reportData.numSamples;
		PCTD_SAMPLE data = (PCTD_SAMPLE)pTaskInfo->pBuf->bufferRx;

		pTaskInfo->pBuf->reportData.samples[index].temp = data->temp;
		pTaskInfo->pBuf->reportData.samples[index].cond = data->cond;

		OSTimeDlyHMSM(0,0,0,100);
		len = 0;
		SB37SI_CTD_Tx(pTaskInfo, cmdDs, 2);
		SB37SI_CTD_Rx(pTaskInfo, pTaskInfo->pBuf->bufferRx, &len);

		for(ntok = 0; ntok < 11; ntok++)
		{
			tokens[ntok] = strtok_r(first, tokenDelimiters, &last);
			first = NULL;
			if(tokens[ntok] == NULL)
			{
				break;
			}
		}

		if ((ntok == 11) && (strcmp(tokens[3], "NO.") == 0))
		{
			pTaskInfo->pBuf->reportData.samples[index].day = atoi(tokens[5]);
			pTaskInfo->pBuf->reportData.samples[index].month = SB37SI_AlphatoMonth(tokens[6]);
			pTaskInfo->pBuf->reportData.samples[index].year = atoi(tokens[7]) - 2000;
			pTaskInfo->pBuf->reportData.samples[index].hour = atoi(tokens[8]);
			pTaskInfo->pBuf->reportData.samples[index].minute = atoi(tokens[9]);
			pTaskInfo->pBuf->reportData.samples[index].second = atoi(tokens[10]);
		}

		pTaskInfo->pBuf->reportData.numSamples++;

		if (pTaskInfo->pBuf->reportData.numSamples >= pTaskInfo->pBuf->reportSampleCount)
			SB37SI_ReportCTDTelemetry(pTaskInfo);
	}	
	return;
}

uint8_t SB37SI_ReportCTDTelemetry(PSEABIRD_CTD_37_SI_TASK pTaskInfo)
{
	if (pTaskInfo->pBuf->reportData.numSamples)
	{
		// queue up telemetry re;port
		SB37SI_CTDReportXmit(pTaskInfo);

		// reset the buffer to empty state
		memset(&pTaskInfo->pBuf->reportData, 0, sizeof(CTD_REPORT));
		memcpy(pTaskInfo->pBuf->reportData.sernum, pTaskInfo->pBuf->serialNumber, 16);
		}

	return SUCCESS;
}


uint8_t SB37SI_AlphatoMonth(char* pChar)
{
	uint8_t month = 0;

	if(strncmp(pChar, "Jan", 3) == 0)
		month = 1;
	if(strncmp(pChar, "Feb", 3) == 0)
		month = 2;
	if(strncmp(pChar, "Mar", 3) == 0)
		month = 3;
	if(strncmp(pChar, "Apr", 3) == 0)
		month = 4;
	if(strncmp(pChar, "May", 3) == 0)
		month = 5;
	if(strncmp(pChar, "Jun", 3) == 0)
		month = 6;
	if(strncmp(pChar, "Jul", 3) == 0)
		month = 7;
	if(strncmp(pChar, "Aug", 3) == 0)
		month = 8;
	if(strncmp(pChar, "Sep", 3) == 0)
		month = 9;
	if(strncmp(pChar, "Oct", 3) == 0)
		month = 10;
	if(strncmp(pChar, "Nov", 3) == 0)
		month = 11;
	if(strncmp(pChar, "Dec", 3) == 0)
		month = 12;

	return month;
}

uint8_t SB37SI_GetSerNum(PSEABIRD_CTD_37_SI_TASK pTaskInfo)
{
	uint8_t retval = FAIL;
	char* last;
	char* first = pTaskInfo->pBuf->bufferRx;
	uint8_t ntok;
	char tokenDelimiters[] = " :,\r\n";
	char* tokens[5];
	uint16_t len;
	char cmdDs[2] = {0x44, 0x53};

	OSTimeDlyHMSM(0,0,0,100);
	len = 0;
	SB37SI_CTD_Tx(pTaskInfo, cmdDs, 2);
	SB37SI_CTD_Rx(pTaskInfo, pTaskInfo->pBuf->bufferRx, &len);

	for(ntok = 0; ntok < 5; ntok++)
	{
		tokens[ntok] = strtok_r(first, tokenDelimiters, &last);
		first = NULL;
		if(tokens[ntok] == NULL)
		{
			break;
		}
	}

	if ((ntok == 5) && (strcmp(tokens[3], "NO.") == 0))
	{
		int32_t sernum = 0;

		sernum = atol(tokens[4]);
		sprintf(pTaskInfo->pBuf->serialNumber, "%ld", sernum);
		retval = SUCCESS;
	}

	return retval;
}

uint8_t SB37SI_CTDReportXmit(PSEABIRD_CTD_37_SI_TASK pTaskInfo)
{
	uint8_t retval = 0;
	uint8_t err = 0;
	PCTD_DATA_XMIT pReport = NULL;

	OSMutexPend(pMutexFloatQueue, INFINITE, &err);

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

	pReport->comm.headers.length  = sizeof(CTD_DATA_XMIT)
							      - sizeof(pReport->ctdReport.samples)
								  + pTaskInfo->pBuf->reportData.numSamples*sizeof(pReport->ctdReport.samples[0])
								  + 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 = 0x00000020;
	pReport->version = 0x01;

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


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

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

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

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

	char* retval = &(*context)[strspn(*context, sep)];
	
	*context = &retval[strlen(retval)];
	return retval;
}

void SB37SI_ParseConsole(PSEABIRD_CTD_37_SI_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, ctd37SIParserID, strlen_P(ctd37SIParserID)) == 0)
		{
			resultCode = 3;		//Unknown command

			command = getCommand(&myCommand, ctd37SICommands[0], CTD37SI_CMD_LEN, NUM_CTD37SI_COMMANDS, ctd37SICmdList, pTaskInfo->pBuf->tokenSeparatorList);

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

	if(goodCommand == TRUE)
	{
		sprintf_P(dbgMsg, PSTR("good message")); SEND_MSG;
		resultCode = SB37SI_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;
		}
	}

	SB37SI_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 SB37SI_QueueConsoleResponse(PSEABIRD_CTD_37_SI_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); 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 SB37SI_ParseCommand(PSEABIRD_CTD_37_SI_TASK pTaskInfo, char command, char* myCommand)
{
	uint8_t retval = FAIL;

	sprintf_P(dbgMsg, PSTR("Parsing Command <%c> <%s>"), command, myCommand); SEND_MSG;
	
	switch (command)
	{
		case 'r':
		case 'R':
			// Run

			sprintf_P(dbgMsg, PSTR("RUN COMMAND")); SEND_MSG;				

			char* reportPeriod = ThreadSafeStrTok(&myCommand, pTaskInfo->pBuf->tokenSeparatorList);
			uint32_t period = atol(reportPeriod);

			if (*reportPeriod && (period != 0))
			{
				char* reportSamples = ThreadSafeStrTok(&myCommand, pTaskInfo->pBuf->tokenSeparatorList);
				uint32_t samples = *reportSamples ? atol(reportSamples) : 8;

				if ((samples >= 1) && (samples <= 8))
				{
					pTaskInfo->pBuf->sampleCTD = TRUE;
					pTaskInfo->pBuf->sampleRate = period;
					pTaskInfo->pBuf->reportSampleCount = samples;
					pTaskInfo->pBuf->nextSample = 0;
					SB37SI_ReportCTDTelemetry(pTaskInfo);
					retval = SUCCESS;
				}
			}
			
			
			if(retval == SUCCESS)
			{				
				memcpy_P(pTaskInfo->pBuf->consoleResponse, PSTR("OK"), 2);
				pTaskInfo->pBuf->responseInfo.status = CONSOLE_RESPONSE_OK;
				pTaskInfo->pBuf->responseInfo.length = 2;
			}

			break;

		case 'x':
		case 'X':
			// Stop

			sprintf_P(dbgMsg, PSTR("STOP COMMAND")); SEND_MSG;

			pTaskInfo->pBuf->sampleCTD = FALSE;
			pTaskInfo->pBuf->nextSample = 0;

			SB37SI_ReportCTDTelemetry(pTaskInfo);
			retval = SUCCESS;

			memcpy_P(pTaskInfo->pBuf->consoleResponse, PSTR("OK"), 2);
			pTaskInfo->pBuf->responseInfo.status = CONSOLE_RESPONSE_OK;
			pTaskInfo->pBuf->responseInfo.length = 2;

			break;

		case 'c':
		case 'C':
			// CMD to the CTD

			sprintf_P(dbgMsg, PSTR("CMD to the CTD")); SEND_MSG;	


			char* tail = ThreadSafeStrTokTail(&myCommand, pTaskInfo->pBuf->tokenSeparatorList);
			uint16_t tailLen = strlen(tail);

			//We have a Command to send to the CTD
			if (tailLen)
			{
				if(pTaskInfo->pBuf->sampleCTD == FALSE)
				{			
					OSTimeDlyHMSM(0,0,0,100);
					SB37SI_CTD_Tx(pTaskInfo, tail, tailLen);
					uint16_t len = 0;
					SB37SI_CTD_Rx(pTaskInfo, pTaskInfo->pBuf->bufferRx, &len);
					if((len > 0) && (len < 1024))
					{
						memcpy(pTaskInfo->pBuf->consoleResponse, pTaskInfo->pBuf->bufferRx, len);
						pTaskInfo->pBuf->responseInfo.length = len;
						pTaskInfo->pBuf->responseInfo.status = CONSOLE_RESPONSE_OK;
						retval = SUCCESS;
					}
				}
				else
				{
					memcpy_P(pTaskInfo->pBuf->consoleResponse, PSTR("You must stop sampling first"), 28);
					pTaskInfo->pBuf->responseInfo.length = strlen(pTaskInfo->pBuf->consoleResponse);
					pTaskInfo->pBuf->responseInfo.status = CONSOLE_RESPONSE_OK;
					retval = SUCCESS;
				}
			}

			break;

		case 'i':
		case 'I':
			// ID

			sprintf_P(dbgMsg, PSTR("ID COMMAND")); SEND_MSG;	


			retval = SB37SI_GetSerNum(pTaskInfo);

			if (retval==SUCCESS)
			{
				sprintf_P(pTaskInfo->pBuf->consoleResponse, PSTR("CTD37SI. Serial Number=%s"), pTaskInfo->pBuf->serialNumber);
				pTaskInfo->pBuf->responseInfo.length = strlen(pTaskInfo->pBuf->consoleResponse);
				pTaskInfo->pBuf->responseInfo.status = CONSOLE_RESPONSE_OK;				
			} else
			{
				sprintf_P(pTaskInfo->pBuf->consoleResponse, PSTR("CTD37SI. Error reading serial number."));
				pTaskInfo->pBuf->responseInfo.length = strlen(pTaskInfo->pBuf->consoleResponse);
				pTaskInfo->pBuf->responseInfo.status = CONSOLE_RESPONSE_OK;
				retval = SUCCESS;
			}

			break;

		case 's':
		case 'S':
			// Status

			sprintf_P(dbgMsg, PSTR("STATUS COMMAND")); SEND_MSG;	

			sprintf_P(pTaskInfo->pBuf->consoleResponse, PSTR("CTD37SI. Sampling is %s"), pTaskInfo->pBuf->sampleCTD ? "ON":"OFF");
			pTaskInfo->pBuf->responseInfo.length = strlen(pTaskInfo->pBuf->consoleResponse);
			pTaskInfo->pBuf->responseInfo.status = CONSOLE_RESPONSE_OK;				

			retval = SUCCESS;

			break;

		default:
			return FAIL;
	}

	return retval;

}


uint8_t SB37SI_ParseFile(PSEABIRD_CTD_37_SI_TASK pTaskInfo, uint32_t fileID)
{
	uint8_t retval = FAIL;
	uint16_t len = 0;
	char* pBuf;

	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[] = { ' ', '\t', '\r', '\n' };
		char* cmdTok = ThreadSafeStrTok(&context, sep);

		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->sampleCTD = FALSE;
				pTaskInfo->pBuf->nextSample = 0;

				SB37SI_ReportCTDTelemetry(pTaskInfo);
				retval = SUCCESS;
			}

			else if(strncmp(subCommand, "STA", 3) == 0)
			{
				char* reportPeriod = ThreadSafeStrTok(&context, sep);
				uint32_t period = atol(reportPeriod);

				if (*reportPeriod && (period != 0))
				{
					char* reportSamples = ThreadSafeStrTok(&context, sep);
					uint32_t samples = *reportSamples ? atol(reportSamples) : 8;

					if ((samples >= 1) && (samples <= 8))
					{
						pTaskInfo->pBuf->sampleCTD = TRUE;
						pTaskInfo->pBuf->sampleRate = period;
						pTaskInfo->pBuf->reportSampleCount = samples;
						pTaskInfo->pBuf->nextSample = 0;
						SB37SI_ReportCTDTelemetry(pTaskInfo);
						retval = SUCCESS;
					}
				}
			}
			
			if(retval == SUCCESS)
			{
				memcpy_P(&fileOut[pTaskInfo->portNum][0], PSTR("OK"), 2);
				fileOutSize[pTaskInfo->portNum] = 2;
			}
		}
		else if(strncmp(cmdTok, "DEV", 3) == 0)
		{
			char* tail = ThreadSafeStrTokTail(&context, sep);
			uint16_t tailLen = strlen(tail);

			//We have a Command to send to the CTD
			if (tailLen)
			{
				if(pTaskInfo->pBuf->sampleCTD == FALSE)
				{			
					OSTimeDlyHMSM(0,0,0,100);
					SB37SI_CTD_Tx(pTaskInfo, tail, tailLen);
					len = 0;
					SB37SI_CTD_Rx(pTaskInfo, pTaskInfo->pBuf->bufferRx, &len);
					if((len > 0) && (len < 1024))
					{
						memcpy(&fileOut[pTaskInfo->portNum][0], pTaskInfo->pBuf->bufferRx, len);
						fileOutSize[pTaskInfo->portNum] = len;
						retval = SUCCESS;
					}
				}
				else
				{
					memcpy_P(&fileOut[pTaskInfo->portNum][0], PSTR("You must stop sampling first"), 28);
					fileOutSize[pTaskInfo->portNum] = 28;
					retval = SUCCESS;
				}
			}
		}
	}

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

	SB37SI_QueueFile(pTaskInfo, fileID);
	
	return retval;
}


uint8_t SB37SI_QueueFile(PSEABIRD_CTD_37_SI_TASK pTaskInfo, uint32_t fileID)
{
	uint8_t retval = FAIL;
	PFS_REPORT pReport;
	uint8_t err = 0;
	tm* dateTime;
	time_t time;
	//OS Specific -- protecting Queue head and tail values
	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, "CTD_Out.txt");
	DotToPad(pReport->newFile.fileName, fileName);

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

	//OS Specific -- protecting Queue head and tail values
	OSMutexPost(pMutexFloatQueue);


	return retval;
}

void SerialPortPassThrough37SI(uint8_t port)
{
	readyForSerialPassThru = TRUE;

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