//------------------------------------------------------------------------------
// <copyright file="DemoSensor.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>
// 	Example implementation of a sensor that logs method calls
// </summary>
//
// <owner>Mike Cookson</owner>
//------------------------------------------------------------------------------

#include "LqrSMACIdl.h"
#include "omniORB4/CORBA.h"
#include "DemoSensor.h"
#include "EventPropertyValue.h"
#include "LqrSMACByteArray.h"
#include "LqrSMACGetErrorDescription.h"
#include "LqrSMACLockableConcept.h"
#include "LqrSMACCmdLineParser.h"
#include "GPSSampleEvent.h"
#include "SendCommandCompleteEvent.h"
#include "TelemetryEventBase.h"
#include <cerrno>
#include <ctime>
#include <iostream>
#include <sstream>
#include <stdexcept>
#include <string>
#include <typeinfo>
#include <stdint.h>
#include <boost/thread.hpp>
#include <sys/types.h>
#include <sys/wait.h>

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

//template<typename T> void ThrowCORBAException2() { LiquidRobotics::SMACFramework::ThrowCORBAException2<T>(); }

DemoSensor::DemoSensor(IObjectBroker& _broker, const DemoSensorSettings& _settings, Logger& _logger)
: EventTransceiver(_settings.GetEventTransceiverSettings(), _broker, _logger)
, broker(_broker)
, logger(_logger)
, settings(_settings)
, gpsEventName(_settings.GetGPSEventSettings().GetName())
, gpsEventType(_settings.GetGPSEventSettings().GetType())
, lat(0)
, lon(0)
{
}

DemoSensor::~DemoSensor()
{
}

void DemoSensor::Initialize(Object_ptr _pServer, PayloadManagerClient::Ptr _pmClient)
{
	try
	{
		// instantiate and initialize the serial port (opens the port, throws on error)
		serialPort = settings.GetSerialPortSettings().CreatePort(logger);

		// create a listener and subscribe to its callbacks, then start it with Initialize()
		serialListener.reset(new SerialListener(*serialPort, settings.GetSerialListenerSettings()));
		connectionToSerialListener = serialListener->Subscribe(boost::bind(&DemoSensor::OnSerialData, this, _1, _2));
		serialListener->Initialize();
	}
	catch(exception& ex)
	{
		Logger::Locked(logger)->Error << "While opening serial port and listener, caught " << typeid(ex).name() << ": " << ex.what() << endl;

		// clear the subscription to SeriallListener if the connection was made
		if (serialListener && connectionToSerialListener)
		{
			ILockableConceptLock lock(serialListener->FiringMutex());
			connectionToSerialListener.reset();
		}

		// shut down the serial listener instance and clear the pointer
		if (serialListener)
		{
			serialListener->Shutdown();
			serialListener.reset();
		}

		// deallocate the serial port
		if (serialPort)
			serialPort.reset();

		// forward the exception back to the caller
		// throw;
		// -- I'm going to let this fall through to prevent crashing if the serial port won't open
	}

	pmClient = _pmClient;					// save a copy of payload manager client

	// register a class factory to wrap incoming GPS events with the GPSSampleEvent class
	RegisterEventClassFactory(gpsEventType, new EventTransceiver::TEventClassFactory<GPSSampleEvent>());

	// create a signal for the GPS event by name and subscribe to it (call GPSSampleHandler on each event)
	InformSignalType& sig0 = SignalForEventName(gpsEventName);
	connectionToGPSEvent.reset( new ScopedConnection(sig0.connect(boost::bind(&DemoSensor::GPSSampleHandler, this, _1, _2))) );

	// create a signal for the Shutdown event by name and subscribe to it (there's no additional data
	// associated with it, so I don't need to register a class factory)
	InformSignalType& sig1 = SignalForEventName(settings.GetSystemShutdownEventSettings().GetName());
	connectionToSystemShutdownEvent.reset( new ScopedConnection(sig1.connect(boost::bind(&DemoSensor::OnSystemShutdown, this, _1, _2))) );

	///! \todo subscribe to more external CORBA messages here
	// TODO: subscribe to more external CORBA messages here: add message to
	// receive to the XML file's "eventNameFilter" list, and add EventSettings
	// for each type of event so that class factories can be registered and
	// callbacks can be set up

	// finish initializing the EventTransceiver base class
	EventTransceiver::Initialize(_pServer);

	//! \todo start background polling loop, if desired
	// TODO: start background polling loop, if desired
}

//! Shut down any resources opened by the server
void DemoSensor::Shutdown()
{
	// clear the subscription to SeriallListener if the connection was made
	if (serialListener && connectionToSerialListener)
	{
		ILockableConceptLock lock(serialListener->FiringMutex());
		connectionToSerialListener.reset();
	}

	// shut down the serial listener instance and clear the pointer
	if (serialListener)
	{
		try { serialListener->Shutdown(); } catch(...) { }
		serialListener.reset();
	}

	// deallocate the serial port
	if (serialPort)
		try { serialPort.reset(); } catch(...) { }

	// shut down the base class event transceiver
	EventTransceiver::Shutdown();
}

::CORBA::Double DemoSensor::Ping()
{
	using namespace LiquidRobotics::SMACFramework;

	::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
	{
		Logger::Locked(logger)->Warning << "DemoSensor::Ping: (gettimeofday returned error #" << errSave << " '" << GetErrorDescription(errSave) << "') " << endl;
		result = 0;
	}

	Logger::Locked(logger)->Info << "DemoSensor::Ping: " << setprecision(15) << result << endl;
	return result;
}

EnumerationData* DemoSensor::GetEnumerationData()
{
	EnumerationData_var result;
	result = new EnumerationData;
	return result._retn();
}

void DemoSensor::PowerOff(::LiquidRobotics::SMAC::PowerOffTimeType offDelay)
{
	logger.Info << "PowerOff offDelay=" << offDelay << endl;
}

void DemoSensor::PowerOn()
{
	logger.Info << "PowerOn" << endl;
}

BoardIDType DemoSensor::boardID()
{
	return pmClient->GetMyBoardID();
}

LiquidRobotics::SMAC::PayloadDescription* DemoSensor::description()
{
	Desc::CORBAEntity_var result = settings.GetPayloadManagerClientSettings().GetPayloadDescription().GetEntity();
	return result._retn();
}

EPayloadEnableState DemoSensor::enableState()
{
	return EPES_ENABLED;
}

PowerOffTimeType DemoSensor::requestOffTime()
{
	return (PowerOffTimeType) 0;
}

PowerOnTimeType DemoSensor::requestOnTime()
{
	return (PowerOnTimeType) 0;
}

ComponentVersion* DemoSensor::softwareVersion()
{
	ComponentVersion_var result;
	result = new ComponentVersion;
	result->versionString = CORBA::string_dup("1.0.1");
	result->majorVersion = 1;
	result->minorVersion = 0;
	result->subminorVersion = 1;

	return result._retn();
}

TaskIDType DemoSensor::taskID()
{
	return pmClient->GetMyTaskID();
}

SensorSample* DemoSensor::SampleNow(const Any& sampleParameter)
{
	using namespace LiquidRobotics::SMACFramework;

	ThrowCORBAException2<BadParameter>();
	return NULL;
}

void DemoSensor::SendCommand(const ::CORBA::Any& command, const char* requester, const char* transactionID, ::CORBA::Any_OUT_arg result)
{
	using namespace LiquidRobotics::SMACFramework;
	const char* cmd0;					// null-terminated form of the "command" parameter

	// convert CORBA "any" to string, and test if the conversion to string
	// actually worked; if not, then throw an exception (parameter must be
	// a string) -- note that with const char*, the data is still owned by
	// the "any"
	if (!(command >>= cmd0))
	{
		logger.Error << "Incoming command type is not a string" << endl;
		ThrowCORBAException2<BadParameter>();
	}

	// make the null-terminated string into full string
	const string cmd = cmd0;

	// logging the invocation
	Logger::Locked(logger)->Info << "DemoSensor::SendCommand command='" << cmd << "' requester='" << requester << "' transactionID='" << transactionID << "'" << endl;

	// *** Nothing is actually implemented here but the incoming command
	// *** could be parsed and the result could be returned immediately,
	// *** or by a parallel thread
	string resultStr;

	///! \todo implement more commands here
	// TODO: implement more commands here

	CmdLineParser parser(cmd);
	const CmdLineParser::StringArray& tokens = parser.GetTokens();
	bool handled = false;

	// (immediate return)
	if (tokens.size() >= 1)
	{
		handled = true;

		if (boost::iequals(tokens[0], "make-report"))
			resultStr = "report text";
		else if (boost::iequals(tokens[0], "repeat-a"))
		{
			if (tokens.size() != 2)
				ThrowCORBAException2<BadParameter>("command", "Unexpected number of arguments to repeat-a command");
			resultStr = string("repeats:")+string(boost::lexical_cast<unsigned>(tokens[1]), 'A');
		}
		// (result will take some time to complete, so return later)
		else if (boost::iequals(tokens[0], "asynch-cmd"))
		{
			if (tokens.size() > 2)
				ThrowCORBAException2<BadParameter>("command", "Unexpected number of arguments to asynch-cmd command");
			unsigned timeout = (tokens.size() > 1) ? boost::lexical_cast<unsigned>(tokens[1]) : 60;
			string req = requester;
			string trans = transactionID;
			boost::thread thd(boost::bind(&DemoSensor::AsynchCmd, this, req, trans, timeout));
			throw LiquidRobotics::SMAC::WillCompleteAsynchronously();
		}
		else
			handled = false;
	}

	if (!handled)
	{
		stringstream msg;
		msg << "Unknown command: " << cmd;
		ThrowCORBAException2<BadParameter>("command", msg.str());
	}

	// return any immediate result as a string
	result.ptr() = new Any;
	*result.ptr() <<= resultStr.c_str();
}

void DemoSensor::AsynchCmd(const string requester, const string transID, unsigned timeoutSec)
{
	// == handle the command ==
	Logger::Locked(logger)->Debug << "AsynchCmd with timeoutSec=" << timeoutSec << endl;
	sleep(timeoutSec);
	const string commandOutput = "12345";

	// SendCommandCompleteEvent contains the packing algorithm for
	// asynchronously-completing commands; first instantiate the
	// event and include the saved requester and transaction ID, then
	// add the command output (Result), which is usually a string
	SendCommandCompleteEvent evt(settings.GetCommandCompleteEventSettings(), broker);
	evt.Complete(true);
	evt.Requester(requester);
	evt.TransactionID(transID);

	EventPropertyValue result(broker);
	result.SetString(commandOutput);
	Any anyResult;
	result.CopyOut(anyResult);
	evt.Result(anyResult);

	// transmit the event via CORBA (using the EvenTransceiver base class)
	TransmitEvent(evt);
}

void DemoSensor::SetSamplePending(const Any& sampleParameter)
{
	using namespace LiquidRobotics::SMACFramework;

	ThrowCORBAException2<BadParameter>();
}

ComponentVersion* DemoSensor::DemoSensor::hardwareVersion()
{
	ComponentVersion_var result;
	result = new ComponentVersion;
	result->versionString = CORBA::string_dup("no version");
	result->majorVersion = 0;
	result->minorVersion = 0;
	result->subminorVersion = 0;

	return result._retn();
}

SensorSampleIntervalType DemoSensor::sensorSampleInterval()
{
	return (SensorSampleIntervalType) 0;
}

void DemoSensor::sensorSampleInterval(SensorSampleIntervalType _v)
{
}

Boolean DemoSensor::samplePending()
{
	return (::CORBA::Boolean) 0;
}

void DemoSensor::GPSSampleHandler(EventTransceiver& sender, const IEventBase& receivedEvent)
{
	// display the event to the log
	stringstream desc;
	receivedEvent.Describe(desc);
	Logger::Locked(logger)->Info << "(DemoSensor::GPSSampleHandler) Received " << desc.str() << endl;

	// assuming IEventBase is actually a GPSSampleEvent, attempt to up-cast it
	const GPSSampleEvent& sample = EventTransceiver::UpcastEventRef<GPSSampleEvent>(receivedEvent, gpsEventType, "DemoSensor::GPSSampleHandler");

	// if the event has a GPS reading, save the data
	if (sample.HasGPS())
	{
		lat = sample.GetLatitude();
		lon = sample.GetLongitude();
	}
}

//! Turns power on or off to the sensor, returns TRUE if successful and FALSE if failed
bool DemoSensor::PowerSensor(
			bool powerOn,				/**< [in] Turn sensor power on (true) or off (false) */
			bool throwException 		/**< [in] TRUE if throwing an exception on error, FALSE if returning FALSE */
		)
{
	Logger::Locked(logger)->Info << "Setting sensor power " << (powerOn ? "ON" : "OFF") << endl;
	const string command = boost::trim_copy(powerOn ? settings.GetPowerOnVerb() : settings.GetPowerOffVerb());
	bool succeeded;

	if (!command.empty())
	{
		int status = system(command.c_str());
		succeeded = WIFEXITED(status) && !WEXITSTATUS(status);

		if (!succeeded && throwException)
		{
			if (WIFEXITED(status))
			{
				const int exitStatus = WEXITSTATUS(status);

				if (exitStatus)
				{
					stringstream msg;
					msg << "Power " << (powerOn ? "ON" : "OFF") << " verb (" << command << ") reported exit code " << exitStatus;
					throw runtime_error(msg.str());
				}
			}
			else if (WIFSIGNALED(status))
			{
				stringstream msg;
				msg << "Power " << (powerOn ? "ON" : "OFF") << " verb (" << command << ") terminated with signal " << WTERMSIG(status);
				if (WCOREDUMP(status)) msg << " and produced a core dump";
				throw runtime_error(msg.str());
			}
			else
			{
				stringstream msg;
				msg << "Power " << (powerOn ? "ON" : "OFF") << " verb (" << command << ") returned with an unexpected code " << status;
				throw runtime_error(msg.str());
			}
		}
	}
	else
		succeeded = true;

	return succeeded;
}

//! Callback for received data from the serial port
void DemoSensor::OnSerialData(
		SerialListener& sender,			/**< [in] Reference to instance reporting data */
		const ByteArray& data			/**< [in] Data received (0 bytes means end of received data) */
		)
{
	if (data.size())
	{
		///! \todo handle incoming data
		// TODO: handle incoming data

		Logger::Locked(logger)->Info << "Received data:" << endl << data.ToStringSplitView() << endl << endl;
	}
}

//! Reports some telemetry as a CORBA event (configure PayloadInterface-telemetry-config.xml to convert this to a binary package)
void DemoSensor::ReportTelemetryExample(
		float data1,					/**< [in] Sensor data item 1 to report */
		uint16_t data2					/**< [in] Sensor data item 2 to report */
		)
{
	///! \todo re-implement packing this as a class so that the encoding and decoding can be reused
	// TODO: re-implement packing this as a class so that the encoding and decoding can be reused

	///! \todo move these settings to settings class so that they can be modified without recompiling
	// TODO: move these settings to settings class so that they can be modified without recompiling
	EventSettings myEventSettings("YoyodyneWaterSample", "YoyodyneWaterSampleType", "DriverMakersInc");
	TelemetryEventBase sample(myEventSettings, broker);
	sample.SetGeneratorAddress(pmClient->GetMyBoardAddress());
	sample.SetGenerationLocation(lat,lon);

	{ EventPropertyValue epvData1(broker);
	  epvData1.SetSingleFloat(data1);
	  sample.SetProperty("YOYODYNE_DATA1", epvData1); }

	{ EventPropertyValue epvData2(broker);
	  epvData2.SetUInt16(data2);
	  sample.SetProperty("YOYODYNE_DATA2", epvData2); }

	TransmitEvent(sample);
}

void DemoSensor::OnSystemShutdown(
		EventTransceiver& sender,		 /**< [in] Reference of EventTransceiver sending the event */
		const IEventBase& receivedEvent /**< [in] Wrapper for the received event */
		)
{
	///! \todo handle system shutdown, save files, etc.
	// TODO: handle system shutdown, save files, etc.
}
