//------------------------------------------------------------------------------
// <copyright file="PayloadPortSlave.cpp" company="LiquidRobotics">
//
// Copyright (c) 2014 Liquid Robotics, Inc.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.
//
// This software is associated with a release.  Liquid Robotics makes no
// representation that the design, interfaces or component distribution will
// remain unchanged in future releases.
//
// </copyright>
//
// <summary>
// 	Declaration of PayloadPortSlave, which implements payload message exchange
//  and is a basis for the SMC implementation of a payload interface
// </summary>
//
// <owner>Mike Cookson</owner>
//------------------------------------------------------------------------------

#include <stdint.h>
#ifndef PAYLOAD_PORT_SLAVE_NO_CORBA
#include "LqrSMACIdl.h"
#endif
#include <boost/foreach.hpp>
#include "PayloadPortSlave.h"
#include "PayloadMessage.h"
#include "PayloadProtocol.h"
#include "LqrSMACPrintfBuffer.h"
#include "nsextend.h"
#include "LqrSMACByteStream.h"
#include "LqrSMACZeroObject.h"

using namespace std;
using namespace LiquidRobotics::SMACFramework;
using namespace LiquidRobotics::PayloadInterface::FCCMessage;

namespace LiquidRobotics {
namespace PayloadInterface {

const unsigned PayloadPortSlave::MaxLength = 560;

PayloadPortSlave::PayloadPortSlave(const SerialPortSettings& _portSettings, Logger& _logger)
: portSettings(_portSettings)
, logger(_logger)
{
}

PayloadPortSlave::~PayloadPortSlave()
{
}

HDLCPort::Ptr PayloadPortSlave::InitHDLC()
{
	HDLCPort::SerialPortPtr pSerialPort = portSettings.CreatePort(logger);
	HDLCPort::Ptr pHDLC(new HDLCPort(pSerialPort, MaxLength, logger));

	pHDLC->SetIntercharacterTimeout(20);
	pHDLC->SetReceiveTimeout(IHDLCPort::NO_TIMEOUT);

	return pHDLC;
}

void PayloadPortSlave::Run()
{
	HDLCPort::Ptr pHDLC = InitHDLC();

	Run(pHDLC);
}

void PayloadPortSlave::Run(HDLCPort::Ptr pHDLC)
{
	while(Running())
	{
		PayloadMessage::Ptr receivedMessage;

		try
		{
			// wait for a message
			Logger::Locked(logger)->Info << "----------" << endl;
			receivedMessage = WaitForMessage(pHDLC);
		}
		catch(exception& ex)
		{
			Logger::Locked(logger)->Error << "Caught " << typeid(ex).name() << " (" << ex.what() << ") while waiting for/receiving message" << endl;
		}

		if (!receivedMessage)
			continue;

		try
		{
			// detect sequential duplicate message
			bool duplicateHeader = lastMessage && (receivedMessage->GetHeader() == lastMessageHeaderReceived);

			// save the new header and cancel valid if not duplicate
			if (!duplicateHeader)
			{
				lastMessageHeaderReceived = receivedMessage->GetHeader();
				lastMessage.reset();
			}

			// get the user's desired response
			PayloadMessage::Ptr responseMessage;

			// if not a duplicate header, then get the app's response
			if (!duplicateHeader)
				responseMessage = OnMessageReceived(receivedMessage, duplicateHeader);

			// at this point, all time-consuming actions should be complete, so flush the HDLC
			// receiver to avoid stacking of messages: master only sends one message at a time
			// and waits for a response; in the case that no ack was requested, don't flush
			// because the master may have sent the next message
			if (!receivedMessage->GetNoAckHaveMessagesFlag())
				pHDLC->FlushInput();

			// if there's a response, send it and save it if it's not a duplicate header
			if (responseMessage)
			{
				Logger::Locked(logger)->Info << "Sending " << *responseMessage << endl;

				PayloadMessage::ByteArray rawResponse;
				responseMessage->ToArray(rawResponse);

				string notStr;
				if (receivedMessage->GetNoAckHaveMessagesFlag()) notStr = "[NOT] ";

				{
					Logger::Locked lockedLog(logger);
					lockedLog->Info << notStr << "Sending HDLC message of " << rawResponse.size() << ":";

					for (PayloadMessage::ByteArray::const_iterator cit = rawResponse.begin(); cit != rawResponse.end(); ++cit)
						lockedLog->Info << " " << ultos(*cit, 16, 2);

					lockedLog->Info << endl;
				}

				if (!receivedMessage->GetNoAckHaveMessagesFlag())
					SendBytes(pHDLC, rawResponse);

				lastMessage = responseMessage;
			}

			// otherwise, if it's duplicate, resend the last message
			else if (duplicateHeader)
			{
				Logger::Locked(logger)->Info << "Resending " << *lastMessage << endl;

				PayloadMessage::ByteArray rawResponse;
				lastMessage->ToArray(rawResponse);

				string notStr;

				if (receivedMessage->GetNoAckHaveMessagesFlag()) notStr = "[NOT] ";

				Logger::Locked(logger)->Info << notStr << "Resending HDLC message of " << rawResponse << endl;

				if (!receivedMessage->GetNoAckHaveMessagesFlag());
					SendBytes(pHDLC, rawResponse);
			}
		}
#ifndef PAYLOAD_PORT_SLAVE_NO_CORBA
		catch(CORBA::Exception& ex)
		{
			cerr << "Caught CORBA exception: " << DecodeCORBAException(ex) << endl;
		}
#endif
		catch(std::exception& ex)
		{
			Logger::Locked(logger)->Error << "Caught " << typeid(ex).name() << ": " << ex.what() << endl;
		}
		catch(...)
		{
			Logger::Locked(logger)->Error << "Caught some generic exception" << endl;
		}
	}

	Logger::Locked(logger)->Info << "Exited message handling loop" << endl;
}

PayloadMessage::Ptr PayloadPortSlave::WaitForMessage(HDLCPort::Ptr pHDLC)
{
	HDLCPort::ByteArray readBlock;
	pHDLC->Receive(readBlock);

	Logger::Locked(logger)->Debug << "HDLC received block of " << readBlock << endl;

	// convert the received message to a payload message
	PayloadMessage::Ptr result;

	if (readBlock.size())
		result.reset(new PayloadMessage(readBlock));

	return result;
}

void PayloadPortSlave::SendBytes(HDLCPort::Ptr pHDLC, const ByteArray& msgBytes)
{
	ByteArray sendBytes;
	HDLCPort::GetRawTransmitBytes(msgBytes, sendBytes);

	Logger::Locked(logger)->Debug << "HDLC sending block of " << sendBytes << endl;
	pHDLC->SendRawBytes(sendBytes);
}

PayloadMessage::Ptr PayloadPortSlave::OnMessageReceived(PayloadMessage::Ptr messageReceived, bool duplicateHeader)
{
	Logger::Locked(logger)->Info << "Received HDLC message { " << *messageReceived << " } duplicateHeader " << duplicateHeader << endl;
	PayloadMessage::Ptr responseMessage;
	const PayloadMessage::EMessageType messageType = (PayloadMessage::EMessageType) messageReceived->GetMessageType();
	const PayloadMessage::ByteArray& messageData = messageReceived->GetData();
	PPSByteStream pyldStream(messageData);
	PPSByteStream resultStream;

	switch(messageType)
	{
	// not requesting acks for requested packets at this time
	//case PayloadMessage::ACK_REQUESTED_PACKET:		break;

	case PayloadMessage::POWER_NOTIFICATION:
	{
		PayloadProtocol::PYLD_POWER_NOTIFICATION notification;
		pyldStream >> notification;
		pyldStream.VerifyExhausted("PYLD_POWER_NOTIFICATION");

		PayloadProtocol::ACK_PYLD_POWER_NOTIFICATION_RECORD ack;
		bool acked = false;
		PayloadProtocol::ENakSummary nakType = PayloadProtocol::NAKSUMMARY_INVALID_DATA;

		ZeroObject(ack);
		OnPowerNotification(notification, ack, acked, nakType);

		if (acked)
		{
			const bool haveMessages = HaveMessagesQueued(messageReceived->GetDestinationAddress());

			if (logger.LogEnabled(Logger::LT_DEBUG) || haveMessages)
				Logger::Locked(logger)->Info << "Have pending messages = " << (haveMessages ? "TRUE" : "false") << endl;

			resultStream << PayloadProtocol::LRIAckCountWrapper(messageType, 1, 1, haveMessages)
			             << ack;

			responseMessage.reset(
					new PayloadMessage(PayloadMessage::LRI_ACK,
							messageReceived->GetDestinationAddress(),
							messageReceived->GetSourceAddress(),
							messageReceived->GetTransactionID(),
							resultStream.AsBytes()));

		}
		else
		{
			resultStream << PayloadProtocol::LRIGenericNak(messageType, nakType);

			responseMessage.reset(
					new PayloadMessage(PayloadMessage::LRI_NAK,
							messageReceived->GetDestinationAddress(),
							messageReceived->GetSourceAddress(),
							messageReceived->GetTransactionID(),
							resultStream.AsBytes()));
		}

		break;
	}

	case PayloadMessage::REQUEST_PIB_STATUS:
	{
		PayloadProtocol::LRI_REQUEST_PIB_STATUS request;
		pyldStream >> request;
		pyldStream.VerifyExhausted("LRI_REQUEST_PIB_STATUS");

		PayloadProtocol::LRI_PIB_STATUS ack;
		bool acked = false;
		PayloadProtocol::ENakSummary nakType = PayloadProtocol::NAKSUMMARY_INVALID_DATA;

		ZeroObject(ack);
		OnStatusRequest(request, ack, acked, nakType);

		if (acked)
		{
			const bool haveMessages = HaveMessagesQueued(messageReceived->GetDestinationAddress());

			if (logger.LogEnabled(Logger::LT_DEBUG) || haveMessages)
				Logger::Locked(logger)->Info << "Have pending messages = " << (haveMessages ? "true" : "false") << endl;

			resultStream << PayloadProtocol::LRIAckCountWrapper(messageType, 1, 1, haveMessages)
			             << ack;

			responseMessage.reset(
					new PayloadMessage(PayloadMessage::LRI_ACK,
							messageReceived->GetDestinationAddress(),
							messageReceived->GetSourceAddress(),
							messageReceived->GetTransactionID(),
							resultStream.AsBytes()));
		}
		else
		{
			resultStream << PayloadProtocol::LRIGenericNak(messageType, nakType);

			responseMessage.reset(
					new PayloadMessage(PayloadMessage::LRI_NAK,
							messageReceived->GetDestinationAddress(),
							messageReceived->GetSourceAddress(),
							messageReceived->GetTransactionID(),
							resultStream.AsBytes()));
		}

		break;
	}

	case PayloadMessage::REQUEST_ID:
	{
		PayloadProtocol::LRI_REQUEST_ID request;
		pyldStream >> request;
		pyldStream.VerifyExhausted("REQUEST_ID");

		LRI_ID_PACKET_ARRAY ackData;
		OnRequestIDReceived(request, ackData);

		if (!messageReceived->GetNoAckHaveMessagesFlag() && ackData.size())
		{
			const int sz = ackData.size();

			resultStream << PayloadProtocol::LRIAckCountWrapper(messageType, sz, sz)
			             << ContainerOut<LRI_ID_PACKET_ARRAY>(ackData);

			responseMessage.reset(
					new PayloadMessage(PayloadMessage::LRI_ACK, ackData[0].address, messageReceived->GetSourceAddress(),
							messageReceived->GetTransactionID(), resultStream.AsBytes()));
		}

		break;
	}

	case PayloadMessage::FLOAT_TELEMETRY_DATA:
	{
		Logger::Locked(logger)->Info << "Received Telemetry" << endl;
		OnTelemetryReceived(messageReceived);
		break;
	}

	case PayloadMessage::REQUEST_WAITING_PACKET:
	{
		PayloadProtocol::ENakSummary nakSummary = PayloadProtocol::NAKSUMMARY_INVALID_DATA;

		if (messageData.size() == 0)
		{
			const BoardAddress dest = messageReceived->GetDestinationAddress();

			if (HaveMessagesQueued(dest))
			{
				responseMessage = DequeueMessage(dest);

				// this message should already be an LRI_ACK of some sort with an AckContainer;
				// the original command should already be set to appropriately, but the flag to
				// indicate if there are more messages available has not been updated yet, so I
				// have to pull the message apart and update the flag, as well as the transaction ID

				PayloadMessage::ByteArray dataCopy;
				responseMessage->GetData(dataCopy);

				//logger.Debug << "dataCopy(0) = " << dataCopy << endl;

				if (dataCopy.size() >= sizeof(PayloadProtocol::LRI_GENERIC_ACK))
				{
					PayloadProtocol::LRI_GENERIC_ACK& genAck = (PayloadProtocol::LRI_GENERIC_ACK&) dataCopy.at(0);

					genAck.originalCmd = messageType & ~0x8000;

					if (HaveMessagesQueued(dest))
						genAck.originalCmd |= 0x8000;

					//logger.Debug << "dataCopy(1) = " << dataCopy << endl;
					responseMessage->SetData(dataCopy);
					responseMessage->SetTransactionID(messageReceived->GetTransactionID());
				}
				else
					responseMessage.reset();
			}
		}

		if (!responseMessage)
		{
			resultStream << PayloadProtocol::LRIGenericNak(messageType, nakSummary);

			responseMessage.reset(
					new PayloadMessage(PayloadMessage::LRI_NAK,
							MainTaskAddress(),
							messageReceived->GetSourceAddress(),
							messageReceived->GetTransactionID(),
							resultStream.AsBytes()));
		}

		break;
	}

	case PayloadMessage::ACK_REQUESTED_PACKET:
	{
		Logger::Locked(logger)->Info << "Received Ack to Requested Packet" << endl;
		OnACKRequestedPacketReceived(messageReceived);
		break;
	}

	case PayloadMessage::SEND_FWD_ICD_CMD:

		responseMessage = OnICDMessageReceived(messageReceived);
		break;

	case PayloadMessage::LRI_NAK:

		Logger::Locked(logger)->Warning << "Unexpectedly received LRI_NAK" << endl;
		break;

	case PayloadMessage::LRI_ACK:

		Logger::Locked(logger)->Warning << "Unexpectedly received LRI_ACK" << endl;
		break;

	case PayloadMessage::DATA_LINK_NAK:

		Logger::Locked(logger)->Warning << "Unexpectedly received DATA_LINK_NAK" << endl;
		break;

	case PayloadMessage::DATA_LINK_ACK:

		Logger::Locked(logger)->Warning << "Unexpectedly received DATA_LINK_ACK" << endl;
		break;

	default:
	{
		resultStream << PayloadProtocol::LRIGenericNak(messageType, PayloadProtocol::NAKSUMMARY_UNKNOWN_COMMAND);

		responseMessage.reset(
				new PayloadMessage(PayloadMessage::LRI_NAK,
						MainTaskAddress(),
						messageReceived->GetSourceAddress(),
						messageReceived->GetTransactionID(),
						resultStream.AsBytes()));

		break;
	}

	}

	return responseMessage;
}



// This method is called when a requested packet is acked or naked.
void PayloadPortSlave::OnACKRequestedPacketReceived(PayloadMessage::Ptr messageReceived)
{
	Logger::Locked(logger)->Info << "Received Ack to Requested Packet: " << *messageReceived << endl;
}

// This method is called when a telemetry packet is received
void PayloadPortSlave::OnTelemetryReceived(PayloadMessage::Ptr messageReceived)
{
	Logger::Locked(logger)->Info << "Received Telemetry: " << *messageReceived << endl;
}

// This method is called whenever an fowarded or copied ICD message is received; the receiver should fill out
// either the ack or the nak response
PayloadMessage::Ptr PayloadPortSlave::OnICDMessageReceived(PayloadMessage::Ptr messageReceived)
{
	PayloadProtocol::SEND_FWD_ICD_MESSAGE_HEADER sendHeader;
	const ByteArray& rxPayload = messageReceived->GetData();
	PPSByteStream ibs0(rxPayload);
	PPSByteStream bsLevel1;
	bool noResponse = false;
	bool acked = false;
	uint8_t nakSummary = PayloadProtocol::NAKSUMMARY_INVALID_DATA;
	uint8_t nakDisp = NAKDISP_DATA;
	uint8_t level2ResponseType = NAK_DATA_ENUMERATED;
	PayloadMessage::Ptr result;

	ibs0 >> sendHeader;

	if (sendHeader.cmdFormat == PayloadProtocol::CF_SHIP_SHORE_ICD)
	{
		Level2Wrapper receivedLevel2;
		ibs0 >> receivedLevel2;

		const uint16_t calculatedCRC = receivedLevel2.CalculateCRC();

		if ((receivedLevel2.crcInclusion == Level2Wrapper::WITH_CRC) && (calculatedCRC != receivedLevel2.crc))
		{
			stringstream msg;
			msg << "CRC in level 2 message body (0x"
			    << utos(receivedLevel2.crc,16,4)
			    << ") does not match calculated value (0x"
			    << utos(calculatedCRC,16,4) << ")";

			throw runtime_error(msg.str());
		}

		if (receivedLevel2.commHeader.size != receivedLevel2.level1Data.size())
		{
			stringstream msg;
			msg << "Level 2 message size field ("
			    << receivedLevel2.commHeader.size
			    << ") does not match the size of the level 1 payload ("
			    << receivedLevel2.level1Data.size()
			    << ")";

			throw runtime_error(msg.str());
		}

		PPSByteStream ibs(receivedLevel2.level1Data);
		CMD_FMT_COMMON cmdFormat;
		ibs >> cmdFormat;

		//Logger::Locked(logger)->Info << "Received header: " << receivedLevel2 << endl;

		if (receivedLevel2.commHeader.type == RCV_CMD)
		switch(cmdFormat.fmtID)
		{
		//case FMT_RCV_CMD:
		//case FMT_RCV_SETTINGS:
		//case FMT_RCV_CRS_LST:
		//case FMT_RCV_SET_PARAMS:
		//case FMT_RCV_STEER:

		case FMT_RCV_INIT_FILE_WRITE:
		{
			if (cmdFormat.command == CMD_INIT_FILE_WRITE)
			{
				FCCMessage::CMD_FMT_18 receivedParams;
				ibs >> receivedParams;

				ByteArray fileData;
				ContainerIn<ByteArray> container(fileData, receivedParams.numDataBytes);
				ibs >> container;
				ibs.VerifyExhausted("CMD_FMT_18");

				if ((strnlen(receivedParams.fileName, sizeof(receivedParams.fileName)) != sizeof(receivedParams.fileName))
				 || (receivedParams.fileID == INVALID_FILE_ID)
				 || (receivedParams.fileSize >= 100*1024*1024)
				 || (receivedParams.fileAttributes & ~FTATTR_READ_ONLY))
				{
					nakDisp = NAKDISP_DATA;
					nakSummary = PayloadProtocol::NAKSUMMARY_INVALID_DATA;
				}
				else
				{
					try
					{
						OnInitFileWrite(messageReceived->GetDestinationAddress(), receivedLevel2, receivedParams, fileData, acked, nakDisp, nakSummary);
					}
					catch(runtime_error& exc)
					{
						acked = false;
						nakDisp = NAKDISP_DATA;
						nakSummary = PayloadProtocol::NAKSUMMARY_INVALID_DATA;
					}
				}
			}
			else
				nakSummary = PayloadProtocol::NAKSUMMARY_UNKNOWN_COMMAND;

			break;
		}

		case FMT_RCV_WRITE_FILE_BLK:
		{
			if (cmdFormat.command == CMD_WRITE_FILE_BLOCK)
			{
				FCCMessage::CMD_FMT_19 receivedParams;
				ibs >> receivedParams;

				ByteArray fileData;
				ContainerIn<ByteArray> container(fileData, receivedParams.numDataBytes);
				ibs >> container;
				ibs.VerifyExhausted("CMD_FMT_19");

				if (receivedParams.fileID == INVALID_FILE_ID)
				{
					nakDisp = NAKDISP_DATA;
					nakSummary = PayloadProtocol::NAKSUMMARY_INVALID_DATA;
				}
				else
				{
					try
					{
						OnWriteFileBlock(messageReceived->GetDestinationAddress(), receivedLevel2, receivedParams, fileData, acked, nakDisp, nakSummary);
					}
					catch(runtime_error& exc)
					{
						acked = false;
						nakDisp = NAKDISP_DATA;
						nakSummary = PayloadProtocol::NAKSUMMARY_INVALID_DATA;
					}
				}
			}
			else
				nakSummary = PayloadProtocol::NAKSUMMARY_UNKNOWN_COMMAND;

			break;
		}

		case FMT_RCV_DELETE_FILE:
		{
			if (cmdFormat.command == CMD_DELETE_FILE)
			{
				FCCMessage::CMD_FMT_1A receivedParams;
				ibs >> receivedParams;
				ibs.VerifyExhausted("CMD_FMT_1A");

				OnDeleteFile(messageReceived->GetDestinationAddress(), receivedLevel2, receivedParams, acked, nakDisp, nakSummary);
			}
			else
				nakSummary = PayloadProtocol::NAKSUMMARY_UNKNOWN_COMMAND;

			break;
		}

		case FMT_RCV_READ_FILE_REQ:
		{
			if (cmdFormat.command == CMD_READ_FILE_REQUEST)
			{
				FCCMessage::CMD_FMT_1B receivedParams;
				ibs >> receivedParams;
				ibs.VerifyExhausted("CMD_FMT_1B");

				boost::shared_ptr<ByteArray> data;
				bool eof = false;

				OnReadFileRequest(messageReceived->GetDestinationAddress(), receivedLevel2, receivedParams, data, eof, nakDisp, nakSummary);
				acked = data;

				if (acked)
				{
					level2ResponseType = ACK_WITH_READ_FILE;

					ACK_READ_FILE ack;
					ack.storageControllerAddress = receivedParams.storageControllerAddress;
					ack.seekOffset = receivedParams.offset;
					ack.eofFlag = eof ? 0xFF : 0;
					ack.numDataBytes = data->size();

					bsLevel1 << ack
					         << ContainerOut<ByteArray>(*data);
				}
			}
			else
				nakSummary = PayloadProtocol::NAKSUMMARY_UNKNOWN_COMMAND;

			break;
		}

		case FMT_RCV_GET_FILE_LIST_D:
		{
			if (cmdFormat.command == CMD_GET_DETAILED_FILE_LIST)
			{
				FCCMessage::CMD_FMT_1C receivedParams;
				ibs >> receivedParams;
				ibs.VerifyExhausted("CMD_FMT_1C");

				ACK_FILE_LIST_COMMON_HEADER ackFileListHeader;
				ACK_FILE_LIST_DETAILED_RECORD_ARRAY ackFileListData;

				OnGetDetailedFileList(messageReceived->GetDestinationAddress(), receivedLevel2, receivedParams,
						ackFileListHeader, ackFileListData, acked, nakDisp, nakSummary);

				if (acked)
				{
					level2ResponseType = ACK_WITH_LIST_DETAILED;

					bsLevel1 << ackFileListHeader
					         << ContainerOut<ACK_FILE_LIST_DETAILED_RECORD_ARRAY>(ackFileListData);
				}
			}
			else
				nakSummary = PayloadProtocol::NAKSUMMARY_UNKNOWN_COMMAND;

			break;
		}

		case FMT_RCV_GET_FILE_LIST:
		{
			if (cmdFormat.command == CMD_GET_FILE_LIST)
			{
				FCCMessage::CMD_FMT_1C receivedParams;
				ibs >> receivedParams;
				ibs.VerifyExhausted("CMD_FMT_1C");

				ACK_FILE_LIST_COMMON_HEADER ackFileListHeader;
				ACK_FILE_LIST_RECORD_ARRAY ackFileListData;

				OnGetFileList(messageReceived->GetDestinationAddress(), receivedLevel2, receivedParams,
						ackFileListHeader, ackFileListData, acked, nakDisp, nakSummary);

				if (acked)
				{
					level2ResponseType = ACK_WITH_FILE_LIST;

					bsLevel1 << ackFileListHeader
					         << ContainerOut<ACK_FILE_LIST_RECORD_ARRAY>(ackFileListData);
				}
			}
			else
				nakSummary = PayloadProtocol::NAKSUMMARY_UNKNOWN_COMMAND;

			break;
		}

		case FMT_RCV_CONSOLE_INPUT:
		{
			level2ResponseType = NACK_CONSOLE_DATA;

			if ((cmdFormat.command == CMD_WRITE_CNSL_CMD_ASCII)
			 || (cmdFormat.command == CMD_WRITE_CNSL_CMD_BINARY))
			{
				nakDisp = CONSOLE_RESPONSE_CMD_FAILED;

				FCCMessage::CMD_FMT_1E receivedParams;
				ibs >> receivedParams;

				ByteArray dataBytes;
				ContainerIn<ByteArray> container(dataBytes, receivedParams.dataLength);
				ibs >> container;

				ibs.VerifyExhausted("CMD_FMT_1E");

				if (receivedParams.dest != messageReceived->GetDestinationAddress())
					nakDisp = CONSOLE_RESPONSE_WRONG_DEVICE;
				else if (!receivedParams.dataLength)
					nakDisp = CONSOLE_RESPONSE_EMPTY_CMD;
				else
				{
					const bool asciiFormat = (cmdFormat.command == CMD_WRITE_CNSL_CMD_ASCII);
					const bool wantsAck = !messageReceived->GetNoAckHaveMessagesFlag();

					try
					{
						OnConsoleMessage(messageReceived->GetSourceAddress(), messageReceived->GetDestinationAddress(),
								receivedLevel2, receivedParams, dataBytes, asciiFormat, acked, nakDisp, nakSummary, wantsAck);
						noResponse = !wantsAck;
					}
					catch(...)
					{
						nakDisp = CONSOLE_RESPONSE_CMD_FAILED;
					}
				}
			}
			else
			{
				nakDisp = CONSOLE_RESPONSE_UNKNOWN_CMD;
				nakSummary = PayloadProtocol::NAKSUMMARY_UNKNOWN_COMMAND;
			}

			break;
		}

		default:
		{
			nakDisp = CONSOLE_RESPONSE_UNKNOWN_CMD;
			nakSummary = PayloadProtocol::NAKSUMMARY_UNKNOWN_COMMAND;
			break;
		}

		}

		if (!noResponse)
		{
			if (!acked)
			{
				NAK_DISPOSITIONS nakInfo;
				nakInfo.countDispositions = 1;
				nakInfo.disposition[0] = nakDisp;

				PPSByteStream bs1;
				bs1 << nakInfo.countDispositions
					<< nakInfo.disposition[0];

				PPSByteStream bs2;
				bs2 << Level2Wrapper(level2ResponseType, receivedLevel2.commHeader.packetID, bsLevel1.AsBytes());

				PPSByteStream bs;
				bs << PayloadProtocol::LRIGenericNak(bs2.AsBytes(), messageReceived->GetMessageType(), nakSummary);

				result.reset(new PayloadMessage(
						PayloadMessage::LRI_NAK,
						messageReceived->GetDestinationAddress(),
						messageReceived->GetSourceAddress(),
						messageReceived->GetTransactionID(),
						bs.AsBytes()));
			}
			else
			{
				const ByteArray& resultData = bsLevel1.AsBytes();

				if (!resultData.size())
					level2ResponseType = ACK_BASIC;

				const bool haveMessages = HaveMessagesQueued(messageReceived->GetDestinationAddress());

				if (logger.LogEnabled(Logger::LT_DEBUG) || haveMessages)
					Logger::Locked(logger)->Info << "Have pending messages = " << (haveMessages ? "TRUE" : "false") << endl;

				PPSByteStream bs;
				bs << PayloadProtocol::LRIGenericAck(messageReceived->GetMessageType(), haveMessages)
				   << Level2Wrapper(level2ResponseType, receivedLevel2.commHeader.packetID, bsLevel1.AsBytes());

				result.reset(new PayloadMessage(
						PayloadMessage::LRI_ACK,
						messageReceived->GetDestinationAddress(),
						messageReceived->GetSourceAddress(),
						messageReceived->GetTransactionID(),
						bs.AsBytes()));
			}
		}
	}

	return result;
}

// This method is called whenever an ASCII- or binary-formatted console command is received
void PayloadPortSlave::OnConsoleMessage(
		BoardAddress source,
		BoardAddress target,
		const Level2Wrapper& receivedLevel2,
		const FCCMessage::CMD_FMT_1E& consoleData,
		const ByteArray& dataBytes,
		bool asciiFormat,
		bool& acked,
		uint8_t& nakDisp,
		uint8_t& nakSummary,
		bool wantsAck)
{
	Logger::Locked(logger)->Warning << "WARNING: no handler available for OnConsoleMessage" << endl;
	acked = false;
	nakSummary = PayloadProtocol::NAKSUMMARY_INVALID_DATA;
	nakDisp = NAKDISP_DATA;
}

// This method is called whenever an Initialize File Write message is sent to the payload
void PayloadPortSlave::OnInitFileWrite(
		BoardAddress target,
		const Level2Wrapper& receivedLevel2,
		const FCCMessage::CMD_FMT_18& params,
		const ByteArray& paramData,
		bool& acked,
		uint8_t& nakDisp,
		uint8_t& nakSummary)
{
	Logger::Locked(logger)->Warning << "WARNING: no handler available for OnInitFileWrite" << endl;
	acked = false;
	nakSummary = PayloadProtocol::NAKSUMMARY_INVALID_DATA;
	nakDisp = NAKDISP_DATA;
}

// This method is called whenever a Write File Block message is sent to the payload
void PayloadPortSlave::OnWriteFileBlock(
		BoardAddress target,
		const Level2Wrapper& receivedLevel2,
		const FCCMessage::CMD_FMT_19& params,
		const ByteArray& paramData,
		bool& acked,
		uint8_t & nakDisp,
		uint8_t& nakSummary)
{
}

// This method is called whenever a Delete File message is sent to the payload
void PayloadPortSlave::OnDeleteFile(
		BoardAddress target,
		const Level2Wrapper& receivedLevel2,
		const FCCMessage::CMD_FMT_1A& params,
		bool& acked,
		uint8_t& nakDisp,
		uint8_t& nakSummary)
{
	Logger::Locked(logger)->Warning << "WARNING: no handler available for OnDeleteFile" << endl;
	acked = false;
	nakSummary = PayloadProtocol::NAKSUMMARY_INVALID_DATA;
	nakDisp = NAKDISP_DATA;
}

// This method is called whenever a Read File Request message is sent to the payload
void PayloadPortSlave::OnReadFileRequest(
		BoardAddress target,
		const Level2Wrapper& receivedLevel2,
		const FCCMessage::CMD_FMT_1B& params,
		boost::shared_ptr<ByteArray>& data,
		bool& eof,
		uint8_t& nakDisp,
		uint8_t& nakSummary)
{
	Logger::Locked(logger)->Warning << "WARNING: no handler available for OnReadFileRequest" << endl;
	nakSummary = PayloadProtocol::NAKSUMMARY_INVALID_DATA;
	nakDisp = NAKDISP_DATA;
}

// This method is called whenever a Get File List message is sent to the payload
void PayloadPortSlave::OnGetFileList(
		BoardAddress target,
		const Level2Wrapper& receivedLevel2,
		const FCCMessage::CMD_FMT_1C& params,
		FCCMessage::ACK_FILE_LIST_COMMON_HEADER& ackFileListHeader,
		ACK_FILE_LIST_RECORD_ARRAY& ackFileListData,
		bool& acked,
		uint8_t& nakDisp,
		uint8_t& nakSummary)
{
	Logger::Locked(logger)->Warning << "WARNING: no handler available for OnGetFileList" << endl;
	acked = false;
	nakSummary = PayloadProtocol::NAKSUMMARY_INVALID_DATA;
	nakDisp = NAKDISP_DATA;
}

// This method is called whenever a Get Detailed File List message is sent to the payload
void PayloadPortSlave::OnGetDetailedFileList(
		BoardAddress target,
		const Level2Wrapper& receivedLevel2,
		const FCCMessage::CMD_FMT_1C& params,
		FCCMessage::ACK_FILE_LIST_COMMON_HEADER& ackFileListHeader,
		ACK_FILE_LIST_DETAILED_RECORD_ARRAY& ackFileListData,
		bool& acked,
		uint8_t& nakDisp,
		uint8_t& nakSummary)
{
	Logger::Locked(logger)->Warning << "WARNING: no handler available for OnGetDetailedFileList" << endl;
	acked = false;
	nakSummary = PayloadProtocol::NAKSUMMARY_INVALID_DATA;
	nakDisp = NAKDISP_DATA;
}

void PayloadPortSlave::OnRequestIDReceived(const PayloadProtocol::LRI_REQUEST_ID& idRequest, std::vector<PayloadProtocol::LRI_ID_PACKET>& responses)
{
	Logger::Locked(logger)->Warning << "WARNING: no handler available for OnRequestIDReceived" << endl;
}

void PayloadPortSlave::OnStatusRequest(
		const PayloadProtocol::LRI_REQUEST_PIB_STATUS& statusRequest,
		PayloadProtocol::LRI_PIB_STATUS& ack,
		bool& acked,
		PayloadProtocol::ENakSummary& nakSummary)
{
	Logger::Locked(logger)->Warning << "WARNING: using default handler OnStatusRequest" << endl;
	ack.version = 1;
	acked = true;
}

// This method is called whenever a power notification message is received; the user should fill out either
// the ack responses or the nak container
void PayloadPortSlave::OnPowerNotification(
		const PayloadProtocol::PYLD_POWER_NOTIFICATION& notification,
		PayloadProtocol::ACK_PYLD_POWER_NOTIFICATION_RECORD& ack,
		bool& acked,
		PayloadProtocol::ENakSummary& nakSummary)
{
	Logger::Locked(logger)->Warning << "WARNING: using default handler for OnPowerNotification" << endl;
	ack.address = MainTaskAddress();
	ack.boardID = GetBoardID(ack.address);
	ack.state = 1;
	ack.delayOff = 5;
	ack.delayOn = 0;
	acked = true;
}


// Attemps to pull the next message from the queue that matches the given board address (i.e. the board ID
// matches); returns an empty smart pointer upon failure
PayloadMessage::Ptr PayloadPortSlave::DequeueMessage(BoardAddress boardAddress)
{
	PayloadMessage::Ptr result;
	BoardID boardID = GetBoardID(boardAddress);

	if (boardID == 0xFF)
		boardID = GetBoardID(MainTaskAddress());

	msgQByBoardID.TryDequeue(boardID, result);
	return result;
}

// Enqueues a message with a wrapper
void PayloadPortSlave::EnqueueMessage(MessageWrapper::Ptr wrapper)
{
	msgQByBoardID.Enqueue(GetBoardID(wrapper->message->GetSourceAddress()), wrapper);
	wrapper->OnEnqueue();
}

// Enqueues a message to be transmitted when polled
void PayloadPortSlave::EnqueueMessage(PayloadMessage::Ptr message)
{
	MessageWrapper::Ptr wrapper;
	wrapper.reset(new MessageWrapper(message));
	EnqueueMessage(wrapper);
}

// Flushes the queue of messages pending transmission that match the pattern
void PayloadPortSlave::FlushMessageQueue(BoardAddress boardAddress)
{
	BoardID boardID = GetBoardID(boardAddress);

	if (boardID == 0xFF)
		boardID = GetBoardID(MainTaskAddress());

	msgQByBoardID.Flush(boardID);
}

// Returns true if there are messages queued to be read out by the Host, false if there are none; this method
// is used to set the "have messages" flag in all responses
bool PayloadPortSlave::HaveMessagesQueued(BoardAddress destinationAddress) const
{
	BoardID boardID = GetBoardID(destinationAddress);

	if (boardID == 0xFF)
		boardID = GetBoardID(MainTaskAddress());

	return !msgQByBoardID.Empty(boardID);
}

bool PayloadPortSlave::SendNaksForAddress(PayloadMessage::BoardAddress destinationAddress) const
{
	return GetBoardID(destinationAddress) != GetBoardID(MainTaskAddress());
}

PayloadMessage::BoardAddress PayloadPortSlave::MainTaskAddress() const
{
	return 0x100;
}

// This method should return true unless intending to exit
bool PayloadPortSlave::Running() const
{
	return true;
}

// Enqueues a debug message
void PayloadPortSlave::EnqueueDebugMessage(const string& message, uint8_t debugLevel)
{
    if (message.length())
    {
        const size_t maxLen = 200;
        const size_t len0 = message.length();
        const size_t len = min(maxLen, len0);
        string addition;
        string temp;

        if (maxLen < len0)
        {
            stringstream stm;
            stm << " (message truncated from " << len0 << " bytes)";
            addition = stm.str();
            temp = message.substr(0,maxLen);
        }
        else
        	temp = message;

        Logger::Locked(logger)->Info << "Enqueuing debug message of " << len << " bytes" << addition << endl;

		PPSByteStream bs;

		bs << PayloadProtocol::LRIGenericAck(0, false)
		   << XBeeDataRequestWrapper(temp, debugLevel);

        PayloadMessage::Ptr payloadMessage;
        PayloadMessage::BoardAddress sourceAddress = MainTaskAddress();

        payloadMessage.reset(new PayloadMessage(PayloadMessage::LRI_ACK, sourceAddress, FLOAT_TARGET_DEBUG_INTERFACE, 0, bs.AsBytes()));
        EnqueueMessage(payloadMessage);
    }
    else
        Logger::Locked(logger)->Warning << "Not enqueuing debug message of 0 length" << endl;
}

}
}
