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

#include "PatchTrack.h"
#include "PatchTrackIF.h"

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

#include <cstdlib>

/// if no "filterWidth" setting is provided, what to use.
const int PatchTrack::DEFAULT_FILTER_WIDTH( 3 );

PatchTrack::PatchTrack( const Str& prefix, const Module* module )
    : Behavior( prefix + PatchTrackIF::NAME, module, true, true ),
      detectFromBaseUnit_( &Units::NONE ),
      advectValuesSetting_( false ),
      detectTroughSetting_( false ),
      beginThresholdSetting_( nanf( "" ) ),
      offPeakFractionSetting_( nanf( "" ) ),
      filterWidthSetting_( DEFAULT_FILTER_WIDTH ),
      centerPeakSetting_( false ),
      detectTimeoutSetting_( Timespan::INVALID_TIMESPAN ),
      filterValues_( true ),
      filterPosition_( 0 ),
      historySize_( 0 ),
      lastValue_(),
      startDetect_(),
      peakDetect_(),
      endDetect_(),
      startTime_( Timestamp::NOT_SET_TIME ),
      detectTimedOut_( false ),
      inPeak_( false )
{

    logger_.syslog( "Construct PatchTrack." );

    // Slate input setting variables
    detectFromSettingReader_ = newSettingReader( PatchTrackIF::DETECT_FROM_SETTING );
    depthFromSettingReader_ = newSettingReader( PatchTrackIF::DEPTH_FROM_SETTING );
    latitudeFromSettingReader_ = newSettingReader( PatchTrackIF::LATITUDE_FROM_SETTING );
    longitudeFromSettingReader_ = newSettingReader( PatchTrackIF::LONGITUDE_FROM_SETTING );
    advectValuesSettingReader_ = newSettingReader( PatchTrackIF::ADVECT_VALUES_SETTING );
    detectTroughSettingReader_ = newSettingReader( PatchTrackIF::DETECT_TROUGH_SETTING );
    beginThresholdSettingReader_ = newSettingReader( PatchTrackIF::BEGIN_THRESHOLD_SETTING );
    offPeakFractionSettingReader_ = newSettingReader( PatchTrackIF::OFF_PEAK_FRACTION_SETTING );
    filterWidthSettingReader_ = newSettingReader( PatchTrackIF::FILTER_WIDTH_SETTING );
    centerPeakSettingReader_ = newSettingReader( PatchTrackIF::CENTER_PEAK_SETTING );
    detectTimeoutSettingReader_ = newSettingReader( PatchTrackIF::DETECT_TIMEOUT_SETTING );

    // Slate input measurements - must be defined at initialization
    distanceWrtSeaWaterReader_ = newUniversalReader( UniversalURI::PLATFORM_DISTANCE_WRT_SEA_WATER );
    depthReader_ = newUniversalReader( UniversalURI::DEPTH );
    latitudeReader_ = newUniversalReader( UniversalURI::LATITUDE );
    longitudeReader_ = newUniversalReader( UniversalURI::LONGITUDE );
    eastwardSeaWaterVelocityReader_ = newUniversalReader( UniversalURI::EASTWARD_SEA_WATER_VELOCITY );
    northwardSeaWaterVelocityReader_ = newUniversalReader( UniversalURI::NORTHWARD_SEA_WATER_VELOCITY );

    // Argument output variables - must be defined at initialization
    peakDetectWriter_ = NULL;
    peakDepthWriter_ = NULL;
    peakLatitudeWriter_ = NULL;
    peakLongitudeWriter_ = NULL;
    peakDistanceWriter_ = NULL;
}

PatchTrack::~PatchTrack()
{}

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

    inPeak_ = false;
    lastValue_.set( nanf( "" ) );
    startDetect_.set( nanf( "" ) );
    peakDetect_.set( nanf( "" ) );
    endDetect_.set( nanf( "" ) );

    filterPosition_ = 0;
    historySize_ = 0;
    startTime_ == Timestamp::Now();
    detectTimedOut_ = false;

    // Let's initialize the detectFromSetting
    if( !detectFromSettingReader_->isActive() )
    {
        logger_.syslog( "Must specify setting: detect", Syslog::CRITICAL );
        return;
    }

    detectFromBaseUnit_ = detectFromSettingReader_->getDefaultUnit() != NULL ? &detectFromSettingReader_->getDefaultUnit()->getBaseUnit() : &Units::NONE;

    if( ! detectTroughSettingReader_->read( Units::BOOL, detectTroughSetting_ ) )
    {
        detectTroughSetting_ = false;
    }

    if( filterWidthSettingReader_->isActive() && !filterWidthSettingReader_->read( Units::COUNT, filterWidthSetting_ ) )
    {
        logger_.syslog( "Could not read filterWidth Setting", Syslog::CRITICAL );
        return;
    }
    filterWidthSetting_ = AuvMath::Max( filterWidthSetting_, 1 );

    if( ! advectValuesSettingReader_->read( Units::BOOL, advectValuesSetting_ ) )
    {
        advectValuesSetting_ = false;
    }

    if( !beginThresholdSettingReader_->read( *detectFromBaseUnit_, beginThresholdSetting_ ) )
    {
        beginThresholdSetting_ = nanf( "" );
    }

    if( !offPeakFractionSettingReader_->isActive() )
    {
        logger_.syslog( "Must specify setting: offPeakFraction", Syslog::CRITICAL );
        return;
    }

    if( !offPeakFractionSettingReader_->read( *detectFromBaseUnit_, offPeakFractionSetting_ )
            || isnan( offPeakFractionSetting_ ) )
    {
        logger_.syslog( "Could not read offPeakFraction", Syslog::CRITICAL );
        return;
    }

    if( centerPeakSettingReader_->isActive() && !centerPeakSettingReader_->read( Units::BOOL, centerPeakSetting_ ) )
    {
        logger_.syslog( "Could not read centerPeak Setting", Syslog::CRITICAL );
        return;
    }

    if( detectTimeoutSettingReader_->isActive() )
    {
        double detectTimeoutSetting;
        if( !detectTimeoutSettingReader_->read( Units::SECOND, detectTimeoutSetting ) )
        {
            logger_.syslog( "Could not read centerPeak Setting", Syslog::CRITICAL );
            return;
        }
        detectTimeoutSetting_ = detectTimeoutSetting;
    }

    // Argument output variables
    peakDetectWriter_ = findArgWriter( PatchTrackIF::PEAK_DETECT_OUTPUT );
    peakDepthWriter_ = findArgWriter( PatchTrackIF::PEAK_DEPTH_OUTPUT );
    peakLatitudeWriter_ = findArgWriter( PatchTrackIF::PEAK_LATITUDE_OUTPUT );
    peakLongitudeWriter_ = findArgWriter( PatchTrackIF::PEAK_LONGITUDE_OUTPUT );
    peakDistanceWriter_ = findArgWriter( PatchTrackIF::PEAK_DISTANCE_OUTPUT );
}

/// Read in the parameters for satisfied or runIfUnsatisfied: return true if OK.
bool PatchTrack::readParams()
{
    if( NULL == detectFromSettingReader_ || !detectFromSettingReader_->isActive()
            || !detectFromSettingReader_->wasTouchedSinceLastRun( this ) )
    {
        return false;
    }

    // Let's read in all the relevant measurements
    bool ok = detectFromSettingReader_->read( *detectFromBaseUnit_, detect_ );
    ok &= depthFromSettingReader_->isActive() ? depthFromSettingReader_->read( Units::METER, depth_ ) : depthReader_->read( Units::METER, depth_ );
    ok &= latitudeFromSettingReader_->isActive() ? latitudeFromSettingReader_->read( Units::RADIAN, latitude_ ) : latitudeReader_->read( Units::RADIAN, latitude_ );
    ok &= longitudeFromSettingReader_->isActive() ? longitudeFromSettingReader_->read( Units::RADIAN, longitude_ ) : longitudeReader_->read( Units::RADIAN, longitude_ );
    ok &= distanceWrtSeaWaterReader_->read( Units::METER, distance_ );

    if( !eastwardSeaWaterVelocityReader_->read( Units::METER_PER_SECOND, eastwardVelocity_ ) )
    {
        eastwardVelocity_ = 0.0f;
    }
    if( !northwardSeaWaterVelocityReader_->read( Units::METER_PER_SECOND, northwardVelocity_ ) )
    {
        northwardVelocity_ = 0.0f;
    }

    return ok;
}

/// Perform the satisfied: return true if envelope "satisfied"
bool PatchTrack::calcSatisfied()
{
    if( historySize_ == 0 )
    {
        startTime_ = Timestamp::Now();
        detectTimedOut_ = false;
    }

    // First, let's advect old values
    if( advectValuesSetting_ )
    {
        const float bearing = atan2( eastwardVelocity_, northwardVelocity_ );
        const float distance = dt_.asFloat() * sqrt( eastwardVelocity_ * eastwardVelocity_ + northwardVelocity_ * northwardVelocity_ );
        startDetect_.advect( bearing, distance );
        peakDetect_.advect( bearing, distance );
        endDetect_.advect( bearing, distance );
    }

    if( detect_ != detect_ )
    {
        detect_ = 0;
    }

    if( lastValue_.differsFrom( depth_, detect_ ) )
    {
        lastValue_.set( detectFromSettingReader_->getMostRecentTimestamp().asDouble(), latitude_, longitude_, distance_, depth_, detect_ );
        PointValue* pv = filterValues_.get( filterPosition_ );
        if( NULL == pv )
        {
            pv = new PointValue();
            filterValues_.set( filterPosition_, pv );
        }
        *pv = lastValue_;
        ++filterPosition_;
        ++historySize_;
        if( filterPosition_ >= filterWidthSetting_ )
        {
            filterPosition_ = 0;
        }
    }

    // If filter not full, return
    if( historySize_ <= filterWidthSetting_ )
    {
        return false;
    }

    // Apply filtering
    // First entry in sliding window is the current filtered value
    filtDetect_.set( 0.0 );
    for( int i = 0; i < filterWidthSetting_; ++i )
    {
        filtDetect_ += *filterValues_.get( i );
    }
    filtDetect_ /= filterWidthSetting_;

    // Return true when exiting patch
    if( !inPeak_ )
    {
        //printf("elapsed=%g s historySize_=%d, detect_=%g, filtDetect=%g\n",startTime_.elapsed().asDouble(), historySize_, detect_, filtDetect_.getValue());
        if( isnan( beginThresholdSetting_ )
                || ( !detectTroughSetting_ && filtDetect_.getValue() >= beginThresholdSetting_ )
                || ( detectTroughSetting_ && filtDetect_.getValue() <= beginThresholdSetting_ ) )
        {
            inPeak_ = true;
            startDetect_ = filtDetect_;
            peakDetect_ = filtDetect_;
        }
        else if( detectTimeoutSetting_ != Timespan::INVALID_TIMESPAN && startTime_.elapsed() > detectTimeoutSetting_ )
        {
            //printf("!!!!!!! Detect Timeout!!!!!!!\n");
            detectTimedOut_ = true;
            return true;
        }
    }
    else
    {
        //printf("inPeak_=%s, historySize_=%d, peakDetect_=%g, detect_=%g, filtDetect=%g\n",inPeak_?"true":"false", historySize_, peakDetect_.getValue(), detect_, filtDetect_.getValue());
        if( ( !detectTroughSetting_ && filtDetect_.getValue() <= ( peakDetect_.getValue() * offPeakFractionSetting_ ) ) ||
                ( detectTroughSetting_ && filtDetect_.getValue() >= ( peakDetect_.getValue() * offPeakFractionSetting_ ) ) )
        {
            inPeak_ = false;
            endDetect_ = filtDetect_;
            //printf("!!!!!!! Got peak!!!!!!!\n");
            return true;
        }
        if( ( !detectTroughSetting_ && filtDetect_.getValue() >= peakDetect_.getValue() ) ||
                ( detectTroughSetting_ && filtDetect_.getValue() <= peakDetect_.getValue() ) )
        {
            peakDetect_ = filtDetect_;
        }
    }

    return false;
}

/// Just do the run: ignore the results of the satisfied
void PatchTrack::run()
{
    runIfUnsatisfied();
}

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

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

    return calcSatisfied();
}

/// Do the run, and return true if envelope "satisfied"
bool PatchTrack::runIfUnsatisfied()
{

    if( detectFromBaseUnit_ == NULL )
    {
        initialize();
        if( detectFromBaseUnit_ == NULL )
        {
            return false;
        }
    }

    if( readParams() )
    {
        if( calcSatisfied() )
        {
            reportPeak();
            initialize();
            return true;
        }
        return false;
    }
    return false;
}

/// Uninit function
void PatchTrack::uninitialize( void )
{
    logger_.syslog( "Uninitialize." );
    if( inPeak_ )
    {
        reportPeak();
        initialize();
    }
}

void PatchTrack::reportPeak()
{
    if( centerPeakSetting_ )
    {
        peakDetect_ = startDetect_;
        if( endDetect_.isSet() )
        {
            peakDetect_ += endDetect_;
        }
        else
        {
            peakDetect_ += lastValue_;
        }
        peakDetect_ /= 2;
    }

    // Report requested values
    if( detectTimedOut_ )
    {
        // maybe do nothing?
    }
    else if( !isnan( peakDetect_.getTime() ) && !isnan( peakDetect_.getLatitude() ) && !isnan( peakDetect_.getLongitude() ) && !isnan( peakDetect_.getDistance() ) && !isnan( peakDetect_.getDepth() ) && !isnan( peakDetect_.getValue() ) )
    {
        if( NULL != peakDetectWriter_ )
        {
            peakDetectWriter_->write( *detectFromBaseUnit_, peakDetect_.getValue(), peakDetect_.getTime() );
        }
        if( NULL != peakDepthWriter_ )
        {
            peakDepthWriter_->write( Units::METER, peakDetect_.getDepth(), peakDetect_.getTime() );
        }
        if( NULL != peakLatitudeWriter_ )
        {
            peakLatitudeWriter_->write( Units::RADIAN, peakDetect_.getLatitude(), peakDetect_.getTime() );
        }
        if( NULL != peakLongitudeWriter_ )
        {
            peakLongitudeWriter_->write( Units::RADIAN, peakDetect_.getLongitude(), peakDetect_.getTime() );
        }

        if( NULL != peakDistanceWriter_ )
        {
            if( advectValuesSetting_ )
            {
                float latitude( nanf( "" ) );
                float longitude( nanf( "" ) );
                latitudeReader_->read( Units::RADIAN, latitude );
                longitudeReader_->read( Units::RADIAN, longitude );
                double distance = Location::GetDistance( latitude, longitude, peakDetect_.getLatitude(), peakDetect_.getLongitude() );
                peakDistanceWriter_->write( Units::METER, distance );
            }
            else
            {
                float distance;
                distanceWrtSeaWaterReader_->read( Units::METER, distance );
                peakDistanceWriter_->write( Units::METER, distance - peakDetect_.getDistance() );
            }
        }
    }
}

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