/** \file
 *
 *  Contains the UniversalDataElement class implementation.
 *
 *  Copyright (c) 2007,2008,2009 MBARI
 *  MBARI Proprietary Information.  All Rights Reserved
 *
 */

#include "UniversalDataElement.h"
#include "component/Component.h"

#include "logger/EventEntry.h"
#include "logger/Logger.h"
#include <math.h>

UniversalDataElement::UniversalDataElement( const UniversalURI& name, const DataValue* dataValue )
    : DataElement( name,
                   NULL == dataValue ? NULL : & dataValue->getUnit().getBaseUnit(),
                   NULL == dataValue ? 0 : dataValue->getTypeSize() ),
      bestElement_( NULL ),
      minPowerElement_( NULL ),
      minErrorElement_( NULL ),
      minLatencyElement_( NULL ),
      maxFrequencyElement_( NULL ),
      accuracy_( DataElement::NO_ACCURACY ),
      elementList_( false )
{}

UniversalDataElement::~UniversalDataElement()
{
}

DataValue* UniversalDataElement::getDataValue( void ) const
{
    if( ( NULL != bestElement_ ) && ( isInvalid() != true ) )
    {
        return bestElement_->getDataValue();
    }
    else
    {
        return NULL;
    }
}

float UniversalDataElement::getAccuracy( const Unit& unit ) const
#ifndef __GAZEBO
throw( UninitializedException )
#endif
{
    if( ( NULL != bestElement_ ) && ( isInvalid() != true ) )
    {
        return bestElement_->getAccuracy( unit );
    }
    else
    {
        throw UninitializedException( /*"Universal Data Element not Initialized"*/ );
    }
}

float UniversalDataElement::getAccuracySI() const
#ifndef __GAZEBO
throw( UninitializedException )
#endif
{
    if( ( NULL != bestElement_ ) && ( isInvalid() != true ) )
    {
        return bestElement_->getAccuracySI();
    }
    else
    {
        throw UninitializedException( /*"Universal Data Element not Initialized"*/ );
    }
}

/// Get the last written DataElement accuracy.
/// Overrides the virtual from DataElement.
///
/// \return Last written accuracy metric for the DataValue.
float UniversalDataElement::getWrittenAccuracy( const Unit& unit ) const
{
    if( ( NULL != bestElement_ ) && ( isInvalid() != true ) )
    {
        return bestElement_->getWrittenAccuracy( unit );
    }
    else
    {
        return NO_ACCURACY;
    }
}

bool UniversalDataElement::isOrphaned( void ) const
{
    //A universal is orphaned strictly if it has no elements.
    return ( elementList_.isEmpty() );
}

void UniversalDataElement::addElement( DataElement& element )
{
    MutexLocker lock( mutex_ );
    elementList_.push( &element );
}

void UniversalDataElement::notifyChangeDone( Component* notifier )
{
    DataElement * currentElement = bestElement_;
    recalculate();
    if( bestElement_ != currentElement && NULL != notifier )
    {
        notifier->getLogger().log( new EventEntry( notifier, bestElement_, EventEntry::DATA_STATE_CHANGE_SET_BEST ) );
    }
}

void UniversalDataElement::notifyChangeDone( DataWriter* notifier )
{
    DataElement * currentElement = bestElement_;
    recalculate();
    if( bestElement_ != currentElement && NULL != notifier )
    {
        notifier->getOwner()->getLogger().log( new EventEntry( notifier->getOwner(), bestElement_, EventEntry::DATA_STATE_CHANGE_SET_BEST ) );
    }
}

// This function could use a little massage
void UniversalDataElement::recalculate( )
{
    MutexLocker lock( mutex_ );
    // Invalid until proven otherwise
    invalid_ = true;

    DataElement *bestElement = NULL;
    float bestAccuracy = DataElement::NO_ACCURACY;
    DataElement *minPowerElement = NULL;
    float minPower = nanf( "" );
    DataElement *minErrorElement = NULL;
    float minError = nanf( "" );
    DataElement *minLatencyElement = NULL;
    float minLatency = nanf( "" );
    DataElement *maxFrequencyElement = NULL;
    float maxFrequency = nanf( "" );

    for( int i = elementList_.getMinIndex(); i <= elementList_.getMaxIndex(); ++i )
    {
        DataElement * element = elementList_[i];
        if( element->isActive() && !element->isInvalid() )
        {
            float thisAccuracy = element->getBaseAccuracy();

            if( thisAccuracy < bestAccuracy )
            {
                bestElement = element;
                bestAccuracy = thisAccuracy;
            }
        }
        if( element->isActivatable() )
        {
            float thisPower = element->getPower();
            if( !isnan( thisPower ) && ( isnan( minPower ) || thisPower < minPower ) )
            {
                minPower = thisPower;
                minPowerElement = element;
            }
            float thisError = element->getError();
            if( !isnan( thisError ) && ( isnan( minError ) || thisError < minError ) )
            {
                minError = thisError;
                minErrorElement = element;
            }
            float thisLatency = element->getLatency();
            if( !isnan( thisLatency ) && ( isnan( minLatency ) || thisLatency < minLatency ) )
            {
                minLatency = thisLatency;
                minLatencyElement = element;
            }
            float thisFrequency = element->getFrequency();
            if( !isnan( thisFrequency ) && ( isnan( maxFrequency ) || thisFrequency > maxFrequency ) )
            {
                maxFrequency = thisFrequency;
                maxFrequencyElement = element;
            }
        }
    }

    if( NULL == minPowerElement )
    {
        minPowerElement = minErrorElement;
    }
    if( NULL == minLatencyElement )
    {
        minLatencyElement = minErrorElement;
    }
    if( NULL == maxFrequencyElement )
    {
        maxFrequencyElement = minErrorElement;
    }

    /// If an element is already "on" for another reason,
    /// it will be the lowest power one to use
    /*if ( NULL != bestElement && !bestElement->isDataRequested() )
    {
        minPowerElement = bestElement;
    }
    /// or maybe an element is specifically requested to be on.
    /// Then it will definitely be the lowest power one to use.
    else if ( NULL != minErrorElement && minErrorElement->isDataRequested() )
    {
        minPowerElement = minErrorElement;
    }
    else if ( NULL != maxFrequencyElement && maxFrequencyElement->isDataRequested() )
    {
        minPowerElement = maxFrequencyElement;
    }
    else if ( NULL != minLatencyElement && minLatencyElement->isDataRequested() )
    {
        minPowerElement = minLatencyElement;
    }
    */

    accuracy_ = bestAccuracy;
    bestElement_ = bestElement;
    minPowerElement_ = minPowerElement;
    minErrorElement_ = minErrorElement;
    minLatencyElement_ = minLatencyElement;
    maxFrequencyElement_ = maxFrequencyElement;

    if( bestElement_ != NULL )
    {
        invalid_ = false;
        hasBeenSet_ = true;
    }
}

bool UniversalDataElement::wasTouchedSinceLastRun( const Component* component )
{
    if( bestElement_ != NULL )
    {
        return bestElement_->wasTouchedSinceLastRun( component );
    }
    return false;
}

/// Returns the timestamp of last write
const Timestamp& UniversalDataElement::getTimestamp() const
{
    if( bestElement_ != NULL )
    {
        return bestElement_->getTimestamp();
    }
    return Timestamp::NOT_SET_TIME;
}

float UniversalDataElement::setAccuracy( const Unit& unit, float accuracy )
{
    //    MutexLocker lock( mutex_ );
    //    accuracy_ = accuracy;
    return accuracy;
}

DataElement* UniversalDataElement::getBestElement( DataAccessor::RequestStrategy strategy ) const
{
    return bestElement_;
}

/// Returns true if one of the registered readers is requesting data
/// and the indicated element is intended to provide the data
bool UniversalDataElement::isDataRequested( DataElement* element )
{
    for( unsigned int i = 0; i < readers_.size(); ++i )
    {
        DataReader* reader = readers_[i];
        switch( reader->getRequestStrategy() )
        {
        case DataAccessor::NO_STRATEGY:
            break;
        case DataAccessor::MIN_POWER:
            if( element == minPowerElement_ )
            {
                return true;
            }
            break;
        case DataAccessor::MIN_ERROR:
            if( element == minErrorElement_ )
            {
                return true;
            }
            break;
        case DataAccessor::MIN_LATENCY:
            if( element == minLatencyElement_ )
            {
                return true;
            }
            break;
        case DataAccessor::MAX_FREQUENCY:
            if( element == maxFrequencyElement_ )
            {
                return true;
            }
            break;
        case DataAccessor::ALL_DEVICES:
            return true;
        default:
            break;
        }
    }
    return false;
}

/// Returns true if one of the registered readers is requesting data
bool UniversalDataElement::isAnyDataRequested()
{
    for( unsigned int i = 0; i < readers_.size(); ++i )
    {
        DataReader* reader = readers_[i];
        if( reader->getRequestStrategy() !=  DataAccessor::NO_STRATEGY )
        {
            return true;
        }
    }
    return false;
}