//------------------------------------------------------------------------------
// <copyright file="PayloadInterfaceController.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>
// 	Implementation of PayloadInterfaceController, which extends PayloadPortSlave
//  to implement the payload interface for the SMC
// </summary>
//
// <owner>Mike Cookson</owner>
//------------------------------------------------------------------------------

#include <iostream>
#include <sstream>
#include <vector>
#include <errno.h>
#include <fcntl.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <time.h>
#include <unistd.h>
#include <boost/foreach.hpp>
#include <boost/function.hpp>
#include <boost/bind.hpp>
#include "PayloadInterfaceController.h"
#include "AddressedPacketEvent.h"
#include "DataSetEvent.h"
#include "GPSSampleEvent.h"
#include "FCCMessageEvent.h"
#include "SendCommandCompleteEvent.h"
#include "FCCMessage_CommHeaderMessage.h"
#include "FCCMessage_Utilities.h"
#include "PayloadProtocol.h"
#include "LqrSMACAsynchronousProcessExecutor.h"
#include "LqrSMACBooleanParser.h"
#include "LqrSMACByteArray.h"
#include "LqrSMACByteStream.h"
#include "LqrSMACCmdLineParser.h"
#include "LqrSMACDetermineFileExtension.h"
#include "LqrSMACGetErrorDescription.h"
#include "LqrSMACLexicalCast.h"
#include "LqrSMACIObjectBroker-inline.h"
#include "LqrSMACPrintfBuffer.h"
#include "LqrSMACTempFile.h"
#include "LqrSMACTimeCounter.h"
#include "nsextend.h"
#include "LqrSMACIdl.h"
#include "LqrSMACZeroObject.h"

using namespace std;
using namespace LiquidRobotics::EventLib;
using namespace LiquidRobotics::PayloadInterface::FCCMessage;
using namespace LiquidRobotics::SMACFramework;
using namespace LiquidRobotics::SMAC;
using namespace CORBA;

namespace LiquidRobotics {
namespace PayloadInterface {

typedef vector<string> StringArray;

inline string DescribeError(int errNum)
{
	stringstream stm;

	stm << "error #" << errNum << " (" << GetErrorDescription(errNum) << ")";
	return stm.str();
}

template<typename T>
void GetHostFormattedFileDetails_WithoutFileID(const StorageController::FileState& source, T& dest)
{
	memcpy(dest.fileName, Unstringize(source.GetReportedFileName().c_str()).paddedFileName, sizeof(dest.fileName));
	dest.fileSize = source.GetFileSize();
	dest.fileAttributes =
		(source.GetReadOnly() ? FTATTR_READ_ONLY : 0)
		 || (source.GetFileChecked() ? FTATTR_CHECKED : 0);
}

template<typename T>
void GetHostFormattedFileDetails(const StorageController::FileState& source, T& dest)
{
	GetHostFormattedFileDetails_WithoutFileID(source, dest);
	dest.fileID = source.GetFileID();
}

template<typename T>
void GetHostFormattedAdditionalDetails(const StorageController::FileState& source, T& dest)
{
	Convert(source.GetCreationTime(), dest.creationDate);
	Convert(source.GetModifiedTime(), dest.modifiedDate);

	try
	{
		dest.providedCRC = source.GetCRC().Get();
	}
	catch(StorageController::FileState::FileCheckValuesNotEstablished& checkValueNotSetError)
	{
		dest.providedCRC = CRC16().Get();
	}

	try
	{
		memcpy(&dest.providedMD5, &source.GetMD5().GetBytes().at(0), sizeof(dest.providedMD5));
	}
	catch(StorageController::FileState::FileCheckValuesNotEstablished& checkValueNotSetError)
	{
		memcpy(&dest.providedMD5, &MD5::Entity().GetBytes().at(0), sizeof(dest.providedMD5));
	}
}

template<typename T>
void GetHostFormattedCalculations(const StorageController::FileState& source, T& dest)
{
	if (source.GetFileChecked())
	{
		dest.calculatedCRC = source.GetCalculatedCRC().Get();
		memcpy(&dest.calculatedMD5, &source.GetCalculatedMD5().GetBytes().at(0), sizeof(dest.calculatedMD5));
	}
	else
	{
		ZeroObject(dest.calculatedCRC);
		ZeroObject(dest.calculatedMD5);
	}
}

SHARED_LIB_PUBLIC PayloadInterfaceController::ReceivedShutdownRequest::ReceivedShutdownRequest()
: runtime_error("Received shutdown request")
{
}

PayloadInterfaceController::PayloadInterfaceController(
		const SerialPortSettings& _portSettings,
		const StorageController::Config& _scSettings,
		const PayloadInterfaceController_Config& _config,
		IObjectBroker& _broker,
		LiquidRobotics::SMACFramework::Logger& _logger,
		const string& _telemConfigPath)

: EventTransceiver(_config.GetEventTransceiverSettings(), _broker, _logger)
//, LocalTransactionCounter
, PayloadManagerClient(_config.GetPayloadManagerClientSettings(), _broker, *this)
, SCLookup(_broker, *this, _logger, _scSettings, _config, *this)
, CommonSendCommand(_broker, *this, _logger, *this, _config, *this)
, PayloadPortSlave(_portSettings, _logger)
, TelemetrySubsystem(_telemConfigPath, *this, _logger, _config, _broker, *this)
//, sc(_scSettings)
, config(_config)
, scSettings(_scSettings)
, broker(_broker)
, halt(false)
, shutdownCompleted(false)
, enqueueStartupTelem(true)
, haveSavedPosition(false)
, savedLatitude(0)
, savedLongitude(0)
, lastGPSEventReceived(0)
{
	const PayloadInterfaceController_Config::SetParamInfoList& spil = config.GetSetParamInfoList();

	for (PayloadInterfaceController_Config::SetParamInfoList::const_iterator ci = spil.begin(); ci != spil.end(); ++ci)
	{
		idToSPIM[ci->paramID] = &(*ci);

		for (PayloadInterfaceController_Config::StringList::const_iterator sci = ci->paramNames.begin(); sci != ci->paramNames.end(); ++sci)
		{
			string temp = *sci;
			boost::to_upper(temp);
			nameToSPIM[temp] = &(*ci);
		}
	}
}

PayloadInterfaceController::~PayloadInterfaceController()
{
}

void PayloadInterfaceController::Initialize(Object_ptr serverRef)
{
	// configure EventTransceiver to create the appropriate derived class for GPS events
	const EventSettings& gpsSampleEvent = config.GetGpsSampleEventSettings();
	RegisterEventClassFactory(gpsSampleEvent.GetType(), new EventTransceiver::TEventClassFactory<GPSSampleEvent>);

	// subscribe to GPS sample events
	InformSignalType& sig1 = SignalForEventName(gpsSampleEvent.GetName());
	connectionToGpsSampleEvent.reset(
			new ScopedConnection(
					sig1.connect(
							boost::bind(&PayloadInterfaceController::GPSReceiveHandler, this, _1, _2)
							)));

	// configure EventTransceiver to create the appropriate derived class for addressable messages
	const EventSettings& addressableMessageEvent = config.GetAddressedMessageEventSettings();
	RegisterEventClassFactory(addressableMessageEvent.GetType(), new EventTransceiver::TEventClassFactory<AddressedPacketEvent>);
	InformSignalType& sig3 = SignalForEventType(addressableMessageEvent.GetType());
	connectionToAddressableMessageEvent.reset(
			new ScopedConnection(
					sig3.connect(
							boost::bind(&PayloadInterfaceController::AddressableMessageHandler, this, _1, _2)
							)));

	// subscribe to FCC Message events
	const EventSettings& FCCIncomingMessageEvent = config.GetFCCIncomingMessageEventSettings();

	InformSignalType& sig4 = SignalForEventName(FCCIncomingMessageEvent.GetName());
	connectionToFCCIncomingMessageEvent.reset(
			new ScopedConnection(
					sig4.connect(
							boost::bind(&PayloadInterfaceController::FCCIncomingMessageEventHandler, this, _1, _2)
							)));

	PayloadManagerClient::Register(serverRef);
	SCLookup::Initialize();
	TelemetrySubsystem::Initialize();
	CommonSendCommand::Initialize();
	EventTransceiver::Initialize(serverRef);
}

void PayloadInterfaceController::FCCIncomingMessageEventHandler(EventTransceiver& sender, const IEventBase& receivedEvent)
{
	Logger::Locked(GetLogger())->Info << "FCC Message Event: " << receivedEvent << endl;
	receivedEvent.Describe(Logger::Locked(GetLogger())->Info);
	Logger::Locked(GetLogger())->Info << endl;

	const FCCMessageEvent* evtPtr;

	try
	{
		evtPtr = static_cast<const FCCMessageEvent*>(&receivedEvent);
	}
	catch(exception& ex)
	{
		Logger::Locked(GetLogger())->Error << "Error making event pointer" << endl;
		Logger::Locked(GetLogger())->Error << "Caught exception " << typeid(ex).name() << " '" << ex.what() << "' while processing event FCC Message Event" << endl;
	}

	if (evtPtr)
	{
		try
		{
			EventPropertyValue::UInt8VectorPtr byteMsg = evtPtr->GetMessage();
			PPSByteStream bs;

			bs << PayloadProtocol::LRIGenericAck(0, false)
			   << ContainerOut<EventPropertyValue::UInt8Vector>(*byteMsg);

			PayloadMessage::Ptr payloadMessage;
			PayloadMessage::BoardAddress sourceAddress = evtPtr->GetSource();
			PayloadMessage::BoardAddress destinationAddress = evtPtr->HasDestination() ? evtPtr->GetDestination() : FCCMessage::FLOAT_TARGET_COMMAND_INTERFACE;

			payloadMessage.reset(new PayloadMessage(PayloadMessage::LRI_ACK, sourceAddress, destinationAddress, 0, bs.AsBytes()));

			Logger::Locked(GetLogger())->Info << "Enqueuing FCC Message" << endl;
			EnqueueMessage(payloadMessage);
		}
		catch(exception& ex)
		{
			Logger::Locked(GetLogger())->Error << "Error when handling FCCMessageEvent" << endl;
			Logger::Locked(GetLogger())->Error << "Caught exception " << typeid(ex).name() << " '" << ex.what() << "' while processing event FCC Message Event" << endl;
		}
	}
	else
	{
		Logger::Locked(GetLogger())->Info << "Event Pointer for FCCMessageEvent is null." << endl;
	}

}

void PayloadInterfaceController::GPSReceiveHandler(EventTransceiver& sender, const IEventBase& receivedEvent)
{
	const EventSettings& gpsSampleEvent = config.GetGpsSampleEventSettings();
	const GPSSampleEvent& evt =
		EventTransceiver::UpcastEventRef<GPSSampleEvent>(receivedEvent, gpsSampleEvent,
				"PayloadInterfaceController::GPSReceiveHandler");

	stringstream ss;

	ss << receivedEvent.GetEventSettings() << ": ";

	if (evt.HasGPS())
	{
		double latitude, longitude;

		if (evt.GetSourceName().length())
			ss << "[" << evt.GetSourceName() << "] ";
		evt.GetGPS(latitude, longitude);
		ss << "latitude=" << setprecision(9) << latitude
		   << " longitude=" << setprecision(10) << longitude
		   << " numSatellites=" << evt.GetNumSatellites();
		haveSavedPosition = true;
		savedLatitude = latitude;
		savedLongitude = longitude;
		GetSystemUpTimeSeconds(lastGPSEventReceived);
		TelemetrySubsystem::SavePosition(latitude, longitude);
		CommonSendCommand::SavePosition(latitude, longitude);
	}
	else
		ss << "no gps data";

	Logger::Locked(GetLogger())->Info << ss.str() << endl;
}

void PayloadInterfaceController::TelemetryReportComplete(TelemetryBuffer& sender, TelemetryReportKey reportKey, TelemetryBuffer::BoardAddress targetAddress, const ByteArray& content)
{
	Logger::Locked(GetLogger())->Info << "TelemetryReportComplete for " << reportKey << " target 0x" << utos(targetAddress, 16, 4) << ": " << content << endl;
	Logger::Locked(GetLogger())->Debug << "  telemetry content =" << endl << content.ToStringSplitView() << endl;

	PPSByteStream bs;

	bs << PayloadProtocol::LRIGenericAck(0, false)
	   << ContainerOut<const ByteArray>(content);

	const ByteArray& ackData = bs.AsBytes();

	Logger::Locked(GetLogger())->Debug << "  telemetry ack content =" << endl << ackData.ToStringSplitView() << endl;

	PayloadMessage::Ptr payloadMessage;
	PayloadMessage::BoardAddress sourceAddress = reportKey.GetPublisherKey().GetBoardAddress();

	payloadMessage.reset(new PayloadMessage(PayloadMessage::LRI_ACK, sourceAddress, targetAddress, 0, ackData));

	Logger::Locked(GetLogger())->Info << "Enqueuing telemetry message " << *payloadMessage << endl;
	EnqueueMessage(payloadMessage);
}

// IPingable operations
::CORBA::Double PayloadInterfaceController::Ping()
{
	using namespace LiquidRobotics::SMACFramework;
	stringstream msg;
	msg << "PayloadInterfaceController::Ping: ";

	::CORBA::Double result;
	timeval tv;
	int status = gettimeofday(&tv, NULL);
	int errSave = errno;

	if (!status)
	{
		result = tv.tv_usec;
		result /= 1000000.0;
		result += tv.tv_sec;
	}
	else
	{
		msg << "(gettimeofday returned error #" << errSave << " '" << GetErrorDescription(errSave) << "') ";
		result = 0;
	}

	Logger::Locked(GetLogger())->Info << msg.str() << setprecision(15) << result << endl;
	return result;
}

bool PayloadInterfaceController::HandleAdditionalCommands(const string& command, const string& requester, const string& transactionID, string& result)
{
	const char* cmdParam = "_command";
	bool handled = false;
	CmdLineParser parser(command);
	const int argCount = parser.Count();

	if (boost::iequals(parser[0], "SETPARAM"))
	{
		handled = true;
		if (argCount < 2)
		{
			stringstream msg;
			msg << "SETPARAM requires at least one argument";

			Logger::Locked(GetLogger())->Error << msg.str() << endl;
			ThrowCORBAException2<BadParameter>(cmdParam, msg.str());
		}

		ThrowIfEmptyOrNull("requester", requester.c_str());
		ThrowIfEmptyOrNull("transactionID", transactionID.c_str());
		PayloadMessage::Ptr outgoingMessage;
		const uint32_t transactionNumber = LocalTransactionCounter::Next();

		try
		{
			outgoingMessage = GetSetParamMessage(parser, 1, transactionNumber);
		}
		catch(exception& ex)
		{
			Logger::Locked(GetLogger())->Error << ex.what() << endl;
			ThrowCORBAException2<BadParameter>(cmdParam, ex.what());
		}

		Logger::Locked(GetLogger())->Info << "Enqueuing set parameter message with transaction 0x" << utos(transactionNumber,16,8) << ": " << *outgoingMessage << endl;
		EnqueueMessage(outgoingMessage);
		StringPtr requesterPtr;
		requesterPtr.reset(new string(requester));
		StringPtr transactionIDPtr;
		transactionIDPtr.reset(new string(transactionID));
		boost::thread thd(boost::bind(&PayloadInterfaceController::AwaitSetParamAck, this, transactionNumber, requesterPtr, transactionIDPtr, 2*60*1000));
		throw WillCompleteAsynchronously();
	}
	else if (boost::iequals(parser[0], "EXECFCC"))
	{
		handled = true;
		if (argCount != 2)
		{
			stringstream msg;
			msg << "EXECFCC requires one argument (the hex string for the ack message less the CRC)";

			Logger::Locked(GetLogger())->Error << msg.str() << endl;
			ThrowCORBAException2<BadParameter>(cmdParam, msg.str());
		}
		else if (!parser[1].length())
		{
			stringstream msg;
			msg << "EXECFCC argument is nil";

			Logger::Locked(GetLogger())->Error << msg.str() << endl;
			ThrowCORBAException2<BadParameter>(cmdParam, msg.str());
		}
		else if (!!(parser[1].length() & 1))
		{
			stringstream msg;
			msg << "EXECFCC expected an even-length string";

			Logger::Locked(GetLogger())->Error << msg.str() << endl;
			ThrowCORBAException2<BadParameter>(cmdParam, msg.str());
		}

		ThrowIfEmptyOrNull("requester", requester.c_str());
		ThrowIfEmptyOrNull("transactionID", transactionID.c_str());
		const uint32_t transactionNumber = LocalTransactionCounter::Next();

		PPSByteStream msgStream;
		msgStream << PayloadProtocol::LRIGenericAckWithFormat(0,false);

		PPSByteStream level2Stream;

		for (unsigned idx = 0; idx < parser[1].length(); idx += 2)
		{
			const string little = parser[1].substr(idx,2);
			char* endptr = NULL;
			const long val = strtol(little.c_str(), &endptr, 16);

			if (endptr && !!*endptr)
			{
				stringstream msg;
				msg << "EXECFCC hex string conversion error at " << idx;

				Logger::Locked(GetLogger())->Error << msg.str() << endl;
				ThrowCORBAException2<BadParameter>(cmdParam, msg.str());
			}

			level2Stream.Put((uint8_t) val);
		}

		const ByteArray& level2Data = level2Stream.AsBytes();
		CRC16 crc;
		crc.Add(level2Data);
		const uint16_t crcResult = crc.Get();

		msgStream << ContainerOut<const ByteArray>(level2Data)
				  << (uint16_t) crcResult;

		PayloadMessage::Ptr outgoingMessage;
		outgoingMessage.reset(new PayloadMessage(PayloadMessage::LRI_ACK, PayloadManagerClient::GetMyBoardAddress(), FLOAT_TARGET_COMMAND_INTERFACE, 0, msgStream.AsBytes()));

		Logger::Locked(GetLogger())->Info << "Enqueuing exec FCC message with transaction 0x" << utos(transactionNumber,16,8) << ": " << *outgoingMessage << endl;
		EnqueueMessage(outgoingMessage);
		StringPtr requesterPtr;
		requesterPtr.reset(new string(requester));
		StringPtr transactionIDPtr;
		transactionIDPtr.reset(new string(transactionID));
		boost::thread thd(boost::bind(&PayloadInterfaceController::AwaitExecFCC, this, transactionNumber, requesterPtr, transactionIDPtr, 2*60*1000));
		throw WillCompleteAsynchronously();
	}
	else if (boost::iequals(parser[0], "FLOATREBOOT"))
	{
		handled = true;
		if (argCount != 1)
		{
			stringstream msg;
			msg << "FLOATREBOOT does not take any parameters";

			Logger::Locked(GetLogger())->Error << msg.str() << endl;
			ThrowCORBAException2<BadParameter>(cmdParam, msg.str());
		}

		PayloadMessage::Ptr outgoingMessage;
		const uint32_t transactionNumber = LocalTransactionCounter::Next();

		try
		{
			outgoingMessage = GetGenericCommandMessage(RCV_CMD, transactionNumber, FMT_RCV_CMD, CMD_FLOAT_REBOOT, 0, 0, 0, 0, 0, 0, 0);
		}
		catch(exception& ex)
		{
			Logger::Locked(GetLogger())->Error << ex.what() << endl;
			ThrowCORBAException2<BadParameter>(cmdParam, ex.what());
		}

		Logger::Locked(GetLogger())->Info << "Enqueuing reboot message with transaction 0x" << utos(transactionNumber,16,8) << ": " << *outgoingMessage << endl;
		EnqueueMessage(outgoingMessage);
		result = "OK";
	}
	else if (boost::iequals(parser[0], "LIGHTOFFTEST"))
	{
		handled = true;
		const uint32_t transactionNumber = LocalTransactionCounter::Next();
		PayloadMessage::Ptr payloadMessage = GetGenericCommandMessage(RCV_CMD, transactionNumber, FMT_RCV_CMD, CMD_LIGHT_OFF, 0, 0, 0, 0, 0, 0, 0);
		Logger::Locked(GetLogger())->Info << "Enqueuing message " << *payloadMessage << endl;
		EnqueueMessage(payloadMessage);
		result = "OK";
	}
	else if (boost::iequals(parser[0], "WCS"))
	{
		handled = true;

		if (argCount <= 2)
		{
			stringstream msg;
			msg << "Missing target address or command detail";
			ThrowCORBAException2<BadParameter>(cmdParam, msg.str());
		}

		const uint16_t boardAddress = LexicalCast<uint16_t>(parser[1]);
		const string tail = boost::trim_copy(parser.GetTail(1));
		const uint32_t transactionNumber = LocalTransactionCounter::Next();

		// set up the level 1 content
		PPSByteStream bsLevel1;
		const uint8_t fmtID = FMT_RCV_CONSOLE_INPUT;
		const uint16_t command = CMD_WRITE_CNSL_CMD_ASCII;
		CMD_FMT_1E wcs;
		wcs.dest = boardAddress;
		wcs.function = 0;
		wcs.dataLength = tail.length();
		bsLevel1 << fmtID << command << wcs << ContainerOut<const string>(tail);

		// form up the level 2 message in the payload protocol packet
		PPSByteStream bs;
		const uint16_t schema = PayloadProtocol::CF_SHIP_SHORE_ICD;
		bs << PayloadProtocol::LRIGenericAck(0, false)
		   << schema
		   << Level2Wrapper(RCV_CMD/*FMT_RCV_CONSOLE_INPUT*/, transactionNumber, bsLevel1.AsBytes());

		// form up the payload protocol packet
		PayloadMessage::Ptr payloadMessage;
		const PayloadMessage::BoardAddress sourceAddress = GetMyBoardAddress();
		const PayloadMessage::BoardAddress destinationAddress = FCCMessage::FLOAT_TARGET_COMMAND_INTERFACE;

		payloadMessage.reset(new PayloadMessage(PayloadMessage::LRI_ACK, sourceAddress, destinationAddress, 0, bs.AsBytes()));

		Logger::Locked(GetLogger())->Info << "Enqueuing FCC Message" << endl;
		EnqueueMessage(payloadMessage);

		StringPtr requesterPtr;
		requesterPtr.reset(new string(requester));
		StringPtr transactionIDPtr;
		transactionIDPtr.reset(new string(transactionID));
		boost::thread thd(boost::bind(&PayloadInterfaceController::AwaitWCS, this, transactionNumber, requesterPtr, transactionIDPtr, 5*60*1000));
		throw WillCompleteAsynchronously();
	}

	return handled;
}


// Overriding this handler to catch the exit from the program
void PayloadInterfaceController::Run(HDLCPort::Ptr pHDLC)
{
	PayloadPortSlave::Run(pHDLC);

	if (!shutdownCompleted)
		try 		{ HandleShutdown(); }
		catch(...) 	{ }

	broker.QueueShutdown();
}

PayloadMessage::Ptr PayloadInterfaceController::WaitForMessage(HDLCPort::Ptr pHDLC)
{
	if (halt)
		HandleShutdown();

	else if (!completedFiles.empty())
	{
		FileIDQueueMemberPtr memPtr = completedFiles.front();
		completedFiles.pop_front();

		if (memPtr->isFile)
			memPtr->proxySC->ExecuteFileCompletion(memPtr->fileID);
		else
			memPtr->proxySC->ExecuteCommand(memPtr->literalData, memPtr->transactionID, memPtr->destID, memPtr->ascii,
					memPtr->retFile, memPtr->wantsAck);
	}

	CheckForTelemetryPublicationTimeout();
	return Base::WaitForMessage(pHDLC);
}

// This method is called by the default OnMessageReceived implementation when an ID/enumeration request is
// received; the user should fill out the responses array for processing on return.
void PayloadInterfaceController::OnRequestIDReceived(const PayloadProtocol::LRI_REQUEST_ID& idRequest, std::vector<PayloadProtocol::LRI_ID_PACKET>& responses)
{
	EnqueueStartupTelemetryMessage(true);
	responses.resize(1);
	PayloadProtocol::LRI_ID_PACKET& response = responses.at(0);

	ZeroObject(response);
	response.address = PayloadManagerClient::GetMyBoardAddress();
	const string::size_type descriptionSize = sizeof(response.description);
	strncpy(response.description, config.GetDescription().c_str(), descriptionSize);
	response.description[descriptionSize-1] = '\0';
	response.deviceType = config.GetDeviceType();
	response.firmwareMajor = config.GetFirmwareMajor();
	response.firmwareMinor = config.GetFirmwareMinor();
	response.firmwareRev = config.GetFirmwareRev();
	response.pollingRate = config.GetPollingRate();
	response.port = 0;

	const string::size_type serNumBufSize = sizeof(response.serialNum);
	ZeroObject(response.serialNum, ' ');
	const string& serNum = config.GetSerialNum();
	const string::size_type serNumLen = serNum.length();
	memcpy(response.serialNum, serNum.c_str(), min(serNumLen, serNumBufSize));

	response.version = 1;

	if (config.GetTelemetryRequested())				response.extraInfo |= PayloadProtocol::EXTRA_INFO_TELEMETRY;
	if (config.GetPowerNotificationRequested())		response.extraInfo |= PayloadProtocol::EXTRA_INFO_POWER;
	if (config.GetEventsRequested())				response.extraInfo |= PayloadProtocol::EXTRA_INFO_EVENTS;
	if (config.GetEchoedAcksRequested())			response.extraInfo |= PayloadProtocol::EXTRA_INFO_ECHOED_ACKS;
}


void PayloadInterfaceController::DistributeGPS(const PayloadProtocol::LRI_REQUEST_PIB_STATUS& statusRequest, const LatLon& fix)
{
	unsigned int numSat = 3;
	tm timer;

	//Logger::Locked(GetLogger())->Info << "DistributeGPS" << endl;

	timer.tm_sec = statusRequest.timestamp.second;
	timer.tm_min = statusRequest.timestamp.minute;
	timer.tm_hour = statusRequest.timestamp.hour;
	timer.tm_mday = statusRequest.timestamp.day;
	timer.tm_mon = statusRequest.timestamp.month-1;
	timer.tm_year = (int) statusRequest.timestamp.year+100;
	timer.tm_wday = 0;
	timer.tm_yday = 0;
	timer.tm_isdst = -1;

	time_t current = mktime(&timer);
	timeval tv;
	tv.tv_sec = current;
	tv.tv_usec = 0;

	GPSSampleEvent gpsEvent(config.GetGpsSampleEventSettings(), broker);
	gpsEvent.SetTime(tv);
	//gpsEvent.SetGPS(statusRequest.latitude, statusRequest.longitude, numSat);
	gpsEvent.SetGPS(fix.GetLatitude().GetAsDouble(), fix.GetLongitude().GetAsDouble(), numSat);
	TransmitEvent(gpsEvent);
}


// This method is called by the default OnMessageReceived implementation when a status request message is
// received; the user should fill out either the Nak structure or the Ack strcture
void PayloadInterfaceController::OnStatusRequest(
		const PayloadProtocol::LRI_REQUEST_PIB_STATUS& statusRequest,
		PayloadProtocol::LRI_PIB_STATUS& ack,
		bool& acked,
		PayloadProtocol::ENakSummary& nakSummary)
{
	LatLon fix;
	const bool validFix = (statusRequest.fixValid == 'Y');

	if (validFix)
	{
		fix = LatLon(statusRequest.latitude, statusRequest.longitude);

		Logger::Locked(GetLogger())->Info
			 << "Valid fix at "
			 << PrintfBuffer("%.4u/%.2u/%.2u %.2u:%.2u:%.2u UTC",
					 2000 + (int) statusRequest.timestamp.year,
					 statusRequest.timestamp.month,
					 statusRequest.timestamp.day,
					 statusRequest.timestamp.hour,
					 statusRequest.timestamp.minute,
					 statusRequest.timestamp.second)
			 << " at " << fix << endl;

		// set the time and date (TODO: publish as event and transfer responsibility to another process)
		string cmd = config.GetDateVerbGMT(
				2000 + (int) statusRequest.timestamp.year,
				statusRequest.timestamp.month,
				statusRequest.timestamp.day,
				statusRequest.timestamp.hour,
				statusRequest.timestamp.minute,
				statusRequest.timestamp.second);

		if (cmd.length())
		{
			Logger::Locked(GetLogger())->Info << "cmd: " << cmd << endl;

			const int systemReturnCode = system(cmd.c_str());
			if (systemReturnCode);
		}

		if (config.GetDistributeGPS() || ((GetSystemUpTimeSecondsDouble()-lastGPSEventReceived) >= 60))
		{
			haveSavedPosition = true;
			savedLatitude = fix.GetLatitude().GetAsDouble();
			savedLongitude = fix.GetLongitude().GetAsDouble();
			TelemetrySubsystem::SavePosition(savedLatitude, savedLongitude);
			CommonSendCommand::SavePosition(savedLatitude, savedLongitude);
		}

		if (config.GetDistributeGPS())
			DistributeGPS(statusRequest, fix);
	}
	else
		Logger::Locked(GetLogger())->Info << "Incoming fix is not valid" << endl;


	// TODO: set position (publish event)

	// TODO: this is just something rigged for the demo, to be removed
	const StringList& demoServerList = config.GetDemoServerList();

	for (StringList::const_iterator it = demoServerList.begin(); it != demoServerList.end(); ++it)
	{
		const string& remoteConfig = *it;
		Logger::Locked(GetLogger())->Info << "Forwarding data to remote server '" << remoteConfig << "'" << endl;

		// for convenience, I'm assuming that the date was set, above
		time_t now = time(NULL);
		CORBA::Double lat = validFix ? fix.GetLatitude().GetAsDouble() : 0;
		CORBA::Double lon = validFix ? fix.GetLongitude().GetAsDouble() : 0;

		try
		{
			IDemoSensor_var sensorV;
			sensorV = broker.GetNamedInstance<IDemoSensor_var,IDemoSensor>(remoteConfig.c_str());

			sensorV->NoteGPSSample(now, validFix, lat, lon);
		}
		catch(CORBA::Exception& exc)
		{
			Logger::Locked(GetLogger())->Error << "Caught CORBA exception: " << DecodeCORBAException(exc) << endl;
		}
		catch(std::exception& exc)
		{
			Logger::Locked(GetLogger())->Error << "Caught exception type "<< typeid(exc).name() << endl;
		}
	}

	if (DeferredStartupTelem())
		EnqueueStartupTelemetryMessage(false);

	ZeroObject(ack);
	ack.version = 1;
	acked = true;
}

// 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 PayloadInterfaceController::HaveMessagesQueued(BoardAddress sourceAddress) const
{
	return Base::HaveMessagesQueued(sourceAddress);
}

bool PayloadInterfaceController::Running() const
{
	return !halt && !shutdownCompleted;
}

// Returns true if the address is one that should be Nak'd, false if the address is unknown
bool PayloadInterfaceController::SendNaksForAddress(BoardAddress destinationAddress) const
{
	return destinationAddress != 0xFFFF;
}

// Returns the board address of the main task
PayloadMessage::BoardAddress PayloadInterfaceController::MainTaskAddress() const
{
	return PayloadManagerClient::GetMyBoardAddress();
}

// This method is called whenever a message is received.  The return value contains the message to be sent
// in response (or send back a null pointer if no response should be made)
//
// Normally, duplicateHeader will be false; if duplicateHeader is true, the receiver should typically return
// a null pointer and the handler will retransmit the message that was previously sent.
PayloadMessage::Ptr PayloadInterfaceController::OnMessageReceived(PayloadMessage::Ptr messageReceived, bool duplicateHeader)
{
	return Base::OnMessageReceived(messageReceived, duplicateHeader);
}

void PayloadInterfaceController::OnACKRequestedPacketReceived(PayloadMessage::Ptr messageReceived)
{
	FCCMessageEvent ackEvent(config.GetFCCOutgoingMessageEventSettings(), broker);

	ByteArray ackByteArray;
	messageReceived->ToArray(ackByteArray);
	const EventPropertyValue::UInt8Vector ack(ackByteArray.begin(), ackByteArray.end());

	ackEvent.SetMessage(ack);
	ackEvent.SetSource(messageReceived->GetSourceAddress());
	ackEvent.SetDestination(messageReceived->GetDestinationAddress());

	TransmitEvent(ackEvent);

	if (messageReceived->GetDestinationAddress() == MainTaskAddress())
		PMQDistributeMessage(messageReceived);
}

void PayloadInterfaceController::OnTelemetryReceived(PayloadMessage::Ptr messageReceived)
{
	FCCMessageEvent telemetryEvent(config.GetFCCOutgoingMessageEventSettings(), broker);

	ByteArray telemetryByteArray;
	messageReceived->ToArray(telemetryByteArray);
	const EventPropertyValue::UInt8Vector telemetry(telemetryByteArray.begin(), telemetryByteArray.end());

	telemetryEvent.SetMessage(telemetry);
	telemetryEvent.SetSource(messageReceived->GetSourceAddress());
	telemetryEvent.SetDestination(messageReceived->GetDestinationAddress());

	TransmitEvent(telemetryEvent);
}

// This method is called whenever an ASCII- or binary-formatted console command is received
void PayloadInterfaceController::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 lockedLog(GetLogger());

		lockedLog->Info
			<< "OnConsoleMessage (" << (asciiFormat ? "ASCII" : "binary") << ")"
			<< " wantsAck=" << wantsAck
			<< " source=0x" << utos(source)
			<< " dest=0x" << utos(consoleData.dest,16,4)
			<< " function=0x" << utos(consoleData.function,16,2)
			<< " dataLength=" << consoleData.dataLength
			<< " data=";

		if (asciiFormat)
		{
			string dataStr;

			if (dataBytes.size())
				dataStr.assign((const char*) &dataBytes.front(), dataBytes.size());

			lockedLog->Info << "\"" << dataStr << "\"" << endl;
		}
		else if (GetLogger().LogEnabled(Logger::LT_DEBUG))
			lockedLog->Info << dataBytes.ToString() << endl;
		else
			lockedLog->Info << dataBytes.size() << " bytes" << endl;
	}

	if (consoleData.function > 1)
	{
		acked = false;
		nakDisp = CONSOLE_RESPONSE_UNSUPPORTED_CMD;
	}
	else
	{

		if (HaveSC(consoleData.dest))
		{
			FileIDQueueMemberPtr itemPtr;
			itemPtr.reset(
					new FileIDQueueMember(
							GetSCPtr(consoleData.dest),
							dataBytes,
							receivedLevel2.commHeader.packetID,
							source,
							asciiFormat,
							consoleData.function == 1,
							wantsAck)
					);

			completedFiles.push_back(itemPtr);
			acked = true;
		}
		else
		{
			acked = false;
			nakDisp = CONSOLE_RESPONSE_UNKNOWN_DESTINATION;
			nakSummary = PayloadProtocol::NAKSUMMARY_DEVICE_NOT_FOUND;
		}
	}
}

// This method is called whenever an Initialize File Write message is sent to the payload
void PayloadInterfaceController::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 lockedLog(GetLogger());
		REPORTED_NAME reportedName;
		memcpy(reportedName.paddedFileName, params.fileName, sizeof(reportedName.paddedFileName));
		const string stringizedName = Stringize(reportedName);
		lockedLog->Info
			<< "OnInitFileWrite"
			<< " storageControllerAddress=0x" << utos(params.storageControllerAddress, 16, 4)
			<< " fileName='" << stringizedName << "'"
			<< " fileID=0x" << ultos(params.fileID, 16, 8)
			<< " fileSize=" << params.fileSize
			<< " creationDate=" << utos(params.creationDate.year, 10, 2) << "/"
			<< utos(params.creationDate.month, 10, 2) << "/"
			<< utos(params.creationDate.day, 10, 2) << " "
			<< utos(params.creationDate.hour, 10, 2) << ":"
			<< utos(params.creationDate.minute, 10, 2) << ":"
			<< utos(params.creationDate.second, 10, 2)
			<< " modifiedDate=" << utos(params.modifiedDate.year, 10, 2) << "/"
			<< utos(params.modifiedDate.month, 10, 2) << "/"
			<< utos(params.modifiedDate.day, 10, 2) << " "
			<< utos(params.modifiedDate.hour, 10, 2) << ":"
			<< utos(params.modifiedDate.minute, 10, 2) << ":"
			<< utos(params.modifiedDate.second, 10, 2)
			<< " fileAttirbutes=0x" << utos(params.fileAttributes, 16, 2)
			<< " overwrite=0x" << utos(params.overwriteFile, 16, 2)
			<< " contentCRC=0x" <<  utos(params.contentCRC, 16, 4)
			<< " contentMD5=";

		BOOST_FOREACH(uint8_t b, params.contentMD5.data)
			lockedLog->Info << utos(b, 16, 2);

		lockedLog->Info
			<< " numDataBytes=" << params.numDataBytes
			<< endl;
	}

	bool caughtException = false;

	acked = false;

	if (HaveSC(params.storageControllerAddress))
	{
		StorageController& sc = GetSC(params.storageControllerAddress);

		StorageController::datetime creationDate;
		Convert(params.creationDate, creationDate);

		StorageController::datetime modifiedDate;
		Convert(params.modifiedDate, modifiedDate);

		StorageController::Lock scLock(sc);

		REPORTED_NAME reportedName;
		memcpy(reportedName.paddedFileName, params.fileName, sizeof(reportedName.paddedFileName));
		const string stringizedName = Stringize(reportedName);

		StorageController::FileState newFileState(
				sc,
				stringizedName,
				params.fileID,
				params.fileSize,
				creationDate,
				modifiedDate,
				!!(params.fileAttributes & FTATTR_READ_ONLY),
				CRC16(params.contentCRC),
				MD5::Entity(params.contentMD5.data),
				!!params.overwriteFile);

		try
		{
			sc.CreateFile(newFileState, !!params.overwriteFile);
		}
		catch(StorageController::FileState::CannotCreateFileExists& fileExistsError)
		{
			nakDisp = NAKDISP_WOULD_OVERWRITE;
			nakSummary = PayloadProtocol::NAKSUMMARY_INVALID_DATA;
			caughtException = true;
		}
		catch(StorageController::FileState::CannotOverwriteReadOnly& readOnlyError)
		{
			nakDisp = NAKDISP_READ_ONLY;
			nakSummary = PayloadProtocol::NAKSUMMARY_INVALID_DATA;
			caughtException = true;
		}
		catch(StorageController::FileState::FileSystemFull& filesystemFullError)
		{
			nakDisp = NAKDISP_STORAGE_FULL;
			nakSummary = PayloadProtocol::NAKSUMMARY_INVALID_DATA;
			caughtException = true;
		}
		catch(StorageController::FileState::FileIOError& fileIOError)
		{
			nakDisp = NAKDISP_STORAGE_IO_ERROR;
			nakSummary = PayloadProtocol::NAKSUMMARY_INVALID_DATA;
			caughtException = true;
		}
		catch(StorageController::FileState::MissingSCDirectory& missingSCDir)
		{
			nakDisp = NAKDISP_STORAGE_IO_ERROR;
			nakSummary = PayloadProtocol::NAKSUMMARY_INVALID_DATA;
			caughtException = true;
		}

		if (paramData.size())
		{
			try
			{
				sc.WriteFile(newFileState.GetFileID(), newFileState.GetReportedFileName(), 0, paramData);
			}
			catch(StorageController::FileState::FileNotFound& fileNotFoundError)
			{
				nakDisp = NAKDISP_DATA;
				nakSummary = PayloadProtocol::NAKSUMMARY_INVALID_DATA;
				caughtException = true;
			}
			catch(StorageController::FileState::CannotOverwriteReadOnly& readOnlyError)
			{
				nakDisp = NAKDISP_READ_ONLY;
				nakSummary = PayloadProtocol::NAKSUMMARY_INVALID_DATA;
				caughtException = true;
			}
			catch(StorageController::FileState::FileIOError& fileIOError)
			{
				nakDisp = NAKDISP_STORAGE_IO_ERROR;
				nakSummary = PayloadProtocol::NAKSUMMARY_INVALID_DATA;
				caughtException = true;
			}
			catch(StorageController::FileState::MissingSCDirectory& missingSCDir)
			{
				nakDisp = NAKDISP_STORAGE_IO_ERROR;
				nakSummary = PayloadProtocol::NAKSUMMARY_INVALID_DATA;
				caughtException = true;
			}
			catch(StorageController::FileState::FileBoundsError& fileBoundsError)
			{
				nakDisp = NAKDISP_DATA;
				nakSummary = PayloadProtocol::NAKSUMMARY_INVALID_DATA;
				caughtException = true;
			}
		}

		acked = !caughtException;

		if (acked && paramData.size())
			WriteFileEOFCheck(params.storageControllerAddress, sc, sc.GetFileState(newFileState.GetActualFileName()), 0,
					paramData.size(), acked, nakDisp, nakSummary);
	}
	else
	{
		nakDisp = NAKDISP_DATA;
		nakSummary = PayloadProtocol::NAKSUMMARY_UNKNOWN_RECIPIENT;
		acked = false;
	}
}

// This method is called whenever a Write File Block message is sent to the payload
void PayloadInterfaceController::OnWriteFileBlock(
		BoardAddress target,
		const Level2Wrapper& receivedLevel2,
		const FCCMessage::CMD_FMT_19& params,
		const ByteArray& paramData,
		bool& acked,
		uint8_t & nakDisp,
		uint8_t& nakSummary)
{
	GetLogger().Info
		<< "OnWriteFileBlock"
		<< " storageControllerAddress=0x" << utos(params.storageControllerAddress, 16, 4)
		<< " fileID=0x" << ultos(params.fileID, 16, 8)
		<< " offset=" << params.offset
		<< " numDataBytes=" << params.numDataBytes
		<< endl;

	REPORTED_NAME emptyName = RealNameToReported(".");
	bool caughtException = false;

	acked = false;

	if (HaveSC(params.storageControllerAddress))
	{
		StorageController& sc = GetSC(params.storageControllerAddress);
		StorageController::Lock scLock(sc);

		try
		{
			sc.WriteFile(params.fileID, emptyName.paddedFileName, params.offset, paramData);
		}
		catch(StorageController::FileState::FileNotFound& fileNotFoundError)
		{
			nakDisp = NAKDISP_DATA;
			nakSummary = PayloadProtocol::NAKSUMMARY_INVALID_DATA;
			caughtException = true;
		}
		catch(StorageController::FileState::CannotOverwriteReadOnly& readOnlyError)
		{
			nakDisp = NAKDISP_READ_ONLY;
			nakSummary = PayloadProtocol::NAKSUMMARY_INVALID_DATA;
			caughtException = true;
		}
		catch(StorageController::FileState::FileIOError& fileIOError)
		{
			nakDisp = NAKDISP_STORAGE_IO_ERROR;
			nakSummary = PayloadProtocol::NAKSUMMARY_INVALID_DATA;
			caughtException = true;
		}
		catch(StorageController::FileState::MissingSCDirectory& missingSCDir)
		{
			nakDisp = NAKDISP_STORAGE_IO_ERROR;
			nakSummary = PayloadProtocol::NAKSUMMARY_INVALID_DATA;
			caughtException = true;
		}
		catch(StorageController::FileState::FileBoundsError& fileBoundsError)
		{
			nakDisp = NAKDISP_DATA;
			nakSummary = PayloadProtocol::NAKSUMMARY_INVALID_DATA;
			caughtException = true;
		}

		acked = !caughtException;

		if (acked)
		{
			StorageController::FileState::PtrList matching = sc.GetFileStateList(params.fileID, emptyName.paddedFileName);

			if (matching.size() != 1)
			{
				acked = false;
				nakDisp = NAKDISP_DATA;
				nakSummary = PayloadProtocol::NAKSUMMARY_INVALID_DATA;
			}
			else
				WriteFileEOFCheck(params.storageControllerAddress, sc, matching.front(), params.offset,
						paramData.size(), acked, nakDisp, nakSummary);
		}
	}
	else
	{
		nakDisp = NAKDISP_DATA;
		nakSummary = PayloadProtocol::NAKSUMMARY_UNKNOWN_RECIPIENT;
		acked = false;
	}
}

// This method is called whenever a Delete File message is sent to the payload
void PayloadInterfaceController::OnDeleteFile(
		BoardAddress target,
		const Level2Wrapper& receivedLevel2,
		const FCCMessage::CMD_FMT_1A& params,
		bool& acked,
		uint8_t& nakDisp,
		uint8_t& nakSummary)
{
	bool caughtException = false;

	if (HaveSC(params.storageControllerAddress))
	{
		StorageController& sc = GetSC(params.storageControllerAddress);

		try
		{
			sc.DeleteFile(params.fileID, params.fileName, !!params.overrideReadOnly);
		}
		catch(StorageController::FileState::FileNotFound& fileNotFoundError)
		{
			nakDisp = NAKDISP_DATA;
			nakSummary = PayloadProtocol::NAKSUMMARY_INVALID_DATA;
			caughtException = true;
		}
		catch(StorageController::FileState::CannotOverwriteReadOnly& readOnlyError)
		{
			nakDisp = NAKDISP_READ_ONLY;
			nakSummary = PayloadProtocol::NAKSUMMARY_INVALID_DATA;
			caughtException = true;
		}
		catch(StorageController::FileState::FileIOError& fileIOError)
		{
			nakDisp = NAKDISP_STORAGE_IO_ERROR;
			nakSummary = PayloadProtocol::NAKSUMMARY_INVALID_DATA;
			caughtException = true;
		}
		catch(StorageController::FileState::MissingSCDirectory& missingSCDir)
		{
			nakDisp = NAKDISP_STORAGE_IO_ERROR;
			nakSummary = PayloadProtocol::NAKSUMMARY_INVALID_DATA;
			caughtException = true;
		}
	}
	else
	{
		nakDisp = NAKDISP_DATA;
		nakSummary = PayloadProtocol::NAKSUMMARY_UNKNOWN_RECIPIENT;
		caughtException = true;
	}

	acked = !caughtException;
}

// This method is called whenever a Read File Request message is sent to the payload
void PayloadInterfaceController::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)
{
	REPORTED_NAME reported;
	memcpy(reported.paddedFileName, params.fileName, sizeof(reported.paddedFileName));
	string reportedFileName = Stringize(reported);

	if (HaveSC(params.storageControllerAddress))
	{
		StorageController& sc = GetSC(params.storageControllerAddress);
		StorageController::Lock scLock(sc);
		StorageController::FileState::PtrList fileDataList = sc.GetFileStateList(StorageController::NO_FILE_ID, reportedFileName);

		if (fileDataList.size() == 1)
		{
			StorageController::FileSizeType readLength = params.readLength;

			if (params.maxResponseSize && (params.maxResponseSize < readLength))
				readLength = params.maxResponseSize;

			try
			{
				boost::shared_ptr<ByteArray> localData;
				localData.reset(new ByteArray);

				sc.ReadFile(*localData, reportedFileName, params.offset, readLength);
				eof = (params.offset+readLength) >= fileDataList.front()->GetFileSize();
				data = localData;
			}
			catch(StorageController::FileState::FileNotFound& fileNotFoundError)
			{
				nakDisp = NAKDISP_DATA;
				nakSummary = PayloadProtocol::NAKSUMMARY_INVALID_DATA;
			}
			catch(StorageController::FileState::FileIOError& fileIOError)
			{
				nakDisp = NAKDISP_STORAGE_IO_ERROR;
				nakSummary = PayloadProtocol::NAKSUMMARY_INVALID_DATA;
			}
			catch(StorageController::FileState::MissingSCDirectory& missingSCDir)
			{
				nakDisp = NAKDISP_STORAGE_IO_ERROR;
				nakSummary = PayloadProtocol::NAKSUMMARY_INVALID_DATA;
			}
			catch(StorageController::FileState::FileBoundsError& fileBoundsError)
			{
				nakDisp = NAKDISP_DATA;
				nakSummary = PayloadProtocol::NAKSUMMARY_INVALID_DATA;
			}
		}
	}
	else
	{
		nakDisp = NAKDISP_DATA;
		nakSummary = PayloadProtocol::NAKSUMMARY_UNKNOWN_RECIPIENT;
	}
}

// This method is called whenever a Get File List message is sent to the payload
void PayloadInterfaceController::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)
{
	StorageController::FileState::PtrList fileDataList;

	ackFileListData.resize(0);
	acked = false;

	if (HaveSC(params.storageControllerAddress))
	{
		StorageController& sc = GetSC(params.storageControllerAddress);
		StorageController::Lock scLock(sc);

		try
		{
			fileDataList = sc.GetFileStateList(params.fileID, params.fileName);
		}
		catch(StorageController::FileState::FileNotFound& exc)
		{
			nakDisp = FCCMessage::NAKDISP_DATA;
			nakSummary = PayloadProtocol::NAKSUMMARY_INVALID_DATA;
			return;
		}


		ackFileListHeader.storageControllerAddress = params.storageControllerAddress;
		ackFileListHeader.offset = 0;
		ackFileListHeader.lastReport = 0xFF;
		ackFileListHeader.numFiles = fileDataList.size();
		ackFileListData.resize(fileDataList.size());

		StorageController::FileState::PtrList::const_iterator it = fileDataList.begin();
		uint16_t idx2 = 0;

		acked = true;

		for (; acked && (it != fileDataList.end()); ++it)
		{
			bool succeeded = true;
			StorageController::FileState::Ptr pData = *it;

			if (params.actionFlags & AF_CHECK_FILE)
			{
				bool crcFail = false;
				bool md5Fail = false;
				bool otherFail = false;
				bool deleteOnFailure = !!(params.actionFlags & AF_DELETE_ON_FAILURE);

				try
				{
					sc.CheckFile(pData->GetFileID(), pData->GetReportedFileName(), deleteOnFailure);
				}
				catch(StorageController::FileState::FileCheckValuesNotEstablished& checkValueNotSetError)
				{
					crcFail = true;
				}
				catch(StorageController::FileState::FileCRCCheckError& crcFailure)
				{
					crcFail = true;
				}
				catch(StorageController::FileState::FileMD5CheckError& md5Failure)
				{
					md5Fail = true;
				}
				catch(StorageController::FileState::FileNotFound& fileNotFoundError)
				{
					nakDisp = NAKDISP_DATA;
					otherFail = true;
				}
				catch(StorageController::FileState::FileIOError& fileIOError)
				{
					nakDisp = NAKDISP_STORAGE_IO_ERROR;
					otherFail = true;
				}
				catch(StorageController::FileState::MissingSCDirectory& missingSCDir)
				{
					nakDisp = NAKDISP_STORAGE_IO_ERROR;
					otherFail = true;
				}

				if (crcFail || md5Fail || otherFail)
				{
					nakSummary = PayloadProtocol::NAKSUMMARY_INVALID_DATA;
					acked = succeeded = false;

					if (crcFail) 		nakDisp = NAKDISP_CRC_FAILED;
					else if (md5Fail) 	nakDisp = NAKDISP_MD5_FAILED;
				}
			}

			if (succeeded)
				GetHostFormattedFileDetails(*pData, ackFileListData[idx2++]);
		}
	}
	else
	{
		nakDisp = NAKDISP_DATA;
		nakSummary = PayloadProtocol::NAKSUMMARY_UNKNOWN_RECIPIENT;
	}
}

// This method is called whenever a Get Detailed File List message is sent to the payload
void PayloadInterfaceController::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)
{
	acked = false;
	ackFileListData.resize(0);

	if (HaveSC(params.storageControllerAddress))
	{
		StorageController& sc = GetSC(params.storageControllerAddress);
		StorageController::Lock scLock(sc);
		StorageController::FileState::PtrList fileDataList;

		try
		{
			fileDataList = sc.GetFileStateList(params.fileID, params.fileName);
		}
		catch(StorageController::FileState::FileNotFound& exc)
		{
			nakDisp = FCCMessage::NAKDISP_DATA;
			nakSummary = PayloadProtocol::NAKSUMMARY_INVALID_DATA;
			return;
		}

		ackFileListHeader.storageControllerAddress = params.storageControllerAddress;
		ackFileListHeader.offset = 0;
		ackFileListHeader.lastReport = 0xFF;
		ackFileListHeader.numFiles = fileDataList.size();
		ackFileListData.resize(fileDataList.size());

		StorageController::FileState::PtrList::const_iterator it = fileDataList.begin();
		uint16_t idx2 = 0;

		acked = true;

		for (; acked && (it != fileDataList.end()); ++it)
		{
			bool succeeded = true;
			const StorageController::FileState::Ptr pData = (*it);

			if (params.actionFlags & AF_CHECK_FILE)
			{
				bool crcFail = false;
				bool md5Fail = false;
				bool otherFail = false;

				try
				{
					sc.CheckFile(pData->GetFileID(), pData->GetReportedFileName());
				}
				catch(StorageController::FileState::FileCheckValuesNotEstablished& checkValueNotSetError)
				{
					crcFail = true;
				}
				catch(StorageController::FileState::FileCRCCheckError& crcFailure)
				{
					crcFail = true;
				}
				catch(StorageController::FileState::FileMD5CheckError& md5Failure)
				{
					md5Fail = true;
				}
				catch(StorageController::FileState::FileNotFound& fileNotFoundError)
				{
					nakDisp = NAKDISP_DATA;
					otherFail = true;
				}
				catch(StorageController::FileState::FileIOError& fileIOError)
				{
					nakDisp = NAKDISP_STORAGE_IO_ERROR;
					otherFail = true;
				}
				catch(StorageController::FileState::MissingSCDirectory& missingSCDir)
				{
					nakDisp = NAKDISP_STORAGE_IO_ERROR;
					otherFail = true;
				}

				if (crcFail || md5Fail || otherFail)
				{
					succeeded = acked = false;
					nakSummary = PayloadProtocol::NAKSUMMARY_INVALID_DATA;

					if (crcFail) 		nakDisp = NAKDISP_CRC_FAILED;
					else if (md5Fail) 	nakDisp = NAKDISP_MD5_FAILED;
				}
			}

			if (succeeded)
			{
				ACK_FILE_LIST_DETAILED_RECORD& rec = ackFileListData[idx2++];

				GetHostFormattedFileDetails(*pData, rec);
				GetHostFormattedAdditionalDetails(*pData, rec);
			}
		}
	}
	else
	{
		nakDisp = NAKDISP_DATA;
		nakSummary = PayloadProtocol::NAKSUMMARY_UNKNOWN_RECIPIENT;
	}
}

// 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 PayloadInterfaceController::OnPowerNotification(
		const PayloadProtocol::PYLD_POWER_NOTIFICATION& notification,
		PayloadProtocol::ACK_PYLD_POWER_NOTIFICATION_RECORD& ack,
		bool& acked,
		PayloadProtocol::ENakSummary& nakSummary)
{
	ack.address = MainTaskAddress();
	ack.boardID = GetBoardID(MainTaskAddress());
	ack.state = 1;
	ack.delayOff = config.GetPowerOffDelay();
	ack.delayOn = config.GetPowerOnDelay();
	acked = true;

	if (config.GetShutdownVerb().length() || !config.GetShutdownEventSettings().Empty())
	{
		Logger::Locked(GetLogger())->Info << "Setting halt flag" << endl;
		halt = true;
	}
	else
		Logger::Locked(GetLogger())->Info << "Not actually halting since shutdown verb is empty" << endl;
}

// callback for CommandComplete
void PayloadInterfaceController::SCCommandComplete(ProxySC& sender, const ByteArray& data, long transNumber, unsigned destID, bool complete, unsigned resultNum, bool success)
{
	Logger& l = GetLogger();

	{
		Logger::Locked l2(l);

		l2->Info
			<< "SCCommandComplete transNumber=" << transNumber
			<< " destID=0x" << utos(destID,16,4)
			<< " complete=" << complete
			<< " resultNum=" << resultNum
			<< " success=" << BooleanToString(success);

		if (l.LogEnabled(Logger::LT_DEBUG))
			l2->Info << " data=" << data.ToString() << endl;
		else
			l2->Info << " data=" << data.size() << " bytes" << endl;
	}

	if (resultNum == 1)
	{
		const uint16_t dataSize = data.size();
		const uint16_t maxReturnedSize = 256;
		uint16_t blockSize = 0;
		unsigned blockNum = 0;

		for (uint16_t offset = 0; (!blockNum || (blockNum < config.GetMaxConsoleOutputBlocks())) &&  offset < dataSize; ++blockNum, offset += blockSize)
		{
			if ((blockSize = dataSize-offset) > maxReturnedSize)
				blockSize = maxReturnedSize;

			const bool sizeOverflow = ((offset+blockSize) < dataSize);
			PPSByteStream bsLevel1;
			uint8_t messageType;

			if (success || !config.GetEnableNakWithDescription())
			{
				ConsoleAck consoleAck(0, sizeOverflow ? CONSOLE_RESPONSE_OK_TRUNCATED : CONSOLE_RESPONSE_OK,
						ByteArray(data.begin()+offset, data.begin()+offset+blockSize));
				bsLevel1 << consoleAck;
				messageType = ACK_CONSOLE_DATA;
			}
			else
			{
				NakWithDescription nakDesc(blockNum, ByteArray(data.begin()+offset, data.begin()+offset+blockSize));
				bsLevel1 << nakDesc;
				messageType = NACK_WITH_DESCRIPTION;
			}

			PPSByteStream bs;
			bs << PayloadProtocol::LRIGenericAck(0, false)
			   << ForwardInfo(UNSOLICITED_SHIP_TO_SHORE, destID, sender.GetAddress())
			   << Level2Wrapper(messageType, transNumber, bsLevel1.AsBytes());

			PayloadMessage::Ptr payloadMessage;
			payloadMessage.reset(new PayloadMessage(PayloadMessage::LRI_ACK, sender.GetAddress(), FLOAT_TARGET_FORWARD_MESSAGE, 0, bs.AsBytes()));

			Logger::Locked(GetLogger())->Info << "(" << blockSize << " bytes at offset " << offset << ") Enqueuing message " << *payloadMessage << endl;

			EnqueueMessage(payloadMessage);
		}
	}
}

// callback for NewFileAvailable
void PayloadInterfaceController::SCNewFileAvailable(ProxySC& sender, const string& actualFileName, string& remoteName)
{
	Logger::Locked(GetLogger())->Info << "New file available sc=0x" << ultos(sender.GetAddress(), 16, 4) << " actualFileName=" << actualFileName << endl;

	StorageController& sc = sender.GetSC();
	StorageController::FileState::Ptr item = sc.GetFileState(actualFileName);

	NewFileAvailable level1;
	level1.storageControllerAddress = sender.GetAddress();
	GetHostFormattedFileDetails_WithoutFileID(*item, level1);
	GetHostFormattedAdditionalDetails(*item, level1);
	GetHostFormattedCalculations(*item, level1);

	PPSByteStream bsLevel1;
	bsLevel1 << level1;

	PPSByteStream bs;
	bs << PayloadProtocol::LRIGenericAck(0, false)
	   << Level2Wrapper(SND_NEW_FILE_AVAILABLE, bsLevel1.AsBytes(), Level2Wrapper::WITHOUT_CRC, false); // FSReport

	PayloadMessage::Ptr payloadMessage;
	payloadMessage.reset(new PayloadMessage(PayloadMessage::LRI_ACK, sender.GetAddress(), FLOAT_TARGET_FILE_TRANSFER_TO_SHORE, 0, bs.AsBytes()));

	Logger::Locked(GetLogger())->Info << "Enqueuing message " << *payloadMessage << endl;

	EnqueueMessage(payloadMessage);
}


// Since GetFileList is not sent from the Float C&C, check if the last part of the file was written and update flags appropriately
void PayloadInterfaceController::WriteFileEOFCheck(PayloadMessage::BoardAddress scAddr, StorageController& sc, StorageController::FileState::Ptr item,
														uint32_t offset, uint16_t writeSize, bool& acked, uint8_t& nakDisp, uint8_t& nakSummary)

{
	if (!acked || (item->GetFileSize() > (offset+writeSize)))
		return;

	Logger::Locked(GetLogger())->Info << "WriteFileEOFCheck for sc=0x" << ultos(scAddr, 16, 4) << " fileID=0x" << ultos(item->GetFileID(), 16, 8) << " actualName='" << item->GetActualFileName() << "'" << endl;

	bool crcFail = false;
	bool md5Fail = false;
	bool otherFail = false;

	try
	{
		sc.CheckFile(item->GetFileID(), item->GetReportedFileName(), false);
	}
	catch(StorageController::FileState::FileCheckValuesNotEstablished& checkValueNotSetError)
	{
		crcFail = true;
	}
	catch(StorageController::FileState::FileCRCCheckError& crcFailure)
	{
		crcFail = true;
	}
	catch(StorageController::FileState::FileMD5CheckError& md5Failure)
	{
		md5Fail = true;
	}
	catch(StorageController::FileState::FileNotFound& fileNotFoundError)
	{
		nakDisp = NAKDISP_DATA;
		otherFail = true;
	}
	catch(StorageController::FileState::FileIOError& fileIOError)
	{
		nakDisp = NAKDISP_STORAGE_IO_ERROR;
		otherFail = true;
	}
	catch(StorageController::FileState::MissingSCDirectory& missingSCDir)
	{
		nakDisp = NAKDISP_STORAGE_IO_ERROR;
		otherFail = true;
	}

	if (crcFail)
		Logger::Locked(GetLogger())->Error << "CRC check failed" << endl;
	else if (md5Fail)
		Logger::Locked(GetLogger())->Error << "MD5 check failed" << endl;
	else if (otherFail)
		Logger::Locked(GetLogger())->Error << "Other failure code " << nakDisp << endl;
	else
		Logger::Locked(GetLogger())->Info << "Check succeeded" << endl;

	if (crcFail || md5Fail || otherFail)
	{
		nakSummary = PayloadProtocol::NAKSUMMARY_INVALID_DATA;

		if (crcFail) 		nakDisp = NAKDISP_CRC_FAILED;
		else if (md5Fail) 	nakDisp = NAKDISP_MD5_FAILED;

		acked = false;
	}
	else
	{
		FileIDQueueMemberPtr itemPtr;
		itemPtr.reset( new FileIDQueueMember(GetSCPtr(scAddr), item->GetFileID()) );

		completedFiles.push_back(itemPtr);
	}
}

PayloadInterfaceController::Logger& PayloadInterfaceController::GetLogger() const
{
	return PayloadPortSlave::logger;
}

void PayloadInterfaceController::HandleShutdown()
{
	InformDemoServerOfShutdown();
	SendShutdownEvent();

	if (config.GetShutdownDelay())
	{
		Logger::Locked(GetLogger())->Info << "Delaying for " << config.GetShutdownDelay() << " seconds while servers shut down" << endl;
		sleep(config.GetShutdownDelay());
	}

	ShutdownSystem();
	shutdownCompleted = true;
	throw ReceivedShutdownRequest();
}

void PayloadInterfaceController::ShutdownSystem()
{
	Logger::Locked(GetLogger())->Info << "Halt action pending" << endl;

	string shutdownVerb = config.GetShutdownVerb();

	if (shutdownVerb.length())
	{
		Logger::Locked(GetLogger())->Info << "cmd: " << shutdownVerb << endl;
		const int systemReturnCode = system(shutdownVerb.c_str());
		if (systemReturnCode);
	}
	else
		Logger::Locked(GetLogger())->Info << "No defined shutdown verb" << endl;
}

void PayloadInterfaceController::SendShutdownEvent()
{
	if (!config.GetShutdownEventSettings().Empty())
	{
		Logger::Locked(GetLogger())->Info << "Sending shutdown event" << endl;

		EventBase shutdownEvent(broker);
		shutdownEvent.SetEventSettings(config.GetShutdownEventSettings());

		TransmitEvent(shutdownEvent);
	}
}

void PayloadInterfaceController::InformDemoServerOfShutdown()
{
	int countInformed = 0;
	const StringList& demoServerList = config.GetDemoServerList();

	for (StringList::const_iterator it = demoServerList.begin(); it != demoServerList.end(); ++it)
	{
		const string& remoteHost = *it;
		Logger::Locked(GetLogger())->Info << "Informing demo server '" << remoteHost << "' of pending shutdown" << endl;

		try
		{
			::CORBA::Any cmd;
			cmd <<= "CMD STOP";

			IDemoSensor_var sensorV;
			::CORBA::Any_var result;
			sensorV = broker.GetNamedInstance<IDemoSensor_var,IDemoSensor>(remoteHost.c_str());
			ICommandableInvoker<IDemoSensor_var> invoker(sensorV);
			invoker(cmd, "PayloadInterfaceController", "", result);

			++countInformed;
		}
		catch(CORBA::Exception& exc)
		{
			Logger::Locked(GetLogger())->Error << "Caught CORBA exception: " << DecodeCORBAException(exc) << " (ignored)" << endl;
		}
		catch(std::exception& exc)
		{
			Logger::Locked(GetLogger())->Error << "Caught exception type "<< typeid(exc).name() << ": " << exc.what() << " (ignored)" << endl;
		}
	}
}

void PayloadInterfaceController::AddressableMessageHandler(EventTransceiver& sender, const IEventBase& receivedEvent)
{
	const EventSettings& addressableMessageEvent = config.GetAddressedMessageEventSettings();
	const AddressedPacketEvent& evt =
		EventTransceiver::UpcastEventRef<AddressedPacketEvent>(receivedEvent, addressableMessageEvent,
				"PayloadInterfaceController::AddressableMessageHandler");

	uint16_t sourceAddr;
	uint8_t destBoard, destTask;
	uint16_t destAddr;
	uint16_t transID;
	ByteArrayPtr msgPtr;
	AddressedPacketEvent::NotifyType notifyType;

	try			{ sourceAddr = evt.GetSourceAddress();	}
	catch(...)	{ Logger::Locked(GetLogger())->Error << "Failed to get source address for addressed packet" << endl; return; }

	try			{ destAddr = evt.GetDestAddress(); evt.GetDestAddress(destBoard, destTask); }
	catch(...)	{ Logger::Locked(GetLogger())->Error << "Failed to get destination address for addressed packet" << endl; return; }

	try			{ transID = evt.GetTransactionID(); }
	catch(...)	{  Logger::Locked(GetLogger())->Error << "Failed to get transaction ID for addressed packet" << endl; return; }

	try			{ if (evt.HasMessage())	msgPtr = evt.GetMessage(); }
	catch(...)	{ Logger::Locked(GetLogger())->Error << "Failed during addressed message extraction" << endl; return; }

	try			{ notifyType = evt.GetNotify();	}
	catch(...)	{ Logger::Locked(GetLogger())->Error << "Failed while reading the notify type from addressed packet" << endl; return;	}

	switch(notifyType)
	{
	case AddressedPacketEvent::NT_PRIMARY:
	case AddressedPacketEvent::NT_SECONDARY:

		if (destBoard == GetBoardID(MainTaskAddress()))
			Logger::Locked(GetLogger())->Info << "Ignoring locally targetted message of type "
											  << AddressedPacketEvent::ToString(notifyType) << endl;

		else if (GetBoardID(sourceAddr) != GetBoardID(MainTaskAddress()))
			Logger::Locked(GetLogger())->Error << "Cannot transmit message from a board ID other than 0x"
											   << ltos(GetBoardID(MainTaskAddress()), 16, 2) << endl;

		else if (!msgPtr)
			Logger::Locked(GetLogger())->Error << "Addressed message missing message data in "
											   << AddressedPacketEvent::ToString(notifyType) << endl;

		else
		{
			EnqueueAddressedMessage(sourceAddr, destAddr, transID, *msgPtr);
		}

		break;

	default:

		Logger::Locked(GetLogger())->Info << "Ignoring addressed '" << AddressedPacketEvent::ToString(notifyType)
										  << "' packet due to status" << endl;
		break;
	}
}

struct AddressedMessageWrapper : public PayloadPortSlave::MessageWrapper
{

	typedef LiquidRobotics::SMACFramework::ByteArray ByteArray;

	AddressedMessageWrapper(PayloadMessage::Ptr _message, EventTransceiver& _evtTrans,
			PayloadInterfaceController::BoardAddress _source, PayloadInterfaceController::BoardAddress _dest,
			const ByteArray& _data, uint16_t _transID, const EventSettings& _evtSettings, IObjectBroker& _broker)
	: PayloadPortSlave::MessageWrapper(_message)
	, evtTrans(_evtTrans)
	, source(_source)
	, dest(_dest)
	, data(_data.begin(), _data.end())
	, transID(_transID)
	, evtSettings(_evtSettings)
	, broker(_broker)
	{
	}

	virtual void OnDiscard()
	{
	}

	virtual void OnEnqueue()
	{
		DoSend(AddressedPacketEvent::NT_ENQUEUED);
	}

	virtual void OnTransmit()
	{
		DoSend(AddressedPacketEvent::NT_TRANSMITTED);
	}

	void DoSend(AddressedPacketEvent::NotifyType notify)
	{
		AddressedPacketEvent pkt(evtSettings, broker, notify);
		pkt.SetSourceAddress(source);
		pkt.SetDestAddress(dest);
		pkt.SetMessage(data);
		pkt.SetTransactionID(transID);
		evtTrans.TransmitEvent(pkt);
	}

	EventTransceiver& evtTrans;
	const PayloadInterfaceController::BoardAddress source;
	const PayloadInterfaceController::BoardAddress dest;
	ByteArray data;
	const uint16_t transID;
	const EventSettings evtSettings;
	IObjectBroker& broker;
};



void PayloadInterfaceController::EnqueueAddressedMessage(BoardAddress source, BoardAddress dest, uint16_t transID,
													const ByteArray& data)
{
	LRI_COMMAND_REQUEST_ACK_WRAPPER cmdReq;
	cmdReq.commandFormat = 1;

	PPSByteStream bs;
	bs << PayloadProtocol::LRIGenericAck(0, false)
	   << cmdReq.commandFormat
	   << ContainerOut<const ByteArray>(data);

	PayloadMessage::Ptr payloadMessage;
	payloadMessage.reset(new PayloadMessage(PayloadMessage::LRI_ACK, source, dest, 0, bs.AsBytes()));

	MessageWrapper::Ptr wrapper;
	wrapper.reset(new AddressedMessageWrapper(payloadMessage, *this, source, dest, data, transID,
			config.GetAddressedMessageEventSettings(), broker));

	Logger::Locked(GetLogger())->Info << "Enqueuing addressed message " << *payloadMessage << endl;
	EnqueueMessage(wrapper);
}

void PayloadInterfaceController::EnqueueStartupTelemetryMessage(bool defer)
{
	if (defer)
	{
		enqueueStartupTelem = true;
		Logger::Locked(GetLogger())->Debug << "Deferred startup set" << endl;
	}
	else
	{
		Logger::Locked(GetLogger())->Debug << "Reporting " << (enqueueStartupTelem ? "deferred " : "") << " startup telemetry" << endl;
		enqueueStartupTelem = false;

		// --- set up the telemetry message ---

		TELEMETRY_91 telem;
		telem.Set(SYSINFO_SMC_PARTITION_STATE, PayloadManagerClient::GetMyBoardAddress(), SYSINFOSMCPARTITIONSTATE_SUBTYPE_0);

		TELEMETRY_91_SAMPLE0 sample0;

		if (haveSavedPosition)
		{
			sample0.latitude = TelemetryEventBase::ConvertDoublePosToInt(savedLatitude);
			sample0.longitude = TelemetryEventBase::ConvertDoublePosToInt(savedLongitude);
		}
		else
			sample0.latitude = sample0.longitude = 0;

		sample0.sampleTime_T = time(NULL);

		PPSByteStream bsLevel1;
		bsLevel1 << telem
		         << sample0
		         << (uint8_t) SYSINFOSMCPARTITIONSTATEVALUE_ENABLED;

		PPSByteStream bs;
		bs << PayloadProtocol::LRIGenericAck(0, false)
		   << Level2Wrapper(SND_SYSTEM_INFO, bsLevel1.AsBytes(), Level2Wrapper::WITHOUT_CRC, true); // FSReport

		// --- now stuff the payload packet bytes into a message (transaction ID set to 0 here, will be corrected later) ---
		PayloadMessage::Ptr payloadMessage;
		payloadMessage.reset(new PayloadMessage(PayloadMessage::LRI_ACK, PayloadManagerClient::GetMyBoardAddress(), FLOAT_TARGET_IRIDIUM_INTERFACE, 0, bs.AsBytes()));

		// --- now queue up the message for transmission later ---
		Logger::Locked(GetLogger())->Info << "Enqueuing startup telemetry message " << *payloadMessage << endl;
		EnqueueMessage(payloadMessage);
	}
}

bool PayloadInterfaceController::DeferredStartupTelem() const
{
	return enqueueStartupTelem;
}

void PayloadInterfaceController::PMQDistributeMessage(PayloadMessage::Ptr pmsg)
{
	LockableConceptMutex::Lock lock(pmqListMutex);

	for (PayloadMessageQueueList::iterator it = pmqList.begin(); it != pmqList.end(); ++it)
		(*it)->WriteExpand(pmsg);
}

void PayloadInterfaceController::PMQRegister(PayloadMessageQueue::Ptr pmq)
{
	LockableConceptMutex::Lock lock(pmqListMutex);
	bool found = false;

	for (PayloadMessageQueueList::iterator it = pmqList.begin(); !found && (it != pmqList.end()); ++it)
		found = (pmq.get() == it->get());

	if (!found)
	{
		pmqList.push_back(pmq);
		lock.unlock();
		Logger::Locked(GetLogger())->Debug << "Registered new payload message queue (new total = " << pmqList.size() << ")" << endl;
	}
	else
	{
		lock.unlock();
		Logger::Locked(GetLogger())->Warning << "Warning: payload message queue is already registered" << endl;
	}
}

void PayloadInterfaceController::PMQUnregister(PayloadMessageQueue::Ptr pmq)
{
	LockableConceptMutex::Lock lock(pmqListMutex);
	bool erased = false;

	for (PayloadMessageQueueList::iterator it = pmqList.begin(); !erased && (it != pmqList.end()); ++it)
		if (pmq.get() == it->get())
		{
			pmqList.erase(it);
			erased = true;
			break;
		}

	const size_t newCount = pmqList.size();
	lock.unlock();

	if (erased)
		Logger::Locked(GetLogger())->Debug << "Payload message queue unregistered (new count = " << newCount << ")" << endl;
	else
		Logger::Locked(GetLogger())->Warning << "Warning: payload message queue was not found, not unregistered" << endl;
}

void PayloadInterfaceController::AwaitSetParamAck(uint32_t transactionNumber, StringPtr requester, StringPtr transactionID, unsigned milliseconds)
{
	PayloadMessageQueue::Ptr pmq;
	PayloadMessage::Ptr msg;
	TimeCounter tcounter;
	string result;
	bool success = false;

	Logger::Locked(GetLogger())->Info << "AwaitSetParamAck for transaction 0x" << utos(transactionNumber,16,8) << " requester=" << *requester << " transactionID=" << *transactionID << " for " << milliseconds << " ms" << endl;

	try
	{
		pmq.reset(new PayloadMessageQueue);
		PMQRegister(pmq);

		bool readOK = false;

		while(!readOK)
		{
			msg.reset();
			msg = pmq->Read(tcounter.Remaining(milliseconds));

			if ((msg->GetDestinationAddress() == MainTaskAddress())
			 && (msg->GetMessageType() == PayloadMessage::ACK_REQUESTED_PACKET))
			{
				PayloadProtocol::LRI_FLOAT_COMMAND_ACK_WRAPPER commandAck;
				COMM_HEADER hdr;
				bool caught;

				try
				{
					const PayloadMessage::ByteArray& messageData = msg->GetData();
					PPSByteStream pyldStream(messageData);

					pyldStream >> commandAck >> hdr;
					caught = false;
					success = true;
				}
				catch(exception& ex)
				{
					Logger::Locked(GetLogger())->Warning << "AwaitSetParamAck: caught exception while unpacking message: " << ex.what() << endl;
					caught = true;
				}

				if (!caught)
				{
					if (hdr.packetID == transactionNumber)
					{
						stringstream stm;

						if (!commandAck.acknowledgementCode)
							stm << "ACK";
						else
							stm << "NAK";

						stm << utos(hdr.type,16, 2);
						result = stm.str();
						readOK = true;
					}
					else
						Logger::Locked(GetLogger())->Debug << "AwaitSetParamAck: transaction ID being acked (0x" << utos(msg->GetTransactionID(),16,8) << ") does not match the command transaction number (0x" << utos(transactionNumber,16,8) << ")" << endl;
				}
			}
			else
				Logger::Locked(GetLogger())->Debug << "AwaitSetParamAck: message not an Ack to an output message or not directed to me" << endl;
		}
	}
	catch(QueueBase::ReadTimeout& rto)
	{
		result = "Timeout";
	}
	catch(exception& ex)
	{
		stringstream stm;
		stm << "Caught exception: " << ex.what();

		result = stm.str();
	}

	Logger::Locked(GetLogger())->Info << "AwaitSetParamAck returning result string '" << result << "'" << endl;

	if (pmq)
		PMQUnregister(pmq);

	SendCommandCompleteEvent evt(config.GetCommandCompleteEventSettings(), broker);
	evt.Complete(true);
	evt.Requester(*requester);
	evt.Successful(success);
	evt.TransactionID(*transactionID);

	EventPropertyValue epv(broker);
	epv.SetString(result);

	Any resultingAny;
	epv.CopyOut(resultingAny);
	evt.Result(resultingAny);

	TransmitEvent(evt);
}

void PayloadInterfaceController::AwaitExecFCC(uint32_t transactionNumber, StringPtr requester, StringPtr transactionID, unsigned milliseconds)
{
	PayloadMessageQueue::Ptr pmq;
	PayloadMessage::Ptr msg;
	TimeCounter tcounter;
	string result;
	bool success = false;

	Logger::Locked(GetLogger())->Info << "AwaitExecFCC for transaction 0x" << utos(transactionNumber,16,8) << " requester=" << *requester << " transactionID=" << *transactionID << " for " << milliseconds << " ms" << endl;

	try
	{
		pmq.reset(new PayloadMessageQueue);
		PMQRegister(pmq);

		bool readOK = false;

		while(!readOK)
		{
			msg.reset();
			msg = pmq->Read(tcounter.Remaining(milliseconds));

			if ((msg->GetDestinationAddress() == MainTaskAddress()) && (msg->GetMessageType() == PayloadMessage::ACK_REQUESTED_PACKET))
			{
				const ByteArray& msgData = msg->GetData();
				PPSByteStream bs(msgData);
				PayloadProtocol::LRI_FLOAT_COMMAND_ACK_WRAPPER ackWrapper;
				COMM_HEADER hdr;
				bool caught = false;

				try
				{
					bs >> ackWrapper.commandFormat >> ackWrapper.acknowledgementCode
					   >> hdr.type >> hdr.packetID >> hdr.size;
				}
				catch(...)
				{
					caught = true;
				}

				if (!caught)
				{
					if (hdr.packetID == transactionNumber)
					{
						stringstream stm;

						if (!ackWrapper.acknowledgementCode)
							stm << "ACK";
						else
							stm << "NAK";

						stm << utos(hdr.type,16, 2);
						result = stm.str();
						readOK = true;
						success = true;
					}
					else
						Logger::Locked(GetLogger())->Debug << "AwaitExecFCC: transaction ID being acked (0x" << utos(hdr.packetID,16,8) << ") does not match the command transaction number (0x" << utos(transactionNumber,16,8) << ")" << endl;
				}
			}
			else
				Logger::Locked(GetLogger())->Debug << "AwaitExecFCC: message not an Ack to an output message or not directed to me" << endl;
		}
	}
	catch(QueueBase::ReadTimeout& rto)
	{
		result = "Timeout";
	}
	catch(exception& ex)
	{
		stringstream stm;
		stm << "Caught exception: " << ex.what();

		result = stm.str();
	}

	Logger::Locked(GetLogger())->Info << "AwaitExecFCC returning result string '" << result << "'" << endl;

	if (pmq)
		PMQUnregister(pmq);

	SendCommandCompleteEvent evt(config.GetCommandCompleteEventSettings(), broker);
	evt.Complete(true);
	evt.Requester(*requester);
	evt.Successful(success);
	evt.TransactionID(*transactionID);

	EventPropertyValue epv(broker);
	epv.SetString(result);

	Any resultingAny;
	epv.CopyOut(resultingAny);
	evt.Result(resultingAny);

	TransmitEvent(evt);
}

void PayloadInterfaceController::AwaitWCS(uint32_t transactionNumber, StringPtr requester, StringPtr transactionID, unsigned milliseconds)
{
	PayloadMessageQueue::Ptr pmq;
	PayloadMessage::Ptr msg;
	TimeCounter tcounter;
	string result;
	bool success = false;

	Logger::Locked(GetLogger())->Info << "AwaitWCS for transaction 0x" << utos(transactionNumber,16,8) << " requester=" << *requester << " transactionID=" << *transactionID << " for " << milliseconds << " ms" << endl;

	try
	{
		pmq.reset(new PayloadMessageQueue);
		PMQRegister(pmq);

		bool readOK = false;

		while(!readOK)
		{
			msg.reset();
			msg = pmq->Read(tcounter.Remaining(milliseconds));

			if ((msg->GetDestinationAddress() == MainTaskAddress()) && (msg->GetMessageType() == PayloadMessage::ACK_REQUESTED_PACKET))
			{
				const ByteArray& msgData = msg->GetData();
				PPSByteStream bs(msgData);
				PayloadProtocol::LRI_FLOAT_COMMAND_ACK_WRAPPER ackWrapper;
				COMM_HEADER hdr;
				bool caught = false;

				try
				{
					bs >> ackWrapper.commandFormat >> ackWrapper.acknowledgementCode
					   >> hdr.type >> hdr.packetID >> hdr.size;
				}
				catch(...)
				{
					caught = true;
				}

				if (!caught)
				{
					if (hdr.packetID == transactionNumber)
					{
						stringstream stm;

						if (!ackWrapper.acknowledgementCode)
							stm << "ACK";
						else
							stm << "NAK";

						stm << utos(hdr.type,16, 2);

						if (hdr.type == ACK_CONSOLE_DATA)
						{
							uint16_t something;
							uint16_t stringLen;
							bs >> something >> stringLen;
							stm.put(':');
							while(stringLen--)
								stm.put((char) bs.Get());
						}

						result = stm.str();
						readOK = true;
						success = true;
					}
					else
						Logger::Locked(GetLogger())->Debug << "AwaitWCS: transaction ID being acked (0x" << utos(hdr.packetID,16,8) << ") does not match the command transaction number (0x" << utos(transactionNumber,16,8) << ")" << endl;
				}
			}
			else
				Logger::Locked(GetLogger())->Debug << "AwaitWCS: message not an Ack to an output message or not directed to me" << endl;
		}
	}
	catch(QueueBase::ReadTimeout& rto)
	{
		result = "Timeout";
	}
	catch(exception& ex)
	{
		stringstream stm;
		stm << "Caught exception: " << ex.what();

		result = stm.str();
	}

	Logger::Locked(GetLogger())->Info << "AwaitWCS returning result string '" << result << "'" << endl;

	if (pmq)
		PMQUnregister(pmq);

	SendCommandCompleteEvent evt(config.GetCommandCompleteEventSettings(), broker);
	evt.Complete(true);
	evt.Requester(*requester);
	evt.Successful(success);
	evt.TransactionID(*transactionID);

	EventPropertyValue epv(broker);
	epv.SetString(result);

	Any resultingAny;
	epv.CopyOut(resultingAny);
	evt.Result(resultingAny);

	TransmitEvent(evt);
}

PayloadMessage::Ptr PayloadInterfaceController::GetGenericCommandMessage(uint8_t l2Type, uint32_t transactionNumber, uint8_t formatID, uint16_t commandID,
		uint8_t field3, uint8_t field4, uint8_t field5, uint8_t field6, uint8_t field7, uint32_t latitude, uint32_t longitude)
{
	CMD_FMT_COMMON type;
	type.fmtID = formatID;
	type.command = commandID;

	CMD_FMT_10 command;
	command.field3 = field3;
	command.field4 = field4;
	command.field5 = field5;
	command.field6 = field6;
	command.field7 = field7;
	command.latitude = latitude;
	command.longitude = longitude;

	PPSByteStream bsLevel1;
	bsLevel1 << type
			 << command;

	PPSByteStream pmStream;
	pmStream << PayloadProtocol::LRIGenericAckWithFormat(0,false)
			 << Level2Wrapper(l2Type, transactionNumber, bsLevel1.AsBytes());

	PayloadMessage::Ptr result;
	result.reset(new PayloadMessage(PayloadMessage::LRI_ACK, PayloadManagerClient::GetMyBoardAddress(), FLOAT_TARGET_COMMAND_INTERFACE, 0, pmStream.AsBytes()));
	return result;
}

PayloadMessage::Ptr PayloadInterfaceController::GetSetParamMessage(const CmdLineParser& parser, unsigned startParam, uint32_t transactionNumber)
{
	uint8_t numParameters = 0;
	PPSByteStream paramStream;
	const CmdLineParser::StringArray& parserArgs = parser.GetTokens();

	for (unsigned idx = startParam; idx < parserArgs.size(); ++idx)
	{
		const string& arg = parserArgs[idx];
		vector<string> sa;
		boost::split(sa, arg, boost::is_any_of("="), boost::token_compress_off);

		if (sa.size() != 2)
		{
			stringstream stm;
			stm << "Expected 2 items like paramName=value in equation '" << arg << "', but " << sa.size() << " found";
			throw invalid_argument(stm.str());
		}

		string temp = sa[0];
		boost::to_upper(temp);
		NameToSetParamInfoMap::const_iterator nci = nameToSPIM.find(temp);

		if (nci == nameToSPIM.end())
		{
			stringstream stm;
			stm << "Unknown item " << sa[0];
			throw invalid_argument(stm.str());
		}

		const PayloadInterfaceController_Config::SetParamInfo& spi = *nci->second;;
		++numParameters;
		paramStream << nci->second->paramID;

		uint8_t len;

		if (boost::iequals(spi.paramType,"BOOLEAN"))
		{
			len = 1;
			uint8_t val = StringToBoolean(sa[1]);
			paramStream << len << val;
		}
		else if (boost::iequals(spi.paramType,"UINT8"))
		{
			len = 1;
			uint8_t val = LexicalCast<uint16_t>(sa[1]);
			paramStream << len << val;
		}
		else if (boost::iequals(spi.paramType,"INT8"))
		{
			len = 1;
			int8_t val = LexicalCast<int16_t>(sa[1]);
			paramStream << len << val;
		}
		else if (boost::iequals(spi.paramType,"UINT16"))
		{
			len = 2;
			uint16_t val = LexicalCast<uint16_t>(sa[1]);
			paramStream << len << val;
		}
		else if (boost::iequals(spi.paramType,"INT16"))
		{
			len = 2;
			int16_t val = LexicalCast<int16_t>(sa[1]);
			paramStream << len << val;
		}
		else if (boost::iequals(spi.paramType,"UINT32"))
		{
			len = 4;
			uint32_t val = LexicalCast<uint32_t>(sa[1]);
			paramStream << len << val;
		}
		else if (boost::iequals(spi.paramType,"INT32"))
		{
			len = 4;
			int32_t val = LexicalCast<int32_t>(sa[1]);
			paramStream << len << val;
		}
		else
		{
			stringstream stm;
			stm << "Item " << sa[0] << " configured for unknown type " << nci->second->paramType;
			throw invalid_argument(stm.str());
		}
	}

	CMD_FMT_COMMON	type;
	type.fmtID = FMT_RCV_SET_PARAMS;
	type.command = CMD_SET_PARAMETERS;

	PPSByteStream bsLevel1;
	bsLevel1 << type;
	bsLevel1.Put(numParameters);
	bsLevel1 << ContainerOut<const ByteArray>(paramStream.AsBytes());

	PPSByteStream msgStream;

	msgStream << PayloadProtocol::LRIGenericAckWithFormat(0,false)
	          << Level2Wrapper(RCV_CMD, transactionNumber, bsLevel1.AsBytes());

	PayloadMessage::Ptr result;
	result.reset(new PayloadMessage(PayloadMessage::LRI_ACK, PayloadManagerClient::GetMyBoardAddress(), FLOAT_TARGET_COMMAND_INTERFACE, 0, msgStream.AsBytes()));

	return result;
}

void PayloadInterfaceController::ReportAlarm(uint16_t senderAddress, const LiquidRobotics::SMACFramework::ByteArray& level1SysInfo)
{
	PPSByteStream bs;
	bs << PayloadProtocol::LRIGenericAck(0, false)
	   << Level2Wrapper(SND_SYSTEM_INFO, level1SysInfo, Level2Wrapper::WITHOUT_CRC, true); // FSReport

	// --- now stuff the payload packet bytes into a message (transaction ID set to 0 here, will be corrected later) ---
	PayloadMessage::Ptr payloadMessage;
	payloadMessage.reset(new PayloadMessage(PayloadMessage::LRI_ACK, PayloadManagerClient::GetMyBoardAddress(), FLOAT_TARGET_IRIDIUM_INTERFACE, 0, bs.AsBytes()));

	// --- now queue up the message for transmission later ---
	Logger::Locked(GetLogger())->Info << "Enqueuing alarm/event message " << *payloadMessage << endl;
	EnqueueMessage(payloadMessage);
}

}
}
