//------------------------------------------------------------------------------
// <copyright file="GPSEventSource_Simulation.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 some methods of GPSEventSource's simulation classes
// </summary>
//
// <owner>Mike Cookson</owner>
//------------------------------------------------------------------------------

#include "GPSEventSource.h"
#include "LqrSMACUptime.h"

using namespace std;
using namespace LiquidRobotics::SMACFramework;

namespace LiquidRobotics {
namespace GPSListenerLib {

GPSEventSource::SimulationMode::SimulationMode()
: simulating(false)
, speedKt(0.0)
, radiusM(0)
, movingCW(false)
, startTime(GetSystemUpTimeSecondsDouble())
, angle(0.0)
{
}

GPSEventSource::SimulationMode::SimulationMode(Geodetic::Location _locCenter, double _speedKt, double _radiusM, bool _movingCW, double _angle)
: simulating(true)
, locCenter(_locCenter)
, speedKt(_speedKt)
, radiusM(_radiusM)
, movingCW(_movingCW)
, startTime(GetSystemUpTimeSecondsDouble())
, angle(_angle)
{
}

double GPSEventSource::SimulationMode::GetAngle() const
{
	return GetAngle(GetSystemUpTimeSecondsDouble()-startTime);
}

double GPSEventSource::SimulationMode::GetAngle(double timeOffset) const
{
	if (!simulating || !locCenter.valid || (radiusM <= 0) || (timeOffset < 0))
		return 0;

	const double knotsToMetersPerSecond = 1852.0/3600.0;  // 1 nautical mile = 1.852 km, 1 kt = 1 nm/hr
	const double distanceTraveledM = abs(timeOffset*speedKt*knotsToMetersPerSecond);
	const double pi = 3.14159265359;
	const double twopi = 2*pi;
	const double circumferenceM = abs(twopi*radiusM);

	double angularOffset = 0.0;

	if (circumferenceM)
	{
		double intPart;
		const double revOffsetM = modf(distanceTraveledM/circumferenceM, &intPart);

		if (revOffsetM)
			angularOffset = 360.0*revOffsetM;
	}

	const double result = movingCW ? (angle+angularOffset) : (angle-angularOffset);
	return result;
}

GPSEventSource::Geodetic::Location GPSEventSource::SimulationMode::GetPosition() const
{
	return GetPosition(GetSystemUpTimeSecondsDouble()-startTime);
}

Geodetic::Location GPSEventSource::SimulationMode::GetPosition(double timeOffset) const
{
	if (!simulating || !locCenter.valid || (radiusM <= 0) || (timeOffset < 0))
		return locCenter;

	const double knotsToMetersPerSecond = 1852.0/3600.0;  // 1 nautical mile = 1.852 km, 1 kt = 1 nm/hr
	const double distanceTraveledM = abs(timeOffset*speedKt*knotsToMetersPerSecond);
	const double pi = 3.14159265359;
	const double twopi = 2*pi;
	const double circumferenceM = abs(twopi*radiusM);

	double angularOffset = 0.0;

	if (circumferenceM)
	{
		double intPart;
		const double revOffsetM = modf(distanceTraveledM/circumferenceM, &intPart);

		if (revOffsetM)
			angularOffset = 360.0*revOffsetM;
	}

	Geodetic::BearingAndDistance bd(movingCW ? (angle+angularOffset) : (angle-angularOffset), 0.001*radiusM);
	Geodetic::Location result = Geodetic::OffsetLocation(locCenter, bd);
	return result;
}

int GPSEventSource::SimulationWorker::DetermineReadTimeout()
{
	int result;

	if (enabled)
	{
		timeval tv;

		if (gettimeofday(&tv,NULL))
			result = -1;
		else
			result = 1000-(tv.tv_usec+500)/1000;
	}
	else
		result = -1;

	return result;
}

void GPSEventSource::StartSimulation(double latCenter, double lonCenter, double speedKt, double radiusM, bool movingCW)
{
	ILockableConcept::Lock lock(simMutex);
	SimulationModePtr newModePtr;
	newModePtr.reset(new SimulationMode(Geodetic::Location(latCenter,lonCenter), speedKt, radiusM, movingCW, simMode.GetAngle()));
	simWorker.EnqueueWorkItem(newModePtr);
}

void GPSEventSource::StopSimulation()
{
	ILockableConcept::Lock lock(simMutex);
	SimulationModePtr newModePtr;
	newModePtr.reset(new SimulationMode());
	newModePtr->angle = simMode.GetAngle();
	simWorker.EnqueueWorkItem(newModePtr);
}

void GPSEventSource::OnSimulationWorkerItem(SimulationWorkerBase& sender, const SimulationModePtr& item)
{
	ILockableConcept::Lock lock(simMutex);
	if (item)
	{
		simMode = *item;
		simWorker.enabled = simMode.simulating;
	}
}

void GPSEventSource::OnSimulationWorkerTimeout(QueueWorkerBase& sender)
{
	ILockableConcept::Lock lock(simMutex);

	if (simMode.simulating)
	{
		const double speedKt = simMode.speedKt;
		Geodetic::Location newLocation = simMode.GetPosition();
		lock.unlock();

		GPSSampleEvent ev(broker);
		timeval tv;

		if (gettimeofday(&tv, NULL))
		{
			tv.tv_sec = 0;
			tv.tv_usec = 0;
		}

		ev.SetTime(tv);
		ev.SetGPS(newLocation.lat, newLocation.lon, 5);
		ev.SetElevation(0.0);
		ev.SetCourse(0.0, 0.0, speedKt, speedKt*1.852);
		ev.SetSimulated(true);

		try { GPSSampleSignal(*this, ev); }
		catch(...) { }
	}
}

}
}
