//----------------------------------------------------------------------------------
// <copyright file="TeledyneATM-88X.h" company="LiquidRobotics">
//	Copyright (c) Liquid Robotics Corporation.  All rights reserved.
// </copyright>
//
// <summary>
// 	This is the source file for managing Teledyne ATM-88X Modem
// </summary>
//
// <owner>Jim Kirklin</owner>
//----------------------------------------------------------------------------------


#include "includes.h"
#include <avr/io.h>
#include <avr/wdt.h>
#include <avr/power.h>
//#include "app_init.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 "TeledyneATM-88X.h"

#include "SerialDebug.h"


#define MODEM_CLASS			0x000B
#define TELEDYNE_ATM_88X	0x0001

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

typedef struct _MODEM_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;
}MODEM_SAMPLE, *PMODEM_SAMPLE;

typedef struct _MODEM_ATX_BLOCK_
{
	int16_t		baud;
	int16_t		acqSNR;
	int16_t		acqMPD;
	int16_t		acqSPD;
	int8_t		hdrCRC;
	int16_t		hdrSNR;
	int16_t		hdrCCERR;
	int16_t		dataMOD;
	int16_t		dataERR;
	int16_t		dataSNR;
	int16_t		dataAGC;
	int16_t		dataSPD;
	int16_t		dataCCERR;
}MODEM_ATX_BLOCK, *PMODEM_ATX_BLOCK;

typedef struct _MODEM_ATX_DATA_
{
	uint8_t			day;
	uint8_t			month;
	uint8_t			year;		//Year - 2000
	uint8_t			hour;
	uint8_t			minute;
	uint8_t			second;
	int32_t			latitude;
	int32_t			longitude;
	MODEM_ATX_BLOCK	atxBlock;
	int32_t			range;
}MODEM_ATX_DATA, *PMODEM_ATX_DATA;

typedef struct _MODEM_ATY_REPORT_
{
//	uint16_t		deviceClass;	//CTD/ADCP/etc.
//	uint16_t		deviceType;		//Seabird 32SI/Seabird 52/Teledyne Citadel/etc.
//	char			sernum[16];
	uint8_t			day;
	uint8_t			month;
	uint8_t			year;		//Year - 2000
	uint8_t			hour;
	uint8_t			minute;
	uint8_t			second;
	int32_t			latitude;
	int32_t			longitude;
	MODEM_ATX_BLOCK	atxBlocks[6];
	int32_t			range;
}MODEM_ATY_REPORT, *PMODEM_ATY_REPORT;


//typedef struct _MODEM_ATX_REPORT_
//{
//	uint16_t	deviceClass;	//CTD/ADCP/etc.
//	uint16_t	deviceType;		//Seabird 32SI/Seabird 52/Teledyne Citadel/etc.
//	char		sernum[16];
//	MODEM_ATX_DATA atxData[4];
//}MODEM_ATX_REPORT, *PMODEM_ATX_REPORT;


//typedef struct _MODEM_DATA_XMIT_ATX_
//{
//	PIB_IRIDIUM_DATA_XMIT	comm;
//	uint32_t				payloadID;
//	uint8_t					version;	
//	MODEM_ATX_REPORT		modemReport;
//}MODEM_DATA_XMIT_ATX, *PMODEM_DATA_XMIT_ATX;

typedef struct _MODEM_DATA_XMIT_ATY_
{
	PIB_IRIDIUM_DATA_XMIT	comm;
	uint32_t				payloadID;
	uint8_t					version;	
	MODEM_ATY_REPORT		modemReport;
}MODEM_DATA_XMIT_ATY, *PMODEM_DATA_XMIT_ATY;


typedef struct _TELEDYNE_ATM_88X_BUFFER_
{
	char 		bufferTx[256];
	char 		bufferRx[1024];
	uint32_t 	sampleRate;
	char 		serialNumber[16];
	BOOL		sampleModem;
	uint32_t 	nextSample;
	BOOL		modemIsOn;
	MODEM_ATY_REPORT reportATYData;
	int16_t 	baud[6];

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

}TELEDYNE_ATM_88X_BUFFER, *PTELEDYNE_ATM_88X_BUFFER;

typedef struct _TELEDYNE_ATM_88X_TASK_
{
	uint8_t	taskID;
	uint8_t portNum;
	uint16_t eepromAddress;
	PTELEDYNE_ATM_88X_BUFFER 	pBuf;
}TELEDYNE_ATM_88X_TASK, *PTELEDYNE_ATM_88X_TASK;



uint8_t TeledyneATM88X_Init(PTELEDYNE_ATM_88X_TASK taskInfo);
uint8_t TeledyneATM88X_Tx(PTELEDYNE_ATM_88X_TASK pTaskInfo, char* pBuffer, uint16_t len);
uint8_t TeledyneATM88X_Rx(PTELEDYNE_ATM_88X_TASK pTaskInfo, char* pBuffer, uint16_t* pLen, char* eol, int eolLen);
void 	TeledyneATM88X_GetModemSample(PTELEDYNE_ATM_88X_TASK pTtaskInfo);
uint8_t TeledyneATM88X_ReportModemTelemetry(PTELEDYNE_ATM_88X_TASK pTaskInfo);
uint8_t TeledyneATM88X_AlphatoMonth(char* pChar);
uint8_t TeledyneATM88X_ModemReportXmit(PTELEDYNE_ATM_88X_TASK pTaskInfo);
uint8_t TeledyneATM88X_GetSerNum(PTELEDYNE_ATM_88X_TASK pTaskInfo);
uint8_t TeledyneATM88X_ParseFile(PTELEDYNE_ATM_88X_TASK pTaskInfo, uint32_t fileID);
uint8_t TeledyneATM88X_QueueFile(PTELEDYNE_ATM_88X_TASK pTaskInfo, uint32_t fileID);

uint8_t TeledyneATM88X_ParseATR(PTELEDYNE_ATM_88X_TASK taskInfo);
uint8_t TeledyneATM88X_ParseATY(PTELEDYNE_ATM_88X_TASK taskInfo);
uint8_t	TeledyneATM88X_ParseATYBaudBlock(char* pBuf, uint16_t len, PMODEM_ATX_BLOCK pResult);

void SerialPortPassThroughATM(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);
extern uint16_t mainTaskAddress;

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

extern int32_t latitude;
extern int32_t longitude;

void TeledyneATM88X_ParseConsole(PTELEDYNE_ATM_88X_TASK pTaskInfo);
uint8_t TeledyneATM88X_ParseCommand(PTELEDYNE_ATM_88X_TASK pTaskInfo, char command, char* cmdParams);
void TeledyneATM88X_QueueConsoleResponse(PTELEDYNE_ATM_88X_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_ATM_COMMANDS	7
#define ATM_CMD_LEN			7

extern const prog_char tokenListConsole[];
prog_char teledyneATM88XCommands[NUM_ATM_COMMANDS][ATM_CMD_LEN] = {"run", "stop", "on", "off", "cmd", "id", "status"};
prog_char teledyneATM88XParserID[] = "ATM";
prog_char teledyneATM88XCmdList[] = "rxofcisRXOFCIS";


uint8_t TeledyneATM88X_Init(PTELEDYNE_ATM_88X_TASK pTaskInfo)
{
	uint8_t retval = SUCCESS;
	uint16_t baseAddr = 0;
	uint16_t len = 0;
	char eol = '>';

	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(TELEDYNE_ATM_88X_BUFFER));

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

	EnablePIBModule(pTaskInfo->portNum, TRUE, FALSE);
	pTaskInfo->pBuf->modemIsOn = TRUE;

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

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

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

	OSTimeDlyHMSM(0,0,0,100);

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

	pTaskInfo->pBuf->baud[0] = 2400;
	pTaskInfo->pBuf->baud[1] = 1200;
	pTaskInfo->pBuf->baud[2] = 1066;
	pTaskInfo->pBuf->baud[3] = 800;
	pTaskInfo->pBuf->baud[4] = 600;
	pTaskInfo->pBuf->baud[5] = 300;

	return retval;
}


void TeledyneATM88X(void* pParam)
{
	TELEDYNE_ATM_88X_TASK taskInfo;
	PTELEDYNE_ATM_88X_TASK pTaskInit = (PTELEDYNE_ATM_88X_TASK)pParam;
	uint8_t retval = FAIL;
	uint32_t myTick = 0;


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

	retval = TeledyneATM88X_Init(&taskInfo);

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

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



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

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

			if(consoleAvailable[taskInfo.portNum] == TRUE)
			{
				sprintf_P(dbgMsg, PSTR("Got something")); SEND_MSG;
				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;

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

				SerialPortPassThroughATM(taskInfo.portNum);
			}
			
			// 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(TELEDYNE_ATM_88X_BUFFER));

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


uint8_t TeledyneATM88X_Tx(PTELEDYNE_ATM_88X_TASK pTaskInfo, char* pBuffer, uint16_t len)
{
	uint8_t retval = FAIL;
	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;

			pBuffer++;
			len--;
		}

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

		retval = SUCCESS;
	}
	return retval;
}

uint8_t TeledyneATM88X_Rx(PTELEDYNE_ATM_88X_TASK pTaskInfo, char* pBuffer, uint16_t* pLen, char* eol, int eolLen)
{
	uint8_t retval = FAIL;
	char* pChar = pBuffer;
	uint16_t count = 0;
//	const char eol[1] = {0x3E};
	BOOL done = FALSE;
	int compare = 0;
	uint8_t err = 0;
	char c = 0;


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

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

	while((done == FALSE) && (count<1024))
	{
		c = (char) OSuartGetCharWait(pTaskInfo->portNum, 1280, &err);	//Wait 10 seconds for characters
			
		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 TeledyneATM88X_GetModemSample(PTELEDYNE_ATM_88X_TASK pTaskInfo)
{
	//char cmdATX[4] = {'A', 'T', 'X', '\r'};
	char cmdATY[3] = {'A', 'T', 'Y'};
	char cmdATR[3] = {'A', 'T', 'R'};

	uint16_t len = 0;
	char eol = '~';
	char c;
	uint8_t err = OS_ERR_NONE;
	tm* dateTime;
	time_t time;

	time=gettime();
	dateTime = gmtime(&time);

	pTaskInfo->pBuf->reportATYData.latitude = latitude;
	pTaskInfo->pBuf->reportATYData.longitude = longitude;
	pTaskInfo->pBuf->reportATYData.hour = dateTime->tm_hour;
	pTaskInfo->pBuf->reportATYData.minute = dateTime->tm_min;
	pTaskInfo->pBuf->reportATYData.second = dateTime->tm_sec;
	pTaskInfo->pBuf->reportATYData.day = dateTime->tm_mday;
	pTaskInfo->pBuf->reportATYData.month = dateTime->tm_mon;
	pTaskInfo->pBuf->reportATYData.year = (uint8_t)(dateTime->tm_year - 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
	}

	TeledyneATM88X_Tx(pTaskInfo, cmdATY, 3);

	TeledyneATM88X_Rx(pTaskInfo, pTaskInfo->pBuf->bufferRx, &len, &eol, 1);

	if(len >=900)
	{
		TeledyneATM88X_ParseATY(pTaskInfo);

		OSTimeDlyHMSM(0,0,0,100);
		memset(pTaskInfo->pBuf->bufferRx, 0, 1024);
		len = 0;
		eol = '>';

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

		TeledyneATM88X_Tx(pTaskInfo, cmdATR, 3);
		TeledyneATM88X_Rx(pTaskInfo, pTaskInfo->pBuf->bufferRx, &len, &eol, 1);

		TeledyneATM88X_ParseATR(pTaskInfo);

		TeledyneATM88X_ReportModemTelemetry(pTaskInfo);
	}
	else
	{
		if(strstr(pTaskInfo->pBuf->bufferRx, "Response Not") != 0)
			memset(pTaskInfo->pBuf->reportATYData.atxBlocks, 0xFF, 6 * sizeof(MODEM_ATX_BLOCK));
		pTaskInfo->pBuf->reportATYData.range = 0xFFFFFFFF;
	}
	
	return;
}

uint8_t TeledyneATM88X_ReportModemTelemetry(PTELEDYNE_ATM_88X_TASK pTaskInfo)
{
	TeledyneATM88X_ModemReportXmit(pTaskInfo);

	// reset the buffer to empty state
	memset(&pTaskInfo->pBuf->reportATYData, 0, sizeof(MODEM_ATY_REPORT));

	return SUCCESS;
}

uint8_t TeledyneATM88X_ModemReportXmit(PTELEDYNE_ATM_88X_TASK pTaskInfo)
{
	uint8_t retval = 0;
	uint8_t err = 0;
	PMODEM_DATA_XMIT_ATY pReport = NULL;

	OSMutexPend(pMutexFloatQueue, INFINITE, &err);

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

	pReport->comm.headers.length  = sizeof(MODEM_DATA_XMIT_ATY) + 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 = 0x00000030;
	pReport->version = 0x01;

	memcpy(&(pReport->modemReport), &(pTaskInfo->pBuf->reportATYData), sizeof(MODEM_ATY_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;
}

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

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

#define MODEM_IS_OFF	10



void TeledyneATM88X_ParseConsole(PTELEDYNE_ATM_88X_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, teledyneATM88XParserID, strlen_P(teledyneATM88XParserID)) == 0)
		{
			resultCode = 3;		//Unknown command

			command = getCommand(&myCommand, teledyneATM88XCommands[0], ATM_CMD_LEN, NUM_ATM_COMMANDS, teledyneATM88XCmdList, pTaskInfo->pBuf->tokenSeparatorList);

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

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

	TeledyneATM88X_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 TeledyneATM88X_QueueConsoleResponse(PTELEDYNE_ATM_88X_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 TeledyneATM88X_ParseCommand(PTELEDYNE_ATM_88X_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);

			period = period * 60;

			if(pTaskInfo->pBuf->modemIsOn == TRUE)
			{
				if (*reportPeriod && (period != 0))
				{
					pTaskInfo->pBuf->sampleModem = TRUE;
					pTaskInfo->pBuf->sampleRate = period;
					pTaskInfo->pBuf->nextSample = 0;
					TeledyneATM88X_ReportModemTelemetry(pTaskInfo);
					retval = SUCCESS;
				}
			}
			else
			{
				retval = MODEM_IS_OFF;
			}
			break;


		case 'x':
		case 'X':
			// Stop
			//sprintf_P(dbgMsg, PSTR("STOP COMMAND")); SEND_MSG;

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

			TeledyneATM88X_ReportModemTelemetry(pTaskInfo);
			retval = SUCCESS;
			break;		

		
		case 'o':
		case 'O':
			// On

			//sprintf_P(dbgMsg, PSTR("ON COMMAND")); SEND_MSG;


			EnablePIBModule(pTaskInfo->portNum, TRUE, FALSE);
			pTaskInfo->pBuf->modemIsOn = TRUE;

			retval = SUCCESS;

			break;


		
		case 'f':
		case 'F':
			// Off

			//sprintf_P(dbgMsg, PSTR("OFF COMMAND")); SEND_MSG;

			// stop sampling if still sampling
			if (pTaskInfo->pBuf->sampleModem)
			{
				pTaskInfo->pBuf->sampleModem = FALSE;
				pTaskInfo->pBuf->nextSample = 0;

				TeledyneATM88X_ReportModemTelemetry(pTaskInfo);				
			}

			EnablePIBModule(pTaskInfo->portNum, FALSE, FALSE);
			pTaskInfo->pBuf->modemIsOn = FALSE;

			retval = SUCCESS;

			break;

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

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


			sprintf_P(pTaskInfo->pBuf->consoleResponse, PSTR("Teledyne ATM-88X"));
			pTaskInfo->pBuf->responseInfo.length = strlen(pTaskInfo->pBuf->consoleResponse);
			pTaskInfo->pBuf->responseInfo.status = CONSOLE_RESPONSE_OK;

			return SUCCESS;

			break;

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

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

			sprintf_P(pTaskInfo->pBuf->consoleResponse, PSTR("Teledyne ATM-88X. Sampling is %s. Power is %s."), pTaskInfo->pBuf->sampleModem ? "ON":"OFF", pTaskInfo->pBuf->modemIsOn ? "ON":"OFF");
			pTaskInfo->pBuf->responseInfo.length = strlen(pTaskInfo->pBuf->consoleResponse);
			pTaskInfo->pBuf->responseInfo.status = CONSOLE_RESPONSE_OK;

			return SUCCESS;

			break;

		case 'c':
		case 'C':
			//We have a Command to send

			sprintf_P(dbgMsg, PSTR("CMD to send")); SEND_MSG;	

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

			//We have a Command to send to the Modem
			if (tailLen)
			{
				if(pTaskInfo->pBuf->sampleModem == FALSE)
				{			
					uint8_t err;
					char c;
					char eol = '>';

					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
					}
		
					TeledyneATM88X_Tx(pTaskInfo, tail, tailLen);
					uint16_t len = 0;
					TeledyneATM88X_Rx(pTaskInfo, pTaskInfo->pBuf->bufferRx, &len, &eol, 1);
					if((len > 0) && (len < 1024))
					{
						memcpy(pTaskInfo->pBuf->consoleResponse, pTaskInfo->pBuf->bufferRx, len);
						pTaskInfo->pBuf->responseInfo.length = strlen(pTaskInfo->pBuf->consoleResponse);
						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;


		default:
			return FAIL;
	}

	if(retval == SUCCESS)
	{
		//memcpy_P(pTaskInfo->pBuf->consoleResponse, PSTR("OK"), 2);
		sprintf_P(pTaskInfo->pBuf->consoleResponse, PSTR("OK"));
		pTaskInfo->pBuf->responseInfo.length = strlen(pTaskInfo->pBuf->consoleResponse);
		pTaskInfo->pBuf->responseInfo.status = CONSOLE_RESPONSE_OK;
	} else if (retval == MODEM_IS_OFF)
	{
		//memcpy_P(pTaskInfo->pBuf->consoleResponse, PSTR("Modem is Off -- Please turn it on first."), 20);
		sprintf_P(pTaskInfo->pBuf->consoleResponse, PSTR("Modem is Off -- Please turn it on first."));
		pTaskInfo->pBuf->responseInfo.length = strlen(pTaskInfo->pBuf->consoleResponse);
		pTaskInfo->pBuf->responseInfo.status = CONSOLE_RESPONSE_OK;
		retval = SUCCESS;
	}

	return retval;

}

uint8_t TeledyneATM88X_ParseFile(PTELEDYNE_ATM_88X_TASK pTaskInfo, uint32_t fileID)
{
	uint8_t retval = FAIL;
	uint16_t len = 0;
	char* pBuf;
	char eol = '>';

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

				TeledyneATM88X_ReportModemTelemetry(pTaskInfo);
				retval = SUCCESS;
			}

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

				period = period * 60;

				if(pTaskInfo->pBuf->modemIsOn == TRUE)
				{
					if (*reportPeriod && (period != 0))
					{
						pTaskInfo->pBuf->sampleModem = TRUE;
						pTaskInfo->pBuf->sampleRate = period;
						pTaskInfo->pBuf->nextSample = 0;
						TeledyneATM88X_ReportModemTelemetry(pTaskInfo);
						retval = SUCCESS;
					}
				}
				else
				{
					retval = MODEM_IS_OFF;
				}
			}
			else if(strncmp(subCommand, "ON", 2) == 0)
			{
				EnablePIBModule(pTaskInfo->portNum, TRUE, FALSE);
				pTaskInfo->pBuf->modemIsOn = TRUE;
			}
			else if(strncmp(subCommand, "OFF", 3) == 0)
			{
				EnablePIBModule(pTaskInfo->portNum, FALSE, FALSE);
				pTaskInfo->pBuf->modemIsOn = FALSE;
			}			
			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 Modem
			if (tailLen)
			{
				if(pTaskInfo->pBuf->sampleModem == FALSE)
				{			
					uint8_t err;
					char c;

					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
					}
		
					TeledyneATM88X_Tx(pTaskInfo, tail, tailLen);
					len = 0;
					TeledyneATM88X_Rx(pTaskInfo, pTaskInfo->pBuf->bufferRx, &len, &eol, 1);
					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;
	}

	if(retval == MODEM_IS_OFF)
	{
		memcpy_P(&fileOut[pTaskInfo->portNum][0], PSTR("Modem is Off -- Please turn it on first."), 20);
		fileOutSize[pTaskInfo->portNum] = 40;
		retval = SUCCESS;
	}

	TeledyneATM88X_QueueFile(pTaskInfo, fileID);
	
	return retval;
}


uint8_t TeledyneATM88X_QueueFile(PTELEDYNE_ATM_88X_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;
}





uint8_t TeledyneATM88X_ParseATR(PTELEDYNE_ATM_88X_TASK pTaskInfo)
{
	char* temp = pTaskInfo->pBuf->bufferRx + 17;

	if(strlen(pTaskInfo->pBuf->bufferRx) > 17)
	{
		pTaskInfo->pBuf->reportATYData.range = atoi(temp);
	}	
	return 0;
}

uint8_t TeledyneATM88X_ParseATY(PTELEDYNE_ATM_88X_TASK pTaskInfo)
{
	char temp[6];
	char* start;

	memset(pTaskInfo->pBuf->reportATYData.atxBlocks, 0, 6 * sizeof(MODEM_ATX_BLOCK));

	for(int i = 0; i<6; i++)
	{
		memset(temp, 0, 6);
		sprintf(temp, "%05d", pTaskInfo->pBuf->baud[i]);
		start = strstr(pTaskInfo->pBuf->bufferRx, temp);


		if(start != (char*)-1)
		{
			TeledyneATM88X_ParseATYBaudBlock(start, 146, &pTaskInfo->pBuf->reportATYData.atxBlocks[i]);
		}
	}

	return 0;
}


uint8_t	TeledyneATM88X_ParseATYBaudBlock(char* pBuf, uint16_t len, PMODEM_ATX_BLOCK pResult)
{
	int8_t negative = 1;

	pResult->baud = atoi(pBuf);

	//Acquisition Stats 
	pBuf += 38;
	if(*pBuf == '-')
		negative = -1;
	pBuf++;
	pResult->acqSNR = atoi(pBuf) * 10;
	pBuf += 3; 
	pResult->acqSNR += atoi(pBuf);
	pResult->acqSNR *= negative;

	pBuf += 6;
	pResult->acqMPD = atoi(pBuf) * 10;
	pBuf += 3; 
	pResult->acqMPD += atoi(pBuf);


	pBuf += 6;
	negative = 1;
	if(*pBuf == '-')
		negative = -1;
	pBuf++;
	pResult->acqSPD = atoi(pBuf) * 10;
	pBuf += 3; 
	pResult->acqSPD += atoi(pBuf);
	pResult->acqSPD *= negative;

	
	
	//Header Stats
	pBuf += 20;
	if(*pBuf == 'P')
		pResult->hdrCRC = 1;
	else
		pResult->hdrCRC = 0;

	pBuf += 9;
	pResult->hdrSNR = atoi(pBuf) * 10;
	pBuf += 3; 
	pResult->hdrSNR += atoi(pBuf);

	pBuf += 8;
	pResult->hdrCCERR = atoi(pBuf);



	//Data
	pBuf += 9;
	pResult->dataMOD = atoi(pBuf);	

	pBuf += 7;
	pResult->dataERR = atoi(pBuf);	

	pBuf += 8;
	pResult->dataSNR = atoi(pBuf) * 10;
	pBuf += 3; 
	pResult->dataSNR += atoi(pBuf);

	pBuf += 6;
	pResult->dataAGC = atoi(pBuf);
	
	pBuf += 7;	
	negative = 1;
	if(*pBuf == '-')
		negative = -1;
	pBuf++;
	pResult->dataSPD = atoi(pBuf) * 10;
	pBuf += 3; 
	pResult->dataSPD += atoi(pBuf);
	pResult->dataSPD *= negative;

	if(pResult->baud != 2400)
	{
		pBuf += 8;
		pResult->dataCCERR = atoi(pBuf);
	}

	return 0;
}

void SerialPortPassThroughATM(uint8_t port)
{
	readyForSerialPassThru = TRUE;

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