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

#include "ValueDetect.h"
#include "ValueDetectIF.h"

#include "data/Slate.h"
#include "data/StrValue.h"
#include "data/UniversalDataReader.h"
#include "units/Units.h"

// Outputs go to Dynamic Control
#include "controlModule/VerticalControlIF.h"

/// if no "numProfilesThreshold" setting is provided, what to use.
const int ValueDetect::DEFAULT_NUM_PROFILES_THRESHOLD( 5 );

ValueDetect::ValueDetect( const Str& prefix, const Module* module )
    : Behavior( prefix + ValueDetectIF::NAME, module, true, true ),
      valueSetting_( nanf( "" ) ),
      deltaSetting_( nanf( "" ) ),
      upperThresholdSetting_( nanf( "" ) ),
      lowerThresholdSetting_( nanf( "" ) ),
      numProfilesThresholdSetting_( DEFAULT_NUM_PROFILES_THRESHOLD ),
      countProfiles_( 0 ),
      lastValue_( nanf( "" ) ),
      baseUnit_( &Units::NONE ),
      depthReader_( NULL ),
      latitudeReader_( NULL ),
      longitudeReader_( NULL ),
      pitchReader_( NULL ),
      headingReader_( NULL ),
      depthWriter_( NULL ),
      latitudeWriter_( NULL ),
      longitudeWriter_( NULL ),
      pitchWriter_( NULL ),
      headingWriter_( NULL )
{
    logger_.syslog( "Construct ValueDetect." );

    // Slate input setting variables
    detectSettingReader_ = newSettingReader( ValueDetectIF::DETECT_SETTING );
    outputSettingReader_ = newSettingReader( ValueDetectIF::OUTPUT_SETTING );
    valueSettingReader_ = newSettingReader( ValueDetectIF::VALUE_SETTING );
    deltaSettingReader_ = newSettingReader( ValueDetectIF::DELTA_SETTING );
    upperThresholdSettingReader_ = newSettingReader( ValueDetectIF::UPPER_THRESHOLD_SETTING );
    lowerThresholdSettingReader_ = newSettingReader( ValueDetectIF::LOWER_THRESHOLD_SETTING );
    numProfilesThresholdSettingReader_ = newSettingReader( ValueDetectIF::NUM_PROFILES_THRESHOLD_SETTING );

    // Slate input measurements - defined at initialization
    outputReader_ = NULL;

    /// Inputs from slate used for outputs - defined at initialization
    depthReader_ = NULL;
    latitudeReader_ = NULL;
    longitudeReader_ = NULL;
    pitchReader_ = NULL;
    headingReader_ = NULL;

    // Argument output variables - defined at initialization
    depthWriter_ = NULL;
    latitudeWriter_ = NULL;
    longitudeWriter_ = NULL;
    pitchWriter_ = NULL;
    headingWriter_ = NULL;
    outputValueWriter_ = NULL;
}

ValueDetect::~ValueDetect()
{}

/// Initialize function
void ValueDetect::initialize( void )
{
    logger_.syslog( "Initialize.", Syslog::INFO );

    // Let's read in all the relevant dataReaders
    if( !detectSettingReader_->isActive() )
    {
        logger_.syslog( "Detect setting not set", Syslog::CRITICAL );
        return;
    }
    baseUnit_ = &detectSettingReader_->getDefaultUnit()->getBaseUnit();

    if( NULL != baseUnit_ )
    {
        valueSettingReader_->read( *baseUnit_, valueSetting_ );
        deltaSettingReader_->read( *baseUnit_, deltaSetting_ );
        upperThresholdSettingReader_->read( *baseUnit_, upperThresholdSetting_ );
        lowerThresholdSettingReader_->read( *baseUnit_, lowerThresholdSetting_ );
    }
    else
    {
        valueSetting_ = nanf( "" );
        deltaSetting_ = nanf( "" );
        upperThresholdSetting_ = nanf( "" );
        lowerThresholdSetting_ = nanf( "" );
    }

    if( !numProfilesThresholdSettingReader_->read( Units::COUNT, numProfilesThresholdSetting_ ) || isnan( numProfilesThresholdSetting_ ) )
    {
        numProfilesThresholdSetting_ = DEFAULT_NUM_PROFILES_THRESHOLD;
    }

    if( !isnan( valueSetting_ ) && ( !isnan( upperThresholdSetting_ ) || !isnan( lowerThresholdSetting_ ) ) )
    {
        logger_.syslog( "Cannot set valueSetting_ (for value triggering) and either or both of upperThresholdSetting_ and upperThresholdSetting_ (for threshold triggering) to non-NaN.", Syslog::CRITICAL );
        return;
    }

    // Output Setting
    Str outputSetting( outputSettingReader_->asString( Units::NONE ) );
    if( NULL != outputReader_ && outputReader_->getUri() != outputSetting )
    {
        delete outputReader_;
        outputReader_ = NULL;
    }
    if( NULL == outputReader_ && outputSetting != Str::EMPTY_STR )
    {
        logger_.syslog( "Initialize with output=" + outputSetting, Syslog::INFO );
        if( outputSetting.find( "." ) == Str::NO_POS )
        {
            UniversalURI* outputURI = UniversalURI::FindURI( outputSetting.cStr() );
            if( NULL != outputURI )
            {
                outputReader_ = Slate::NewUniversalReader( *outputURI, this, NULL );
            }
            else
            {
                logger_.syslog( "No UniversalURI called " + outputSetting, Syslog::CRITICAL );
            }
        }
        else
        {
            ElementURI* outputURI = ElementURI::FindURI( outputSetting.cStr() );
            if( NULL != outputURI )
            {
                outputReader_ = Slate::NewReader( *outputURI, this, NULL );
            }
            else
            {
                logger_.syslog( "No ElementURI called " + outputSetting, Syslog::CRITICAL );
            }
        }
        if( NULL == outputReader_ )
        {
            logger_.syslog( "Could not create Reader called " + outputSetting, Syslog::CRITICAL );
            return;
        }
    }

    lastValue_ = nanf( "" );

    // Argument output/input  variables
    if( NULL == depthWriter_ )
    {
        depthWriter_ = findArgWriter( ValueDetectIF::DEPTH_OUTPUT );
        if( NULL != depthWriter_ )
        {
            depthReader_ = newUniversalReader( UniversalURI::DEPTH );
        }
    }
    if( NULL == latitudeWriter_ )
    {
        latitudeWriter_ = findArgWriter( ValueDetectIF::LATITUDE_OUTPUT );
        if( NULL != latitudeWriter_ )
        {
            latitudeReader_ = newUniversalReader( UniversalURI::LATITUDE );
        }
    }
    if( NULL == longitudeWriter_ )
    {
        longitudeWriter_ = findArgWriter( ValueDetectIF::LONGITUDE_OUTPUT );
        if( NULL != longitudeWriter_ )
        {
            longitudeReader_ = newUniversalReader( UniversalURI::LONGITUDE );
        }
    }
    if( NULL == pitchWriter_ )
    {
        pitchWriter_ = findArgWriter( ValueDetectIF::PITCH_OUTPUT );
        if( NULL != pitchWriter_ )
        {
            pitchReader_ = newUniversalReader( UniversalURI::PLATFORM_PITCH_ANGLE );
        }
    }
    if( NULL == headingWriter_ )
    {
        headingWriter_ = findArgWriter( ValueDetectIF::HEADING_OUTPUT );
        if( NULL != headingWriter_ )
        {
            headingReader_ = newUniversalReader( UniversalURI::PLATFORM_ORIENTATION );
        }
    }
    if( NULL == outputValueWriter_ )
    {
        outputValueWriter_ = findArgWriter( ValueDetectIF::OUTPUT_VALUE_OUTPUT );
    }
}

/// Read in the parameters for satisfied or runIfUnsatisfied: return true if OK.
bool ValueDetect::readParams( float& param )
{
    //if( !detectSettingReader_->isActive() )
    if( !detectSettingReader_->isActive() || !detectSettingReader_->wasTouchedSinceLastRun( this ) )
    {
        return false;
    }

    // Let's read in all the relevant measurements
    return detectSettingReader_->read( *baseUnit_, param );
}

/// Perform the satisfied: return true if envelope "satisfied"
bool ValueDetect::calcSatisfied( const float param )
{
    // Read these each time in case they are set

    if( NULL != baseUnit_ )
    {
        valueSettingReader_->read( *baseUnit_, valueSetting_ );
        deltaSettingReader_->read( *baseUnit_, deltaSetting_ );
        upperThresholdSettingReader_->read( *baseUnit_, upperThresholdSetting_ );
        lowerThresholdSettingReader_->read( *baseUnit_, lowerThresholdSetting_ );
    }
    else
    {
        valueSetting_ = nanf( "" );
        deltaSetting_ = nanf( "" );
        upperThresholdSetting_ = nanf( "" );
        lowerThresholdSetting_ = nanf( "" );
    }

    if( isnan( valueSetting_ ) && isnan( upperThresholdSetting_ ) && isnan( lowerThresholdSetting_ ) )
    {
        return false;
    }

    if( !isnan( valueSetting_ ) )
    {
        if( !isnan( deltaSetting_ ) )
        {
            return fabs( valueSetting_ - param ) < deltaSetting_;
        }
        else
        {
            bool retValue = !isnan( lastValue_ )
                            && ( ( lastValue_ <= valueSetting_ && param >= valueSetting_ )
                                 || ( lastValue_ >= valueSetting_ && param <= valueSetting_ ) );

            lastValue_ = param;
            return retValue;
        }
    }

    if( !isnan( upperThresholdSetting_ ) && !isnan( lowerThresholdSetting_ ) )
    {
        if( ( param >= lowerThresholdSetting_ ) && ( param <= upperThresholdSetting_ ) )
            countProfiles_++;
        else
            countProfiles_ = 0;

        if( countProfiles_ >= numProfilesThresholdSetting_ )
            return true;
        else
            return false;
    }
    else
    {
        if( !isnan( lowerThresholdSetting_ ) )
            if( param >= lowerThresholdSetting_ )
                countProfiles_++;
            else
                countProfiles_ = 0;
        else if( param <= upperThresholdSetting_ )
            countProfiles_++;
        else
            countProfiles_ = 0;

        if( countProfiles_ >= numProfilesThresholdSetting_ )
            return true;
        else
            return false;
    }

    return false;
}

/// Just do the run: ignore the results of the satisfied
void ValueDetect::run()
{
    if( runIfUnsatisfied() )
    {
        // Do nothing, we don't care about the return value, but this should satisfy Coverity
    }
}

/// Just do the satisfied: return true if envelope "satisfied"
bool ValueDetect::isSatisfied()
{

    float param;

    if( !readParams( param ) )
    {
        return false;
    }

    return calcSatisfied( param );
}

/// Do the run, and return true if envelope "satisfied"
bool ValueDetect::runIfUnsatisfied()
{
    if( baseUnit_ == NULL )
    {
        initialize();
        if( baseUnit_ == NULL )
        {
            return false;
        }
    }

    float param;

    if( readParams( param ) )
    {
        if( calcSatisfied( param ) )
        {
            Timestamp now( Timestamp::Now() );
            // Report requested values
            if( NULL != depthWriter_ )
            {
                float depth;
                if( depthReader_->read( Units::METER, depth ) )
                {
                    depthWriter_->write( Units::METER, depth, now );
                }
            }
            if( NULL != latitudeWriter_ )
            {
                double latitude;
                if( latitudeReader_->read( Units::RADIAN, latitude ) )
                {
                    latitudeWriter_->write( Units::RADIAN, latitude, now );
                }
            }
            if( NULL != longitudeWriter_ )
            {
                double longitude;
                if( longitudeReader_->read( Units::RADIAN, longitude ) )
                {
                    longitudeWriter_->write( Units::RADIAN, longitude, now );
                }
            }
            if( NULL != pitchWriter_ )
            {
                float pitch;
                if( pitchReader_->read( Units::RADIAN, pitch ) )
                {
                    pitchWriter_->write( Units::RADIAN, pitch, now );
                }
            }
            if( NULL != headingWriter_ )
            {
                float heading;
                if( headingReader_->read( Units::RADIAN, heading ) )
                {
                    headingWriter_->write( Units::RADIAN, heading, now );
                }
            }
            if( NULL != outputValueWriter_ && NULL != outputReader_ )
            {
                float output;
                if( outputReader_->read( *outputValueWriter_->getElement().getBaseUnit(), output ) )
                {
                    outputValueWriter_->write( *outputValueWriter_->getElement().getBaseUnit(), output, now );
                }
            }

            // Finally return
            return true;
        }
        return false;
    }
    return false;
}

/// Uninit function
void ValueDetect::uninitialize( void )
{
    logger_.syslog( "Uninitialize." );
}

/// Mission Component factory interface
Behavior* ValueDetect::CreateBehavior( const Str& prefix, const Module* module )
{
    return new ValueDetect( prefix, module );
}