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

#include "DataElement.h"
#include "component/Component.h"
#include "component/ComponentRegistry.h"
#include "data/Slate.h"
#include "data/UniversalDataElement.h"
#include "logger/EventEntry.h"
#include "logger/Logger.h"
#include "units/Unit.h"

#include <math.h>


//const double DataElement::NoAccuracy_s = nan("");
const float DataElement::NO_ACCURACY = 3.25e30;

/// Destructor
DataElement::~DataElement()
{}

DataReader* DataElement::findReader( const ElementURI& uri )
{
    MutexLocker lock( mutex_ );
    for( int i = readers_.getMaxIndex(); i >= readers_.getMinIndex(); --i )
    {
        if( readers_[i]->getUri() == uri )
        {
            return readers_[i];
        }
    }
    return NULL;
}

DataWriter* DataElement::findWriter( const ElementURI& uri )
{
    MutexLocker lock( mutex_ );
    for( int i = writers_.getMaxIndex(); i >= writers_.getMinIndex(); --i )
    {
        if( writers_[i]->getUri() == uri )
        {
            return writers_[i];
        }
    }
    return NULL;
}

//---------------------------------------------------------
/// Property getter and ter Methods

const ElementURI& DataElement::getUri( void ) const
{
    return name_;
}

//-------------------------------------------------------------------
//-- Functions dealing with the Elements "state" --

bool DataElement::isUnavailable( void ) const
{
    return unavailable_ || isOwnersFailed();
}

bool DataElement::isInvalid( void ) const
{
    return invalid_ || isUnavailable();
}

bool DataElement::isOwnersFailed( void ) const
{
    //MutexLocker lock( mutex_ );
    bool ownersOk = true;
    if( !unavailable_ && !isUniversal() )
    {
        ownersOk = false;
        for( unsigned int i = 0; i < writers_.size(); ++i )
        {
            DataWriter* writer = writers_.get( i );
            if( !writer->getOwner()->isFailed() )
            {
                ownersOk = true;
                break;
            }
        }
    }
    return !ownersOk;
}

bool DataElement::isOrphaned( void ) const
{
    return ( writerCount() == 0 );
}

bool DataElement::isOwnersActive( void ) const
{
    bool ownersActive = true;
    if( !isUniversal() )
    {
        ownersActive = false;
        for( unsigned int i = 0; i < writers_.size(); ++i )
        {
            DataWriter* writer = writers_.get( i );
            if( writer->getOwner()->isActive() )
            {
                ownersActive = true;
                break;
            }
        }
    }
    return ownersActive;
}

bool DataElement::isActive( void ) const
{
    return ( isActivatable() && isOwnersActive() && hasBeenSet_ );
}

bool DataElement::isActivatable( void ) const
{
    return ( !isUnavailable() && !isOrphaned() );
}

bool DataElement::isBest( DataAccessor::RequestStrategy strategy ) const
{
    return NULL != universal_ &&
           universal_->getBestElement( strategy ) == this;
}

const Unit* DataElement::getUnit( void ) const
{
    DataValue *dataValue = getDataValue();
    return NULL == dataValue ? NULL : &dataValue->getUnit();
}
//-------------------------------------------------------------------
//-- Registration functions --
// Associates this element to a universal
void DataElement::registerUniversal( UniversalDataElement *abstract )
{
    MutexLocker lock( mutex_ );
    if( universal_ != this )
    {
        universal_ = abstract;
    }
    //universalQueue_.push_back( abstract );
}

void DataElement::registerReader( DataReader *reader )
{
    MutexLocker lock( mutex_ );
    readers_.push( reader );
}

void DataElement::unregisterReader( DataReader *reader )
{
    MutexLocker lock( mutex_ );
    for( int i = readers_.getMaxIndex(); i >= readers_.getMinIndex(); --i )
    {
        if( readers_[i] == reader )
        {
            readers_.pop( i );
            break;
        }
    }
}

unsigned int DataElement::readerCount( void ) const
{
    return readers_.size();
}

/// Returns true if one of the registered readers is requesting data
bool DataElement::isDataRequested()
{
    bool result( false );
    for( unsigned int i = 0; i < readers_.size(); ++i )
    {
        DataReader* reader = readers_[i];
        if( reader->isRequestingData() )
        {
            result = true;
            break;
        }
    }
    if( !result && NULL != universal_ )
    {
        result = universal_->isDataRequested( this );
    }
    return result;
}

/// Returns true if there is a non-failed "implementor" reader of this element
bool DataElement::isImplemented()
{
    for( unsigned int i = 0; i < readers_.size(); ++i )
    {
        DataReader* reader = readers_[i];
        if( reader != NULL
                && reader->isImplementor()
                && reader->getOwner() != NULL
                && !reader->getOwner()->isFailed() )
        {
            return true;
        }
    }
    if( NULL != universal_ )
    {
        return universal_->isImplemented() && !universal_->isUnavailable();
    }
    return false;
}

void DataElement::registerWriter( DataWriter *writer )
{
    MutexLocker lock( mutex_ );
    writers_.push( writer );
    if( wasOrphaned_ )
    {
        wasOrphaned_ = false;
        writer->getOwner()->getLogger().log( new EventEntry( writer->getOwner(), this, EventEntry::DATA_STATE_CHANGE_UNORPHANED ) );
        registerChangePending( writer );
        registerChangeDone();
    }
}

void DataElement::unregisterWriter( DataWriter *writer )
{
    MutexLocker lock( mutex_ );
    for( int i = writers_.getMaxIndex(); i >= writers_.getMinIndex(); --i )
    {
        if( writers_[i] == writer )
        {
            writers_.pop( i );
            break;
        }
    }
    if( isOrphaned() && !wasOrphaned_ )
    {
        wasOrphaned_ = true;
        writer->getOwner()->getLogger().log( new EventEntry( writer->getOwner(), this, EventEntry::DATA_STATE_CHANGE_ORPHANED ) );
        registerChangePending( writer );
        registerChangeDone();
    }
}

unsigned int DataElement::writerCount( void ) const
{
    return writers_.size();
}


//-------------------------------------------------------------------
//== Get/set function ==

bool DataElement::get( DataReader &reader, const Unit& unit, bool& readTo )
#ifndef __GAZEBO
throw( DataElement::UninitializedException )
#endif
{
    MutexLocker lock( mutex_ );
    if( isActive() && NULL != getDataValue() && getDataValue() ->copyTo( unit, readTo ) )
    {
        registerRead( reader );
        return true;
    }
    return false;
}

bool DataElement::get( DataReader &reader, const Unit& unit, unsigned char& readTo )
#ifndef __GAZEBO
throw( DataElement::UninitializedException )
#endif
{
    MutexLocker lock( mutex_ );
    if( isActive() && NULL != getDataValue() && getDataValue() ->copyTo( unit, readTo ) )
    {
        registerRead( reader );
        return true;
    }
    return false;
}

bool DataElement::get( DataReader &reader, const Unit& unit, double& readTo )
#ifndef __GAZEBO
throw( DataElement::UninitializedException )
#endif
{
    MutexLocker lock( mutex_ );
    if( isActive() && NULL != getDataValue() && getDataValue() ->copyTo( unit, readTo ) )
    {
        registerRead( reader );
        return true;
    }
    return false;
}

bool DataElement::get( DataReader &reader, const Unit& unit, float& readTo )
#ifndef __GAZEBO
throw( DataElement::UninitializedException )
#endif
{
    MutexLocker lock( mutex_ );
    if( isActive() && NULL != getDataValue() && getDataValue() ->copyTo( unit, readTo ) )
    {
        registerRead( reader );
        return true;
    }
    return false;
}

bool DataElement::get( DataReader &reader, const Unit& unit, int &readTo )
#ifndef __GAZEBO
throw( DataElement::UninitializedException )
#endif
{
    MutexLocker lock( mutex_ );
    if( isActive() && NULL != getDataValue() && getDataValue() ->copyTo( unit, readTo ) )
    {
        registerRead( reader );
        return true;
    }
    return false;
}

bool DataElement::get( DataReader &reader, DataValue& readTo )
#ifndef __GAZEBO
throw( DataElement::UninitializedException )
#endif
{
    MutexLocker lock( mutex_ );
    if( isActive() && NULL != getDataValue() && getDataValue() ->copyTo( readTo ) )
    {
        registerRead( reader );
        return true;
    }
    return false;
}

//-------------------------------------------

const double DataElement::asDouble( DataReader &reader, const Unit& unit )
#ifndef __GAZEBO
throw( DataElement::UninitializedException )
#endif
{
    double value = 0;
    if( get( reader, unit, value ) )
    {
        return value;
    }
    throw UninitializedException();
}
const int DataElement::asInt( DataReader &reader, const Unit& unit )
#ifndef __GAZEBO
throw( DataElement::UninitializedException )
#endif
{
    int value = 0;
    if( get( reader, unit, value ) )
    {
        return value;
    }
    throw UninitializedException();
}
const Str DataElement::asString( DataReader &reader, const Unit& unit )
#ifndef __GAZEBO
throw( DataElement::UninitializedException )
#endif
{
    /// First two lines just force logging of read.
    double value = 0;
    get( reader, unit, value );
    DataValue* dataValue = getDataValue();
    if( NULL != dataValue )
    {
        return dataValue->toString( unit );
    }
    throw UninitializedException();
}

//===============================================================
/// Set functions
/// set int value
bool DataElement::set( DataWriter &writer, const Unit& unit, const unsigned char value, const Timestamp& timestamp )
{
    try
    {
        MutexLocker lock( mutex_ );
        setTimestamp( timestamp );
        setWrittenAccuracy();
        DataValue* dataValue = getDataValue();
        if( NULL == dataValue )
        {
            setDataValue( unit( value ) );
            dataValue = getDataValue();
        }
        if( NULL != dataValue )
        {
            bool changed = dataValue->setFrom( unit, value ) || !hasBeenSet_;
            if( !hasBeenSet_ )
            {
                hasBeenSet_ = true;
                registerChangePending( &writer );
                registerChangeDone();
            }
            registerWrite( writer, changed );
            return true;
        }
    }
    catch( UninitializedException & ue )
    {}
    return false;
}

bool DataElement::set( DataWriter &writer, const Unit& unit, const bool value, const Timestamp& timestamp )
{
    try
    {
        MutexLocker lock( mutex_ );
        setTimestamp( timestamp );
        setWrittenAccuracy();
        DataValue* dataValue = getDataValue();
        if( NULL == dataValue )
        {
            setDataValue( unit( value ) );
            dataValue = getDataValue();
        }
        if( NULL != dataValue )
        {
            bool changed = dataValue->setFrom( unit, value ) || !hasBeenSet_;
            if( !hasBeenSet_ )
            {
                hasBeenSet_ = true;
                registerChangePending( &writer );
                registerChangeDone();
            }
            registerWrite( writer, changed );
            return true;
        }
    }
    catch( UninitializedException & ue )
    {}
    return false;
}

bool DataElement::set( DataWriter &writer, const Unit& unit, const double value, const Timestamp& timestamp )
{
    try
    {
        MutexLocker lock( mutex_ );
        setTimestamp( timestamp );
        setWrittenAccuracy();
        DataValue* dataValue = getDataValue();
        if( NULL == dataValue )
        {
            setDataValue( unit( value ) );
            dataValue = getDataValue();
        }
        if( NULL != dataValue )
        {
            bool changed = dataValue->setFrom( unit, value )  || !hasBeenSet_;
            if( !hasBeenSet_ )
            {
                hasBeenSet_ = true;
                registerChangePending( &writer );
                registerChangeDone();
            }
            registerWrite( writer, changed );
            return true;
        }
    }
    catch( UninitializedException & ue )
    {}
    return false;
}

bool DataElement::set( DataWriter &writer, const Unit& unit, const float value, const Timestamp& timestamp )
{
    try
    {
        MutexLocker lock( mutex_ );
        setTimestamp( timestamp );
        setWrittenAccuracy();
        DataValue* dataValue = getDataValue();
        if( NULL == dataValue )
        {
            setDataValue( unit( value ) );
            dataValue = getDataValue();
        }
        if( NULL != dataValue )
        {
            bool changed = dataValue->setFrom( unit, value )  || !hasBeenSet_;
            if( !hasBeenSet_ )
            {
                hasBeenSet_ = true;
                registerChangePending( &writer );
                registerChangeDone();
            }
            registerWrite( writer, changed );
            return true;
        }
    }
    catch( UninitializedException & ue )
    {}
    return false;
}

bool DataElement::set( DataWriter &writer, const Unit& unit, const int value, const Timestamp& timestamp )
{
    try
    {
        MutexLocker lock( mutex_ );
        setTimestamp( timestamp );
        setWrittenAccuracy();
        DataValue* dataValue = getDataValue();
        if( NULL == dataValue )
        {
            setDataValue( unit( value ) );
            dataValue = getDataValue();
        }
        if( NULL != dataValue )
        {
            bool changed = dataValue ->setFrom( unit, value )  || !hasBeenSet_;
            if( !hasBeenSet_ )
            {
                hasBeenSet_ = true;
                registerChangePending( &writer );
                registerChangeDone();
            }
            registerWrite( writer, changed );
            return true;
        }
    }
    catch( UninitializedException & ue )
    {}
    return false;
}
bool DataElement::set( DataWriter &writer, const DataValue& value, const Timestamp& timestamp )
{
    try
    {
        MutexLocker lock( mutex_ );
        setTimestamp( timestamp );
        setWrittenAccuracy();
        DataValue* dataValue = getDataValue();
        if( NULL == dataValue ) //|| ( NULL != dataValue && dataValue->getBinaryType() == MULTIVALUE && NULL != value && value.getBinaryType() == MULTIVALUE ) )
        {
            setDataValue( value.copy() );
            dataValue = getDataValue();
        }
        if( NULL != dataValue )
        {
            bool changed = dataValue ->setFrom( value )  || !hasBeenSet_;
            if( !hasBeenSet_ )
            {
                hasBeenSet_ = true;
                registerChangePending( &writer );
                registerChangeDone();
            }
            registerWrite( writer, changed );
            return true;
        }
    }
    catch( UninitializedException & ue )
    {}
    return false;
}



//-----------------------------------------------
/// Set w/ Accuracy functions

bool DataElement::setWithAccuracy( DataWriter &writer, const Unit& unit, const bool value, const float accuracy, const Timestamp& timestamp )
{
    setAccuracy( writer, unit, accuracy );
    return set( writer, unit, value, timestamp );
}

bool DataElement::setWithAccuracy( DataWriter &writer, const Unit& unit, const unsigned char value, const float accuracy, const Timestamp& timestamp )
{
    setAccuracy( writer, unit, accuracy );
    return set( writer, unit, value, timestamp );
}

bool DataElement::setWithAccuracy( DataWriter &writer, const Unit& unit, const double value, const float accuracy, const Timestamp& timestamp )
{
    setAccuracy( writer, unit, accuracy );
    return set( writer, unit, value, timestamp );
}

bool DataElement::setWithAccuracy( DataWriter &writer, const Unit& unit, const int value, const float accuracy, const Timestamp& timestamp )
{
    setAccuracy( writer, unit, accuracy );
    return set( writer, unit, value, timestamp );
}

bool DataElement::setWithAccuracy( DataWriter &writer, const DataValue& value, const float accuracy, const Timestamp& timestamp )
{
    setAccuracy( writer, value.getBaseUnit(), accuracy );
    return set( writer, value, timestamp );
}


//-----------------------------------------------
/// Set Accuracy functions
/// set double value
bool DataElement::setAccuracy( DataWriter &writer, const Unit& unit, const float accuracy )
{
    bool doChange = false;
    try
    {
        doChange = getAccuracySI() != unit.getSI( accuracy );
    }
    catch( UninitializedException & ui )
    {
        doChange = true;
    }
    if( doChange )
    {
        MutexLocker lock( mutex_ );
        float newAccuracy = unit.getSI( accuracy );
        float oldAccuracy = getBaseAccuracy();
        bool changedForLogging = ( *( unsigned int* ) & oldAccuracy & 0xFFFF0000 ) !=
                                 ( *( unsigned int* ) & newAccuracy & 0xFFFF0000 );
        setAccuracy( unit, accuracy );
        if( changedForLogging )
        {
            writer.getOwner()->getLogger().logSetAccuracy( writer.getOwner(), this );
        }
        registerChangePending( &writer );
        registerChangeDone();
        return true;
    }
    return false;
}

// Sets the unavailable flag
bool DataElement::setUnavailable( bool unavailable, DataWriter* notifier )
{
    if( unavailable_ != unavailable )
    {
        unavailable_ = unavailable;
        if( NULL != notifier )
        {
            notifier->getOwner()->getLogger().log( new EventEntry( notifier->getOwner(), this,
                                                   unavailable ?
                                                   EventEntry::DATA_STATE_CHANGE_UNAVAILABLE :
                                                   EventEntry::DATA_STATE_CHANGE_AVAILABLE ) );
        }
        registerChangePending( notifier );
        registerChangeDone();
    }
    return unavailable_;
}

// Sets the unavailable flag
bool DataElement::setUnavailable( bool unavailable, Component* notifier )
{
    if( unavailable_ != unavailable )
    {
        unavailable_ = unavailable;
        if( NULL != notifier )
        {
            notifier->getLogger().log( new EventEntry( notifier, this,
                                       unavailable ?
                                       EventEntry::DATA_STATE_CHANGE_UNAVAILABLE :
                                       EventEntry::DATA_STATE_CHANGE_AVAILABLE ) );
        }
        registerChangeDone( notifier );
    }
    return unavailable_;
}

// Sets the invalid flag
bool DataElement::setInvalid( bool invalid, DataWriter* notifier )
{
    if( invalid_ != invalid )
    {
        invalid_ = invalid;
        if( NULL != notifier )
        {
            notifier->getOwner()->getLogger().log( new EventEntry( notifier->getOwner(), this,
                                                   invalid ?
                                                   EventEntry::DATA_STATE_CHANGE_INVALID :
                                                   EventEntry::DATA_STATE_CHANGE_VALID ) );
        }
        registerChangePending( notifier );
        registerChangeDone();
    }
    return invalid_;
}

// Sets the invalid flag
bool DataElement::setInvalid( bool invalid, Component* notifier )
{
    if( invalid_ != invalid )
    {
        invalid_ = invalid;
        if( NULL != notifier )
        {
            notifier->getLogger().log( new EventEntry( notifier, this,
                                       invalid ?
                                       EventEntry::DATA_STATE_CHANGE_INVALID :
                                       EventEntry::DATA_STATE_CHANGE_VALID ) );
        }
        registerChangeDone( notifier );
    }
    return invalid_;
}

/// Sets the timestamp of last write
/// Used if a more acurate timestamp is available
void DataElement::setTimestamp( const Timestamp& timestamp )
{
    touchtime_ = Timestamp::Now();
    timestamp_ = timestamp == Timestamp::NOT_SET_TIME ? Timestamp::Now() : timestamp;
    if( NULL != universal_ )
    {
        universal_->setTimestamp( timestamp_ );
    }
};

//-------------------------------------------------------------------
// Logging functions
void DataElement::registerRead( DataReader &reader )
{
    //logger_.logData( reader, false );
}

void DataElement::registerWrite( DataWriter &writer, bool changed )
{
    writer.getOwner()->getLogger().logData( writer, changed );
}

void DataElement::registerChangeDone( Component* notifier )
{
    if( NULL != universal_ )
    {
        universal_->notifyChangeDone( notifier );
    }
}

void DataElement::registerChangeDone( void )
{
    if( NULL != universal_ )
    {
        universal_->notifyChangeDone( notifier_ );
    }
    notifier_ = NULL;
}

void DataElement::registerChangePending( DataWriter* writer )
{
    notifier_ = writer;
}

bool DataElement::wasTouchedSinceLastRun( const Component* component )
{
    return touchtime_ != Timestamp::NOT_SET_TIME && touchtime_ >= component->getTimeOfLastRun();
}


//===================================================================
/// Protected Constructor
DataElement::DataElement( const ElementURI& name, const Unit* baseUnit, size_t typeSize )
    : name_( name ),
      universal_( NULL ),
      notifier_( NULL ),
      //universalQueue_(),
      writers_( false ), // Do not delete contents
      readers_( false ), // Do not delete contents
      unavailable_( false ),
      invalid_( false ),
      wasOrphaned_( false ),
      mutex_(),
      writtenAccuracy_( NO_ACCURACY ),
      timestamp_( Timestamp::NOT_SET_TIME ),
      touchtime_( Timestamp::NOT_SET_TIME ),
      hasBeenSet_( false ),
      baseUnit_( baseUnit ),
      typeSize_( typeSize ),
      power_( nanf( "" ) ),
      latency_( nanf( "" ) ),
      frequency_( nanf( "" ) )
{
    Slate::CodifyElementURI( name_ );
}

/// Called before set commands to update the written accuracy
bool DataElement::setWrittenAccuracy()
{
    double newWrittenAccuracy;
    try
    {
        newWrittenAccuracy = getBaseAccuracy();
    }
    catch( UninitializedException& ue )
    {
        newWrittenAccuracy = NO_ACCURACY;
    }
    if( writtenAccuracy_ != newWrittenAccuracy )
    {
        writtenAccuracy_ = newWrittenAccuracy;
        return true;
    }
    return false;
}