//-------------------------------------------------------------------------------
// <copyright file="GPSEventSource.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 GPSEventSource class, which patches data received from
//   a serial port into the NMEA receiver and fires events to clients.
// </summary>
//
// <owner>Mike Cookson</owner>
//-------------------------------------------------------------------------------

#include "GPSEventSource.h"
#include "GPSSampleEvent.h"
#include "LqrSMACTZTools.h"
#include <boost/foreach.hpp>
#include <boost/bind.hpp>
#include <boost/lexical_cast.hpp>
#include <time.h>

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

template<typename T>
bool DoCast(const string& input, T& output, const string& varName, const string& section, bool doThrow = true)
{
	bool success;

	try
	{
		output = boost::lexical_cast<T>(input);
		success = true;
	}
	catch(...)
	{
		stringstream ss;
		ss << "Failed to convert " << varName << " value \"" << input << "\" to " << typeid(output).name() << " in " << section;

		if (doThrow)
			throw runtime_error(ss.str());

		success = false;
	}

	return success;
}

namespace LiquidRobotics {
namespace GPSListenerLib {

IMPLEMENT_INNER_EXCEPTION(GPSEventSource,AlreadyInitialized,std::logic_error);
IMPLEMENT_INNER_EXCEPTION(GPSEventSource,NotInitialized,std::logic_error);

const string alreadyInitialized("This class instance was already initialized");
const string notInitialized("This class instance has not been initialized");

const string GPSEventSource::NMEATime = "ZDA";
const string GPSEventSource::NMEALocation = "GGA";
const string GPSEventSource::NMEASpeed = "VTG";
const string GPSEventSource::NMEARecommendedMinimum = "RMC";
const string GPSEventSource::NMEAAccuracy = "GST";

GPSEventSource::GPSEventSource(IObjectBroker& _broker, ISerialPort& _sp, string::size_type _maxString, const ISerialPort::TimeSpan _readTimeout,
							const BaudRateVectorType& brvect, Logger& _logger, Logger& _tonedDownLogger)
: gpsSampleSubscriber(GPSSampleSignal, firingMutex)
, nmeaSubscriber(NMEASignal, firingMutex)
, statusSubscriber(statusSignal, firingMutex)
, broker(_broker)
, sp(_sp)
, maxString(_maxString)
, readTimeout(_readTimeout)
, logger(_logger)
, tonedDownLogger(_tonedDownLogger)
, serialPortReceiver(*this)
, initialized(false)
, shutDown(false)
, brTracker(_logger,_sp, brvect)
, gpsOnlineTrackerExit(false)
, goodGPSReport(0)
{
	expectedMap[NMEATime] = 7;
	expectedMap[NMEALocation] = 15;
	expectedMap[NMEASpeed] = 10;
	expectedMap[NMEARecommendedMinimum] = 13;
	expectedMap[NMEAAccuracy] = 9;
}

GPSEventSource::~GPSEventSource()
{
}

void GPSEventSource::Initialize()
{
	if (initialized)
		throw AlreadyInitialized(alreadyInitialized);

	bool slInit = false;

	try
	{
		serialListener.reset(new SerialListener(sp, maxString, readTimeout));
		serialPortReceiverConnection.reset(new ScopedConnection(serialListener->hereIsSerialData_Signal.connect(serialPortReceiver)));

		serialListener->Initialize();
		slInit = true;
		simWorkerWorkItemProcessedConnection = simWorker.SubscribeWorkItemProcessed(boost::bind(&GPSEventSource::OnSimulationWorkerItem, this, _1, _2));
		simWorkerReadTimeoutConnection = simWorker.SubscribeReadTimeout(boost::bind(&GPSEventSource::OnSimulationWorkerTimeout, this, _1));
		simWorker.Initialize();
		brTracker.Initialize();
		gpsOnlineTracker.reset(new boost::thread(boost::bind(&GPSEventSource::GPSOnlineTrackerBackground, this)));
		initialized = true;
	}
	catch(...)
	{
		simWorkerWorkItemProcessedConnection.reset();
		simWorkerReadTimeoutConnection.reset();
		serialPortReceiverConnection.reset();
		if (slInit)
			serialListener->Shutdown();
		serialListener.reset();
		brTracker.Shutdown();
		throw;
	}

}

void GPSEventSource::Shutdown()
{
	if (!initialized)
		throw NotInitialized(notInitialized);

	else if (!shutDown)
	{
		shutDown = true;

		if (gpsOnlineTracker) {
			gpsOnlineTrackerExit = true;
			goodGPSReport.Post();
			gpsOnlineTracker->join();
			gpsOnlineTracker.reset();
		}

		brTracker.Shutdown();
		simWorkerWorkItemProcessedConnection.reset();
		simWorkerReadTimeoutConnection.reset();
		serialPortReceiverConnection.reset();

		simWorker.Shutdown();
		serialListener->Shutdown();
		serialListener.reset();
	}
}

void GPSEventSource::NoteParsingError(NMEAParser& sender, const string& errorMessage)
{
	Logger::Locked(tonedDownLogger)->Error << "Parsing error: " << errorMessage << endl;
}

void GPSEventSource::ReceivedSerialData(const ByteArray& receivedData)
{
	if (receivedData.size())
		Logger::Locked(tonedDownLogger)->Notice << "ReceivedSerialData with " << receivedData.size() << " bytes" << endl;

	const ByteArray* pData = NULL;

	if (receivedData.size() == 0)
		pData = &pendingBytes;
	else if (receivedData.size() == maxString)
		pendingBytes.insert(pendingBytes.end(), receivedData.begin(), receivedData.end());
	else if (pendingBytes.size() == 0)
		pData = &receivedData;
	else
	{
		pendingBytes.insert(pendingBytes.end(), receivedData.begin(), receivedData.end());
		pData = &pendingBytes;
	}

	if (pData && pData->size())
	{
		NMEAParser::BulkParsed bp(*pData, maxString);

		for (NMEAParser::BulkParsed::ErrorArrayType::const_iterator ci = bp.errorArray.begin(); ci != bp.errorArray.end(); ++ci)
			Logger::Locked(tonedDownLogger)->Error << "Parsing error: " << **ci << endl;

		for (NMEAParser::BulkParsed::ItemArrayType::const_iterator ci = bp.itemArray.begin(); ci != bp.itemArray.end(); ++ci)
		{
			const bool last = ((ci+1) == bp.itemArray.end());
			const NMEAParser::BulkParsedItemPtr& ia = *ci;
			ReceivedSentence(ia->rawSentence, ia->parsedSentence, last);
		}

		if (bp.errorArray.size() || !bp.itemArray.size())
			brTracker.NoteBadSample();
		else
			brTracker.NoteGoodSample();
	}

	pendingBytes.clear();
}

void GPSEventSource::ReceivedSentence(const string& rawLine, const StringArray& sentence, bool last)
{
	try { ILockableConceptLock lock(firingMutex); NMEASignal(*this, rawLine); }
	catch(...) { }

	goodGPSReport.Post();

	const StringArray::size_type sentenceSize = sentence.size();
	stringstream msg;

	try
	{
		msg << "ReceivedSentence '" << rawLine << "' with " << sentenceSize << " tokens";

		if (sentenceSize)
		{
			const string& sentenceName = sentence[0];

			if (sentenceName.length() == 5)
			{
				string sentenceNameTail = sentenceName.substr(2);
				ExpectedMapType::const_iterator expectedIt = expectedMap.find(sentenceNameTail);
				const bool isMatching = (expectedIt != expectedMap.end());;
				const bool isExpected = isMatching && (sentenceSize == expectedIt->second);

				if (!isMatching || isExpected)
				{
					SentenceDataPtr sentenceData;
					sentenceData.reset(new SentenceData);
					sentenceData->sentence = rawLine;
					sentenceData->sentenceArray = CopySentence(sentence);
					sentenceMap[sentenceNameTail] = sentenceData;

					msg << " (stored)";
				}
				else
					msg << " (token count not matching)";
			}
			else
				msg << " (header must be 5 characters long, not stored)";
		}
		else
			msg << " (empty, not stored)";
	}
	catch(...)
	{
		if (msg.rdbuf()->in_avail())
			Logger::Locked(logger)->Notice << msg.str() << endl;

		throw;
	}

	if (msg.rdbuf()->in_avail())
		Logger::Locked(logger)->Notice << msg.str() << endl;

	if (last)
		EndTransmission();
}

bool GPSEventSource::GetLatLon(const string& tag, const string& latNum, const string& latHemi, const string& lonNum,
							const string& lonHemi, double& latResult, double& lonResult)
{
	double tempLat;
	DoCast(latNum, tempLat, "latitude", tag);
	latResult = ConvertPositionValue(tempLat);

	if (!latHemi.compare("S"))
		latResult = -latResult;
	else if (latHemi.compare("N"))
	{
		Logger::Locked(logger)->Warning << tag << ": hemisphere indication '" << latHemi << "' is invalid for latitude" << endl;
		return false;
	}

	double tempLon;
	DoCast(lonNum, tempLon, "longitude", tag);
	lonResult = ConvertPositionValue(tempLon);

	if (!lonHemi.compare("W"))
		lonResult = -lonResult;
	else if (lonHemi.compare("E"))
	{
		Logger::Locked(logger)->Warning << tag << ": hemisphere indication '" << lonHemi << "' is invalid for longitude" << endl;
		return false;
	}

	return true;
}

inline void ConvertTime(const string& ddmmyyStr, const string& hhmmss_ssStr, timeval& resultTime, const string& section)
{
	memset(&resultTime, '\0', sizeof(resultTime));

	int ddmmyy;
	DoCast(ddmmyyStr, ddmmyy, "packed date", section);
	double hhmmss_ss;
	DoCast(hhmmss_ssStr, hhmmss_ss, "packed time", section);
	int packed_time = (int) floor(hhmmss_ss);

	resultTime.tv_sec = UTCToLocal(
					2000+(ddmmyy % 100),
					(ddmmyy/100) % 100,
					ddmmyy/10000,
					packed_time/10000,
					(packed_time/100) % 100,
					packed_time % 100);

	resultTime.tv_usec = floor(1000000*(hhmmss_ss-packed_time));
}

inline void ConvertTime(const string& yr, const string& mo, const string& moday, const string& hhmmss_ssStr, timeval& resultTime, const string& sentence)
{
	memset(&resultTime, '\0', sizeof(resultTime));

	double hhmmss_ss;
	DoCast(hhmmss_ssStr, hhmmss_ss, "hour/minute/seconds packed time", sentence);
	int packed_time = (int) floor(hhmmss_ss);
	int iYear, iMonth, iMonthDay;

	DoCast(yr, iYear, "year", sentence);
	DoCast(mo, iMonth, "month", sentence);
	DoCast(moday, iMonthDay, "day of month", sentence);

	resultTime.tv_sec = UTCToLocal(
					iYear,
					iMonth,
					iMonthDay,
					packed_time/10000,
					(packed_time/100) % 100,
					packed_time % 100);

	resultTime.tv_usec = floor(1000000*(hhmmss_ss-packed_time));
}

inline double RMS(double v1, double v2, double v3)
{
	double result = sqrt((v1*v1 + v2*v2 + v3*v3)/3);
	return result;
}

void GPSEventSource::EndTransmission()
{
	{
		ILockableConcept::Lock simLock(simMutex);
		if (simMode.simulating)
		{
			sentenceMap.clear();
			return;
		}
	}

	GPSSampleEvent ev(broker);
	bool timeErr = (sentenceMap.find(NMEATime) == sentenceMap.end());
	bool locationErr = (sentenceMap.find(NMEALocation) == sentenceMap.end());
	bool speedErr = (sentenceMap.find(NMEASpeed) == sentenceMap.end());
	bool recMinErr = (sentenceMap.find(NMEARecommendedMinimum) == sentenceMap.end());
	bool accuracyErr = (sentenceMap.find(NMEAAccuracy) == sentenceMap.end());

	if (timeErr)
		Logger::Locked(logger)->Debug << "No time string present in NMEA sentence set" << endl;
	else
	{
		timeval tv;

		try
		{
			StringArrayPtr sent0 = sentenceMap[NMEATime]->sentenceArray;
			const StringArray& sent = *sent0;

			ConvertTime(sent[4], sent[3], sent[2], sent[1], tv, NMEATime);
		}
		catch(exception& ex)
		{
			timeErr = true;
			repeatedErrorFilter.ReportError(Logger::Locked(logger)->Warning, ex.what());
		}

		if (!timeErr)
			ev.SetTime(tv);
	}

	if (accuracyErr)
		Logger::Locked(logger)->Debug << "No accuracy string present in NMEA sentence set" << endl;
	else
	{
		double stddevLatitude = 0, stddevLongitude = 0, stddevAltitude = 0;

		try
		{
			StringArrayPtr sent0 = sentenceMap[NMEAAccuracy]->sentenceArray;
			const StringArray& sent = *sent0;

			DoCast(sent[6], stddevLatitude, "latitude standard deviation", NMEAAccuracy);
			DoCast(sent[7], stddevLongitude, "longitude standard deviation", NMEAAccuracy);
			DoCast(sent[8], stddevAltitude, "altitude standard deviation", NMEAAccuracy);
		}
		catch(exception& ex)
		{
			accuracyErr = true;
			repeatedErrorFilter.ReportError(Logger::Locked(logger)->Warning, ex.what());
		}

		if (!accuracyErr)
		{
			const double rmsAccuracy = RMS(stddevLatitude, stddevLongitude, stddevAltitude);
			ev.SetRMSAccuracy(rmsAccuracy);
		}
	}

	if (locationErr)
		Logger::Locked(logger)->Debug << "No location string present in NMEA sentence set" << endl;
	else
	{
		double lat = 0, lon = 0;
		unsigned numSat = 0;
		double antennaAltitude = 0/*, geoidalSeparation = 0*/;

		try
		{
			StringArrayPtr sent0 = sentenceMap[NMEALocation]->sentenceArray;
			const StringArray& sent = *sent0;

			int gpsQuality;
			DoCast(sent[6], gpsQuality, "GPS quality type", NMEALocation);

			if ((gpsQuality != 1) && (gpsQuality != 2))
			{
				locationErr = true;

				Logger::Lock lock(logger);
				logger.Warning << "Location quality value " << gpsQuality << " is not sufficient" << endl;
			}

			if (!locationErr)
				locationErr = !GetLatLon(NMEALocation, sent[2], sent[3], sent[4], sent[5], lat, lon);

			if (!locationErr)
			{
				DoCast(sent[7], numSat, "number of satellites", NMEALocation);
				locationErr = !!sent[10].compare("M") || !!sent[12].compare("M");
				DoCast(sent[9], antennaAltitude, "antenna altitude", NMEALocation);
				//geoidalSeparation boost::lexical_cast<double>(sent[11]);
			}
		}
		catch(exception& ex)
		{
			locationErr = true;
			repeatedErrorFilter.ReportError(Logger::Locked(logger)->Warning, ex.what());
		}

		if (!locationErr)
		{
			ev.SetGPS(lat, lon, numSat);
			ev.SetElevation(antennaAltitude);
		}
	}

	if (speedErr)
	{
		Logger::Lock lock(logger);
		logger.Debug << "No speed string present in NMEA sentence set" << endl;
	}
	else
	{
		double degreesTrue = 0, degreesMagnetic = 0, speedKnots = 0, speedKmh = 0;

		try
		{
			StringArrayPtr sent0 = sentenceMap[NMEASpeed]->sentenceArray;
			const StringArray& sent = *sent0;

			if (sent[2].compare("T") || sent[4].compare("M") || sent[6].compare("N") || sent[8].compare("K"))
			{
				speedErr = true;

				Logger::Lock lock(logger);
				logger.Warning << "One or more sentinel characters (TMNK) are not in the designated locations in speed sentence" << endl;
			}

			if (!speedErr && sent[9].compare("A") && sent[9].compare("D"))
			{
				speedErr = true;

				Logger::Lock lock(logger);
				logger.Warning << "Speed mode indicator '" << sent[9] << "' is not supported" << endl;
			}

			if (!speedErr)
			{
				DoCast(sent[1], degreesTrue, "course degrees true", NMEASpeed);
				DoCast(sent[3], degreesMagnetic, "course degrees magnetic", NMEASpeed);
				DoCast(sent[5], speedKnots, "speed in knots", NMEASpeed);
				DoCast(sent[7], speedKmh, "speed in km/h", NMEASpeed);
			}
		}
		catch(exception& ex)
		{
			speedErr = true;
			repeatedErrorFilter.ReportError(Logger::Locked(logger)->Warning, ex.what());
		}

		if (!speedErr)
			ev.SetCourse(degreesTrue, degreesMagnetic, speedKnots, speedKmh);
	}

	if (!recMinErr)
	{
		try
		{
			StringArrayPtr sent0 = sentenceMap[NMEARecommendedMinimum]->sentenceArray;
			const StringArray& sent = *sent0;

			if (sent[2].compare("A"))
			{
				recMinErr = true;
				Logger::Locked(logger)->Warning << NMEARecommendedMinimum << " sentence reports a navigation receiver warning" << endl;
			}

			const string validModes = "ADEMS";

			if ((sent[12].length() != 1) || (validModes.find(sent[12][0]) == string::npos))
			{
				recMinErr = true;
				Logger::Locked(logger)->Warning << NMEARecommendedMinimum << " sentence reports invalid mode indicator (" << sent[12] << ")" << endl;
			}

			double lat = 0, lon = 0;

			if (!recMinErr)
				recMinErr = !GetLatLon(NMEARecommendedMinimum, sent[3], sent[4], sent[5], sent[6], lat, lon);

			timeval tv;

			if (!recMinErr)
				ConvertTime(sent[9], sent[1], tv, NMEARecommendedMinimum);

			double magDec = 0;

			if (!recMinErr)
			{
				DoCast(sent[10], magDec, "magnetic declination", NMEARecommendedMinimum);

				const string& magDecHemi = sent[11];

				if (!magDecHemi.compare("W"))
					magDec = -magDec;
				else if (magDecHemi.compare("E"))
				{
					recMinErr = true;
					Logger::Locked(logger)->Warning << "Invalid magnetic declination hemisphere (" << magDecHemi << ")" << endl;
				}
			}

			if (!recMinErr)
			{
				if (timeErr)		ev.SetTime(tv);
				if (locationErr)	ev.SetGPS(lat, lon, 1);
				ev.SetMagneticDeclination(magDec);
			}
		}
		catch(exception& ex)
		{
			recMinErr = true;
			repeatedErrorFilter.ReportError(Logger::Locked(logger)->Warning, ex.what());
		}

	}

	if (sentenceMap.size())
	{
		StringArray sentences;

		for (SentenceMap::const_iterator it = sentenceMap.begin(); it != sentenceMap.end(); ++it)
			sentences.push_back(it->second->sentence);

		ev.SetSentences(sentences);
		sentenceMap.clear();
	}

	{
		Logger::Lock lock(logger);
		logger.Notice << "Formed GPS event ";
		ev.Describe(logger.Notice);
		logger.Notice << endl;
	}

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

GPSEventSource::SerialPortReceiver::SerialPortReceiver(GPSEventSource& _eventSource)
: eventSource(_eventSource)
{
}

void GPSEventSource::SerialPortReceiver::operator()(SerialListener& sender, const ByteArray& receivedData)
{
	eventSource.ReceivedSerialData(receivedData);

	//if (receivedData.size() == 0)
	//	eventSource.EndTransmission();
}

GPSEventSource::StringArrayPtr GPSEventSource::CopySentence(const StringArray& sentence)
{
	StringArrayPtr result;
	result.reset(new StringArray);
	result->reserve(sentence.size());

	BOOST_FOREACH(string s, sentence)
		result->push_back(s);

	return result;
}

double GPSEventSource::ConvertPositionValue(double nmeaVal)
{
	const int sign = (nmeaVal < 0) ? -1 : 1;
	const double absVal = fabs(nmeaVal);
	const int mantissa = (int) floor(absVal);
	const double minutes = (mantissa%100) + (absVal-mantissa);
	const double degrees = floor(mantissa/100.0);
	const double result = sign * (degrees + minutes/60.0);

	Logger::Lock lock(logger);
	logger.Debug << "ConvertPositionValue " << nmeaVal << " ==> " << result << endl;
	return result;
}

GPSEventSource::BaudRateTracker::BaudRateTracker(Logger& _logger, ISerialPort& _sp, const BaudRateVectorType& _brvect)
: sp(_sp)
, logger(_logger)
, brvect(_brvect.begin(), _brvect.end())
, sem(0)
, requestShutdown(false)
, baudRateSolid(_brvect.size() == 0)
, numBadSamples(0)
, numGoodSamples(0)
{
}

void GPSEventSource::BaudRateTracker::Initialize()
{
	if (brvect.size())
	{
		Logger::Locked(logger)->Warning << "Trying " << ISerialPort::BaudRate2Num(sp.GetBaudRate()) << " baud for GPS ..." << endl;
		thd =  boost::thread(boost::bind(&BaudRateTracker::Background, this));
	}
}

void GPSEventSource::BaudRateTracker::Shutdown()
{
	if (brvect.size())
	{
		requestShutdown = true;
		sem.Post();
	}
}

bool GPSEventSource::BaudRateTracker::BaudRateSolid() const
{
	return baudRateSolid;
}

void GPSEventSource::BaudRateTracker::NoteGoodSample()
{
	ILockableConceptLock lock(mut);

	if (!baudRateSolid)
	{
		Logger::Locked(logger)->Notice << "- received a good GPS sample" << endl;

		if (++numGoodSamples == 2)
		{
			Logger::Locked(logger)->Notice << "GPS baud rate confirmed at " << ISerialPort::BaudRate2Num(sp.GetBaudRate()) << " baud" << endl;
			baudRateSolid = true;
			numGoodSamples = numBadSamples = 0;
		}

		sem.Post();  // this keeps the background from timing out
	}
}

void GPSEventSource::BaudRateTracker::NoteBadSample()
{
	ILockableConceptLock lock(mut);

	if (!baudRateSolid)
	{
		Logger::Locked(logger)->Notice << "- received a bad GPS sample" << endl;
		++numBadSamples;
		sem.Post();
	}
}

void GPSEventSource::BaudRateTracker::Background()
{
	ILockableConceptLock lock(mut);
	double lastPrint = -60.0;
	Logger::Locked(logger)->Notice << "GPS baud rate tracker starting" << endl;

	while(!requestShutdown && !baudRateSolid)
	{
		double now = GetSystemUpTimeSecondsDouble();
		if ((now-lastPrint) >= 60) { lastPrint = now; Logger::Locked(logger)->Warning << "Trying baud rates for GPS" << endl; }
		lock.unlock();
		const bool tw = sem.TimedWait(6000, false);
		lock.lock();
		if (!tw)
			Logger::Locked(logger)->Debug << "- nothing received from GPS port" << endl;
		if (!requestShutdown && !baudRateSolid && (!tw || (numBadSamples > 2)))
		{
			numGoodSamples = numBadSamples = 0;

			try
			{
				unsigned curBr = ISerialPort::BaudRate2Num(sp.GetBaudRate());
				unsigned idx;

				for (idx = 0; idx < brvect.size(); ++idx)
					if (brvect[idx] == curBr)
						break;

				ISerialPort::BaudRate br = ISerialPort::NumToBaudRate(((idx+1) >= brvect.size()) ? brvect[0] : brvect[idx+1]);
				Logger::Locked(logger)->Debug << "Trying " << ISerialPort::BaudRate2Num(br) << " baud for GPS ..." << endl;
				sp.SetBaudRate(br);
				sp.FlushAll();
			}
			catch(exception& ex)
			{
				Logger::Locked(logger)->Error << "Caught " << typeid(ex).name() << " while adjusting baud rate: " << ex.what() << endl;
			}
		}
	}

	Logger::Locked(logger)->Notice << "GPS baud rate tracker exiting" << endl;
}

void GPSEventSource::RepeatedErrorFilter::ReportError(Logger::LogStream& stm, const string& errorMessage)
{
	RepeatedErrorMapType::const_iterator ci = repeatedErrorMap.find(errorMessage);
	const time_t timeNow = time(NULL);

	if ((ci == repeatedErrorMap.end()) || ((timeNow-ci->second) >= 60))
		stm << errorMessage << endl;

	repeatedErrorMap[errorMessage] = timeNow;
}

void GPSEventSource::GPSOnlineTrackerBackground()
{
	bool online = false;
	bool reported = false;

	Logger::Locked(logger)->Info << "Background GPS online tracker started" << endl;

	while(!gpsOnlineTrackerExit)
	{
		const unsigned timeoutValue = (reported && !online) ? 60*1000 : 12*1000;
		const bool goodReport = goodGPSReport.TimedWait(timeoutValue);

		if (!gpsOnlineTrackerExit && (!online || !reported || (online != goodReport)))
		{
			online = goodReport;
			reported = true;

			if (online)
				Logger::Locked(logger)->Info << "System GPS is now online" << endl;
			else
				Logger::Locked(logger)->Info << "System GPS is now offline" << endl;

			ILockableConceptLock lock(firingMutex);
			statusSignal(*this, online);
		}
	}

	Logger::Locked(logger)->Info << "Background GPS online tracker thread exiting" << endl;
}

}
}
