//------------------------------------------------------------------------------
// <copyright file="SSQController.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 SSQController class, which implements the controller
//  role business logic for the interface to the SSQ sensor.
// </summary>
//
// <owner>Mike Cookson</owner>
//------------------------------------------------------------------------------

#include "LqrSMACByteArray.h"
#include "LqrSMACLogger.h"
#include "SSQController.h"
#include "SensorControllerRunFileCmd.h"
#include "nsextend.h"
#include <boost/algorithm/string.hpp>
#include <boost/foreach.hpp>

#include "SSQEnqueueCmd.h"
#include "SSQStartCmd.h"
#include "SSQStopCmd.h"
#include "LqrSMACSignalSubscriber.h"

using namespace std;
using namespace LiquidRobotics::SMACFramework;

namespace LiquidRobotics {
namespace SimpleSBDQueue {

SSQController::SSQController(const SSQControllerSettings& _settings, IObjectBroker& _broker, Logger& _logger, GPSSignalSubscriber::Ptr _gpsSubscriber, ShutdownSignalSubscriber::Ptr _shutdownSubscriber)
: SSQControllerBase(_broker, _logger, _gpsSubscriber, _shutdownSubscriber)
, settings(_settings)
, initialized(false)
, lat(0)
, lon(0)
, hasGPS(false)
, nextCookie(1)
, nextMessageCookie(0)
{
}

SSQController::~SSQController()
{
}

void SSQController::Initialize()
{
    Logger::Locked(logger)->Info << "SSQController::Initialize" << endl;
    MutexLock lock(mutex);

    if (!initialized)
    {
        bool shutdownBase = false;

        try
        {
            SSQControllerBase::Initialize();
            shutdownBase = true;

            if (settings.GetAutoStart())
                DoStart();
        }
        catch(...)
        {
            if (commPtr)
            {
                try
                {
                    ILockableConcept::Lock eventLock(commPtr->GetEventLock());
                    receivedPacketConnection.reset();
                    requestDataConnection.reset();
                    sentPacketConnection.reset();
                    statsConnection.reset();
                    eventLock.unlock();
                }
                catch(...) { }

                commPtr.reset();
            }

            if (shutdownBase)
                try { Base::Shutdown() ; } catch(...) { }

            throw;
        }

        initialized = true;
    }
}

void SSQController::Shutdown()
{
    Logger::Locked(logger)->Info << "SSQController::Shutdown" << endl;
    MutexLock lock(mutex);

    if (initialized)
    {
        gpsConnection.reset();

        SSQCommunicator::Ptr temp = commPtr;
        commPtr.reset();
        initialized = false;
        lock.unlock();

        if (temp)
        {
            try
            {
                ILockableConcept::Lock eventLock(commPtr->GetEventLock());
                receivedPacketConnection.reset();
                requestDataConnection.reset();
                sentPacketConnection.reset();
                statsConnection.reset();
                eventLock.unlock();
                temp->Shutdown();
            }
            catch(...) { }

            temp.reset();
        }

        try { Base::Shutdown() ; } catch(...) { }
    }
}

void SSQController::Start()
{
    Logger::Locked(logger)->Info << "SSQController::Start" << endl;
    {
        MutexLock lock(mutex);

        if (!initialized)
            throw runtime_error("Not initialized");

        DoStart();
    }
    Logger::Locked(logger)->Info << "SSQController::Start returning" << endl;
}

void SSQController::DoStart()
{
    MutexLock lock(mutex);

    if (!commPtr)
    {
        try
        {
            commPtr.reset(new SSQCommunicator(settings.GetCommunicatorSettings(), logger));
            receivedPacketConnection = commPtr->SubscribeReceivedData(boost::bind(&SSQController::OnReceivedPacket, this, _1, _2, _3));
            requestDataConnection = commPtr->SubscribeRequestDataToSend(boost::bind(&SSQController::OnRequestData, this, _1, _2, _3, _4, _5));
            sentPacketConnection = commPtr->SubscribeSentData(boost::bind(&SSQController::OnSentPacket, this, _1, _2, _3));
            statsConnection = commPtr->SubscribeStatistics(boost::bind(&SSQController::OnStatistics, this, _1, _2));
            commPtr->Initialize();
        }
        catch(...)
        {
            SSQCommunicator::Ptr temp = commPtr;
            commPtr.reset();
            lock.unlock();

            if (temp)
            {
                ILockableConcept::Lock eventLock(temp->GetEventLock());
                receivedPacketConnection.reset();
                requestDataConnection.reset();
                sentPacketConnection.reset();
                statsConnection.reset();
                eventLock.unlock();
                try { temp->Shutdown(); }
                catch(...) { }
            }
        }
    }
}

void SSQController::Stop()
{
    Logger::Locked(logger)->Info << "SSQController::Stop" << endl;
    MutexLock lock(mutex);

    if (!initialized)
        throw runtime_error("Not initialized");

    SSQCommunicator::Ptr temp = commPtr;
    commPtr.reset();
    lock.unlock();

    if (temp)
    {
        ILockableConcept::Lock eventLock(temp->GetEventLock());
        receivedPacketConnection.reset();
        requestDataConnection.reset();
        sentPacketConnection.reset();
        statsConnection.reset();
        eventLock.unlock();
        try { temp->Shutdown(); }
        catch(...) { }
    }
}

void SSQController::EnqueueMessage(const PriorityType priority, const ByteArray& message, const SenderIDType senderID, const SenderTransactionType senderTransaction,
                        const QueuePosition queuePos, const TTLType timeToLive)
{
    Logger::Locked(logger)->Info << "SSQController::EnqueueMessage priority=" << priority << " message=(" << message.size() << " bytes)"
        << " senderID=0x" << utos(senderID, 16, 2) << " senderTransaction=" << senderTransaction << " queuePos=" << (unsigned) queuePos
        << " timeToLive=" << timeToLive << endl;

    MutexLock lock(mutex);

    if (!initialized)
        throw runtime_error("Not initialized");


    QueueEntryPtr qep;
    const CookieType cookie = PullCookie();
    qep.reset(new QueueEntry(priority, message, senderID, senderTransaction, queuePos, timeToLive, cookie));

    QueueMap::iterator qit = queueMap.find(priority);
    QueuePtr q;

    if (qit == queueMap.end())
    {
        q.reset(new Queue);
        queueMap[priority] = q;
    }
    else
        q = qit->second;

    if (q)
    {
        // shuffle the latest to the the back if it was 'queue latest at front'
        if (!q->empty())
        {
            QueueEntryPtr temp = q->front();

            if (temp->priority == QUEUE_LATEST_AT_FRONT)
            {
                temp->priority = QUEUE_END;
                q->pop_front();
                q->push_back(temp);
            }
        }

        // push the next item on the front or the back, depending on position
        switch(queuePos)
        {
            case QUEUE_LATEST_AT_FRONT:
            case QUEUE_FRONT:

                q->push_front(qep);
                break;

            default:

                q->push_back(qep);
                break;
        }
    }

    cookieMap[cookie] = qep;

    SSQCommunicator::Ptr temp = commPtr;

    if (temp)
        temp->RequestSession();
    else
        Logger::Locked(logger)->Warning << "SSQController::EnqueueMessage: no communicator" << endl;

    lock.unlock();
}

void SSQController::ForwardGPS(double latitude, double longitude)
{
    Logger::Locked(logger)->Info << "SSQController::ForwardGPS latitude=" << latitude << " longitude=" << longitude << endl;
    MutexLock lock(mutex);

    if (initialized)
    {
        SSQCommunicator::Ptr temp;

        if (temp)
            temp->SetPosition(latitude, longitude);
    }
}

void SSQController::RequestMailboxCheck()
{
    Logger::Locked(logger)->Info << "SSQController::RequestMailboxCheck" << endl;
    MutexLock lock(mutex);

    if (!initialized)
        throw runtime_error("Not initialized");

    SSQCommunicator::Ptr temp = commPtr;

    if (temp)
        temp->RequestSession();
    else
        Logger::Locked(logger)->Warning << "SSQController::RequestMailboxCheck: no communicator" << endl;
}

void SSQController::OnGPSSampleEvent(const GPSSampleEvent& sample)
{
    try
    {
        if (sample.HasGPS())
        {
            SSQCommunicator::Ptr temp = commPtr;
            if (temp)
            {
                double lat, lon;
                sample.GetGPS(lat, lon);
                temp->SetPosition(lat, lon);
            }
        }
    }
    catch(...)
    {
        Logger::Locked(logger)->Warning << "Caught exception while forwarding GPS data to communicator" << endl;
    }
}

void SSQController::OnShutdownEvent()
{
	Base::OnShutdownEvent();
}

SSQController::CommandPtr SSQController::ParseCommand(ParsingContext& context, bool internalCmd)
{
	CmdLineParser parser(context.GetCmd());
	CommandPtr result;
    const string serverVerb = "SSQ";

	if (!!(result = EventLib::SensorControllerRunFileCmd<TCon,TCmd,TCID,TEx>::Parse(serverVerb, "--run", *this, parser, context, logger))
	 || !!(result = SSQEnqueueCmd::Parse(serverVerb, "--enqueue", false, *this, parser, context, logger))
	 || !!(result = SSQEnqueueCmd::Parse(serverVerb, "--mbcheck", true, *this, parser, context, logger))
	 || !!(result = SSQStartCmd::Parse(serverVerb, "--start", *this, parser, context, logger))
	 || !!(result = SSQStopCmd::Parse(serverVerb, "--stop", *this, parser, context, logger))
			)
	{
	}
	else
	{
		stringstream msg;
		msg << "Unrecognized command '" << context.GetCmd() << "'";

		throw invalid_argument(msg.str());
	}

	return result;
}

void SSQController::OnReceivedPacket(SSQCommunicator& sender, const ByteArray& packetData, unsigned MTMSN)
{
    Logger::Locked(logger)->Debug << "SSQController::OnRecievedPacket: MTMSN=" << MTMSN << " packetData=" << packetData.ToHexString() << endl;
    ILockableConcept::Lock firingLock(firingMutex);
    packetReceivedSignal(*this, packetData, MTMSN);
}

void SSQController::OnRequestData(SSQCommunicator& sender, size_t maxPacketSize, ByteArray& packetData, CookieType& cookie, unsigned& newMailboxCheckTimeMS)
{
    //Logger::Locked(logger)->Debug << "SSQController::OnRequestData: maxPacketSize=" << maxPacketSize << endl;
    MutexLock lock(mutex);
    QueueEntryPtr qep = GetNextMessageBufferQueueEntry();

    if (!qep)
    {
    	ValidateQueueContent(maxPacketSize);

    	if (settings.GetConcatenateSentPackets())
    	{
    		PackNextMessageBuffer(maxPacketSize);
    		qep = GetNextMessageBufferQueueEntry();
    	}
    	else
    		qep = GetNextQueueEntry(true);
    }

	if (qep)
	{
		packetData.assign(qep->message.begin(), qep->message.end());
		cookie = qep->cookie;
	}

    lock.unlock();

    stringstream stm;
    stm << "SSQController::OnRequestData: maxPacketSize=" << maxPacketSize << " returning ";

    if (qep)
    {
        stm << "packetData=" << packetData.size() << " cookie=" << cookie;
        if (cookie == nextMessageCookie)
        	stm << " (packed buffer)";
    }
    else
        stm << "nothing to send";

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

void SSQController::UnlinkQueueEntry(QueueEntryPtr qep)
{
    MutexLock lock(mutex);
    const CookieType cookie = qep->cookie;

    if (cookie == nextMessageCookie)
    {
    	nextMessageCookie = 0;
    	nextMessageQueue.clear();
    }
    else
    {
		CookieMap::iterator cookit = cookieMap.find(cookie);

		if (cookit == cookieMap.end())
			Logger::Locked(logger)->Warning << "SSQController::UnlinkQueueEntry: no cookie found matching " << cookie << endl;
		else
			cookieMap.erase(cookit, cookit);

		QueueMap::iterator qmit = queueMap.find(qep->priority);

		if (qmit != queueMap.end())
		{
			QueuePtr qp = qmit->second;

			if (!qp->empty() && (qp->front()->cookie == cookie))
				qp->pop_front();
			else if (!qp->empty() && (qp->back()->cookie == cookie))
				qp->pop_back();
			else
			{
				bool found = false;

				for (Queue::iterator qpit = qp->begin(); !found && (qpit != qp->end()); ++qpit)
					if ((*qpit)->cookie == cookie)
					{
						qp->erase(qpit);
						found = true;
					}

				if (!found)
					Logger::Locked(logger)->Warning << "SSQController::UnlinkQueueEntry: no entry found matching cookie " << cookie << endl;
			}
		}
    }
}

void SSQController::OnSentPacket(SSQCommunicator& sender, CookieType cookie, unsigned MOMSN)
{
    Logger::Locked(logger)->Debug << "SSQController::OnSentPacket: cookie=" << cookie << " MOMSN=" << MOMSN << endl;
    MutexLock lock(mutex);

    if (cookie == nextMessageCookie)
    {
    	for (Queue::const_iterator ci = nextMessageQueue.begin(); ci != nextMessageQueue.end(); ++ci)
    	{
    		QueueEntryPtr qep = *ci;

            {
                ILockableConcept::Lock firingLock(firingMutex);
                packetSendDispositionSignal(*this, qep->priority, qep->senderID, qep->senderTransaction, MOMSN, SDS_SUCCESS);
            }
    	}

    	nextMessageCookie = 0;
    	nextMessageQueue.clear();
    }
    else
    {
		CookieMap::iterator cookit = cookieMap.find(cookie);

		if (cookit == cookieMap.end())
			Logger::Locked(logger)->Warning << "SSQController::OnSentPacket: no cookie found matching " << cookie << endl;
		else
		{
			QueueEntryPtr qep = cookit->second;

			{
				ILockableConcept::Lock firingLock(firingMutex);
				packetSendDispositionSignal(*this, qep->priority, qep->senderID, qep->senderTransaction, MOMSN, SDS_SUCCESS);
			}

			UnlinkQueueEntry(qep);
		}
    }
}

void SSQController::OnStatistics(SSQCommunicator& sender, const SSQCommunicator::Statistics& stats)
{
    Logger::Locked(logger)->Debug << "OnStatistics: " << stats.ToString() << endl;
    ILockableConcept::Lock firingLock(firingMutex);
    channelStatsSignal(*this, stats);
}

SSQController::ScopedConnectionPtr SSQController::SubscribePacketReceived(PacketReceivedSignalType::slot_type slot)
{
    ScopedConnectionPtr result;
    result.reset(new ScopedConnection(packetReceivedSignal.connect(slot)));
    return result;
}

SSQController::ScopedConnectionPtr SSQController::SubscribePacketSendDisposition(PacketSendDispositionSignalType::slot_type slot)
{
    ScopedConnectionPtr result;
    result.reset(new ScopedConnection(packetSendDispositionSignal.connect(slot)));
    return result;
}

SSQController::ScopedConnectionPtr SSQController::SubscribeChannelStats(ChannelStatsSignalType::slot_type slot)
{
    ScopedConnectionPtr result;
    result.reset(new ScopedConnection(channelStatsSignal.connect(slot)));
    return result;
}

ILockableConcept& SSQController::GetFiringMutex()
{
    return firingMutex;
}

//! Returns the next message to be sent (nil if nothing to send); fires event timeouts and zero length item errors
SSQController::QueueEntryPtr SSQController::GetNextQueueEntry(
		bool peek												/**< [in] true if just looking, false if extracting the message from queue (default) */)
{
    MutexLock lock(mutex);
    QueueMap::const_iterator ciBeg = queueMap.begin();          // beginning of the queue map by priority
    QueueMap::const_iterator ciEnd = queueMap.end();            // end of the queue map by priority
    QueueEntryPtr qep;                                          // item that was found

    // stepping backward through priorities starting at the greatest, pick off the first element of each queue
    if (ciBeg != ciEnd)
        while(!qep && (ciEnd-- != ciBeg))
            while(!qep && !ciEnd->second->empty())
            {
                qep = ciEnd->second->front();

                // kill off the item if its time has expired
                if (qep->timeToLive && !qep->queueTime.Remaining(qep->timeToLive))
                {
                    UnlinkQueueEntry(qep);
                    ILockableConcept::Lock firingLock(firingMutex);
                    packetSendDispositionSignal(*this, qep->priority, qep->senderID, qep->senderTransaction, 0, SDS_TIMEOUT);
                    qep.reset();
                }
                // kill off the item if it's empty
                else if (!qep->message.size())
                {
                    UnlinkQueueEntry(qep);
                    ILockableConcept::Lock firingLock(firingMutex);
                    packetSendDispositionSignal(*this, qep->priority, qep->senderID, qep->senderTransaction, 0, SDS_ZERO_LENGTH);
                    qep.reset();
                }
                // otherwise (entry is ok), if not peeking, then delete the item from the queue
                else if (!peek)
                    UnlinkQueueEntry(qep);
            }

    lock.unlock();
    return qep;
}

SSQController::CookieType SSQController::PullCookie()
{
	ILockableConcept::Lock lock(cookieMutex);
	CookieType result = nextCookie++;
	if (!result)
		result = nextCookie++;
	return result;
}

SSQController::QueueEntryPtr SSQController::GetNextMessageBufferQueueEntry() const
{
	QueueEntryPtr qep;

	if (nextMessageCookie && nextMessageQueue.size())
	{
		ByteArray temp;
		for (Queue::const_iterator ci = nextMessageQueue.begin(); ci != nextMessageQueue.end(); ++ci)
			temp.insert(temp.end(), (*ci)->message.begin(), (*ci)->message.end());

		qep.reset(new QueueEntry(100,temp,0,0,QUEUE_END,0,nextMessageCookie));
	}

	return qep;
}

void SSQController::PackNextMessageBuffer(
		size_t maxPacketSize									/**< [in] the maximum number of bytes for a message */)
{
	size_t currentSize = 0;

	for (Queue::const_iterator ci = nextMessageQueue.begin(); ci != nextMessageQueue.end(); ++ci)
		currentSize += (*ci)->message.size();

	if (currentSize)
		Logger::Locked(logger)->Warning << "PackNextMessageBuffer was called with " << currentSize << " bytes in the buffer and cookie=" << nextMessageCookie << endl;

	QueueEntryPtr qep;
	bool added = false;
	bool overflow = false;

	do
	{
		qep = GetNextQueueEntry(true);

		if (qep)
		{
			if ((currentSize+qep->message.size()) <= maxPacketSize)
			{
				added = true;
				UnlinkQueueEntry(qep);
				currentSize += qep->message.size();
				nextMessageQueue.push_back(qep);
			}
			else
				overflow = true;
		}

	} while(qep && !overflow && (currentSize <= maxPacketSize));

	if (added)
		nextMessageCookie = PullCookie();

	stringstream ss;
	ss << "PackNextMessageBuffer exiting with " << currentSize << " bytes in the buffer and cookie=" << nextMessageCookie << ":";
	for (Queue::const_iterator ci = nextMessageQueue.begin(); ci != nextMessageQueue.end(); ++ci)
	{
		if (ci != nextMessageQueue.begin()) ss << ",";
		ss << " cookie " << (*ci)->cookie << " size " << (*ci)->message.size();
	}
	Logger::Locked(logger)->Info << ss.str() << endl;
}

void SSQController::ValidateQueueContent(
		size_t maxPacketSize									/**< [in] the maximum number of bytes for any message */)
{
	// first create a queue of the queue entries to handle (can't affect a list or a map while iterating over it)
	typedef std::pair<QueueEntryPtr,SDSDisposition> DispoPair;
	typedef std::deque<DispoPair> DispoPairQueue;
	DispoPairQueue dpq;

	for (CookieMap::const_iterator ci = cookieMap.begin(); ci != cookieMap.end(); ++ci)
		if (ci->second->message.size() > maxPacketSize)
		{
			Logger::Locked(logger)->Warning << "ValidateQueueContent: cookie " << ci->second->cookie << " with " << ci->second->message.size() << " bytes exceeds " << maxPacketSize << " byte limit" << endl;
			const DispoPair dp(ci->second,SDS_TOO_LONG);
			dpq.push_back(dp);
		}
		else if (ci->second->timeToLive && !ci->second->queueTime.Remaining(ci->second->timeToLive))
		{
			Logger::Locked(logger)->Warning << "ValidateQueueContent: cookie " << ci->second->cookie << " with " << ci->second->message.size() << " bytes timed out (TTL=" << ci->second->timeToLive << ")" << endl;
			const DispoPair dp(ci->second,SDS_TIMEOUT);
			dpq.push_back(dp);
		}

	// process the queue and fire events
	while(!dpq.empty())
	{
		const DispoPair dp = dpq.front();
		dpq.pop_front();
		QueueEntryPtr qep = dp.first;
		UnlinkQueueEntry(qep);
		packetSendDispositionSignal(*this, qep->priority, qep->senderID, qep->senderTransaction, 0, dp.second);
	}
}

}
}
