//------------------------------------------------------------------------------
// <copyright file="GPSListenerTestProgram.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 the main module of GPS listener test program
// </summary>
//
// <owner>Mike Cookson</owner>
//------------------------------------------------------------------------------

#include "LqrSMACDummyLogger.h"
#include "LqrSMACStdOutLogger.h"
#include "LqrSMACObjectBroker.h"
#include "EventTransceiver.h"
#include "EventTransceiver_Settings.h"
#include "GPSSampleEvent.h"
#include <time.h>

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

static void Wait(int numSec)
{
	if (numSec > 0)
	{
		timespec tsStart;

		if (clock_gettime(CLOCK_REALTIME, &tsStart))
		{
			time_t tNow = time(NULL);
			time_t tNext = tNow-(tNow%numSec)+numSec;

			do
			{
				if (sleep(tNext-tNow) || (abs(tNext-time(NULL)) > numSec))
					tNow = tNext;
				else
					tNow = time(NULL);

			} while(tNext > tNow);
		}
		else
		{
			timespec tsNext;
			tsNext.tv_sec = tsStart.tv_sec-(tsStart.tv_sec%numSec)+numSec;
			tsNext.tv_nsec = 0;

			timespec tsWait;
			tsWait.tv_nsec = tsNext.tv_nsec-tsStart.tv_nsec;
			tsWait.tv_sec = tsNext.tv_sec-tsStart.tv_sec;

			if (tsWait.tv_nsec < 0)
			{
				tsWait.tv_nsec += 1000000000;
				--tsWait.tv_sec;
			}

			if (tsWait.tv_sec >= 0)
			{
				timespec tsRem;

				do
				{
					const int nanoStatus = nanosleep(&tsWait, &tsRem);
					const int timeShift = abs(tsNext.tv_sec-time(NULL)) > numSec;

					if (!(nanoStatus && (errno == EINTR)) || timeShift)
						memset(&tsRem, '\0', sizeof(tsRem));

				} while(tsRem.tv_sec || tsRem.tv_nsec);
			}
		}
	}
}

extern "C" SHARED_LIB_PUBLIC int GPSListenerTestProgram(int argc, char* argv[])
{
	tzset();

	ObjectBroker broker(false);
	broker.Initialize(argc, argv);

	EventTransceiverSettings ets;
	ets.SetEventServiceName("EventManager");
	ets.SetEventServiceKind("");
	ets.SetReceiveEnable(false);
	ets.ClearEventFilter();
	ets.RemoveAllAliasesForEvents();

	//DummyLogger logger;
	StdOutLogger logger("none", 1, Logger::LT_DEBUG);
	string eventName("GPSSample");
	EventType eventType("GPSSample", "LiquidR-SMC");

	try
	{
		EventTransceiver et(ets, broker, logger);
		et.Initialize();

		while(true)
		{
			Wait(5);

			{
				GPSSampleEvent evt(eventName, eventType, broker);
				timeval tv;
				gettimeofday(&tv, NULL);
				evt.SetTime(tv);
				evt.SetGPS(37,-122,6);
				evt.SetCourse(0,1,12,6);
				et.TransmitEvent(evt);
			}

			Wait(5);

			{
				GPSSampleEvent evt(eventName, eventType, broker);
				timeval tv;
				gettimeofday(&tv, NULL);
				evt.SetTime(tv);
				evt.SetGPS(37,-122,6);
				et.TransmitEvent(evt);
			}

			Wait(5);

			{
				GPSSampleEvent evt(eventName, eventType, broker);
				timeval tv;
				gettimeofday(&tv, NULL);
				evt.SetTime(tv);
				et.TransmitEvent(evt);
			}

			Wait(5);

			{
				GPSSampleEvent evt(eventName, eventType, broker);
				timeval tv;
				gettimeofday(&tv, NULL);
				evt.SetTime(tv);
				evt.SetCourse(0,1,12,6);
				et.TransmitEvent(evt);
			}
		}
	}
	catch(CORBA::Exception& ex)
	{
		cout << "Caught CORBA exception " << DecodeCORBAException(ex) << endl;
	}
	catch(exception& ex)
	{
		cout << "Caught exception " << typeid(ex).name() << ": " << ex.what() << endl;
	}

	return 0;
}
