//----------------------------------------------------------------------------------
// <copyright file="SV3RemoteController.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 SV3RemoteController, which talks to Regulus and bridges to the
//  CORBA-based IPC that the SMC uses.
// </summary>
//
// <owner>Mike Cookson</owner>
//----------------------------------------------------------------------------------

#include "SV3RemoteController.h"
#include "FCCMessage.h"
#include "GPSSampleEvent.h"
#include "LqrSMACBooleanParser.h"
#include "LqrSMACCmdLineParser.h"
#include "LqrSMACDetermineFileExtension.h"
#include "LqrSMACGetErrorDescription.h"
#include "LqrSMACIdl.h"
#include "LqrSMACIObjectBroker-inline.h"
#include "LqrSMACLexicalCast.h"
#include "nsextend.h"
#include <vector>
#include <ctype.h>
#include <boost/filesystem.hpp>
#include "LqrSMACIdl.h"

using namespace std;
using namespace LiquidRobotics::SMACFramework;
using namespace LiquidRobotics::EventLib;
using namespace LiquidRobotics::SV3RemotingLib;

namespace LiquidRobotics {
namespace PayloadInterface {

SV3RemoteController::SV3RemoteController(
		IObjectBroker& _broker,
		Logger& _logger,
		const string& _telemConfigPath,
		const SV3NetInterfaceSettings& _sv3NetCfg,
		const PayloadInterfaceController_Config& _config,
		const StorageController::Config& _scSettings)
: LiquidRobotics::EventLib::EventTransceiver(_config.GetEventTransceiverSettings(), _broker, _logger)
//, LocalTransactionCounter()
, PayloadManagerClient(_config.GetPayloadManagerClientSettings(), _broker, *this)
, CommonSendCommand(_broker, *this, _logger, *this, _config, *this)
, SCLookup(_broker, *this, _logger, _scSettings, _config, *this)
, TelemetrySubsystem(_telemConfigPath, *this, _logger, _config, _broker, *this)
, broker(_broker)
, logger(_logger)
, config(_config)
, sv3NetCfg(_sv3NetCfg)
, sv3NetIf(_sv3NetCfg, logger)
, messageForwarder(sv3NetIf, logger)
, pendingStartupSysInfo(true)
, lastStartupSysInfo(0)
, haveSavedPosition(false)
, savedLatitude(0.0)
, savedLongitude(0.0)
, systemGPSOnline(true)
, manualGPSRequested(false)
{
}

SV3RemoteController::~SV3RemoteController()
{
}

void SV3RemoteController::Initialize(Object_ptr serverRef)
{
	EventTransceiver::Initialize(serverRef);			// initialize the event transceiver base class
	PayloadManagerClient::Register(serverRef);			// initialize the payload manager client base class and pass parameters to register it
	SCLookup::Initialize();								// initialize storage controllers
	TelemetrySubsystem::Initialize(true);				// initialize the telemetry subsystem
	CommonSendCommand::Initialize();					// initialize the alarm subsystem

	// before continuing, use the settings to update the JSON registration files
	const string json = PayloadManagerClient::GetJSON();

	RegulusREST::PRHardware::Vector hw;

	try 					{ RegulusREST::PRHardware::InitFromRegJSON(json,hw); }
	catch(exception& ex)	{ Logger::Locked(logger)->Error << "Error decoding hardware JSON (" << ex.what() << ")" << endl; }

	RegulusREST::PRInterface sw;

	try						{ sw.InitFromRegJSON(json); }
	catch(exception& ex)	{ Logger::Locked(logger)->Error << "Error decoding interface JSON (" << ex.what() << ")" << endl; }

	stringstream swverstr;

	swverstr << (unsigned) config.GetFirmwareMajor() << "."
			 << (unsigned) config.GetFirmwareMinor() << "."
			 << (unsigned) config.GetFirmwareRev();

	sw.swver = swverstr.str();
	try {
		PayloadManagerClient::UpdateJSON(hw,sw);
	} catch(exception& ex) {
		Logger::Locked(logger)->Warning << "While updating JSON with initial values, caught " << typeid(ex).name() << ": " << ex.what() << endl;
	}

	// initialize the interface to the VMC
	remoteCommandSubscription = sv3NetIf.remoteCommandSubscriber.Subscribe(boost::bind(&SV3RemoteController::OnRemoteCommand, this, _1, _2, _3, _4, _5, _6));
	shutdownSubscription = sv3NetIf.shutdownSubscriber.Subscribe(boost::bind(&SV3RemoteController::OnShutdown, this, _1, _2));
	commstatusSubscription = sv3NetIf.commstatusSubscriber.Subscribe(boost::bind(&SV3RemoteController::OnCommStatus, this, _1, _2));
	hostGPSSubscription = sv3NetIf.gpsSubscriber.Subscribe(boost::bind(&SV3RemoteController::OnHostGPS, this, _1, _2, _3, _4, _5));
	sv3NetIf.Initialize();
	messageForwarder.Initialize();

	// set up callbacks from payload manager to update the list of registrations
	payloadAddedSubscription = PayloadManagerClient::payloadAddedSubscriber.Subscribe(boost::bind(&SV3RemoteController::OnPayloadAdded, this, _1, _2));
	payloadChangedSubscription = PayloadManagerClient::payloadChangeSubscriber.Subscribe(boost::bind(&SV3RemoteController::OnPayloadChanged, this, _1, _2));
	payloadRemovedSubscription = PayloadManagerClient::payloadRemovedSubscriber.Subscribe(boost::bind(&SV3RemoteController::OnPayloadRemoved, this, _1, _2));
	PayloadManagerClient::SubscribeCallbacks(*this);

	// read the descriptions of all initial devices and note them (callbacks will continue to register new devices as they
	// come up)
	PayloadDescriptionList::Ptr descriptions = PayloadManagerClient::GetPayloadDescriptions(true);
	if (descriptions)
		for (PayloadDescriptionListBase::const_iterator cit = descriptions->begin(); cit != descriptions->end(); ++cit)
		{
			if (*cit)
			{
				PayloadDescription::Ptr payloadDesc = *cit;
				const uint16_t boardAddress = (payloadDesc->GetBoardID() << 8) | payloadDesc->GetTaskID();
				Logger::Locked(logger)->Info << "Registering payload: 0x" << utos(boardAddress,16,4) << " " << payloadDesc->GetReadableName() << endl;
				sv3NetIf.NoteClient(boardAddress, payloadDesc->GetReadableName(), payloadDesc->GetDescription(), payloadDesc->GetDescriptiveJSON());
			}
		}

	// 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(&SV3RemoteController::GPSReceiveHandler, this, _1, _2)
							)));

	// subscribe to events that announce whether the system GPS is up (enable/disable polling of GPS)
	const EventSettings& systemGPSUp = config.GetSystemGPSOnlineSettings();
	InformSignalType& sig2 = SignalForEventName(systemGPSUp.GetName());
	connectionToGpsSystemGPSOnline.reset(
			new ScopedConnection(
					sig2.connect(
							boost::bind(&SV3RemoteController::OnSystemGPSOnline, this, _1, _2)
							)));

	const EventSettings& systemGPSDown = config.GetSystemGPSOfflineSettings();
	InformSignalType& sig3 = SignalForEventName(systemGPSDown.GetName());
	connectionToGpsSystemGPSOffline.reset(
			new ScopedConnection(
					sig3.connect(
							boost::bind(&SV3RemoteController::OnSystemGPSOffline, this, _1, _2)
							)));
}

// IPingable operations
::CORBA::Double SV3RemoteController::Ping()
{
	using namespace LiquidRobotics::SMACFramework;
	stringstream msg;
	msg << "SV3RemoteController::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(logger)->Info << msg.str() << setprecision(15) << result << endl;
	return result;
}

static bool IsPrintableASCII(const string& str)
{
	bool result = true;

	for (string::const_iterator ci = str.begin(); result && (ci != str.end()); ++ci)
		result = isprint(*ci) || isspace(*ci);

	return result;
}

void SV3RemoteController::OnRemoteCommand(SV3NetInterface& sender, uint16_t boardAddress, const string& transactionReference,
		uint32_t conformingReference, bool returnFile, const string& cmdLine)
{
	// == deciding how much of cmdline should be displayed in the log ==
	string debugCmdLine;
	bool isASCIICommand;

	if (!!(isASCIICommand = IsPrintableASCII(cmdLine))) {

		if (cmdLine.length() > 43)
			debugCmdLine = cmdLine.substr(0,20)+"..."+cmdLine.substr(cmdLine.length()-20,20);
		else
			debugCmdLine = cmdLine;

		// here I'm translating some control characters to C-standard format
		stringstream ss;
		const char* const literal = "\r\n\t\f";
		const string translated[] = { "\\r", "\\n", "\\t", "\\f" };

		for (string::const_iterator ci = debugCmdLine.begin(); ci != debugCmdLine.end(); ++ci) {
			const char* fnd = strchr(literal, *ci);
			if (fnd)					ss << translated[fnd-literal];
			else if (iscntrl(*ci))		ss << "\\x" << utos(*ci,16,2);
			else						ss.put(*ci);
		}

		debugCmdLine = ss.str();
	}
	else {
		stringstream ss;
		ss << "(" << cmdLine.length() << " bytes)";
		debugCmdLine = ss.str();
	}

	Logger::Locked(logger)->Info << "OnRemoteCommand boardAddress=0x" << utos(boardAddress,16,4) << " ref=" << transactionReference
			<< " conformingRef=0x" << ultos(conformingReference,16,8) << " returnFile=" << BooleanToString(returnFile)
			<< " cmd=" << debugCmdLine << endl;

	ByteArray literalData;
	literalData.InitFromLiteralString(cmdLine);

	SV3NetChannel::Ptr channelp = sender.FindChannel(boardAddress);
	const bool haveSC = channelp ? HaveSC(boardAddress) : false;
	stringstream ssError;
	bool haveError = false;
	ProxySC::Ptr proxyp;

	if (!channelp) {
		ssError << "No channel found for board address 0x" << utos(boardAddress,16,4);
		haveError = true;
	}
	else if (!haveSC) {
		ssError << "No storage controller for board address 0x" << utos(boardAddress,16,4);
		haveError = true;
	}
	else {
		proxyp = GetSCPtr(boardAddress);
		proxyp->ExecuteCommand(literalData, conformingReference, boardAddress, isASCIICommand, returnFile, true);
	}

	if (haveError) {
		Logger::Locked(logger)->Warning << "OnRemoteCommand: " << ssError.str() << endl;
		try						{ channelp->NoteCommandOutput(transactionReference, ssError.str(), true, false); }
		catch(exception& ex)	{ Logger::Locked(logger)->Warning << ex.what() << endl; }

	}

	if (!proxyp)
		try 					{ sender.RemoveTransactionReference(transactionReference); }
		catch(exception& ex)	{ Logger::Locked(logger)->Warning << ex.what() << endl; }
}

void SV3RemoteController::OnShutdown(SV3NetInterface& sender, SV3NetChannel& channel)
{
	Logger::Locked(logger)->Info << "Got shutdown notification" << endl;

	if (config.GetPowerOffDelay()) {
		Logger::Locked(logger)->Info << "Requesting shutdown delay of " << (unsigned) config.GetPowerOffDelay() << " sec" << endl;
		try						{ channel.DelayShutdown(config.GetPowerOffDelay()); }
		catch(exception& ex)	{ Logger::Locked(logger)->Warning << ex.what() << endl; }
	}

	if (config.GetShutdownVerb().length() || !config.GetShutdownEventSettings().Empty())
	{
		InformDemoServerOfShutdown();
		SendShutdownEvent();

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

		ShutdownSystem();

		Logger::Locked(logger)->Info << "Waiting while system shuts down" << endl;
		Semaphore haltSem(0);
		haltSem.Wait();
	}
	else
		Logger::Locked(logger)->Info << "Not actually halting since shutdown verb is empty" << endl;
}

void SV3RemoteController::OnPayloadAdded(PayloadManagerClient& sender, const PayloadDescription& payloadDesc)
{
	const uint16_t boardAddress = PayloadManagerClient::TranslateBoardAddress((payloadDesc.GetBoardID() << 8) | payloadDesc.GetTaskID());
	Logger::Locked(logger)->Info << "Payload added: 0x" << utos(boardAddress,16,4) << " " << payloadDesc.GetReadableName() << endl;
	ILockableConceptLock lock(mutex);
	sv3NetIf.NoteClient(boardAddress, payloadDesc.GetReadableName(), payloadDesc.GetDescription(), payloadDesc.GetDescriptiveJSON());
}

void SV3RemoteController::OnPayloadChanged(PayloadManagerClient& sender, const PayloadDescription& payloadDesc)
{
	const uint16_t boardAddress = PayloadManagerClient::TranslateBoardAddress((payloadDesc.GetBoardID() << 8) | payloadDesc.GetTaskID());
	Logger::Locked(logger)->Info << "Payload changed: 0x" << utos(boardAddress,16,4) << " " << payloadDesc.GetReadableName() << endl;
	ILockableConceptLock lock(mutex);
	sv3NetIf.NoteClient(boardAddress, payloadDesc.GetReadableName(), payloadDesc.GetDescription(), payloadDesc.GetDescriptiveJSON());
}

void SV3RemoteController::OnPayloadRemoved(PayloadManagerClient& sender, const PayloadDescription& payloadDesc)
{
	const uint16_t boardAddress = PayloadManagerClient::TranslateBoardAddress((payloadDesc.GetBoardID() << 8) | payloadDesc.GetTaskID());
	Logger::Locked(logger)->Info << "Payload removed: 0x" << utos(boardAddress,16,4) << " " << payloadDesc.GetReadableName() << " (ignored)" << endl;
	//ILockableConceptLock lock(mutex);
	//sv3NetIf.NoteClient(boardAddress, payloadDesc.GetReadableName(), payloadDesc.GetDescription(), payloadDesc.GetDescriptiveJSON());
}

// callback for CommandComplete
void SV3RemoteController::SCCommandComplete(ProxySC& sender, const ByteArray& data, long transNumber, unsigned destID, bool complete, unsigned resultNum, bool success)
{
	{
		stringstream ss;

		ss	<< "SCCommandComplete transNumber=" << transNumber
			<< " destID=0x" << utos(destID,16,4)
			<< " complete=" << complete
			<< " resultNum=" << resultNum
			<< " success=" << BooleanToString(success);

		if (logger.LogEnabled(Logger::LT_DEBUG) && (data.size() < 100))
			ss << " data=" << data.ToString() << endl;
		else
			ss << " data=" << data.size() << " bytes" << endl;

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

	const uint16_t boardAddress = sender.GetAddress();
	SV3NetChannel::Ptr channelp = sv3NetIf.FindChannel(boardAddress);
	string remoteRef;

	if (!channelp) {
		Logger::Locked(logger)->Warning << "(SCCommandComplete) no channel matching destination" << endl;
		try {
			remoteRef = sv3NetIf.ConformingReferenceToTransactionReference(boardAddress);
			sv3NetIf.RemoveTransactionReference(remoteRef);
		}
		catch(...) { }
		return;
	}

	// for now, I'm only supporting one output

	try {

		remoteRef = sv3NetIf.ConformingReferenceToTransactionReference(transNumber);

		if (resultNum == 1)
		{
#if 0
			const uint16_t maxReturnedSize = 256*config.GetMaxConsoleOutputBlocks();

			if (data.size() <= maxReturnedSize) {
				Logger::Locked(logger)->Info << "Replying with " << data.size() << " bytes for remote reference " << remoteRef << endl;
				channelp->NoteCommandOutput(remoteRef, data.ToASCIIString(), true, true);
			}
			else {
				ByteArray data2(data.begin(), data.begin()+maxReturnedSize);
				Logger::Locked(logger)->Info << "Replying with " << data2.size() << " bytes for remote reference " << remoteRef << endl;
				channelp->NoteCommandOutput(remoteRef, data2.ToASCIIString(), true, true);
			}
#else
			string summary;
			if (!success) summary = " -- FAILED";
			Logger::Locked(logger)->Info << "Replying with " << data.size() << " bytes for remote reference " << remoteRef << summary << endl;
			channelp->NoteCommandOutput(remoteRef, data.ToASCIIString(), true, success);
#endif
		}
	}
	catch(exception& ex) {
		Logger::Locked(logger)->Warning << ex.what() << endl;
		return;
	}

	try {
		Logger::Locked(logger)->Debug << "Removing remote reference " << remoteRef << endl;
		sv3NetIf.RemoveTransactionReference(remoteRef);
	}
	catch(exception& ex) {
		Logger::Locked(logger)->Warning << ex.what() << endl;
	}
}

// callback for NewFileAvailable
void SV3RemoteController::SCNewFileAvailable(ProxySC& sender, const string& actualFileName, string& remoteFileName)
{
	using namespace boost::filesystem;
	path scPath(sender.GetSC().GetConfig().GetStorageDirectoryName());
	path filePath(actualFileName);
	path fullPath = scPath / filePath;
	string actualFileNameFullPath = fullPath.string();
	string ext = fullPath.extension().string();
	time_t timeNow = time(NULL);

	remoteFileName = ultos((unsigned long) timeNow, 16, 8)+ext;

	Logger::Locked(logger)->Info << "New file available sc=0x" << ultos(sender.GetAddress(), 16, 4)
			<< " actualFileName=" << actualFileName << " (" << actualFileNameFullPath << ")"
			<< " remoteFileName=" << remoteFileName << endl;

	const uint16_t boardAddress = sender.GetAddress();
	SV3NetChannel::Ptr channelp = sv3NetIf.FindChannel(boardAddress);
	string remoteRef;

	if (!channelp) {
		Logger::Locked(logger)->Warning << "(SCNewFileAvailable) no channel matching destination" << endl;
		return;
	}

	try {
		Logger::Locked(logger)->Info << "Calling SendFile for actual file " << actualFileNameFullPath << " remote file " << remoteFileName << " (legacy)" << endl;
		channelp->SendFile(actualFileNameFullPath, remoteFileName, true);
	}
	catch(exception& ex) {
		Logger::Locked(logger)->Warning << "(SCNewFileAvailable) Caught " << typeid(ex).name() << " while sending " << actualFileName << ": " << ex.what() << endl;
	}
}

void SV3RemoteController::InformDemoServerOfShutdown()
{
	using namespace LiquidRobotics::SMAC;

	int countInformed = 0;
	typedef PayloadInterfaceController_Config::StringList StringList;
	const StringList& demoServerList = config.GetDemoServerList();

	for (StringList::const_iterator it = demoServerList.begin(); it != demoServerList.end(); ++it)
	{
		const string& remoteHost = *it;
		Logger::Locked(logger)->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(logger)->Error << "Caught CORBA exception: " << DecodeCORBAException(exc) << " (ignored)" << endl;
		}
		catch(std::exception& exc)
		{
			Logger::Locked(logger)->Error << "Caught exception type "<< typeid(exc).name() << ": " << exc.what() << " (ignored)" << endl;
		}
	}
}

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

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

		TransmitEvent(shutdownEvent);
	}
}

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

	string shutdownVerb = config.GetShutdownVerb();

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

void SV3RemoteController::OnCommStatus(SV3NetInterface& sender, bool commStatus)
{
	Logger::Locked(logger)->Info << "New comm status = " << BooleanToString(commStatus) << endl;
	if (commStatus)
		ReportEncryptedPartitionStatus(true);
	messageForwarder.SetCommunicating(commStatus);
}

void SV3RemoteController::GPSReceiveHandler(EventTransceiver& sender, const IEventBase& receivedEvent)
{
	const EventSettings& gpsSampleEvent = config.GetGpsSampleEventSettings();
	const GPSSampleEvent& evt =
		EventTransceiver::UpcastEventRef<GPSSampleEvent>(receivedEvent, gpsSampleEvent,
				"SV3RemoteController::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;
		TelemetrySubsystem::SavePosition(latitude, longitude);
		CommonSendCommand::SavePosition(latitude, longitude);
	}
	else
		ss << "no gps data";

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

void SV3RemoteController::ReportEncryptedPartitionStatus(bool newState)
{
	using namespace LiquidRobotics::PayloadInterface::FCCMessage;
	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) (newState ? SYSINFOSMCPARTITIONSTATEVALUE_ENABLED : SYSINFOSMCPARTITIONSTATEVALUE_DISABLED);

	PPSByteStream bs;
	bs << Level2Wrapper(SND_SYSTEM_INFO, 0, bsLevel1.AsBytes(), Level2Wrapper::WITH_CRC);

	const ByteArray& bytes = bs.AsBytes();
	SV3NetChannel::Ptr channelp = sv3NetIf.FindChannel(GetMyBoardAddress());

	if (channelp) {
		Logger::Locked(logger)->Info << "Sending encrypted partition status (" << bytes.ToHexString() << ")" << endl;
		try						{ channelp->SendOutputMessage(bytes); }
		catch(exception& ex)	{ Logger::Locked(logger)->Warning << ex.what() << endl; }
	}
	else
		Logger::Locked(logger)->Warning << "No channel found for 0x" << utos(GetMyBoardAddress(),16,4) << ", cannot report encrypted partition status" << endl;
}

void SV3RemoteController::OnSystemGPSOffline(EventTransceiver& sender, const IEventBase& receivedEvent)
{
	Logger::Locked(logger)->Info << "System GPS is now offline" << endl;
	ILockableConceptLock lock(mutex);
	systemGPSOnline = false;
	sv3NetIf.NoteGPSRequired(!systemGPSOnline || manualGPSRequested);
}

void SV3RemoteController::OnSystemGPSOnline(EventTransceiver& sender, const IEventBase& receivedEvent)
{
	Logger::Locked(logger)->Info << "System GPS is now online" << endl;
	ILockableConceptLock lock(mutex);
	systemGPSOnline = true;
	sv3NetIf.NoteGPSRequired(!systemGPSOnline || manualGPSRequested);
}

void SV3RemoteController::OnHostGPS(SV3NetInterface& sender, uint16_t boardAddress, time_t timestamp, double latitude, double longitude)
{
	GPSSampleEvent hostSample(config.GetHostGPSEventSettings(), broker);
	hostSample.SetGeneratorAddress(GetMyBoardAddress());
	hostSample.SetGPS(latitude, longitude, 0);
	timeval tv = { timestamp, 0 };
	hostSample.SetTime(tv);
	hostSample.SetSourceName("host");
	TransmitEvent(hostSample);
}

void SV3RemoteController::TelemetryReportComplete(TelemetryBuffer& sender, TelemetryReportKey reportKey,
		TelemetryBuffer::BoardAddress targetAddress, const LiquidRobotics::SMACFramework::ByteArray& content)
{
	using namespace LiquidRobotics::PayloadInterface::FCCMessage;

	Logger::Locked(logger)->Info << "TelemetryReportComplete for " << reportKey << " target 0x" << utos(targetAddress, 16, 4) << ": " << content << endl;
	Logger::Locked(logger)->Debug << "  telemetry content =" << endl << content.ToStringSplitView() << endl;

	const uint16_t sourceAddress = reportKey.GetPublisherKey().GetBoardAddress();
	SV3NetChannel::Ptr channelp = sv3NetIf.FindChannel(sourceAddress);

	if (!channelp)
	{
		Logger::Locked(logger)->Warning << "No channel found for address 0x" << utos(sourceAddress,16,4) << endl;
		return;
	}

	// messages come in without being fully formed: they need to have the 4-byte transaction number (0) added and they need
	// the checksum recalculated
	PPSByteStream input(content);
	COMM_HEADER hdr;
	ByteArray type1Content;

	try
	{
		input >> hdr.type >> hdr.size;
		ContainerIn<ByteArray> container(type1Content, hdr.size);
		input >> container;
		input.VerifyExhausted();
	}
	catch(exception& ex)
	{
		Logger::Locked(logger)->Warning << "Failed to pull out telemetry information (" << content.ToHexString() << "): " << ex.what();
		return;
	}

	PPSByteStream output;
	output << Level2Wrapper(hdr.type, 0, type1Content, Level2Wrapper::WITH_CRC);

	const ByteArray& outputBytes = output.AsBytes();
	Logger::Locked(logger)->Info << "Queued " << outputBytes.size() << "-byte telemetry message from 0x" << utos(sourceAddress,16,4) << endl;
	ByteArray::Ptr outputp;
	outputp.reset(new ByteArray(outputBytes.begin(), outputBytes.end()));
	messageForwarder.PushPendingMessage(sourceAddress, outputp);

	/*
	Logger::Locked(logger)->Info << "Sending telemetry message to channel 0x" << utos(sourceAddress,16,4) << " (" << outputBytes.ToHexString() << ")" << endl;

	try						{ channelp->SendOutputMessage(outputBytes); }
	catch(exception& ex)	{ Logger::Locked(logger)->Warning << "Output message failed: " << ex.what() << endl; }
	*/
}

bool SV3RemoteController::HandleAdditionalCommands(const string& command, const string& requester, const string& transactionID, string& result)
{
	using namespace LiquidRobotics::SMAC;

	const char* cmdParam = "_command";
	bool handled = true;
	CmdLineParser parser(command);
	const int argCount = parser.Count();
	const string hostInvoke = "HOSTINVOKE";
	const string setAMPSSwitch = "SETAMPSSW";

	if ((argCount > 0) && boost::iequals(parser[0], "HOSTGPS")) {
		if (argCount == 2) {
			bool newState;
			try 					{ newState = StringToBoolean(parser[1]); }
			catch(exception& ex)	{ ThrowCORBAException2<BadParameter>(cmdParam, ex.what()); }

			ILockableConceptLock lock(mutex);
			manualGPSRequested = newState;
			sv3NetIf.NoteGPSRequired(!systemGPSOnline || manualGPSRequested);
			result = "OK";
		}
		else
			ThrowCORBAException2<BadParameter>(cmdParam, "Unexpected number of arguments for HOSTGPS command");
	}
	else if ((argCount > 0) && boost::iequals(parser[0], "KILLPOWER")) {

		if (argCount != 1)
			ThrowCORBAException2<BadParameter>(cmdParam, "Unexpected number of arguments for KILLPOWER command");

		uint16_t boardAddress = GetMyBoardAddress();
		SV3NetChannel::Ptr channelp = sv3NetIf.FindChannel(boardAddress);

		if (!channelp)
		{
			stringstream ss;
			ss << "No channel found for board address " << boardAddress;
			Logger::Locked(logger)->Warning << ss.str() << endl;
			ThrowCORBAException2<BadParameter>(cmdParam, ss.str());
		}

		Logger::Locked(logger)->Info << "Calling TurnPayloadOff on Host from 0x" << utos(boardAddress,16,4) << endl;

		try {
			channelp->TurnPayloadOff();
			result = "OK";
		}
		catch(exception& ex) {
			stringstream ss;
			ss << "Caught " << typeid(ex).name() << " while invoking TurnPayloadOff from 0x" << utos(boardAddress,16,4) << ": " << ex.what() << endl;
			Logger::Locked(logger)->Warning << ss.str() << endl;
			result = ss.str();
		}

	}
	else if ((argCount > 0) && boost::iequals(parser[0], hostInvoke))
	{
		uint16_t boardAddress = GetMyBoardAddress();
		size_t idx = 1;
		const string boardAddressStr = "--boardAddress=";

		if ((argCount > 1) && boost::istarts_with(parser[idx], boardAddressStr))
		{
			try 					{ boardAddress = LexicalCast<uint16_t>(parser[1].substr(boardAddressStr.length())); ++idx; }
			catch(exception& ex)	{ ThrowCORBAException2<BadParameter>(cmdParam, ex.what()); }
		}

		const string tail = boost::trim_copy(parser.GetTail(idx-1));
		SV3NetChannel::Ptr channelp = sv3NetIf.FindChannel(boardAddress);

		if (!channelp)
		{
			stringstream ss;
			ss << "No channel found for board address " << boardAddress;
			Logger::Locked(logger)->Warning << ss.str() << endl;
			ThrowCORBAException2<BadParameter>(cmdParam, ss.str());
		}

		Logger::Locked(logger)->Info << "Invoking from 0x" << utos(boardAddress,16,4) << " script \"" << tail << "\"" << endl;

		try
		{
			result = channelp->InvokeOnServer(tail);
			Logger::Locked(logger)->Info << "VMC replied with " << result.length() << " bytes" << endl;
		}
		catch(exception& ex)
		{
			stringstream ss;
			ss << "Caught " << typeid(ex).name() << " while invoking script from 0x" << utos(boardAddress,16,4) << ": " << ex.what();
			Logger::Locked(logger)->Warning << ss.str() << endl;
			result = ss.str();
		}
	}
	else if ((argCount > 0) && boost::iequals(parser[0], setAMPSSwitch))
	{
		uint16_t boardAddress = GetMyBoardAddress();
		int idx = 1;
		const string boardAddressStr = "--boardAddress=";

		if ((argCount > 1) && boost::istarts_with(parser[idx], boardAddressStr))
		{
			try 					{ boardAddress = LexicalCast<uint16_t>(parser[1].substr(boardAddressStr.length())); ++idx; }
			catch(exception& ex)	{ ThrowCORBAException2<BadParameter>(cmdParam, ex.what()); }
		}

		if ((argCount < (idx+1)) || (argCount > (idx+2))) {
			stringstream ss;
			ss << "Unexpected number of arguments for " << setAMPSSwitch << " command";
			ThrowCORBAException2<BadParameter>(cmdParam, ss.str());
		}

		bool newState;
		try {
			newState = StringToBoolean(parser[argCount-1]);
		}
		catch(exception& ex) {
			ThrowCORBAException2<BadParameter>(cmdParam, ex.what());
		}

		SV3NetChannel::Ptr channelp = sv3NetIf.FindChannel(boardAddress);

		if (!channelp)
		{
			stringstream ss;
			ss << "No channel found for board address " << boardAddress;
			Logger::Locked(logger)->Warning << ss.str() << endl;
			ThrowCORBAException2<BadParameter>(cmdParam, ss.str());
		}

		if (argCount == (idx+2)) {
			const string switchName = parser[argCount-2];
			Logger::Locked(logger)->Info << "Setting payload AMPS switch " << switchName << " for 0x" << utos(boardAddress,16,4) << " " << (newState ? "on" : "off") << endl;
			try
			{
				channelp->SetPayloadAMPSSwitch(switchName, newState);
				result = "OK";
			}
			catch(exception& ex)
			{
				stringstream ss;
				ss << "Caught " << typeid(ex).name() << " while setting payload AMPS switch for 0x" << utos(boardAddress,16,4) << ": " << ex.what();
				Logger::Locked(logger)->Warning << ss.str() << endl;
				result = ss.str();
			}
		}
		else {
			Logger::Locked(logger)->Info << "Setting payload AMPS switch for 0x" << utos(boardAddress,16,4) << " " << (newState ? "on" : "off") << endl;

			try
			{
				channelp->SetPayloadAMPSSwitch(newState);
				result = "OK";
			}
			catch(exception& ex)
			{
				stringstream ss;
				ss << "Caught " << typeid(ex).name() << " while setting payload AMPS switch for 0x" << utos(boardAddress,16,4) << ": " << ex.what();
				Logger::Locked(logger)->Warning << ss.str() << endl;
				result = ss.str();
			}
		}
	}
	else
		handled = false;

	return handled;
}

SV3RemoteController::MessageForwarder::MessageForwarder(SV3NetInterface& _sv3NetIf, Logger& _logger)
: sv3NetIf(_sv3NetIf)
, logger(_logger)
, wakeup(0)
, communicating(false)
, shutdown(false)
{
}

void SV3RemoteController::MessageForwarder::Initialize()
{
	threadp.reset(new boost::thread(boost::bind(&SV3RemoteController::MessageForwarder::Background, this)));
}

void SV3RemoteController::MessageForwarder::Shutdown()
{
	if (threadp && !shutdown) {
		shutdown = true;
		wakeup.Post();
		threadp->join();
		threadp.reset();
	}
}

void SV3RemoteController::MessageForwarder::PushPendingMessage(uint16_t sourceAddress, ByteArray::Ptr contentp)
{
	ItemPtr messagep;
	messagep.reset(new Item);
	messagep->sourceAddress = sourceAddress;
	messagep->contentp = contentp;
	pendingMessages.WriteExpand(messagep);
	wakeup.Post();
}

void SV3RemoteController::MessageForwarder::SetCommunicating(bool _communicating)
{
	communicating = _communicating;
	wakeup.Post();
}

void SV3RemoteController::MessageForwarder::Background()
{
	Logger::Locked(logger)->Info << "Starting MessageForwarder background thread" << endl;
	ItemPtr itemp;
	bool fail = false;

	while(!shutdown) {

		while(!fail && communicating && (itemp || pendingMessages.ReadTest(itemp))) {
			if (itemp) {
				SV3NetChannel::Ptr channelp = sv3NetIf.FindChannel(itemp->sourceAddress);
				if (!channelp)
					Logger::Locked(logger)->Warning << "(MessageForwarder) No channel found for source address 0x" << utos(itemp->sourceAddress,16,4) << endl;
				else {
					Logger::Locked(logger)->Info << "(MessageForwarder) Sending message from 0x" << utos(itemp->sourceAddress,16,4) << " (" << itemp->contentp->ToHexString() << ")" << endl;

					try						{ channelp->SendOutputMessage(*itemp->contentp); itemp.reset(); }
					catch(exception& ex)	{ fail = true; Logger::Locked(logger)->Warning << "(MessageForwarder) Output message failed (buffered): " << ex.what() << endl; }
				}
			}
			if (fail && !shutdown) {
				fail = false;
				const unsigned ms = 15000;
				Logger::Locked(logger)->Info << "(MessageForwarder) Waiting for " << ms << " ms due to last failure" << endl;
				wakeup.TimedWait(ms,false);
			}
		}

		if (!shutdown) {
			int count = pendingMessages.Size();
			if (itemp) ++count;
			Logger::Locked(logger)->Info << "(MessageForwarder) Waiting (communicating=" << BooleanToString(communicating) << " messages=" << count << ")" << endl;
			wakeup.Wait();
		}

	}

	Logger::Locked(logger)->Warning << "Exiting MessageForwarder background thread" << endl;
}

void SV3RemoteController::ReportAlarm(uint16_t senderAddress, const LiquidRobotics::SMACFramework::ByteArray& level1SysInfo)
{
	using namespace LiquidRobotics::PayloadInterface::FCCMessage;

	PPSByteStream output;
	output << Level2Wrapper(SND_SYSTEM_INFO, 0, level1SysInfo, Level2Wrapper::WITH_CRC);

	const ByteArray& outputBytes = output.AsBytes();
	Logger::Locked(logger)->Info << "Queued " << outputBytes.size() << "-byte alarm/event message from 0x" << utos(senderAddress,16,4) << endl;
	ByteArray::Ptr outputp;
	outputp.reset(new ByteArray(outputBytes.begin(), outputBytes.end()));
	messageForwarder.PushPendingMessage(senderAddress, outputp);
}

}
}
