//----------------------------------------------------------------------------------
// <copyright file="MBARI CO2.c" company="LiquidRobotics">
//	Copyright (c) Liquid Robotics Corporation.  All rights reserved.
// </copyright>
//
// <summary>
// 	This is the source file for managing the MBARI_CO2
//
//  Now with less guesswork than before
//
// </summary>
//
// <owner>Arash Ushani</owner>
//----------------------------------------------------------------------------------



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

#include "SerialDebug.h"

#define CO2_SOF 				0x7E
#define CO2_MAX_BLOCK_SIZE		255
#define CO2_POWER_DOWN_DELAY	60			// Wait 60 seconds after power down message before actually powering down

typedef struct _MBARI_CO2_REPORT_
{
	uint8_t		length;
	uint8_t		data[CO2_MAX_BLOCK_SIZE + CRC16_SIZE];
	//uint16_t	crc16;
}MBARI_CO2_REPORT, *PMBARI_CO2_REPORT;


typedef struct _MBARI_CO2_XMIT_
{
	PIB_IRIDIUM_DATA_XMIT	comm;
	TYPE_91_REPORT_HEADER	reportHeader;
	TYPE_90_91_DATA_HEADER	dataHeader;
	uint8_t 				data[CO2_MAX_BLOCK_SIZE];
}MBARI_CO2_XMIT, *PMBARI_CO2_XMIT;

/*
typedef struct _MBARI_CO2_XMIT_
{
	PIB_IRIDIUM_DATA_XMIT	comm;
	uint32_t				payloadID;
	uint8_t					version;	
	uint8_t					data[CO2_MAX_BLOCK_SIZE];
	//MBARI_CO2_REPORT		report;
}MBARI_CO2_XMIT, *PMBARI_CO2_XMIT;
*/

typedef struct _MBARI_CO2_BUFFER_
{
	char 					bufferTx[256];
	char 					bufferRx[1024];
	
	BOOL					isPowered;
	BOOL					isSampling;

	BOOL					pendingPowerDown;
	uint32_t				powerDownTime;

	uint8_t					initState;

	MBARI_CO2_REPORT		reportData;

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

}MBARI_CO2_BUFFER, *PMBARI_CO2_BUFFER;

typedef struct _MBARI_CO2_TASK_
{
	uint8_t	taskID;
	uint8_t portNum;
	uint16_t eepromAddress;
	PMBARI_CO2_BUFFER 	pBuf;
}MBARI_CO2_TASK, *PMBARI_CO2_TASK;


uint8_t MBARI_CO2_Init(PMBARI_CO2_TASK pTaskInfo);
uint8_t MBARI_CO2_PowerOff(PMBARI_CO2_TASK pTaskInfo);
uint8_t MBARI_CO2_PowerDown(PMBARI_CO2_TASK pTaskInfo);

uint8_t MBARI_CO2_Tx(PMBARI_CO2_TASK pTaskInfo, uint8_t len);
uint8_t MBARI_CO2_Rx(PMBARI_CO2_TASK pTaskInfo);

uint8_t MBARI_CO2_RxDataSample(PMBARI_CO2_TASK pTaskInfo);
uint8_t MBARI_CO2_GetDataSample(PMBARI_CO2_TASK pTaskInfo);
uint8_t MBARI_CO2_ReportXmit(PMBARI_CO2_TASK pTaskInfo);

void MBARI_CO2_ParseConsole(PMBARI_CO2_TASK pTaskInfo);
void MBARI_CO2_QueueConsoleResponse(PMBARI_CO2_TASK pTaskInfo);
uint8_t MBARI_CO2_ParseCommand(PMBARI_CO2_TASK pTaskInfo, char command, char* myCommand);

void SerialPortPassThroughMBARI_CO2(uint8_t port);

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

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

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

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

extern int32_t latitude;
extern int32_t longitude;

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

extern uint8_t delay;

void MBARI_CO2_ParseConsole(PMBARI_CO2_TASK pTaskInfo);
uint8_t MBARI_CO2_ParseCommand(PMBARI_CO2_TASK pTaskInfo, char command, char* cmdParams);
void MBARI_CO2_QueueConsoleResponse(PMBARI_CO2_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_CO2_COMMANDS		8
#define LEN_CO2_COMMANDS		10

//Autorun State stored at offset 0x0040 from PM base -- 1 byte
#define CO2_AUTORUN_STATE_OFFSET		0x0040


extern const prog_char tokenListConsole[];
prog_char MBARI_CO2_Commands[NUM_CO2_COMMANDS][LEN_CO2_COMMANDS] = {"on", "off", "autorun", "run", "stop", "status", "id", "cmd"};
prog_char MBARI_CO2_ParserID[] = "CO2";
prog_char MBARI_CO2_CmdList[] = "ofarxsicOFARXSIC";

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

uint8_t MBARI_CO2_Init(PMBARI_CO2_TASK pTaskInfo)
{
	uint8_t retval = SUCCESS;

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


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

	OSuartInitPort(pTaskInfo->portNum, 38400, UART_MODE_EIGHT_DATA_BITS, UART_MODE_ONE_STOP_BIT, UART_MODE_NO_PARITY);	
	
	OSTimeDlyHMSM(0,0,1,0);
	
	
	EnablePIBModule(pTaskInfo->portNum, FALSE, FALSE);
	pTaskInfo->pBuf->isPowered = FALSE;
	pTaskInfo->pBuf->isSampling = FALSE;

	pTaskInfo->pBuf->initState = 0xFF;
	EepromReadByte((void*)(pTaskInfo->eepromAddress + CO2_AUTORUN_STATE_OFFSET), (void*)&pTaskInfo->pBuf->initState);
	if((pTaskInfo->pBuf->initState != AUTORUN_ON) && (pTaskInfo->pBuf->initState != AUTORUN_OFF))
	{
		pTaskInfo->pBuf->initState = AUTORUN_ON;
		EepromWriteByteUnSigned((void*)(pTaskInfo->eepromAddress + CO2_AUTORUN_STATE_OFFSET), pTaskInfo->pBuf->initState);
	}

	pTaskInfo->pBuf->isPowered = FALSE;
	pTaskInfo->pBuf->isSampling = FALSE;

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



//	EnablePIBModule(pTaskInfo->portNum, TRUE, FALSE);
//	pTaskInfo->pBuf->isPowered = TRUE;
//	pTaskInfo->pBuf->isSampling = TRUE;

	if(delay < CO2_POWER_DOWN_DELAY)
		delay = CO2_POWER_DOWN_DELAY;

	/*
	EnablePIBModule(pTaskInfo->portNum, TRUE, FALSE);
	pTaskInfo->pBuf->isPowered = TRUE;
	pTaskInfo->pBuf->isSampling = TRUE;
	*/
	return retval;
}


void MBARI_CO2(void* pParam)
{
	MBARI_CO2_TASK taskInfo;
	PMBARI_CO2_TASK pTaskInit = (PMBARI_CO2_TASK)pParam;
	uint8_t retval = FAIL;

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

	retval = MBARI_CO2_Init(&taskInfo);

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

			sprintf_P(dbgMsg, PSTR("MBARI CO2 for PM%d"), taskInfo.portNum);
			SEND_MSG;

			if(taskInfo.pBuf->isSampling == TRUE)
			{
				MBARI_CO2_GetDataSample(&taskInfo);
			}

			if (powerDown[taskInfo.portNum] == TRUE)
			{
				//sprintf_P(dbgMsg, PSTR("Power down CO2...")); SEND_MSG;				
				MBARI_CO2_PowerOff(&taskInfo);
				powerDown[taskInfo.portNum] = FALSE;
			}

			if (taskInfo.pBuf->pendingPowerDown == TRUE)
			{
				uint32_t tickNow = getsecondtick();
				if (tickNow > taskInfo.pBuf->powerDownTime)
					MBARI_CO2_PowerDown(&taskInfo);
					
			}
		
			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);

				SerialPortPassThroughMBARI_CO2(taskInfo.portNum);
			}


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

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

			// Check to see if we need to end this task and release resources
			if (kill[taskInfo.portNum] == TRUE) 
			{
				uint8_t val = 0xFF;
				uint16_t startAddr = 0;

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

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

					memset(taskInfo.pBuf, 0, sizeof(MBARI_CO2_BUFFER));

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

uint8_t MBARI_CO2_PowerOff(PMBARI_CO2_TASK pTaskInfo)
{
	memset(pTaskInfo->pBuf->bufferTx, 0, sizeof(pTaskInfo->pBuf->bufferTx));
	sprintf_P(pTaskInfo->pBuf->bufferTx, PSTR("Power down\r"));

	uint8_t retval = MBARI_CO2_Tx(pTaskInfo, 11);

	
	// Don't power down here
	// Must wait for 60 seconds before actually turning power off
	pTaskInfo->pBuf->pendingPowerDown = TRUE;
	pTaskInfo->pBuf->powerDownTime = getsecondtick() + CO2_POWER_DOWN_DELAY;

	return retval;

}

uint8_t MBARI_CO2_PowerDown(PMBARI_CO2_TASK pTaskInfo)
{
	uint8_t retval = EnablePIBModule(pTaskInfo->portNum, FALSE, FALSE);
	
	if (retval == SUCCESS)
	{
		pTaskInfo->pBuf->isPowered = FALSE;
		pTaskInfo->pBuf->isSampling = FALSE;

		pTaskInfo->pBuf->pendingPowerDown = FALSE;
		pTaskInfo->pBuf->powerDownTime = -1;
	}

	return retval;
}


uint8_t MBARI_CO2_Tx(PMBARI_CO2_TASK pTaskInfo, uint8_t len)
{
	uint8_t retval = FAIL;
	uint8_t err = 0;
	
	uint8_t i = 0;

	if((len > 0) && (len <= 255))
	{
		while(len > i)
		{
			char c = pTaskInfo->pBuf->bufferTx[i++];
			err = OSuartPutCharWait(pTaskInfo->portNum, c, 10);
			if(err != OS_ERR_NONE)
				return retval;
		}

		retval = SUCCESS;
	}

	return retval;
}

uint8_t MBARI_CO2_Rx(PMBARI_CO2_TASK pTaskInfo)
{
	uint8_t retval = FAIL;
	uint8_t err = 0;
	
	uint8_t i = 0;

	while(i < sizeof(pTaskInfo->pBuf->bufferRx))
	{
		char c = OSuartGetCharWait(pTaskInfo->portNum, 64, &err);

		if (err != OS_ERR_NONE)
			return SUCCESS;

		pTaskInfo->pBuf->bufferRx[i++] = c;

	}

	return retval;
}

uint8_t MBARI_CO2_RxDataSample(PMBARI_CO2_TASK pTaskInfo)
{
	uint8_t retval = FAIL;

	uint16_t count = 0;
	uint8_t err = 0;

	uint8_t c = 0;

	
	do
	{
		c = (uint8_t) OSuartGetCharWait(pTaskInfo->portNum, 64, &err);	//Read until you get to CO2_SOF
	} while (c!=CO2_SOF && err == OS_ERR_NONE);
			
	if(err != OS_ERR_NONE)
	{
		return retval;
	}

	if (c != CO2_SOF)
	{
		//sprintf_P(dbgMsg, PSTR("wrong char <%c>"), c); SEND_MSG;
		return retval;
	}


	pTaskInfo->pBuf->reportData.length = 0;	
	pTaskInfo->pBuf->reportData.length = (uint8_t) OSuartGetCharWait(pTaskInfo->portNum, 64, &err);
	
	if(err != OS_ERR_NONE || pTaskInfo->pBuf->reportData.length == 0)
		return retval;
	
	count = 0;

	// Get Data and CRC
	while(count < pTaskInfo->pBuf->reportData.length + CRC16_SIZE)
	{
		c = (uint8_t) OSuartGetCharWait(pTaskInfo->portNum, 64, &err);
			
		if(err != OS_ERR_NONE)
		{
			return retval;
		}

		pTaskInfo->pBuf->reportData.data[count++] = c;

	}

	if(ValidateCRC16((uint8_t*) (&pTaskInfo->pBuf->reportData.length), (uint16_t) pTaskInfo->pBuf->reportData.length + 1 + CRC16_SIZE, 0xFFFF))
	{
		// WE HAVE A GOOD CRC-16
		//sprintf_P(dbgMsg, PSTR("Good CRC")); SEND_MSG;
		return SUCCESS;
	}
	else
	{
		// WE HAVE A BAD CRC-16

		//uint16_t crc = pTaskInfo->pBuf->reportData.data[pTaskInfo->pBuf->reportData.length+1];
		//crc <<= 8;
		//crc += pTaskInfo->pBuf->reportData.data[pTaskInfo->pBuf->reportData.length];		
		//sprintf_P(dbgMsg, PSTR("CRC is <0x%x>, should be <0x%x>"), crc, CalculateCRC16((uint8_t*) (&pTaskInfo->pBuf->reportData.length), pTaskInfo->pBuf->reportData.length + 1, 0xFFFF)); SEND_MSG;
		return FAIL;
	}


	//return SUCCESS;
}


uint8_t MBARI_CO2_GetDataSample(PMBARI_CO2_TASK pTaskInfo)
{

	memset(&(pTaskInfo->pBuf->reportData), 0, sizeof(MBARI_CO2_REPORT));

	uint8_t retval = MBARI_CO2_RxDataSample(pTaskInfo);

	if(retval == SUCCESS)
	{
		retval = MBARI_CO2_ReportXmit(pTaskInfo);
	}

	return retval;
}

uint8_t MBARI_CO2_ReportXmit(PMBARI_CO2_TASK pTaskInfo)
{
	uint8_t err = 0;
	PMBARI_CO2_XMIT pReport = NULL;

	OSMutexPend(pMutexFloatQueue, INFINITE, &err);

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

	pReport->reportHeader.payloadType = 0x90;
	pReport->reportHeader.boardID = mainBoardID;
	pReport->reportHeader.taskID = pTaskInfo->taskID;
	pReport->reportHeader.dataFormat = 1;
	pReport->reportHeader.parser = 1;
	pReport->reportHeader.priority = 1;
	pReport->reportHeader.repeatCount = 1;

	pReport->comm.headers.length = sizeof(PIB_IRIDIUM_DATA_XMIT)+sizeof(TYPE_91_REPORT_HEADER)+sizeof(TYPE_90_91_DATA_HEADER)+pTaskInfo->pBuf->reportData.length + CRC16_SIZE;
	pReport->comm.headers.destination = FLOAT_IRIDIUM_INTERFACE;

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

	pReport->dataHeader.latitude = latitude;
	pReport->dataHeader.longitude = longitude;
	pReport->dataHeader.timestamp = gettime();

	memcpy(&(pReport->data), &(pTaskInfo->pBuf->reportData.data), pTaskInfo->pBuf->reportData.length);


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

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

	//sprintf_P(dbgMsg, PSTR("going back to shore")); SEND_MSG;

	OSMutexPost(pMutexFloatQueue);

	return SUCCESS;
}

void MBARI_CO2_ParseConsole(PMBARI_CO2_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, MBARI_CO2_ParserID, strlen_P(MBARI_CO2_ParserID)) == 0)
		{
			resultCode = 3;		//Unknown command

			command = getCommand(&myCommand, MBARI_CO2_Commands[0], LEN_CO2_COMMANDS, NUM_CO2_COMMANDS, MBARI_CO2_CmdList, pTaskInfo->pBuf->tokenSeparatorList);

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

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

	MBARI_CO2_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 MBARI_CO2_QueueConsoleResponse(PMBARI_CO2_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);
	}
	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;
}

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

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

uint8_t MBARI_CO2_ParseCommand(PMBARI_CO2_TASK pTaskInfo, char command, char* myCommand)
{
	uint8_t retval = FAIL;

	switch(command)
	{
		case 'o':
		case 'O':
		case 'r':
		case 'R':
			// ON

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

			retval = SUCCESS;

			break;

		case 'x':
		case 'X':
		case 'f':
		case 'F':
			// OFF
			retval = MBARI_CO2_PowerOff(pTaskInfo);			

			break;


		case 's':
		case 'S':

			sprintf_P(pTaskInfo->pBuf->consoleResponse, PSTR("Power is %s. Sampling is %s. Autorun is %s"), pTaskInfo->pBuf->isPowered ? (pTaskInfo->pBuf->pendingPowerDown ? "PENDING OFF":"ON"):"OFF", pTaskInfo->pBuf->isSampling ? "ON":"OFF", (pTaskInfo->pBuf->initState == AUTORUN_ON) ? "ON":"OFF");
			pTaskInfo->pBuf->responseInfo.status = CONSOLE_RESPONSE_OK;
			pTaskInfo->pBuf->responseInfo.length = strlen(pTaskInfo->pBuf->consoleResponse);	

			return SUCCESS;

			break;

		case 'c':
		case 'C':

			OSTimeDlyHMSM(0,0,0,100);

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

			//We have a Command to send

			if (tailLen)
			{	
				uint8_t err;
				

				memset(pTaskInfo->pBuf->bufferRx, 0, 1024);
				memcpy(pTaskInfo->pBuf->bufferTx, tail, tailLen);

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

				memcpy(pTaskInfo->pBuf->consoleResponse, pTaskInfo->pBuf->bufferRx, strlen(pTaskInfo->pBuf->bufferRx));
				pTaskInfo->pBuf->responseInfo.length = strlen(pTaskInfo->pBuf->consoleResponse);
				pTaskInfo->pBuf->responseInfo.status = CONSOLE_RESPONSE_OK;
				retval = SUCCESS;
				
			}

			break;

		case 'i':
		case 'I':

			sprintf_P(pTaskInfo->pBuf->consoleResponse, PSTR("MBARI CO2 Sensor"));
			pTaskInfo->pBuf->responseInfo.status = CONSOLE_RESPONSE_OK;
			pTaskInfo->pBuf->responseInfo.length = strlen(pTaskInfo->pBuf->consoleResponse);			

			return SUCCESS;

			break;
		case 'a':
		case 'A':
		{
			char* result = NULL;

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

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

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

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


				if(retval == SUCCESS)
				{
					EepromWriteByteUnSigned((void*)(pTaskInfo->eepromAddress + CO2_AUTORUN_STATE_OFFSET), pTaskInfo->pBuf->initState);

					sprintf_P(pTaskInfo->pBuf->consoleResponse, PSTR("AutoRun is %s"), (pTaskInfo->pBuf->initState==AUTORUN_ON) ? "ON":"OFF");
					pTaskInfo->pBuf->responseInfo.length = strlen(pTaskInfo->pBuf->consoleResponse);
					pTaskInfo->pBuf->responseInfo.status = CONSOLE_RESPONSE_OK;
				}

			}
			else
			{
				retval = FAIL;
			}
			break;
		}
	}

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

	return retval;
}

void SerialPortPassThroughMBARI_CO2(uint8_t port)
{
	readyForSerialPassThru = TRUE;

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