//----------------------------------------------------------------------------------
// <copyright file="FlPyldComms.c" company="LiquidRobotics">
//	Copyright (c) Liquid Robotics Corporation.  All rights reserved.
// </copyright>
//
// <summary>
// 	This is the source for managing communication between the PIB, Float, and 
//  other payloads daisychained off the PIB
// </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 "User_Health.h"
#include "boot.h"

#include "SerialDebug.h"


#define FILE_INIT_WRITE		0x9000
#define FILE_WRITE_BLOCK	0x9001
#define FILE_DELETE			0x9002
#define FILE_READ_BLOCK		0x9003
#define FILE_GET_DETAIL		0x9004
#define FILE_GET_INFO		0x9008

#define SEND_FWD_ICD_CMD			0x0024


//Nak Dispositions
#define	NAKDISP_SUCCEEDED			(3)				// this item succeeded, other items failed
#define	NAKDISP_DATA				(5)				// something was wrong with the data
#define	NAKDISP_FILE_IN_USE			(7)				// file exists and is in use
#define	NAKDISP_WOULD_OVERWRITE		(8)				// file exists (new file would overwrite)
#define	NAKDISP_READ_ONLY			(9)				// file is marked as read-only
#define	NAKDISP_MISSING_STORAGE		(10)			// storage device is missing, cannot be mounted
#define	NAKDISP_CRC_FAILED			(11)			// calculated CRC of file content does not match provided number
#define	NAKDISP_MD5_FAILED			(12)			// calculated MD5 of file content does not match provided signature
#define	NAKDISP_STORAGE_FULL		(13)			// storage device is full
#define	NAKDISP_STORAGE_IO_ERROR	(14)			// there was a problem talking to the storage device
#define	NAKDISP_BAD_FILE_CONTROL_BLOCK (15)			// there is a problem with the control blocks that describe the file
#define	NAKDISP_BAD_PROG_SELECT		(16)			// there is a problem with the configuration of program selection
#define	NAKDISP_TOO_MANY_COMMANDS	(17)			// there are too many active commands


#define MAX_PYLD_FILE_BLOCK_SIZE	192

#define SHIP_SHORE_ICD		0x0001
#define EXECUTE_CMD			1
#define INFORMATION			0





typedef struct _ACK_READ_FILE_
{
	uint16_t	storageControllerAddress;	// storage controller address from request
	uint32_t	seekOffset;			// offset from start of file
	uint8_t		eofFlag;			// EOF flag (0 = not EOF, 0xFF = EOF)
	uint16_t	numDataBytes;		// number of data bytes to follow
	uint8_t		data[192];			// data bytes (content of file; may be larger, but at least 64)
	uint16_t	crc;				// placeholder for CRC
}ACK_READ_FILE, *PACK_READ_FILE;

typedef struct _NAK_FILE_
{
	uint16_t	count;		//always one for this application
	uint8_t		result;
	uint16_t	crc;
}NAK_FILE, *PNAK_FILE;

typedef struct _FILE_ACKS_
{
	LRI_COMM_WRAPPER headers;
	LRI_GENERIC_ACK	 ackInfo;
	COMM_HEADER		toShoreheader;
	union
	{
		ACK_READ_FILE ackRead;
		ACK_FILE_LIST ackFileInfo;
		uint16_t crc;
	};

}FILE_ACKS, *PFILE_ACKS;

typedef struct _FILE_NACKS_
{
	LRI_COMM_WRAPPER headers;
	LRI_GENERIC_NAK	 nakInfo;
	COMM_HEADER		toShoreheader;
	union
	{
		NAK_FILE	nakFile;
		uint16_t crc;
	};
}FILE_NAKS, *PFILE_NAKS;

#define NO_WRITE_IN_PROGRESS	0
#define WRITE_IN_PROGRESS		1
#define WRITE_COMPLETE			2

typedef struct _FILE_WRITE_IN_PROGRESS_
{
	uint8_t		writeStatus;
	char		tempFileName[13];
	char		finalFileName[13];
	uint32_t	tickCount;
	uint16_t	storageControllerAddress;
	char		fileName[11];
	uint32_t	fileID;
	uint32_t	fileSize;
	DATE_TIME	creationDate;
	DATE_TIME	modifiedDate;
	uint8_t		fileAttributes;
	uint8_t		overwriteFile;
	uint16_t	contentCRC;
	MD5			contentMD5;
	uint16_t	numDataBytes;
}FILE_WRITE_IN_PROGRESS, *PFILE_WRITE_IN_PROGRESS;



void PIBRcvdFloatCmd(PPYLD_MGR_FILE_RX_BUFFER pHeaders);
void CancelWriteInProgress();

void FileInitializeWrite(PPYLD_MGR_FILE_RX_BUFFER pHeaders);
void FileWriteBlock(PPYLD_MGR_FILE_RX_BUFFER pHeaders);
void FileDelete(PPYLD_MGR_FILE_RX_BUFFER pHeaders);
void FileReadBlock(PPYLD_MGR_FILE_RX_BUFFER pHeaders);
// JGK	void FileReadComplete(PPYLD_MGR_FILE_RX_BUFFER pHeaders);
void FileGetInfo(PPYLD_MGR_FILE_RX_BUFFER pHeaders);
void FileGetDetail(PPYLD_MGR_FILE_RX_BUFFER pHeaders);
void SelectProgram(PPYLD_MGR_FILE_RX_BUFFER pHeaders);

void SerialPortPassThroughFloat(uint8_t port);

uint8_t	PadToDot(char* pad, char* dot);
uint8_t	DotToPad(char* pad, char* dot);


FILE_ACKS		pyldFileAcks __attribute__((section(".xdata")));
FILE_NAKS		pyldFileNaks __attribute__((section(".xdata")));

FILE_WRITE_IN_PROGRESS	writingFile __attribute__((section(".xdata")));


BOOL fileAllowed[NUM_TASK_FILES] __attribute__((section(".xdata")));

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

CONSOLE_CMD_FORMAT consoleIn[NUM_TASK_FILES] __attribute__((section(".xdata")));
BOOL consoleAvailable[NUM_TASK_FILES] __attribute__((section(".xdata")));	// = {0};

BOOL kill[NUM_TASK_FILES] __attribute__((section(".xdata")));
BOOL powerDown[NUM_TASK_FILES] __attribute__((section(".xdata")));

BOOL serialPortPassThrough[NUM_TASK_FILES] __attribute__((section(".xdata")));	// = {0};
BOOL serialPortPassThroughOK __attribute__((section(".xdata")));	// = FALSE;

BOOL readyForSerialPassThru __attribute__((section(".xdata")));	// = FALSE;

void ConsoleInput(PPYLD_MGR_FILE_RX_BUFFER pHeaders);

const prog_char tokenListConsole[] = " ,:\r\n\t";

extern prog_char emptyParserID[];
extern prog_char powerParserID[];
extern prog_char gpctdParserID[];
extern prog_char ctd37SIParserID[];
extern prog_char SB37SIParserID[];
extern prog_char teledyneATM88XParserID[];
extern prog_char c3OrigParserID[];
extern prog_char MBARI_CO2_ParserID[];

#define MAX_COMM_TARGETS	4

typedef struct _PIB_GENERIC_ACK_
{
	LRI_COMM_WRAPPER 		headers;
	LRI_GENERIC_ACK			ackInfo;
	LRI_ACK_COUNT_WRAPPER	pibCount;
	uint16_t				crc;
}PIB_GENERIC_ACK, *PPIB_GENERIC_ACK;

typedef struct _PIB_ID_RESPONSE_
{
	LRI_COMM_WRAPPER 		headers;
	LRI_GENERIC_ACK			ackInfo;
	LRI_ACK_COUNT_WRAPPER	idCount;
	LRI_ID_PACKET			idPacket[MAX_COMM_TARGETS + 1];
	uint16_t				crc;
}PIB_ID_RESPONSE, *PPIB_ID_RESPONSE;

typedef struct _PIB_STATUS_RESPONSE_
{
	LRI_COMM_WRAPPER 		headers;
	LRI_GENERIC_ACK			ackInfo;
	LRI_ACK_COUNT_WRAPPER	pibCount;
	LRI_PIB_STATUS			pibPacket;
	uint16_t				crc;
}PIB_STATUS_RESPONSE, *PPIB_STATUS_RESPONSE;


typedef struct _PIB_POWER_RESPONSE_
{
	LRI_COMM_WRAPPER 					headers;
	LRI_GENERIC_ACK						ackInfo;
	LRI_ACK_COUNT_WRAPPER				powerCount;
	ACK_PYLD_POWER_NOTIFICATION_RECORD	powerState;
	uint16_t							crc;
}PIB_POWER_RESPONSE, *PPIB_POWER_RESPONSE;


PIB_ID_RESPONSE 	pibIDResponse 		__attribute__((section(".xdata")));
PIB_STATUS_RESPONSE	pibStatusResponse 	__attribute__((section(".xdata")));
PIB_POWER_RESPONSE  pibPowerResponse 	__attribute__((section(".xdata")));
PIB_GENERIC_ACK		pibGenericAck 		__attribute__((section(".xdata")));
PLRI_GENERIC_NAK_PACKET pGenericNak __attribute__((section(".xdata")));

char rxFloatData[HDLC_MAX_LENGTH] __attribute__((section(".xdata")));
char rxPMData[HDLC_MAX_LENGTH] __attribute__((section(".xdata")));
char txHDLCBuffer[HDLC_MAX_LENGTH] __attribute__((section(".xdata")));




void TxHDLC(uint8_t* pTxHDLCBuf, uint16_t activeTxLen);
BOOL PM_TxHDLC(uint8_t* pTxHDLCBuf, uint16_t activeTxLen, uint8_t port, uint16_t timeout);
uint16_t RxHDLC(char *pHDLCBuf);
uint16_t PM_RxHDLC(char *pHDLCBuf, uint8_t port, uint16_t timeout);


uint8_t ValidateCRC16(uint8_t* pBuffer, uint16_t size, uint16_t initValue);
uint16_t CalculateCRC16(uint8_t* pBuffer, uint16_t size, uint16_t initValue);

void PIBToFloatSendID(uint16_t command, uint16_t msgSeq, uint16_t destination);
void PIBToFloatSendPIBStatus(uint16_t command, uint16_t msgSeq, uint16_t destination);
void PIBToFloatSendWaitingData(uint16_t command, uint16_t msgSeq, uint16_t destination);
void PIBToFloatSendNAK(uint16_t command, uint16_t msgSeq, uint16_t destination);

void PIBToPMGetID(PLRI_COMM_WRAPPER pHeaders);
void PIBToPMForwardPacket(PLRI_COMM_WRAPPER pHeaders);
void PIBToPMBroadcastPacket(PLRI_COMM_WRAPPER pHeaders);

void PIBToFloatSendPowerReply(PLRI_COMM_WRAPPER pBuffer);


void FloatCommParseConsole(PPYLD_MGR_FILE_RX_BUFFER pHeaders);
void FloatCommsQueueConsoleResponse(PPYLD_MGR_FILE_RX_BUFFER pHeaders, char* response);


char rxFloatData[HDLC_MAX_LENGTH] __attribute__((section(".xdata")));
char rxPMData[HDLC_MAX_LENGTH] __attribute__((section(".xdata")));


FLOAT_TX_BUFFER txFloatData[MAX_FLOAT_BUFFERS] __attribute__((section(".xdata")));
uint8_t	headFloatTx = 0;
uint8_t tailFloatTx = 0;


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

//#define MAIN_TASK_ADDRESS	0x0100

uint16_t mainTaskAddress = 0x0100;
uint8_t mainBoardID 	 = 0x01;


int32_t latitude __attribute__((section(".xdata")));	//;
int32_t longitude __attribute__((section(".xdata")));	//;
char	fixValid __attribute__((section(".xdata")));	//;

BOOL handledStatusRequest = FALSE;

uint8_t delay;

LRI_ID_PACKET personalityIDs[MAX_COMM_TARGETS] __attribute__((section(".xdata")));
uint8_t		commTargets = 0;

uint8_t newData = 0;
uint8_t newStop = 0;
uint8_t newParity = 0;

PYLD_COMMS_INIT commPortState __attribute__((section(".xdata")));

uint16_t* pCRC __attribute__((section(".xdata")));


void Float_Comm_Init(void)
{
	uint8_t temp;

	memset(&txFloatData[0], 0, sizeof(FLOAT_TX_BUFFER) * MAX_FLOAT_BUFFERS);
	OSuartInitPort(FLOAT_PORT, 115200, UART_MODE_EIGHT_DATA_BITS, UART_MODE_ONE_STOP_BIT, UART_MODE_NO_PARITY);


	memset((char*)fileOutSize , 0, sizeof(uint16_t) * sizeof(NUM_TASK_FILES));
	memset((char*)fileInSize , 0, sizeof(uint16_t) * sizeof(NUM_TASK_FILES));
	memset((char*)fileAvailable , 0, sizeof(BOOL) * sizeof(NUM_TASK_FILES));
	memset((char*)fileAvailableID , 0, sizeof(uint32_t) * sizeof(NUM_TASK_FILES));
	memset((char*)transmitPending , 0, sizeof(BOOL) * sizeof(NUM_TASK_FILES));
	memset((char*)consoleAvailable , 0, sizeof(BOOL) * sizeof(NUM_TASK_FILES));
	memset((char*)serialPortPassThrough , 0, sizeof(BOOL) * sizeof(NUM_TASK_FILES));

	serialPortPassThroughOK = FALSE;
	readyForSerialPassThru = FALSE;

	memset(personalityIDs, 0, MAX_COMM_TARGETS * sizeof(LRI_ID_PACKET));

	EepromReadByte((void*)(EEPROM_SYSTEM_PARAMS_START + EEPROM_BOARD_ID_OFFSET), &temp);

	if((temp == 0xFF) || (temp == 0x00))
	{
		temp = (mainTaskAddress >> 8);
		EepromWriteByteUnSigned((void*)(EEPROM_SYSTEM_PARAMS_START + EEPROM_BOARD_ID_OFFSET), temp);
	}

	EepromReadByte((void*) (EEPROM_SYSTEM_PARAMS_START + EEPROM_POWER_DOWN_DELAY_OFFSET), &delay);
	if(delay == 0xFF)
	{
		delay = 1;
		EepromWriteByteUnSigned((void*)(EEPROM_SYSTEM_PARAMS_START + EEPROM_POWER_DOWN_DELAY_OFFSET), delay);
	}

	mainTaskAddress = temp;
	mainBoardID = temp;
	mainTaskAddress = mainTaskAddress<<8;

	memset(fileIn , 0, NUM_TASK_FILES*MAX_FILE_SIZE_IN);
	memset(fileOut , 0, NUM_TASK_FILES*MAX_FILE_SIZE_OUT);

	for(int i=0; i<NUM_TASK_FILES; i++)
	{
		serialPortPassThrough[i] = FALSE;
	}

	for(int i = USER_PORT_1; i<=USER_PORT_3; i++)
	{
		switch(commPortState.pmState[i])
		{
			case PM_STANDARD_COMM:
				OSuartInitPort(i, 115200, UART_MODE_EIGHT_DATA_BITS, UART_MODE_ONE_STOP_BIT, UART_MODE_NO_PARITY);
				EnablePIBModule(i, TRUE, TRUE);
				break;
			case PM_SMC_COMM:
				OSuartInitPort(i, 115200, UART_MODE_EIGHT_DATA_BITS, UART_MODE_TWO_STOP_BITS, UART_MODE_NO_PARITY);
				EnablePIBModule(i, TRUE, TRUE);
				break;
			default:
				//Do Nothing
				break;
		}
	}
}


void FloatComms(void* pParam)
{

	PLRI_COMM_WRAPPER pHeaders= (PLRI_COMM_WRAPPER)(rxFloatData + 1);
	PLRI_REQUEST_PIB_STATUS pStatus = (PLRI_REQUEST_PIB_STATUS)(rxFloatData + 1 + sizeof(LRI_COMM_WRAPPER));
	PLRI_REQUEST_SERIAL_PORT_PASS_THROUGH pPassThrough;
 	pGenericNak = (PLRI_GENERIC_NAK_PACKET)txHDLCBuffer;

	uint8_t nakCode = 0;

	uint8_t err = 0;

	for(int i = 0; i<4; i++)
	{
		OSuartFlushBuffers(i, UART_ALL_BUFFERS);
		kill[i] = FALSE;
	}

	while(1)
	{
		pHeaders= (PLRI_COMM_WRAPPER)(rxFloatData + 1);
		pStatus = (PLRI_REQUEST_PIB_STATUS)(rxFloatData + 1 + sizeof(LRI_COMM_WRAPPER));
		pPassThrough = (PLRI_REQUEST_SERIAL_PORT_PASS_THROUGH)(rxFloatData + 1 + sizeof(LRI_COMM_WRAPPER));
		uint16_t msgLen;

		memset(rxFloatData, 0, HDLC_MAX_LENGTH);
		msgLen = RxHDLC(rxFloatData);
		
		if(msgLen != 0)
		{
			if(ValidateCRC16((uint8_t*)pHeaders, pHeaders->length, OTHER_CRC_START) == SUCCESS)
			{
				dbgMsg[0] = 0; SEND_MSG;
				sprintf_P(dbgMsg, PSTR("RECV:\tSrc:0x%04X\tDest:0x%04X\tType:0x%04X"), pHeaders->source, pHeaders->destination, pHeaders->msgType & ~0x8000);
				SEND_MSG;

				if(((pHeaders->destination & 0xFF00) == (mainTaskAddress & 0xFF00)) ||
					(((pHeaders->destination & 0xFF00) == 0xFF00) && (pHeaders->msgType == REQUEST_ID)))
				{	
						
					switch(pHeaders->msgType & ~0x8000)
					{
						case REQUEST_ID:
						{
							sprintf_P(dbgMsg, PSTR("Received id request"));
							SEND_MSG;

							OSuartFlushBuffers(FLOAT_PORT, UART_RX_BUFFER);
							if(pHeaders->length == 0x10)
							{
								PIBToPMGetID(pHeaders);
								PIBToFloatSendID(pHeaders->msgType, pHeaders->msgID, pHeaders->source);
							}
							break;
						}
						case REQUEST_PIB_STATUS:
						{
							sprintf_P(dbgMsg, PSTR("Received PIB status request"));
							SEND_MSG;

							tm time;

							OSuartFlushBuffers(FLOAT_PORT, UART_RX_BUFFER);

							time.tm_sec = pStatus->timestamp.second;
							time.tm_min = pStatus->timestamp.minute;
							time.tm_hour = pStatus->timestamp.hour;
							time.tm_mday = pStatus->timestamp.day;
							time.tm_mon = pStatus->timestamp.month;
							time.tm_year = pStatus->timestamp.year + 2000;

							settime(mktime(time));

							latitude = pStatus->latitude;
							longitude = pStatus->longitude;
							fixValid = pStatus->fixValid;
							PIBToFloatSendPIBStatus(pHeaders->msgType, pHeaders->msgID, pHeaders->source);

							handledStatusRequest = TRUE;
							break;
						}
						case REQUEST_WAITING_PACKET:
						{
							sprintf_P(dbgMsg, PSTR("Received request waiting packet"));
							SEND_MSG;

							OSuartFlushBuffers(FLOAT_PORT, UART_RX_BUFFER);

							if((headFloatTx != tailFloatTx) || (headFloatAckTx != tailFloatAckTx))
								PIBToFloatSendWaitingData(pHeaders->msgType, pHeaders->msgID, pHeaders->source);
							else
								nakCode = NAKSUMMARY_INVALID_DATA;
					
							break;
						}
						case POWER_NOTIFICATION:
						{
							sprintf_P(dbgMsg, PSTR("Received power notification"));
							SEND_MSG;

							OSuartFlushBuffers(FLOAT_PORT, UART_RX_BUFFER);

							for (uint8_t i=0; i<NUM_TASK_FILES; i++)
								powerDown[i] = TRUE;

							PIBToFloatSendPowerReply(pHeaders);
							break;
						}
						case SEND_FWD_ICD_CMD:
						{
							sprintf_P(dbgMsg, PSTR("Received forward command"));
							SEND_MSG;

							OSuartFlushBuffers(FLOAT_PORT, UART_RX_BUFFER);
							//printf("Received File Request\r\n");
							PIBRcvdFloatCmd((PPYLD_MGR_FILE_RX_BUFFER)pHeaders);
							break;
						}
						case REQUEST_SERIAL_PORT_PASS_THRU:
						{
							sprintf_P(dbgMsg, PSTR("Received serial port pass through request"));
							SEND_MSG;

							uint8_t myTOCount = 0;
							serialPortPassThroughOK = FALSE;
							serialPortPassThrough[pPassThrough->personalityModuleNumber] = TRUE;

							while((serialPortPassThroughOK == FALSE) && (myTOCount < 15))
							{
								myTOCount++;
								OSTimeDlyHMSM(0,0,1,0);
							}

							if(serialPortPassThroughOK == TRUE)
								pibGenericAck.headers.msgType = LRI_ACK;
							else
								pibGenericAck.headers.msgType = LRI_NAK;

							pibGenericAck.headers.length = sizeof(PIB_GENERIC_ACK);
							pibGenericAck.headers.destination = pHeaders->source;
							pibGenericAck.headers.source = mainTaskAddress;
							pibGenericAck.headers.msgID = pHeaders->msgID;

							pibGenericAck.ackInfo.originalCmd = pHeaders->msgType;

							pibGenericAck.pibCount.totalNumber = 1;
							pibGenericAck.pibCount.numberOfReplies = 1;

							pibGenericAck.crc = CalculateCRC16((uint8_t*)&pibGenericAck, pibGenericAck.headers.length - CRC16_SIZE, OTHER_CRC_START);

							TxHDLC((uint8_t*)&pibGenericAck, pibGenericAck.headers.length);

							OSTimeDlyHMSM(0,0,2,0);

							if(serialPortPassThroughOK == TRUE)
							{
								switch(pPassThrough->passThroughDataBits)
								{
									case 5:
										newData = UART_MODE_FIVE_DATA_BITS;
										break;
									case 6:
										newData = UART_MODE_SIX_DATA_BITS;
										break;
									case 7:
										newData = UART_MODE_SEVEN_DATA_BITS;
										break;
									default:
										newData = UART_MODE_EIGHT_DATA_BITS;
										break;
								}

								switch(pPassThrough->passThroughStopBits)
								{
									case 2:
										newStop = UART_MODE_TWO_STOP_BITS;
										break;
									default:
										newStop = UART_MODE_ONE_STOP_BIT;
										break;
								}

								switch(pPassThrough->passThroughParity)
								{
									case 'E':
									case 'e':
										newParity = UART_MODE_EVEN_PARITY;
										break;
									case 'O':
									case 'o':
										newParity = UART_MODE_ODD_PARITY;
										break;
									default:
										newParity = UART_MODE_NO_PARITY;
										break;
								}

								OSuartUpdatePort(FLOAT_PORT, 
												pPassThrough->passThroughBaud, 
												newData, 
												newStop, 
												newParity);

								SerialPortPassThroughFloat(pPassThrough->personalityModuleNumber);

							}
							else
							{
								serialPortPassThrough[pPassThrough->personalityModuleNumber] = FALSE;
							}
							
							break;
						}
						default:
						{
							sprintf_P(dbgMsg, PSTR("Received unknown message type: 0x%x"), pHeaders->msgType & ~0x8000);
							SEND_MSG;

							nakCode = NAKSUMMARY_UNKNOWN_COMMAND;
							//Unknown packet
							err = 1;
							break;
						}
					}
				}
				else
				{
	                OSuartFlushBuffers(FLOAT_PORT, UART_RX_BUFFER);

				

					if((pHeaders->destination & 0xFF00) == 0xFF00)
					{
						PIBToPMBroadcastPacket(pHeaders);
					}
					else
					{
						sprintf_P(dbgMsg, PSTR("Forwarding message to 0x%04x\r\n"), pHeaders->destination);
						SEND_MSG;
						PIBToPMForwardPacket(pHeaders);
					}
				}

				if(nakCode != 0)
				{
					uint16_t* pCRC;

					OSuartFlushBuffers(FLOAT_PORT, UART_RX_BUFFER);

					pGenericNak->headers.destination = pHeaders->source;
					pGenericNak->headers.source = pHeaders->destination;
					pGenericNak->headers.msgID = pHeaders->msgID;
					pGenericNak->headers.msgType = LRI_ACK;

					pGenericNak->headers.msgType = LRI_NAK;
					pGenericNak->headers.length = sizeof(LRI_GENERIC_NAK_PACKET) + CRC16_SIZE;
					pGenericNak->nakInfo.nakSummary = nakCode;

					pCRC = (uint16_t*)(txHDLCBuffer + pGenericNak->headers.length - CRC16_SIZE);
					*pCRC = CalculateCRC16((uint8_t*)pGenericNak, pGenericNak->headers.length - CRC16_SIZE, OTHER_CRC_START);
					TxHDLC((uint8_t*)pGenericNak, pGenericNak->headers.length);
				}

			}
		}
	}
	return;
}

//Only support No ACK on this one currently
void PIBToPMBroadcastPacket(PLRI_COMM_WRAPPER pHeaders)
{

	int i = 0;
	BOOL error;


	for(i=1; i<4; i++)
	{
		if((commPortState.pmState[i] == PM_STANDARD_COMM) || (commPortState.pmState[i] == PM_SMC_COMM))
		    error = PM_TxHDLC((uint8_t*)pHeaders, pHeaders->length, i, 10);
	}


	return;
}


void PIBToPMForwardPacket(PLRI_COMM_WRAPPER pHeaders)
{
	int i = 0;
	uint16_t destID;
	int16_t index = -1;
	destID = pHeaders->destination;
	PLRI_COMM_WRAPPER pReply= (PLRI_COMM_WRAPPER)(rxPMData + 1);
	BOOL error = FALSE;
	uint16_t length;

	uint16_t myTimeout = 64;

	switch (pHeaders->msgType)
	{
		case IPIB_SECURE_ON:
		case IPIB_SECURE_AUTHENTICATE:
		case IPIB_SECURE_REGISTER_CLIENT:
		case IPIB_SECURE_FAILED:
		case IPIB_SECURE_OFF:
		case IPIB_SECURE_ON_2:
		case IPIB_ENCRYPT_DECRYPT_PACKET:
		case IPIB_ENCRYPT_DECRYPT_BLOCK_DOWN_START:
		case IPIB_ENCRYPT_DECRYPT_BLOCK_DOWN:
		case IPIB_ENCRYPT_DECRYPT_BLOCK_DOWN_END:
		case IPIB_ENCRYPT_DECRYPT_BLOCK_UP_START:
		case IPIB_ENCRYPT_DECRYPT_BLOCK_UP:
		case IPIB_ENCRYPT_DECRYPT_BLOCK_UP_END:
		{
			myTimeout = 128 * 45;
			break;
		}
		default:
		{
			myTimeout = 64;
			break;
		}
	}

	for(i=0; i<MAX_COMM_TARGETS; i++)
	{
		if((personalityIDs[i].address & 0xFF00) == (destID & 0xFF00))
		{
			index = i;
			i=MAX_COMM_TARGETS;
		}	
	}

	if((index >= 0) && (index < MAX_COMM_TARGETS))
	{
		if(personalityIDs[index].address != 0000)
		{
			error = PM_TxHDLC((uint8_t*)pHeaders, pHeaders->length, personalityIDs[index].port, 10);
			if(error == FALSE)
			{
				length = PM_RxHDLC(rxPMData, personalityIDs[index].port, myTimeout);
				if(length != 0)
				{
					TxHDLC((uint8_t*)pReply, pReply->length);
				}
			}
		}
		else
		{
			//Send NAK
			sprintf_P(dbgMsg, PSTR("Address==0000.")); 
			SEND_MSG;
		}
	}
	else
	{
		//Send NAK
		sprintf_P(dbgMsg, PSTR("Address out of range.")); 
		SEND_MSG;
	}

	return;
}



void PIBToPMGetID(PLRI_COMM_WRAPPER pHeaders)
{
	int i = 0;
	PPIB_ID_RESPONSE pIDReply = (PPIB_ID_RESPONSE)(rxPMData + 1);
	BOOL error;
	uint16_t length;
	uint8_t loopcount = 0;
	commTargets = 0;

	for(i=1; i<4; i++)
	{
		if((commPortState.pmState[i] == PM_STANDARD_COMM) || (commPortState.pmState[i] == PM_SMC_COMM))
		{
		    uint8_t retryCt = 0;
		    while(retryCt < 2)
		    {
		    	retryCt++;
			    memset(rxPMData, 0, HDLC_MAX_LENGTH);
			    error = PM_TxHDLC((uint8_t*)pHeaders, pHeaders->length, i, 0);
			    if(error == FALSE)
			    {
			    	length = PM_RxHDLC(rxPMData, i, 64);
				    if(length > 0)
				    {
					    if(ValidateCRC16((uint8_t*)pIDReply, pIDReply->headers.length, OTHER_CRC_START) == SUCCESS)
					    {
						    loopcount = 0;
							while((commTargets < MAX_COMM_TARGETS) && (pIDReply->idCount.totalNumber >= 1))
							{
								memcpy(&personalityIDs[commTargets], &pIDReply->idPacket[loopcount], sizeof(LRI_ID_PACKET));
							    personalityIDs[commTargets].port = i;
								loopcount++;
							    commTargets++;
								pIDReply->idCount.totalNumber--;
							}
						    retryCt = 3;
					    }
				    }
			    }
		    }
	    }
	}

	return;
}



void PIBToFloatSendID(uint16_t command, uint16_t msgSeq, uint16_t destination)
{
	pCRC = NULL;

	memset(&pibIDResponse, 0, sizeof(PIB_ID_RESPONSE));
	pibIDResponse.headers.length = sizeof(LRI_COMM_WRAPPER) + sizeof(LRI_GENERIC_ACK) + sizeof(LRI_ACK_COUNT_WRAPPER)
									+ sizeof(LRI_ID_PACKET) * (commTargets + 1) + sizeof(uint16_t);
	pibIDResponse.headers.destination = destination;
	pibIDResponse.headers.source = mainTaskAddress;
	pibIDResponse.headers.msgID = msgSeq;
	pibIDResponse.headers.msgType = LRI_ACK;

	pibIDResponse.ackInfo.originalCmd = command;

	pibIDResponse.idCount.totalNumber = commTargets + 1;
	pibIDResponse.idCount.numberOfReplies = commTargets + 1;

	pibIDResponse.idPacket[0].version = 0x0001;
	pibIDResponse.idPacket[0].deviceType = 0x01;
	pibIDResponse.idPacket[0].address = mainTaskAddress;
	memcpy(&(pibIDResponse.idPacket[0].serialNum[0]), "123456", 6);
	pibIDResponse.idPacket[0].port = 0;
	pibIDResponse.idPacket[0].pollingRate = 20;
	pibIDResponse.idPacket[0].extraInfo = 0;
	pibIDResponse.idPacket[0].firmwareMajor = MAJOR;
	pibIDResponse.idPacket[0].firmwareMinor = MINOR;
	pibIDResponse.idPacket[0].firmwareRev = BUILD;
	memcpy(&(pibIDResponse.idPacket[0].description[0]), "Full PIB            ", 20);

	for(int i = 0; i<commTargets; i++)
	{
		memcpy(&(pibIDResponse.idPacket[i + 1]), &personalityIDs[i], sizeof(LRI_ID_PACKET));
	}

	pCRC = (uint16_t*)(((uint8_t*)&pibIDResponse) + pibIDResponse.headers.length - CRC16_SIZE);
	*pCRC = CalculateCRC16((uint8_t*)&pibIDResponse, pibIDResponse.headers.length - CRC16_SIZE, OTHER_CRC_START);

	TxHDLC((uint8_t*)&pibIDResponse, pibIDResponse.headers.length);

	return;
}


void PIBToFloatSendPIBStatus(uint16_t command, uint16_t msgSeq, uint16_t destination)
{
	pibStatusResponse.headers.length = sizeof(PIB_STATUS_RESPONSE);
	pibStatusResponse.headers.destination = destination;
	pibStatusResponse.headers.source = mainTaskAddress;
	pibStatusResponse.headers.msgID = msgSeq;
	pibStatusResponse.headers.msgType = LRI_ACK;

	pibStatusResponse.ackInfo.originalCmd = command;

	pibStatusResponse.pibCount.totalNumber = 1;
	pibStatusResponse.pibCount.numberOfReplies = 1;

	pibStatusResponse.pibPacket.version = 1;
	if((headFloatTx != tailFloatTx) || (headFloatAckTx != tailFloatAckTx))
	{
		pibStatusResponse.pibPacket.version |= 0x8000;
	}
	pibStatusResponse.pibPacket.address = mainTaskAddress;
	pibStatusResponse.pibPacket.alarms = 0;
	pibStatusResponse.pibPacket.leak1 = 0;
	pibStatusResponse.pibPacket.leak2 = 0;
	pibStatusResponse.pibPacket.humidTemp = pibTempHum;
	pibStatusResponse.pibPacket.humidity = pibHumidity;
	pibStatusResponse.pibPacket.presTemp = pibTempPres;	// pibPressure;
	pibStatusResponse.pibPacket.pressure = pibPressure;	// pibTempPres;

	pibStatusResponse.crc = CalculateCRC16((uint8_t*)&pibStatusResponse, pibStatusResponse.headers.length - CRC16_SIZE, OTHER_CRC_START);
	
	TxHDLC((uint8_t*)&pibStatusResponse, pibStatusResponse.headers.length);


	return;
}


uint8_t* tail1 __attribute__((section(".xdata")));
uint8_t* head1 __attribute__((section(".xdata")));
PPIB_IRIDIUM_DATA_XMIT comms __attribute__((section(".xdata")));
uint8_t tempTail __attribute__((section(".xdata")));
void PIBToFloatSendWaitingData(uint16_t command, uint16_t msgSeq, uint16_t destination)
{
	uint8_t tempTail = 0;
	uint16_t* pCRC = NULL;
	uint8_t* temp;
	uint8_t err = 0;
	uint8_t numBuffs = MAX_FLOAT_BUFFERS;

	pCRC = NULL;
	tail1 = NULL;
	head1 = NULL;
	comms = NULL;
	tempTail = 0;

	OSMutexPend(pMutexFloatQueue, INFINITE, &err);

	if(headFloatAckTx != tailFloatAckTx)
	{
		tail1 = &tailFloatAckTx;
		head1 = &headFloatAckTx;
		numBuffs = MAX_ACK_BUFFERS;

		temp = (uint8_t*)&(txFloatAck[*tail1]);
		comms = (PPIB_IRIDIUM_DATA_XMIT)&(txFloatAck[*tail1]);
	}
	else
	{
		tail1 = &tailFloatTx;
		head1 = &headFloatTx;
		numBuffs = MAX_FLOAT_BUFFERS;

		temp = (uint8_t*)&(txFloatData[*tail1]);
		comms = (PPIB_IRIDIUM_DATA_XMIT)&(txFloatData[*tail1]);
	}		

	tempTail = *tail1 + 1;
	if(tempTail >= numBuffs) tempTail = 0;

	comms->headers.destination |= (destination & 0xFF00);		// | FLOAT_IRIDIUM_INTERFACE;
	comms->headers.source = mainTaskAddress;
	comms->headers.msgID = msgSeq;
	comms->headers.msgType = LRI_ACK;

	comms->ackInfo.originalCmd = command;
	if(tempTail != *head1)
	{
		comms->ackInfo.originalCmd |= 0x8000;
	}
	
	temp += comms->headers.length;
	temp -= CRC16_SIZE;

	pCRC = (uint16_t*)temp;
	*pCRC = CalculateCRC16((uint8_t*)comms, comms->headers.length - CRC16_SIZE, OTHER_CRC_START);

	TxHDLC((uint8_t*)comms, comms->headers.length);

	(*tail1 < numBuffs - 1) ? (*tail1 = *tail1 + 1) : (*tail1 = 0);

	OSMutexPost(pMutexFloatQueue);

	return;
}


void PIBToFloatSendPowerReply(PLRI_COMM_WRAPPER pBuffer)
{
	pibPowerResponse.headers.length = sizeof(PIB_POWER_RESPONSE);
	pibPowerResponse.headers.destination = pBuffer->source;
	pibPowerResponse.headers.source = mainTaskAddress;
	pibPowerResponse.headers.msgID = pBuffer->msgID;
	pibPowerResponse.headers.msgType = LRI_ACK;

	pibPowerResponse.ackInfo.originalCmd = pBuffer->msgType;

	pibPowerResponse.powerCount.totalNumber = 1;
	pibPowerResponse.powerCount.numberOfReplies = 1;

	pibPowerResponse.powerState.address = mainTaskAddress;
	pibPowerResponse.powerState.boardID = mainBoardID;
	pibPowerResponse.powerState.state = 1;

	pibPowerResponse.powerState.delayOff = delay;	
	pibPowerResponse.powerState.delayOn = 1;

	pibPowerResponse.crc = CalculateCRC16((uint8_t*)&pibPowerResponse, pibPowerResponse.headers.length - CRC16_SIZE, OTHER_CRC_START);
	
	TxHDLC((uint8_t*)&pibPowerResponse, pibPowerResponse.headers.length);

	return;
}


void PIBToFloatSendNAK(uint16_t command, uint16_t msgSeq, uint16_t destination)
{

	return;
}


void TxHDLC(uint8_t* pTxHDLCBuf, uint16_t activeTxLen)
{
	static BOOL fltEscaped = FALSE;

	if((activeTxLen >0) && (activeTxLen < HDLC_MAX_LENGTH))
	{
		OSuartPutCharWait(FLOAT_PORT, HDLC_SOF, INFINITE);

		while(activeTxLen > 0)
		{
			switch (*pTxHDLCBuf)
			{
				case HDLC_SOF:
				case HDLC_ESC:
				{
					OSuartPutCharWait(FLOAT_PORT, HDLC_ESC, INFINITE);
					*pTxHDLCBuf &= HDLC_TX_AND_MASK;
					fltEscaped = TRUE;
					break;
				}
				default:
				{
					OSuartPutCharWait(FLOAT_PORT, *pTxHDLCBuf, INFINITE);
					if(fltEscaped)
					{
						*pTxHDLCBuf |= HDLC_RX_OR_MASK;
						fltEscaped = FALSE;
					}
					pTxHDLCBuf++;
					activeTxLen--;
					break;
				}
			}
		}
	}
	return;
}


BOOL PM_TxHDLC(uint8_t* pTxHDLCBuf, uint16_t activeTxLen, uint8_t port, uint16_t timeout)
{
	uint8_t err;
	BOOL error = TRUE;
	static BOOL escaped[4] = {FALSE, FALSE, FALSE, FALSE};

	if((activeTxLen >0) && (activeTxLen < HDLC_MAX_LENGTH))
	{
		error = FALSE;

		// --- Before starting, flush the buffers to ignore any earlier transmissions ---
		OSuartFlushBuffers(port, UART_ALL_BUFFERS);

		if(commPortState.pmState[port] == PM_SMC_COMM)
		{
			for (err = 10; err--; )
				OSuartPutCharWait(port, 0, timeout);

			OSuartPutCharWait(port, 0x55, timeout);
			OSuartPutCharWait(port, 0xAA, timeout);
		}
		err = OSuartPutCharWait(port, HDLC_SOF, timeout);
		if(err != OS_ERR_NONE)
		{
			activeTxLen = 0;
			error = TRUE;
		}

		while(activeTxLen > 0)
		{
			switch (*pTxHDLCBuf)
			{
				case HDLC_SOF:
				case HDLC_ESC:
				{
					err = OSuartPutCharWait(port, HDLC_ESC, timeout);
					*pTxHDLCBuf &= HDLC_TX_AND_MASK;

					escaped[port] = TRUE;
					break;
				}
				default:
				{
					err = OSuartPutCharWait(port, *pTxHDLCBuf, timeout);
					if(escaped[port])
					{
						*pTxHDLCBuf |= HDLC_RX_OR_MASK;
						escaped[port] = FALSE;
					}
					pTxHDLCBuf++;
					activeTxLen--;
					break;
				}
			}
			if(err != OS_ERR_NONE)
			{
				activeTxLen = 0;
				error = TRUE;
			}
		}
	}
	return error;
}


uint16_t RxHDLC(char *pHDLCBuf)
{
	uint16_t msgLength = 3;
	BOOL rxInProgHDLC = FALSE;
	uint16_t lenRxHDLCBuf = 0;
	BOOL escapedHDLC = FALSE;
	uint16_t*	lenHDLCMsg = (uint16_t*)(pHDLCBuf + 1);
	BOOL waitingForPacket = TRUE;
	uint8_t err = 0;
	char c;

	while(waitingForPacket == TRUE)
	{
		c = (char) OSuartGetCharWait(FLOAT_PORT, INFINITE, &err);

		if(rxInProgHDLC == TRUE)
		{
			switch (c)
			{
				case HDLC_ESC:
				{
					escapedHDLC = TRUE;
					break;
				}
				default:
				{
					switch (escapedHDLC)
					{
						case TRUE:
						{
							c |= HDLC_RX_OR_MASK;
							escapedHDLC = FALSE;
							*pHDLCBuf = c;
							pHDLCBuf++;
							lenRxHDLCBuf++;
							break;
						}
						case FALSE:
						{
							*pHDLCBuf = c;
							pHDLCBuf++;
							lenRxHDLCBuf++;
							break;
						}
					}

					break;
				}
			}
			if(lenRxHDLCBuf == 3)
			{
				msgLength = *lenHDLCMsg;
				if(msgLength >= HDLC_MAX_LENGTH)
				{
					msgLength = 0;
					waitingForPacket = FALSE;
				}
			}
			if(lenRxHDLCBuf >= msgLength + 1) 
			{
				pHDLCBuf = (char*)&(rxFloatData[0]);
				waitingForPacket = FALSE;		
				//msgLength = 3;
				escapedHDLC = FALSE;
			}
		}
		else
		{
			if(c == HDLC_SOF)
			{
				rxInProgHDLC = TRUE;
				*pHDLCBuf = c;
				pHDLCBuf++;
				lenRxHDLCBuf++;
				escapedHDLC = FALSE;
			}
		}
	}
	return lenRxHDLCBuf;
}

uint16_t PM_RxHDLC(char *pHDLCBuf, uint8_t port, uint16_t timeout)
{
	uint16_t msgLength = 3;
	BOOL rxInProgHDLC = FALSE;
	uint16_t lenRxHDLCBuf = 0;
	BOOL escapedHDLC = FALSE;
	uint16_t*	lenHDLCMsg = (uint16_t*)(pHDLCBuf + 1);
	BOOL waitingForPacket = TRUE;
	uint8_t err = 0;
	char c;
	BOOL error = FALSE;

	while(waitingForPacket == TRUE)
	{
		c = (char) OSuartGetCharWait(port, timeout, &err);

		if(err != OS_ERR_NONE)
		{
			waitingForPacket = FALSE;
			error = TRUE;
			return 0;
		}

		if(rxInProgHDLC == TRUE)
		{
			switch (c)
			{
				case HDLC_ESC:
				{
					escapedHDLC = TRUE;
					break;
				}
				default:
				{
					switch (escapedHDLC)
					{
						case TRUE:
						{
							c |= HDLC_RX_OR_MASK;
							escapedHDLC = FALSE;
							*pHDLCBuf = c;
							pHDLCBuf++;
							lenRxHDLCBuf++;
							break;
						}
						case FALSE:
						{
							*pHDLCBuf = c;
							pHDLCBuf++;
							lenRxHDLCBuf++;
							break;
						}
					}

					break;
				}
			}
			if(lenRxHDLCBuf == 3)
			{
				msgLength = *lenHDLCMsg;
				if(msgLength >= HDLC_MAX_LENGTH)
				{
					msgLength = 0;
					waitingForPacket = FALSE;
				}
			}
			if(lenRxHDLCBuf >= msgLength + 1) 
			{
				pHDLCBuf = (char*)&(rxPMData[0]);
				waitingForPacket = FALSE;		
				//msgLength = 3;
				escapedHDLC = FALSE;
			}
		}
		else
		{
			if(c == HDLC_SOF)
			{
				rxInProgHDLC = TRUE;
				*pHDLCBuf = c;
				pHDLCBuf++;
				lenRxHDLCBuf++;
				escapedHDLC = FALSE;
			}
		}
	}

	if(error == TRUE)
		lenRxHDLCBuf = 0;

	return lenRxHDLCBuf;
}





//---------------------------------------------------------------------------------
//
//	Validates A CRC value passed in a communication -- assumes crc is passed at the end of the buffer
//
//---------------------------------------------------------------------------------
uint8_t ValidateCRC16(uint8_t* pBuffer, uint16_t size, uint16_t initValue)
{
	uint8_t retval = FALSE;
	uint16_t crc = initValue;
	uint16_t* pRxCRC = NULL;
	int i = 0;

	for(i = 0; i < (size - CRC16_SIZE); i++)
	{
		crc = _crc16_update(crc, *(pBuffer + i));
	}

	pRxCRC = (uint16_t*)(pBuffer + i);

	if(crc == *pRxCRC)
	{
		retval = SUCCESS;
	}

	return retval;
}


//---------------------------------------------------------------------------------
//
//	Calculates A CRC value for an outbound communication
//
//---------------------------------------------------------------------------------
uint16_t CalculateCRC16(uint8_t* pBuffer, uint16_t size, uint16_t initValue)
{
	uint16_t crc = initValue;

	for(int i = 0; i < (size); i++)
	{
		crc = _crc16_update(crc, *(pBuffer + i));
	}

	return crc;
}


//File Handling routines

void CancelWriteInProgress()
{
	//Delete writingFile.tempFileName
	memset(&writingFile, 0, sizeof(FILE_WRITE_IN_PROGRESS));
	writingFile.writeStatus = NO_WRITE_IN_PROGRESS;

	return;
}

void PIBRcvdFloatCmd(PPYLD_MGR_FILE_RX_BUFFER pHeaders)
{
	switch(pHeaders->type.command)
	{
	case FILE_INIT_WRITE:
		{
			FileInitializeWrite(pHeaders);
			break;
		}
	case FILE_WRITE_BLOCK:
		{
			FileWriteBlock(pHeaders);
			break;
		}
	case FILE_DELETE:
		{
			FileDelete(pHeaders);
			break;
		}
	case FILE_READ_BLOCK:
		{
			FileReadBlock(pHeaders);
			break;
		}
	case FILE_GET_INFO:
		{
			FileGetInfo(pHeaders);
			break;
		}
	case FILE_GET_DETAIL:
		{
			FileGetDetail(pHeaders);
			break;
		}
	case CMD_SELECT_PROGRAM:
		{
			SelectProgram(pHeaders);
			break;
		}
	case CMD_WRITE_CNSL_CMD_ASCII:
	case CMD_WRITE_CNSL_CMD_BINARY:
		{
			ConsoleInput(pHeaders);
			break;
		}
	}	
	return;
}

void FileInitializeWrite(PPYLD_MGR_FILE_RX_BUFFER pHeaders)
{
	char fileName[13] = {0};
	BOOL proceed = TRUE;
	int nakType = 0;
	uint8_t retval = FAIL;

	uint8_t mytaskid = (pHeaders->wrapper.destination & 0x00FF);
	uint8_t myboardid = (pHeaders->wrapper.destination & 0xFF00) >> 8;
			
	memcpy(fileName, pHeaders->initFileWrite.fileName, 11);


	memset(fileName, 0, 13);

	//Check for WGMS File ID in logs to see if it already exists for current files -- if so, then this new
	//one will delete the old one, even if different names

	if(writingFile.writeStatus == NO_WRITE_IN_PROGRESS)
	{
		retval = PadToDot(pHeaders->initFileWrite.fileName, fileName);
		if(retval == FAIL)
		{
			nakType = NAKDISP_DATA;
			proceed = FALSE;
		}
	}
	else
	{
		nakType = NAKDISP_TOO_MANY_COMMANDS;
		proceed = FALSE;
	}
	

	if((proceed == TRUE) && (pHeaders->initFileWrite.fileSize >= MAX_FILE_SIZE_IN))
	{
		nakType = NAKDISP_STORAGE_FULL;
		proceed = FALSE;
	}

	
	if(proceed)
	{
		if((myboardid != mainBoardID) && (mytaskid >= NUM_TASK_FILES))
		{
			nakType = NAKDISP_MISSING_STORAGE;
			proceed = FALSE;
		}
		if(proceed)
		{
			if(fileAllowed[mytaskid] == FALSE)
			{
				nakType = NAKDISP_MISSING_STORAGE;
				proceed = FALSE;
			}
		}
	}

	if(proceed)
	{
		if(fileOutSize[mytaskid] > 0)
		{
			nakType = NAKDISP_TOO_MANY_COMMANDS;
			proceed = FALSE;
		}
	}

	if(proceed)
	{
		memcpy(writingFile.finalFileName, fileName, 13);
		writingFile.contentCRC = pHeaders->initFileWrite.contentCRC;
		memcpy(&writingFile.contentMD5, &(pHeaders->initFileWrite.contentMD5), sizeof(MD5));
		memcpy(&writingFile.creationDate, &(pHeaders->initFileWrite.creationDate), sizeof(DATE_TIME));
		memcpy(&writingFile.modifiedDate, &(pHeaders->initFileWrite.modifiedDate), sizeof(DATE_TIME));
		writingFile.fileAttributes = pHeaders->initFileWrite.fileAttributes;
		writingFile.fileID = pHeaders->initFileWrite.fileID;
		memcpy(writingFile.fileName, pHeaders->initFileWrite.fileName, 11);
		PadToDot(pHeaders->initFileWrite.fileName, writingFile.tempFileName);
		writingFile.fileSize = pHeaders->initFileWrite.fileSize;
		writingFile.writeStatus = WRITE_IN_PROGRESS;
		// if want a time-out on file writes -- writingFile.tickCount = (current Tick value)

		//Log WGMS ID, file attributes, creation date, modified date, CRC, MD5 with file name for future reference
		//Need to at least save WGMS ID and File Name correlation -- others are nice but may not be necessary
		//or use writingFile structure with only 1 allowed write at a time
		if(pHeaders->initFileWrite.overwriteFile == 0)
		{
			//check for file
			nakType = NAKDISP_WOULD_OVERWRITE;
			proceed = FALSE;
		}

		if(proceed)
		{
			memset(&(fileIn[mytaskid][0]), 0, MAX_FILE_SIZE_IN);
			fileInSize[mytaskid] = pHeaders->initFileWrite.fileSize;
		}
	}
	if(nakType == 0)
	{
		pyldFileAcks.headers.destination = pHeaders->wrapper.source;
		pyldFileAcks.headers.source = pHeaders->wrapper.destination;
		pyldFileAcks.headers.msgID = pHeaders->wrapper.msgID;
		pyldFileAcks.headers.length = sizeof(LRI_COMM_WRAPPER) + sizeof(LRI_GENERIC_ACK) + sizeof(COMM_HEADER) + CRC16_SIZE;
		pyldFileAcks.headers.msgType = LRI_ACK;		//0x05;
		pyldFileAcks.ackInfo.originalCmd = pHeaders->wrapper.msgType;
		pyldFileAcks.toShoreheader.packetID = pHeaders->header.packetID;
		pyldFileAcks.toShoreheader.size = 0;
		pyldFileAcks.toShoreheader.type = 0x13;
		pyldFileAcks.crc = CalculateCRC16((uint8_t*) &pyldFileAcks, sizeof(LRI_COMM_WRAPPER) + sizeof(LRI_GENERIC_ACK) + sizeof(COMM_HEADER), OTHER_CRC_START);
		TxHDLC((uint8_t*)&pyldFileAcks, pyldFileAcks.headers.length);
	}
	else
	{
		memset(&writingFile, 0, sizeof(FILE_WRITE_IN_PROGRESS));
		fileInSize[mytaskid] = 0;
		writingFile.writeStatus = NO_WRITE_IN_PROGRESS;
		pyldFileNaks.headers.destination = pHeaders->wrapper.source;
		pyldFileNaks.headers.source = pHeaders->wrapper.destination;
		pyldFileNaks.headers.msgID = pHeaders->wrapper.msgID;
		pyldFileNaks.headers.length = sizeof(LRI_COMM_WRAPPER) + sizeof(LRI_GENERIC_NAK) + sizeof(COMM_HEADER) + sizeof(NAK_FILE);
		pyldFileNaks.headers.msgType = 0x04;
		pyldFileNaks.nakInfo.originalCmd = pHeaders->wrapper.msgType;
		pyldFileNaks.nakInfo.nakSummary = 0;
		pyldFileNaks.toShoreheader.packetID = pHeaders->header.packetID;
		pyldFileNaks.toShoreheader.size = 3;
		pyldFileNaks.toShoreheader.type = 0x24;
		pyldFileNaks.nakFile.count = 1;
		pyldFileNaks.nakFile.result = nakType;
		pyldFileNaks.nakFile.crc = CalculateCRC16((uint8_t*) &pyldFileAcks, sizeof(LRI_COMM_WRAPPER) + sizeof(LRI_GENERIC_NAK) + sizeof(COMM_HEADER) + sizeof(NAK_FILE) - CRC16_SIZE, OTHER_CRC_START);
		TxHDLC((uint8_t*)&pyldFileAcks, pyldFileAcks.headers.length);
	}
	return;
}


void FileWriteBlock(PPYLD_MGR_FILE_RX_BUFFER pHeaders)
{
	char fileName[13] = {0};
	uint32_t hyperionID = 0;
	BOOL proceed = TRUE;
	int nakType = 0;
	int numBytes = 0;


	uint8_t mytaskid = (pHeaders->wrapper.destination & 0x00FF);
	uint8_t myboardid = (pHeaders->wrapper.destination & 0xFF00) >> 8;
			
	hyperionID = pHeaders->writeFile.fileID;

	//Get Filename from log file based on Hyperion ID or use writingFile structure with only 1 allowed write at a time
	//Get final File Size from Hyperion ID
	if((writingFile.writeStatus == WRITE_IN_PROGRESS) && (hyperionID == writingFile.fileID))
		memcpy(fileName, writingFile.tempFileName, 13);
	else
		fileName[0] = 0;

	if(fileName[0] == 0)
	{
		nakType = NAKDISP_DATA;
		proceed = FALSE;
	}

	if(proceed)
	{
		if((myboardid != mainBoardID) && (mytaskid >= NUM_TASK_FILES))
		{
			nakType = NAKDISP_MISSING_STORAGE;
			proceed = FALSE;
		}
	}

	if(proceed)
	{
		numBytes = pHeaders->writeFile.numDataBytes;
		if((numBytes + pHeaders->writeFile.offset) <= MAX_FILE_SIZE_IN)
			memcpy(&(fileIn[mytaskid][pHeaders->writeFile.offset]), pHeaders->writeFile.data, numBytes);
		else
			nakType = NAKDISP_DATA;

		if((pHeaders->writeFile.offset + numBytes) >= writingFile.fileSize)
		{
			//Rename writingFile.tempFileName to writingFile.finalFileName
			//Log information in log file 
			memset(&writingFile, 0, sizeof(FILE_WRITE_IN_PROGRESS));
			writingFile.writeStatus = NO_WRITE_IN_PROGRESS;
			fileAvailableID[mytaskid] = hyperionID;
			fileAvailable[mytaskid] = TRUE;
			//Set flag for task that data is available
		}
	}

	if(nakType == 0)
	{
		pyldFileAcks.headers.destination = pHeaders->wrapper.source;
		pyldFileAcks.headers.source = pHeaders->wrapper.destination;
		pyldFileAcks.headers.msgID = pHeaders->wrapper.msgID;
		pyldFileAcks.headers.length = sizeof(LRI_COMM_WRAPPER) + sizeof(LRI_GENERIC_ACK) + sizeof(COMM_HEADER) + CRC16_SIZE;
		pyldFileAcks.headers.msgType = LRI_ACK;		//0x05;
		pyldFileAcks.ackInfo.originalCmd = pHeaders->wrapper.msgType;
		pyldFileAcks.toShoreheader.packetID = pHeaders->header.packetID;
		pyldFileAcks.toShoreheader.size = 0;
		pyldFileAcks.toShoreheader.type = 0x13;
		pyldFileAcks.crc = CalculateCRC16((uint8_t*) &pyldFileAcks, sizeof(LRI_COMM_WRAPPER) + sizeof(LRI_GENERIC_ACK) + sizeof(COMM_HEADER), OTHER_CRC_START);
		TxHDLC((uint8_t*)&pyldFileAcks, pyldFileAcks.headers.length);
	}
	else
	{
		memset(&writingFile, 0, sizeof(FILE_WRITE_IN_PROGRESS));
		fileInSize[mytaskid] = 0;
		writingFile.writeStatus = NO_WRITE_IN_PROGRESS;
		pyldFileNaks.headers.destination = pHeaders->wrapper.source;
		pyldFileNaks.headers.source = pHeaders->wrapper.destination;
		pyldFileNaks.headers.msgID = pHeaders->wrapper.msgID;
		pyldFileNaks.headers.length = sizeof(LRI_COMM_WRAPPER) + sizeof(LRI_GENERIC_NAK) + sizeof(COMM_HEADER) + sizeof(NAK_FILE);
		pyldFileNaks.headers.msgType = 0x04;
		pyldFileNaks.nakInfo.originalCmd = pHeaders->wrapper.msgType;
		pyldFileNaks.nakInfo.nakSummary = 0;
		pyldFileNaks.toShoreheader.packetID = pHeaders->header.packetID;
		pyldFileNaks.toShoreheader.size = 3;
		pyldFileNaks.toShoreheader.type = 0x24;
		pyldFileNaks.nakFile.count = 1;
		pyldFileNaks.nakFile.result = nakType;
		pyldFileNaks.nakFile.crc = CalculateCRC16((uint8_t*) &pyldFileAcks, sizeof(LRI_COMM_WRAPPER) + sizeof(LRI_GENERIC_NAK) + sizeof(COMM_HEADER) + sizeof(NAK_FILE) - CRC16_SIZE, OTHER_CRC_START);
		TxHDLC((uint8_t*)&pyldFileAcks, pyldFileAcks.headers.length);
	}
	return;
}


void FileDelete(PPYLD_MGR_FILE_RX_BUFFER pHeaders)
{
	char fileName[13] = {0};
	uint32_t hyperionID;
	BOOL proceed = TRUE;
	int nakType = 0;
	uint8_t retval = FAIL;
			
	hyperionID = pHeaders->deleteFile.fileID;
	if(hyperionID != 0xFFFFFFFF)
	{
		//Find Hyperion ID in log file and get file name
		if(fileName[0] == 0)
		{
			nakType = NAKDISP_DATA;
			proceed = FALSE;
		}
		if(proceed)
		{
			//Don't need to do anything
		}
	}
	else
	{
		retval = PadToDot(pHeaders->deleteFile.fileName, fileName);

		if(retval == FAIL)
		{
			nakType = NAKDISP_DATA;
			proceed = FALSE;
		}

		if(proceed)
		{
			//Don't need to do anything
		}
	}

	if(nakType == 0)
	{
		pyldFileAcks.headers.destination = pHeaders->wrapper.source;
		pyldFileAcks.headers.source = pHeaders->wrapper.destination;
		pyldFileAcks.headers.msgID = pHeaders->wrapper.msgID;
		pyldFileAcks.headers.length = sizeof(LRI_COMM_WRAPPER) + sizeof(LRI_GENERIC_ACK) + sizeof(COMM_HEADER) + CRC16_SIZE;
		pyldFileAcks.headers.msgType = LRI_ACK;		//0x05;
		pyldFileAcks.ackInfo.originalCmd = pHeaders->wrapper.msgType;
		pyldFileAcks.toShoreheader.packetID = pHeaders->header.packetID;
		pyldFileAcks.toShoreheader.size = 0;
		pyldFileAcks.toShoreheader.type = 0x13;
		pyldFileAcks.crc = CalculateCRC16((uint8_t*) &pyldFileAcks, sizeof(LRI_COMM_WRAPPER) + sizeof(LRI_GENERIC_ACK) + sizeof(COMM_HEADER), OTHER_CRC_START);
		TxHDLC((uint8_t*)&pyldFileAcks, pyldFileAcks.headers.length);
	}
	else
	{
		pyldFileNaks.headers.destination = pHeaders->wrapper.source;
		pyldFileNaks.headers.source = pHeaders->wrapper.destination;
		pyldFileNaks.headers.msgID = pHeaders->wrapper.msgID;
		pyldFileNaks.headers.length = sizeof(LRI_COMM_WRAPPER) + sizeof(LRI_GENERIC_NAK) + sizeof(COMM_HEADER) + sizeof(NAK_FILE);
		pyldFileNaks.headers.msgType = 0x04;
		pyldFileNaks.nakInfo.originalCmd = pHeaders->wrapper.msgType;
		pyldFileNaks.nakInfo.nakSummary = 0;
		pyldFileNaks.toShoreheader.packetID = pHeaders->header.packetID;
		pyldFileNaks.toShoreheader.size = 3;
		pyldFileNaks.toShoreheader.type = 0x24;
		pyldFileNaks.nakFile.count = 1;
		pyldFileNaks.nakFile.result = nakType;
		pyldFileNaks.nakFile.crc = CalculateCRC16((uint8_t*) &pyldFileAcks, sizeof(LRI_COMM_WRAPPER) + sizeof(LRI_GENERIC_NAK) + sizeof(COMM_HEADER) + sizeof(NAK_FILE) - CRC16_SIZE, OTHER_CRC_START);
		TxHDLC((uint8_t*)&pyldFileAcks, pyldFileAcks.headers.length);
	}
	return;
}


void FileReadBlock(PPYLD_MGR_FILE_RX_BUFFER pHeaders)
{
	char fileName[13] = {0};
	BOOL proceed = TRUE;
	int nakType = 0;
	int bytesRead = 0;
	uint8_t retval = FAIL;

	uint8_t mytaskid = (pHeaders->wrapper.destination & 0x00FF);
	uint8_t myboardid = (pHeaders->wrapper.destination & 0xFF00) >> 8;
			
	retval = PadToDot(pHeaders->readFile.fileName, fileName);

	if(retval == FAIL)
	{
		nakType = NAKDISP_DATA;
		proceed = FALSE;
	}

	if(proceed)
	{
		if((myboardid != mainBoardID) && (mytaskid >= NUM_TASK_FILES))
		{
			nakType = NAKDISP_MISSING_STORAGE;
			proceed = FALSE;
		}
	}

	if(proceed)
	{
		bytesRead = fileOutSize[mytaskid] - pHeaders->readFile.offset;
		if(bytesRead > pHeaders->readFile.readLength)
			bytesRead = pHeaders->readFile.readLength;

		if(bytesRead < 0)
		{
			nakType = NAKDISP_DATA;
			proceed = FALSE;
		}
	}

	if((bytesRead + pHeaders->readFile.offset) >= fileOutSize[mytaskid])
	{
		fileOutSize[mytaskid] = 0;
	}

	if((nakType == 0) && (proceed == TRUE))
	{
		pyldFileAcks.headers.destination = pHeaders->wrapper.source;
		pyldFileAcks.headers.source = pHeaders->wrapper.destination;
		pyldFileAcks.headers.msgID = pHeaders->wrapper.msgID;
		pyldFileAcks.headers.length = sizeof(LRI_COMM_WRAPPER) + sizeof(LRI_GENERIC_ACK) + sizeof(COMM_HEADER) + sizeof(ACK_READ_FILE);
		pyldFileAcks.headers.msgType = LRI_ACK;		//0x05;
		pyldFileAcks.ackInfo.originalCmd = pHeaders->wrapper.msgType;
		pyldFileAcks.toShoreheader.packetID = pHeaders->header.packetID;
		pyldFileAcks.toShoreheader.size = sizeof(ACK_READ_FILE) - CRC16_SIZE;
		pyldFileAcks.toShoreheader.type = 0x1B;
		pyldFileAcks.ackRead.numDataBytes = bytesRead;
		pyldFileAcks.ackRead.seekOffset = pHeaders->readFile.offset;
		memcpy(pyldFileAcks.ackRead.data, &(fileOut[mytaskid][pHeaders->readFile.offset]), bytesRead);
		pyldFileAcks.ackRead.eofFlag = 0xFF;
		pyldFileAcks.ackRead.storageControllerAddress = pHeaders->readFile.storageControllerAddress;
		pyldFileAcks.ackRead.crc = CalculateCRC16((uint8_t*) &pyldFileAcks, sizeof(LRI_COMM_WRAPPER) + sizeof(LRI_GENERIC_ACK) + sizeof(COMM_HEADER)+ sizeof(ACK_READ_FILE) - CRC16_SIZE, OTHER_CRC_START);
		TxHDLC((uint8_t*)&pyldFileAcks, pyldFileAcks.headers.length);
	}
	else
	{
		pyldFileNaks.headers.destination = pHeaders->wrapper.source;
		pyldFileNaks.headers.source = pHeaders->wrapper.destination;
		pyldFileNaks.headers.msgID = pHeaders->wrapper.msgID;
		pyldFileNaks.headers.length = sizeof(LRI_COMM_WRAPPER) + sizeof(LRI_GENERIC_NAK) + sizeof(COMM_HEADER) + sizeof(NAK_FILE);
		pyldFileNaks.headers.msgType = 0x04;
		pyldFileNaks.nakInfo.originalCmd = pHeaders->wrapper.msgType;
		pyldFileNaks.nakInfo.nakSummary = 0;
		pyldFileNaks.toShoreheader.packetID = pHeaders->header.packetID;
		pyldFileNaks.toShoreheader.size = 3;
		pyldFileNaks.toShoreheader.type = 0x24;
		pyldFileNaks.nakFile.count = 1;
		pyldFileNaks.nakFile.result = nakType;
		pyldFileNaks.nakFile.crc = CalculateCRC16((uint8_t*) &pyldFileAcks, sizeof(LRI_COMM_WRAPPER) + sizeof(LRI_GENERIC_NAK) + sizeof(COMM_HEADER) + sizeof(NAK_FILE) - CRC16_SIZE, OTHER_CRC_START);
		TxHDLC((uint8_t*)&pyldFileAcks, pyldFileAcks.headers.length);
	}
	return;
}



void FileGetInfo(PPYLD_MGR_FILE_RX_BUFFER pHeaders)
{
	char fileName[13] = {0};
	uint32_t hyperionID;
	BOOL proceed = TRUE;
	int nakType = 0;
	uint8_t fileAttributes = 0;
	uint32_t fileSize = 0;
	uint8_t retval = FAIL;

	uint8_t mytaskid = (pHeaders->wrapper.destination & 0x00FF);

	hyperionID = pHeaders->getFileList.fileID;

	hyperionID = 0xFFFFFFFF;

	if(hyperionID != 0xFFFFFFFF)
	{
		//Get File Name from Hyperion ID
		if(fileName[0] == 0)
		{
			nakType = NAKDISP_DATA;
			proceed = FALSE;
		}
	}
	else
	{
		retval = PadToDot(pHeaders->getFileList.fileName, fileName);
		if(retval == FAIL)
		{
			//NAK -- invalid name
			nakType = NAKDISP_DATA;
			proceed = FALSE;
		}
	}

	if(proceed)
	{
		fileSize = fileOutSize[mytaskid];
	}

	//Return File information (size, etc.)
	if(nakType == 0)
	{
		pyldFileAcks.headers.destination = pHeaders->wrapper.source;
		pyldFileAcks.headers.source = pHeaders->wrapper.destination;
		pyldFileAcks.headers.msgID = pHeaders->wrapper.msgID;
		pyldFileAcks.headers.length = sizeof(LRI_COMM_WRAPPER) + sizeof(LRI_GENERIC_ACK) + sizeof(COMM_HEADER) + sizeof(ACK_FILE_LIST);
		pyldFileAcks.headers.msgType = LRI_ACK;		//0x05;
		pyldFileAcks.ackInfo.originalCmd = pHeaders->wrapper.msgType;
		pyldFileAcks.toShoreheader.packetID = pHeaders->header.packetID;
		pyldFileAcks.toShoreheader.size = sizeof(ACK_FILE_LIST) - CRC16_SIZE;
		pyldFileAcks.toShoreheader.type = 0x31;
		pyldFileAcks.ackFileInfo.common.lastReport = 0xFF;
		pyldFileAcks.ackFileInfo.common.numFiles = 1;
		pyldFileAcks.ackFileInfo.common.offset = 0;
		pyldFileAcks.ackFileInfo.common.storageControllerAddress = pHeaders->getFileList.storageControllerAddress;

		pyldFileAcks.ackFileInfo.records[0].fileAttributes = fileAttributes;		//File Attributes fneed to be filled in
		pyldFileAcks.ackFileInfo.records[0].fileID = hyperionID;	//Hyperion ID needs to be filled in
		pyldFileAcks.ackFileInfo.records[0].fileSize = fileSize;	//File Size needs to be filled in

		DotToPad(pyldFileAcks.ackFileInfo.records[0].fileName, fileName);

		pyldFileAcks.ackFileInfo.crc = CalculateCRC16((uint8_t*) &pyldFileAcks, sizeof(LRI_COMM_WRAPPER) + sizeof(LRI_GENERIC_ACK) + sizeof(COMM_HEADER)+ sizeof(ACK_FILE_LIST) - CRC16_SIZE, OTHER_CRC_START);

		TxHDLC((uint8_t*)&pyldFileAcks, pyldFileAcks.headers.length);
	}
	else
	{
		pyldFileNaks.headers.destination = pHeaders->wrapper.source;
		pyldFileNaks.headers.source = pHeaders->wrapper.destination;
		pyldFileNaks.headers.msgID = pHeaders->wrapper.msgID;
		pyldFileNaks.headers.length = sizeof(LRI_COMM_WRAPPER) + sizeof(LRI_GENERIC_NAK) + sizeof(COMM_HEADER) + sizeof(NAK_FILE);
		pyldFileNaks.headers.msgType = 0x04;
		pyldFileNaks.nakInfo.originalCmd = pHeaders->wrapper.msgType;
		pyldFileNaks.nakInfo.nakSummary = 0;
		pyldFileNaks.toShoreheader.packetID = pHeaders->header.packetID;
		pyldFileNaks.toShoreheader.size = 3;
		pyldFileNaks.toShoreheader.type = 0x24;
		pyldFileNaks.nakFile.count = 1;
		pyldFileNaks.nakFile.result = nakType;
		pyldFileNaks.nakFile.crc = CalculateCRC16((uint8_t*) &pyldFileAcks, sizeof(LRI_COMM_WRAPPER) + sizeof(LRI_GENERIC_NAK) + sizeof(COMM_HEADER) + sizeof(NAK_FILE) - CRC16_SIZE, OTHER_CRC_START);
		TxHDLC((uint8_t*)&pyldFileAcks, pyldFileAcks.headers.length);
	}
	return;
}


void FileGetDetail(PPYLD_MGR_FILE_RX_BUFFER pHeaders)
{
	char fileName[13] = {0};
	uint32_t hyperionID;
	BOOL proceed = TRUE;
	int nakType = 0;
	uint8_t fileAttributes = 0;
	uint32_t fileSize = 0;
	uint8_t retval = FAIL;

	uint8_t mytaskid = (pHeaders->wrapper.destination & 0x00FF);

	hyperionID = pHeaders->getFileList.fileID;
	if(hyperionID != 0xFFFFFFFF)
	{
		//Get File Name from Hyperion ID
		if(fileName[0] == 0)
		{
			nakType = NAKDISP_DATA;
			proceed = FALSE;
		}
	}
	else
	{
		retval = PadToDot(pHeaders->getFileList.fileName, fileName);

		if(retval == FAIL)
		{
			//NAK -- invalid name
			nakType = NAKDISP_DATA;
			proceed = FALSE;
		}
	}

	if(proceed)
	{
		fileSize = fileOutSize[mytaskid];
	}

	//Return File information (size, etc.) -- need to expand to Detailed Ack
	if(nakType == 0)
	{
		pyldFileAcks.headers.destination = pHeaders->wrapper.source;
		pyldFileAcks.headers.source = pHeaders->wrapper.destination;
		pyldFileAcks.headers.msgID = pHeaders->wrapper.msgID;
		pyldFileAcks.headers.length = sizeof(LRI_COMM_WRAPPER) + sizeof(LRI_GENERIC_ACK) + sizeof(COMM_HEADER) + sizeof(ACK_FILE_LIST);
		pyldFileAcks.headers.msgType = LRI_ACK;		//0x05;
		pyldFileAcks.ackInfo.originalCmd = pHeaders->wrapper.msgType;
		pyldFileAcks.toShoreheader.packetID = pHeaders->header.packetID;
		pyldFileAcks.toShoreheader.size = sizeof(ACK_FILE_LIST) - CRC16_SIZE;
		pyldFileAcks.toShoreheader.type = 0x31;
		pyldFileAcks.ackFileInfo.common.lastReport = 0xFF;
		pyldFileAcks.ackFileInfo.common.numFiles = 1;
		pyldFileAcks.ackFileInfo.common.offset = 0;
		pyldFileAcks.ackFileInfo.common.storageControllerAddress = pHeaders->getFileList.storageControllerAddress;

		pyldFileAcks.ackFileInfo.records[0].fileAttributes = fileAttributes;		//File Attributes fneed to be filled in
		pyldFileAcks.ackFileInfo.records[0].fileID = hyperionID;	//Hyperion ID needs to be filled in
		pyldFileAcks.ackFileInfo.records[0].fileSize = fileSize;	//File Size needs to be filled in

		DotToPad(pyldFileAcks.ackFileInfo.records[0].fileName, fileName);

		pyldFileAcks.ackFileInfo.crc = CalculateCRC16((uint8_t*) &pyldFileAcks, sizeof(LRI_COMM_WRAPPER) + sizeof(LRI_GENERIC_ACK) + sizeof(COMM_HEADER)+ sizeof(ACK_FILE_LIST) - CRC16_SIZE, OTHER_CRC_START);

		TxHDLC((uint8_t*)&pyldFileAcks, pyldFileAcks.headers.length);
	}
	else
	{
		pyldFileNaks.headers.destination = pHeaders->wrapper.source;
		pyldFileNaks.headers.source = pHeaders->wrapper.destination;
		pyldFileNaks.headers.msgID = pHeaders->wrapper.msgID;
		pyldFileNaks.headers.length = sizeof(LRI_COMM_WRAPPER) + sizeof(LRI_GENERIC_NAK) + sizeof(COMM_HEADER) + sizeof(NAK_FILE);
		pyldFileNaks.headers.msgType = 0x04;
		pyldFileNaks.nakInfo.originalCmd = pHeaders->wrapper.msgType;
		pyldFileNaks.nakInfo.nakSummary = 0;
		pyldFileNaks.toShoreheader.packetID = pHeaders->header.packetID;
		pyldFileNaks.toShoreheader.size = 3;
		pyldFileNaks.toShoreheader.type = 0x24;
		pyldFileNaks.nakFile.count = 1;
		pyldFileNaks.nakFile.result = nakType;
		pyldFileNaks.nakFile.crc = CalculateCRC16((uint8_t*) &pyldFileAcks, sizeof(LRI_COMM_WRAPPER) + sizeof(LRI_GENERIC_NAK) + sizeof(COMM_HEADER) + sizeof(NAK_FILE) - CRC16_SIZE, OTHER_CRC_START);
		TxHDLC((uint8_t*)&pyldFileAcks, pyldFileAcks.headers.length);
	}
	return;
}

void SelectProgram(PPYLD_MGR_FILE_RX_BUFFER pRX)
{
	uint8_t retval = NAKSUMMARY_UNKNOWN_COMMAND;
	BOOL reboot = FALSE;
	
	if ((pRX->flagSendFwd == EXECUTE_CMD) 
	 && (pRX->cmdFormat == SHIP_SHORE_ICD))
	{
		if (pRX->header.type == RCV_CMD)
		{
			if (pRX->type.fmtID == FMT_RCV_CMD)
			{
				if (pRX->type.command == CMD_SELECT_PROGRAM)
				{
					uint16_t storageController = (((uint16_t) pRX->command.field4) << 8) + pRX->command.field3;
					uint16_t numItems = (((uint16_t) pRX->command.field6) << 8) + pRX->command.field5;
					PFT_PROGSEL_ITEM pItems = (PFT_PROGSEL_ITEM) &pRX->command.field7;

					// TODO: verify the packet length for the number of items selected

					if ((numItems != 1)
					 || (pItems->operation > 2)
					 || (storageController != mainTaskAddress)
					 || (pItems->boardAddress != mainTaskAddress))
					
						retval = NAKSUMMARY_INVALID_DATA;

					else
					{
						BootSelectProgram(pItems->operation == 1);
						reboot = TRUE;
						retval = SUCCESS;
					}
				}
			}
		}
	}
	else if (pRX->flagSendFwd == INFORMATION)
		retval = SUCCESS;

	if (!(pRX->wrapper.msgType & 0x8000))
	{
		PLRI_COMM_WRAPPER response = (PLRI_COMM_WRAPPER) txHDLCBuffer;
		response->source = mainTaskAddress;
		response->destination = pRX->wrapper.source;
		response->msgID = pRX->wrapper.msgID;
		response->msgType = (retval == SUCCESS) ? LRI_ACK : LRI_NAK;

		uint8_t* pStart = (uint8_t*) response;
		uint8_t* pEnd;
	
		if (retval == SUCCESS)
		{
			PLRI_GENERIC_ACK pAck = (PLRI_GENERIC_ACK) &response[1];
			pAck->originalCmd = pRX->wrapper.msgType;
			pEnd = (uint8_t*) &pAck[1];
		}
		else
		{
			PLRI_GENERIC_NAK pNak = (PLRI_GENERIC_NAK) &response[1];
			pNak->originalCmd = pRX->wrapper.msgType;
			pNak->nakSummary = retval;
			pEnd = (uint8_t*) &pNak[1];
		}

		response->length = pEnd-pStart+CRC16_SIZE;
		*((uint16_t*) pEnd) = CalculateCRC16(pStart, response->length - CRC16_SIZE, OTHER_CRC_START);
		TxHDLC(pStart, response->length);
	}

	if (reboot)
		BootRestart(TRUE);
}


uint8_t	PadToDot(char* pad, char* dot)
{
	uint8_t retval = FAIL;
	int dotpos = -1;
	int i = 0;

	memset(dot, 0, 13);
	memcpy(dot, pad, 8);
	for(i = 7; i>=0; i--)
	{
		if(pad[i] != ' ')
		{
			dotpos = i+1;
			retval = SUCCESS;
			break;
		}
	}

	if(dotpos > 0)
	{
		dot[dotpos] = '.';
		memcpy(&dot[dotpos + 1], &pad[8], 3);
		for(i = (dotpos + 3); i>dotpos; i--)
		{
			if(dot[i] == ' ')
				dot[i] = 0;
		}
	}

	return retval;
}

uint8_t	DotToPad(char* pad, char* dot)
{
	uint8_t retval = FAIL;
	int dotpos = -1;
	int len = -1;
	int i = 0;

	memset(pad, ' ', 11);
	len = strlen(dot);
	
	for(i = 0; i<len; i++)
	{
		if(dot[i] == '.')
		{
			dotpos = i;
			break;
		}
	}

	if((len > 0) && (len < 13))
	{
		if(dotpos > 0)
		{
			memcpy(pad, dot, dotpos);
			if(len > dotpos)
				memcpy(&pad[8], &dot[dotpos+1], (len-(dotpos+1)));
		}
		else
		{
			memcpy(pad, dot, len);
		}
		retval = SUCCESS;
	}

	return retval;
}

void ConsoleInput(PPYLD_MGR_FILE_RX_BUFFER pHeaders)
{
	BOOL proceed = TRUE;
	uint8_t myTaskID = (pHeaders->wrapper.destination & 0x00FF);
	uint8_t myBoardID = (pHeaders->wrapper.destination & 0xFF00) >> 8;
	uint8_t nakType = 0;
	
	sprintf_P(dbgMsg, PSTR("Processing Console Input...."));
	SEND_MSG;
			
	if((proceed == TRUE) && (pHeaders->consoleInput.dataLength >= MAX_FILE_SIZE_IN))
	{
		nakType = CONSOLE_RESPONSE_INVALID_CMD_LENGTH;	//Need new NAK type for Console
		proceed = FALSE;
		sprintf_P(dbgMsg, PSTR("Length too large")); SEND_MSG;
	}

	if((proceed == TRUE) && (pHeaders->consoleInput.dataLength < 0))
	{
		nakType = CONSOLE_RESPONSE_INVALID_CMD_LENGTH;	//Need new NAK type for invalid command length
		proceed = FALSE;
		sprintf_P(dbgMsg, PSTR("Length too small")); SEND_MSG;
	}

	if(proceed)
	{
		if((myBoardID != mainBoardID) && (myTaskID >= NUM_TASK_FILES))
		{
			nakType = CONSOLE_RESPONSE_UNKNOWN_DESTINATION;	//Need new NAK type for Console
			proceed = FALSE;
			sprintf_P(dbgMsg, PSTR("Unknown destination")); SEND_MSG;
		}
	}

	if(proceed)
	{
		if(consoleAvailable[myTaskID] == TRUE)
		{
			nakType = NAKDISP_DATA;
			proceed = FALSE;
			nakType = NAKDISP_TOO_MANY_COMMANDS;
			proceed = FALSE;
			sprintf_P(dbgMsg, PSTR("Too many commands")); SEND_MSG;
		}
	}
		
	if(proceed)
	{
		consoleIn[myTaskID].cmdSrcAddress = pHeaders->wrapper.source;
		consoleIn[myTaskID].functionCode = pHeaders->consoleInput.function;
		consoleIn[myTaskID].cmdSize = pHeaders->consoleInput.dataLength;
		consoleIn[myTaskID].id = pHeaders->header.packetID;
		memset(consoleIn[myTaskID].cmd, 0, 256);
		memcpy(consoleIn[myTaskID].cmd, pHeaders->consoleInput.consoleData, pHeaders->consoleInput.dataLength);
		consoleAvailable[myTaskID] = TRUE;
	}

	if(nakType == 0)
	{
		pyldFileAcks.headers.destination = pHeaders->wrapper.source;
		pyldFileAcks.headers.source = pHeaders->wrapper.destination;
		pyldFileAcks.headers.msgID = pHeaders->wrapper.msgID;
		pyldFileAcks.headers.length = sizeof(LRI_COMM_WRAPPER) + sizeof(LRI_GENERIC_ACK) + sizeof(COMM_HEADER) + CRC16_SIZE;
		pyldFileAcks.headers.msgType = LRI_ACK;
		pyldFileAcks.ackInfo.originalCmd = pHeaders->wrapper.msgType;
		pyldFileAcks.toShoreheader.packetID = pHeaders->header.packetID;
		pyldFileAcks.toShoreheader.size = 0;
		pyldFileAcks.toShoreheader.type = 0x13;
		pyldFileAcks.crc = CalculateCRC16((uint8_t*) &pyldFileAcks, sizeof(LRI_COMM_WRAPPER) + sizeof(LRI_GENERIC_ACK) + sizeof(COMM_HEADER), OTHER_CRC_START);
		TxHDLC((uint8_t*)&pyldFileAcks, pyldFileAcks.headers.length);
		
		sprintf_P(dbgMsg, PSTR("Sending ACK to 0x%04x from 0x%04x"), pyldFileAcks.headers.destination, pyldFileAcks.headers.source); 
		SEND_MSG;
	}
	else
	{
		sprintf_P(dbgMsg, PSTR("Sending NAK")); SEND_MSG;
		pyldFileNaks.headers.destination = pHeaders->wrapper.source;
		pyldFileNaks.headers.source = pHeaders->wrapper.destination;
		pyldFileNaks.headers.msgID = pHeaders->wrapper.msgID;
		pyldFileNaks.headers.length = sizeof(LRI_COMM_WRAPPER) + sizeof(LRI_GENERIC_NAK) + sizeof(COMM_HEADER) + sizeof(NAK_FILE);
		pyldFileNaks.headers.msgType = 0x04;
		pyldFileNaks.nakInfo.originalCmd = pHeaders->wrapper.msgType;
		pyldFileNaks.nakInfo.nakSummary = 0;
		pyldFileNaks.toShoreheader.packetID = pHeaders->header.packetID;
		pyldFileNaks.toShoreheader.size = 3;
		pyldFileNaks.toShoreheader.type = 0x26;
		pyldFileNaks.nakFile.count = 1;
		pyldFileNaks.nakFile.result = nakType;
		pyldFileNaks.nakFile.crc = CalculateCRC16((uint8_t*) &pyldFileAcks, sizeof(LRI_COMM_WRAPPER) + sizeof(LRI_GENERIC_NAK) + sizeof(COMM_HEADER) + sizeof(NAK_FILE) - CRC16_SIZE, OTHER_CRC_START);
		TxHDLC((uint8_t*)&pyldFileAcks, pyldFileAcks.headers.length);

		sprintf_P(dbgMsg, PSTR("Sent NAK...")); SEND_MSG;
	}

	if(myTaskID == 0)
	{
		//Process Console Command)
		FloatCommParseConsole(pHeaders);
		consoleAvailable[myTaskID] = FALSE;
	}

}


prog_char floatCommSeparators[] = " ,:=\r\n";

#define NUM_PIB_COMMANDS	7
#define LEN_PIB_COMMANDS	8

prog_char floatCommCommands[NUM_PIB_COMMANDS][LEN_PIB_COMMANDS] = {"pm1", "pm2", "pm3", "cfg", "boardID", "setDly", "id"};
prog_char floatCommCmdList[] = "123cbdi123CBDI";
prog_char floatCommParserID[] = "PIB";


char floatCommSeparatorList[7] __attribute__((section(".xdata")));

uint8_t floatCommsConsoleResponse = 1;

char consoleResponse[80] __attribute__((section(".xdata")));
char pmCfg[20] __attribute__((section(".xdata")));

void FloatCommParseConsole(PPYLD_MGR_FILE_RX_BUFFER pHeaders)
{
	char* myCommand = consoleIn[0].cmd; 
	char* result = NULL;
	char command = 0;
	BOOL goodCommand = FALSE;
	uint8_t resultCode = 0;
	
	sprintf_P(dbgMsg, PSTR("Processing console input: <%s>"), myCommand); SEND_MSG;

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

	memset(floatCommSeparatorList, 0, 7);
	strcat_P(floatCommSeparatorList, floatCommSeparators);

	result = strtok_r(myCommand, floatCommSeparatorList, &myCommand);
	resultCode = 1;				//No Command Received
	if(result != NULL)
	{
		resultCode = 2;			//Unknown Parser Code
		if(strncasecmp_P(result, PSTR("PIB"), 3) == 0)
		{
			resultCode = 3;		//Unknown command
			command = getCommand(&myCommand, floatCommCommands[0], LEN_PIB_COMMANDS, NUM_PIB_COMMANDS, floatCommCmdList, floatCommSeparatorList);

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

//	char consoleResponse[80];
	memset(consoleResponse, 0, 80);

	if(goodCommand == TRUE)
	{
		//sprintf_P(dbgMsg, PSTR("Good command")); SEND_MSG;
		resultCode = FloatCommsParseCommand(command, myCommand, consoleResponse);
	} 
	else
	{
		sprintf_P(dbgMsg, PSTR("Bad command. Result code: <%d>"), resultCode); 
		SEND_MSG;
	}

	if(resultCode != SUCCESS)
	{
		switch(resultCode)
		{
			case FAIL:
				floatCommsConsoleResponse = CONSOLE_RESPONSE_CMD_FAILED;
				break;
			case 1:
				floatCommsConsoleResponse = CONSOLE_RESPONSE_EMPTY_CMD;
				break;
			case 2:
				floatCommsConsoleResponse = CONSOLE_RESPONSE_WRONG_DEVICE;
				break;
			case 3:
				floatCommsConsoleResponse = CONSOLE_RESPONSE_UNKNOWN_CMD;
				break;
			default:
				floatCommsConsoleResponse = CONSOLE_RESPONSE_CMD_FAILED;
				break;
		}
	}
	else
	{
		floatCommsConsoleResponse = CONSOLE_RESPONSE_OK;
	}

	FloatCommsQueueConsoleResponse(pHeaders, consoleResponse);

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

	return;
}

uint8_t FloatCommsParseCommand(char command, char* cmdParams, char* response)
{
	// Advance cmdParams past token separator
	//cmdParams++;
	cmdParams = strtok_r(cmdParams, floatCommSeparatorList, &cmdParams);

	sprintf_P(dbgMsg, PSTR("Command is: <%c>"), command);
	SEND_MSG;

	sprintf_P(dbgMsg, PSTR("Command params: <%s>"), cmdParams);
	SEND_MSG;

	uint8_t pm = 2;
	uint8_t deviceToSet, deviceCurrent;
	uint16_t eepromAddress;
	
	uint8_t powerDelay, boardID;

	switch(command)
	{
		case '1':
			pm--;
		case '2':
			pm--;
		case '3':
			// Set the appropriate PM to the appropriate sensor

			if (strncasecmp_P(cmdParams, emptyParserID, strlen_P(emptyParserID))==0)
			{
				deviceToSet = EMPTY;
			}
			else if (strncasecmp_P(cmdParams, powerParserID, strlen_P(powerParserID))==0)
			{
				deviceToSet = POWER;
			}
			else if (strncasecmp_P(cmdParams, gpctdParserID, strlen_P(gpctdParserID))==0)
			{
				deviceToSet = GPCTD;
			}
			else if (strncasecmp_P(cmdParams, ctd37SIParserID, strlen_P(SB37SIParserID))==0)
			{
				deviceToSet = CTD37SI;
			}
			else if (strncasecmp_P(cmdParams, teledyneATM88XParserID, strlen_P(teledyneATM88XParserID))==0)
			{
				deviceToSet = ATM;
			}
			else if (strncasecmp_P(cmdParams, c3OrigParserID, strlen_P(c3OrigParserID))==0)
			{
				deviceToSet = C3;
			}
			else if (strncasecmp_P(cmdParams, MBARI_CO2_ParserID, strlen_P(MBARI_CO2_ParserID))==0)
			{
				deviceToSet = CO2;
			}
			else if (strncasecmp_P(cmdParams, PSTR("COMM"), 4)==0)
			{
				deviceToSet = PYLDCOMM;
			}
			else if (strncasecmp_P(cmdParams, PSTR("SMC"), 3)==0)
			{
				deviceToSet = SMC;
			}
			else if (*cmdParams>='0' && *cmdParams<='9')
			{
				sprintf_P(dbgMsg, PSTR("number...")); SEND_MSG;
				deviceToSet = atoi(cmdParams);
			}
			else
			{
				sprintf_P(dbgMsg, PSTR("Failing...")); SEND_MSG;
				return FAIL;
			}

			deviceCurrent = -1;
			eepromAddress = EEPROM_SYSTEM_PARAMS_START + EEPROM_TASK1_OFFSET + ( pm * EEPROM_TASK_SIZE);

			EepromReadByte((void*)eepromAddress, (void*) &deviceCurrent);

			if (deviceToSet!=deviceCurrent)
			{
				kill[pm+1] = TRUE;				
				EepromWriteByteUnSigned((void*)eepromAddress, deviceToSet);
			}
			else
			{
				sprintf_P(dbgMsg, PSTR("PM%d, Device already here"), pm+1, deviceToSet);
				SEND_MSG;
			}

			// Send the sensor configuration back
			//break;

		case 'c':
		case 'C':
			// Send sensor configuration information
			
			for(uint8_t i=0; i<3; i++)
			{
				uint16_t eepromAddress;
				uint8_t task;
				eepromAddress = EEPROM_SYSTEM_PARAMS_START + EEPROM_TASK1_OFFSET + (i * EEPROM_TASK_SIZE);
				EepromReadByte((void*)eepromAddress, (void*)&task);
				memset(pmCfg, 0, 20);

				char parserID[10];
				memset(parserID, 0, 10);
								
				switch(task)
				{
					case EMPTY:
						memcpy_P(parserID, emptyParserID, strlen_P(emptyParserID));
						break;

					case PYLDCOMM:
						sprintf_P(parserID, PSTR("COMM"), 8);
						break;

					case SMC:
						sprintf_P(parserID, PSTR("SMC"), 3);
						break;
					
					case CTD37SI:
						memcpy_P(parserID, ctd37SIParserID, strlen_P(ctd37SIParserID));
						break;

					case GPCTD:
						memcpy_P(parserID, gpctdParserID, strlen_P(gpctdParserID));
						break;

					case C3:
						memcpy_P(parserID, c3OrigParserID, strlen_P(c3OrigParserID));
						break;

					case POWER:
						memcpy_P(parserID, powerParserID, strlen_P(powerParserID));
						break;

					case ATM:
						memcpy_P(parserID, teledyneATM88XParserID, strlen_P(teledyneATM88XParserID));
						break;

					case CO2:
						memcpy_P(parserID, MBARI_CO2_ParserID, strlen_P(MBARI_CO2_ParserID));
						break;

				}

				sprintf_P(pmCfg, PSTR("PM%d=%s "), i+1, parserID);
				strcat(response, pmCfg);
			}

			EepromReadByte((void*)(EEPROM_SYSTEM_PARAMS_START + EEPROM_POWER_DOWN_DELAY_OFFSET), &powerDelay);
			sprintf_P(pmCfg, PSTR("Power Down Delay = %d seconds"), powerDelay); 
			strcat(response, pmCfg);

			sprintf_P(dbgMsg, PSTR("cfg result: <%s>"), response); SEND_MSG;

			break;
			
		


		case 'b':
		case 'B':
		{
			// Set board ID
			int32_t templong = 0;

			templong = strtol(cmdParams, NULL, 0);

			if((templong >= 0x100) && (templong < 0xFF00) && ((templong & 0x000000FF) == 0))
				templong = templong >> 8;

			if (templong>0x00 && templong<0xFF)
			{
				boardID = templong;
				EepromWriteByteUnSigned((void*)(EEPROM_SYSTEM_PARAMS_START + EEPROM_BOARD_ID_OFFSET), boardID);
			}
			else 
				return FAIL;

			sprintf_P(response, PSTR("OK -- Board ID will be %02X after payload reboot"), boardID);

			break;
		}
			
		case 'd':
		case 'D':
			// Set Power Down Delay

			if (!*cmdParams)
				return FAIL;

			powerDelay = atoi(cmdParams);
			if(powerDelay < 255)
			{
				EepromWriteByteUnSigned((void*)(EEPROM_SYSTEM_PARAMS_START + EEPROM_POWER_DOWN_DELAY_OFFSET), powerDelay);
				sprintf_P(response, PSTR("OK -- Power down delay set to %d seconds"), powerDelay);
			}
			else
				return FAIL;
			break;

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

			EepromReadByte((void*)(EEPROM_SYSTEM_PARAMS_START + EEPROM_BOARD_ID_OFFSET), &boardID);
			EepromReadByte((void*)(EEPROM_SYSTEM_PARAMS_START + EEPROM_POWER_DOWN_DELAY_OFFSET), &powerDelay);

			sprintf_P(response, PSTR("PIB ver. %d.%d.%d. Board ID = 0x%02X. Power Down Delay = %d sec."), MAJOR, MINOR, BUILD, boardID, powerDelay);

			break;


		default:
			return FAIL;
	}

	return SUCCESS;
}


void FloatCommsQueueConsoleResponse(PPYLD_MGR_FILE_RX_BUFFER pHeaders, char* response)
{

	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 = pHeaders->wrapper.source;
	pReport->comm.fwdInfo.source = mainTaskAddress;	

	if(floatCommsConsoleResponse < CONSOLE_RESPONSE_UNKNOWN_CMD)
	{

		pReport->comm.headers.length = sizeof(PIB_DATA_FORWARD_MESSAGE) + sizeof(CONSOLE_ACK) + strlen(response) + CRC16_SIZE;

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

		pReport->consoleAck.len = strlen(response);
		pReport->consoleAck.disposition = 3;
		pReport->consoleAck.format = 0;
		pData = pData + sizeof(PIB_DATA_FORWARD_MESSAGE) + sizeof(CONSOLE_ACK);
		memcpy(pData, response, strlen(response));
		memcpy(dbgMsg, pData, strlen(response));
		dbgMsg[strlen(response)]=0;
		SEND_MSG;
		
		sprintf_P(dbgMsg, PSTR("Sending ACK from 0x%04X to 0x%04X"), pReport->comm.fwdInfo.source, pReport->comm.fwdInfo.destination);
		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] = floatCommsConsoleResponse;

		sprintf_P(dbgMsg, PSTR("Sending NAK from 0x%04X to 0x%04X"), pReport->comm.fwdInfo.source, pReport->comm.fwdInfo.destination);
		SEND_MSG;
	}

	pReport->comm.iridiumInfo.headerICD.packetID = consoleIn[0].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;
}
void SerialPortPassThroughFloat(uint8_t port)
{
	uint8_t c = _BV(RXC0);

	UCSR0B = _BV(RXEN0) | _BV(TXEN0);
	UCSR1B = _BV(RXEN0) | _BV(TXEN0);
	UCSR2B = _BV(RXEN0) | _BV(TXEN0);
	UCSR3B = _BV(RXEN0) | _BV(TXEN0);

	while(readyForSerialPassThru == FALSE)
		OSTimeDlyHMSM(0,0,0,100);

	wdt_disable();  	
	cli();

	switch(port)
	{
		case 1:
		{
			while(1)
			{
				if(UCSR0A & c)
					UDR1 = UDR0;
				if(UCSR1A & c)
					UDR0 = UDR1;
			}
			break;
		}
		case 2:
		{
			while(1)
			{
				if(UCSR0A & c)
					UDR2 = UDR0;
				if(UCSR2A & c)
					UDR0 = UDR2;
			}
			break;
		}
		case 3:
		{
			while(1)
			{
				if(UCSR0A & c)
					UDR3 = UDR0;
				if(UCSR3A & c)
					UDR0 = UDR3;
			}
			break;
		}
		default:
		{
			while(1)
			{}
			break;
		}
	}
}

uint8_t SendDebugMsg(PDEBUG_OUTPUT_INFO pDbgInfo)
{
	uint8_t retval = SUCCESS;

	if(pDbgInfo->lenBuffer <= 192)
	{
		PDEBUG_MSG_OUTPUT pReport = NULL;

		OSMutexPend(pMutexFloatQueue, INFINITE, &retval);
		{
			pReport = (PDEBUG_MSG_OUTPUT)&(txFloatData[headFloatTx]);

			pReport->comm.headers.length  = sizeof(COMM_XBEE_DATA_XMIT) + pDbgInfo->lenBuffer + CRC16_SIZE;
			pReport->comm.headers.destination = FLOAT_DEBUG_INTERFACE;

			pReport->comm.debugInfo.outputFormat = pDbgInfo->fmtBuffer;
			pReport->comm.debugInfo.debugLevel 	 = pDbgInfo->dbgLevel;
			pReport->comm.debugInfo.size         = pDbgInfo->lenBuffer;

			memcpy(pReport->msg, pDbgInfo->pBuffer, pDbgInfo->lenBuffer);

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

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

		retval = SUCCESS;
	}
	else
	{
		retval = FAIL;
	}

	return retval;
}