/**
 ******************************************************************************
 *
 * @file       telemetry.cpp
 * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
 *             Parts by Nokia Corporation (qt-info@nokia.com) Copyright (C) 2009.
 * @brief      
 * @see        The GNU Public License (GPL) Version 3
 * @defgroup   
 * @{
 * 
 *****************************************************************************/
/* 
 * This program is free software; you can redistribute it and/or modify 
 * it under the terms of the GNU General Public License as published by 
 * the Free Software Foundation; either version 3 of the License, or 
 * (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful, but 
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY 
 * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License 
 * for more details.
 * 
 * You should have received a copy of the GNU General Public License along 
 * with this program; if not, write to the Free Software Foundation, Inc., 
 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 */

#include "telemetry.h"
#include <QTime>

/**
 * Constructor
 */
Telemetry::Telemetry(UAVTalk* utalk, UAVObjectManager* objMngr)
{
    this->utalk = utalk;
    this->objMngr = objMngr;
    mutex = new QMutex(QMutex::Recursive);
    // Process all objects in the list
    QList< QList<UAVObject*> > objs = objMngr->getObjects();
    for (int objidx = 0; objidx < objs.length(); ++objidx)
    {
        registerObject(objs[objidx][0]); // we only need to register one instance per object type
    }
    // Listen to new object creations
    connect(objMngr, SIGNAL(newObject(UAVObject*)), this, SLOT(newObject(UAVObject*)));
    connect(objMngr, SIGNAL(newInstance(UAVObject*)), this, SLOT(newInstance(UAVObject*)));
    // Listen to transaction completions
    connect(utalk, SIGNAL(transactionCompleted(UAVObject*)), this, SLOT(transactionCompleted(UAVObject*)));
    // Setup transaction timer
    transPending = false;
    transTimer = new QTimer(this);
    transTimer->stop();
    connect(transTimer, SIGNAL(timeout()), this, SLOT(transactionTimeout()));
    // Setup and start the periodic timer
    timeToNextUpdateMs = 0;
    updateTimer = new QTimer(this);
    connect(updateTimer, SIGNAL(timeout()), this, SLOT(processPeriodicUpdates()));
    updateTimer->start(1000);
    // Setup and start the stats timer
    statsObj = dynamic_cast<GCSTelemetryStats*>( objMngr->getObject(GCSTelemetryStats::NAME) );
    txErrors = 0;
    txRetries = 0;
    statsTimer = new QTimer(this);
    connect(statsTimer, SIGNAL(timeout()), this, SLOT(processStatsUpdates()));
    statsTimer->start(STATS_UPDATE_PERIOD_MS);
}

/**
 * Register a new object for periodic updates (if enabled)
 */
void Telemetry::registerObject(UAVObject* obj)
{
    // Setup object for periodic updates
    addObject(obj);

    // Setup object for telemetry updates
    updateObject(obj);
}

/**
 * Add an object in the list used for periodic updates
 */
void Telemetry::addObject(UAVObject* obj)
{
    // Check if object type is already in the list
    for (int n = 0; n < objList.length(); ++n)
    {
        if ( objList[n].obj->getObjID() == obj->getObjID() )
        {
            // Object type (not instance!) is already in the list, do nothing
            return;
        }
    }

    // If this point is reached, then the object type is new, let's add it
    ObjectTimeInfo timeInfo;
    timeInfo.obj = obj;
    timeInfo.timeToNextUpdateMs = 0;
    timeInfo.updatePeriodMs = 0;
    objList.append(timeInfo);
}

/**
 * Update the object's timers
 */
void Telemetry::setUpdatePeriod(UAVObject* obj, qint32 periodMs)
{
    // Find object type (not instance!) and update its period
    for (int n = 0; n < objList.length(); ++n)
    {
        if ( objList[n].obj->getObjID() == obj->getObjID() )
        {
            objList[n].updatePeriodMs = periodMs;
            objList[n].timeToNextUpdateMs = 0;
        }
    }
}

/**
 * Connect to all instances of an object depending on the event mask specified
 */
void Telemetry::connectToObjectInstances(UAVObject* obj, quint32 eventMask)
{
    QList<UAVObject*> objs = objMngr->getObjectInstances(obj->getObjID());
    for (int n = 0; n < objs.length(); ++n)
    {
        // Disconnect all
        objs[n]->disconnect(this);
        // Connect only the selected events
        if ( (eventMask&EV_UNPACKED) != 0)
        {
            connect(objs[n], SIGNAL(objectUnpacked(UAVObject*)), this, SLOT(objectUnpacked(UAVObject*)));
        }
        if ( (eventMask&EV_UPDATED) != 0)
        {
            connect(objs[n], SIGNAL(objectUpdatedAuto(UAVObject*)), this, SLOT(objectUpdatedAuto(UAVObject*)));
        }
        if ( (eventMask&EV_UPDATED_MANUAL) != 0)
        {
            connect(objs[n], SIGNAL(objectUpdatedManual(UAVObject*)), this, SLOT(objectUpdatedManual(UAVObject*)));
        }
        if ( (eventMask&EV_UPDATE_REQ) != 0)
        {
            connect(objs[n], SIGNAL(updateRequested(UAVObject*)), this, SLOT(updateRequested(UAVObject*)));
        }
    }
}

/**
 * Update an object based on its metadata properties
 */
void Telemetry::updateObject(UAVObject* obj)
{
    // Get metadata
    UAVObject::Metadata metadata = obj->getMetadata();

    // Setup object depending on update mode
    qint32 eventMask;
    if ( metadata.gcsTelemetryUpdateMode == UAVObject::UPDATEMODE_PERIODIC )
    {
        // Set update period
        setUpdatePeriod(obj, metadata.gcsTelemetryUpdatePeriod);
        // Connect signals for all instances
        eventMask = EV_UPDATED_MANUAL | EV_UPDATE_REQ;
        if( dynamic_cast<UAVMetaObject*>(obj) != NULL )
        {
            eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events)
        }
        connectToObjectInstances(obj, eventMask);
    }
    else if ( metadata.gcsTelemetryUpdateMode == UAVObject::UPDATEMODE_ONCHANGE )
    {
        // Set update period
        setUpdatePeriod(obj, 0);
        // Connect signals for all instances
        eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ;
        if( dynamic_cast<UAVMetaObject*>(obj) != NULL )
        {
            eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events)
        }
        connectToObjectInstances(obj, eventMask);
    }
    else if ( metadata.gcsTelemetryUpdateMode == UAVObject::UPDATEMODE_MANUAL )
    {
        // Set update period
        setUpdatePeriod(obj, 0);
        // Connect signals for all instances
        eventMask = EV_UPDATED_MANUAL | EV_UPDATE_REQ;
        if( dynamic_cast<UAVMetaObject*>(obj) != NULL )
        {
            eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events)
        }
        connectToObjectInstances(obj, eventMask);
    }
    else if ( metadata.gcsTelemetryUpdateMode == UAVObject::UPDATEMODE_NEVER )
    {
        // Set update period
        setUpdatePeriod(obj, 0);
        // Disconnect from object
        connectToObjectInstances(obj, 0);
    }
}

/**
 * Called when a transaction is successfully completed (uavtalk event)
 */
void Telemetry::transactionCompleted(UAVObject* obj)
{
    // Check if there is a pending transaction and the objects match
    if ( transPending && transInfo.obj->getObjID() == obj->getObjID() )
    {
        // Complete transaction
        transTimer->stop();
        transPending = false;
        // Process new object updates from queue
        processObjectQueue();
    }
}

/**
 * Called when a transaction is not completed within the timeout period (timer event)
 */
void Telemetry::transactionTimeout()
{
    transTimer->stop();
    // Proceed only if there is a pending transaction
    if ( transPending )
    {
        // Check if more retries are pending
        if (transInfo.retriesRemaining > 0)
        {
            --transInfo.retriesRemaining;
            processObjectTransaction();
            ++txRetries;
        }
        else
        {
            // Terminate transaction
            utalk->cancelTransaction();
            transPending = false;
            // Process new object updates from queue
            processObjectQueue();
            ++txErrors;
        }
    }
}

/**
 * Start an object transaction with UAVTalk, all information is stored in transInfo
 */
void Telemetry::processObjectTransaction()
{
    if (transPending)
    {
        // Initiate transaction
        if (transInfo.objRequest)
        {
            utalk->sendObjectRequest(transInfo.obj, transInfo.allInstances);
        }
        else
        {
            UAVObject::Metadata metadata = transInfo.obj->getMetadata();
            utalk->sendObject(transInfo.obj, metadata.gcsTelemetryAcked, transInfo.allInstances);
        }
        // Start timer
        transTimer->start(REQ_TIMEOUT_MS);
    }
}

/**
 * Process the event received from an object
 */
void Telemetry::processObjectUpdates(UAVObject* obj, EventMask event, bool allInstances)
{
    // Push event into queue
    ObjectQueueInfo objInfo;
    objInfo.obj = obj;
    objInfo.event = event;
    objInfo.allInstances = allInstances;
    objQueue.enqueue(objInfo);

    // If there is no transaction in progress then process event
    if (!transPending)
    {
        processObjectQueue();
    }
}

/**
 * Process events from the object queue
 */
void Telemetry::processObjectQueue()
{
    // If the queue is empty there is nothing to do
    if (objQueue.isEmpty())
    {
        return;
    }

    // Get updated object from the queue
    ObjectQueueInfo objInfo = objQueue.dequeue();

    // Setup transaction
    transInfo.obj = objInfo.obj;
    transInfo.allInstances = objInfo.allInstances;
    transInfo.retriesRemaining = MAX_RETRIES;
    if ( objInfo.event == EV_UPDATED || objInfo.event == EV_UPDATED_MANUAL )
    {
        transInfo.objRequest = false;
    }
    else if ( objInfo.event == EV_UPDATE_REQ )
    {
        transInfo.objRequest = true;
    }
    // Start transaction
    transPending = true;
    processObjectTransaction();

    // If this is a metaobject then make necessary telemetry updates
    UAVMetaObject* metaobj = dynamic_cast<UAVMetaObject*>(objInfo.obj);
    if ( metaobj != NULL )
    {
        updateObject( metaobj->getParentObject() );
    }
}

/**
 * Check is any objects are pending for periodic updates
 * TODO: Clean-up
 */
void Telemetry::processPeriodicUpdates()
{
    QMutexLocker locker(mutex);

    // Stop timer
    updateTimer->stop();

    // Iterate through each object and update its timer, if zero then transmit object.
    // Also calculate smallest delay to next update (will be used for setting timeToNextUpdateMs)
    qint32 minDelay = MAX_UPDATE_PERIOD_MS;
    ObjectTimeInfo objinfo;
    qint32 elapsedMs = 0;
    QTime time;
    for (int n = 0; n < objList.length(); ++n)
    {
        objinfo = objList[n];
        // If object is configured for periodic updates
        if (objinfo.updatePeriodMs > 0)
        {
            objinfo.timeToNextUpdateMs -= timeToNextUpdateMs;
            // Check if time for the next update
            if (objinfo.timeToNextUpdateMs <= 0)
            {
                // Reset timer
                objinfo.timeToNextUpdateMs = objinfo.updatePeriodMs;
                // Send object
                time.start();
                processObjectUpdates(objinfo.obj, EV_UPDATED_MANUAL, true);
                elapsedMs = time.elapsed();
                // Update timeToNextUpdateMs with the elapsed delay of sending the object;
                timeToNextUpdateMs += elapsedMs;
            }
            // Update minimum delay
            if (objinfo.timeToNextUpdateMs < minDelay)
            {
                minDelay = objinfo.timeToNextUpdateMs;
            }
        }
    }

    // Check if delay for the next update is too short
    if (minDelay < MIN_UPDATE_PERIOD_MS)
    {
        minDelay = MIN_UPDATE_PERIOD_MS;
    }

    // Done
    timeToNextUpdateMs = minDelay;

    // Restart timer
    updateTimer->start(timeToNextUpdateMs);
}

void Telemetry::processStatsUpdates()
{
    QMutexLocker locker(mutex);

    // Get UAVTalk stats
    UAVTalk::ComStats utalkStats = utalk->getStats();
    utalk->resetStats();

    // Update stats object
    GCSTelemetryStats::DataFields stats = statsObj->getData();
    if (utalkStats.rxBytes > 0)
    {
        stats.Connected = GCSTelemetryStats::CONNECTED_TRUE;
    }
    else
    {
        stats.Connected = GCSTelemetryStats::CONNECTED_FALSE;
    }
    stats.RxDataRate = (float)utalkStats.rxBytes / ((float)STATS_UPDATE_PERIOD_MS/1000.0);
    stats.TxDataRate = (float)utalkStats.txBytes / ((float)STATS_UPDATE_PERIOD_MS/1000.0);
    stats.RxFailures += utalkStats.rxErrors;
    stats.TxFailures += txErrors;
    stats.TxRetries += txRetries;
    txErrors = 0;
    txRetries = 0;
    statsObj->setData(stats);
}

void Telemetry::objectUpdatedAuto(UAVObject* obj)
{
    QMutexLocker locker(mutex);
    processObjectUpdates(obj, EV_UPDATED, false);
}

void Telemetry::objectUpdatedManual(UAVObject* obj)
{
    QMutexLocker locker(mutex);
    processObjectUpdates(obj, EV_UPDATED_MANUAL, false);
}

void Telemetry::objectUnpacked(UAVObject* obj)
{
    QMutexLocker locker(mutex);
    processObjectUpdates(obj, EV_UNPACKED, false);
}

void Telemetry::updateRequested(UAVObject* obj)
{
    QMutexLocker locker(mutex);
    processObjectUpdates(obj, EV_UPDATE_REQ, false);
}

void Telemetry::newObject(UAVObject* obj)
{
    QMutexLocker locker(mutex);
    registerObject(obj);
}

void Telemetry::newInstance(UAVObject* obj)
{
    QMutexLocker locker(mutex);
    registerObject(obj);
}