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

#include "PeakDetectHorizontal.h"
#include "PeakDetectHorizontalIF.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 PeakDetectHorizontal::DEFAULT_FILTER_WIDTH( 3 );

/// if no "numProfilesSlidingwindow" setting is provided, what to use
const int PeakDetectHorizontal::DEFAULT_NUMPROFILES_SLIDINGWINDOW( 100 );

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

    logger_.syslog( "Construct PeakDetectHorizontal." );

    // Slate input setting variables
    detectFromSettingReader_ = newSettingReader( PeakDetectHorizontalIF::DETECT_FROM_SETTING );
    depthFromSettingReader_ = newSettingReader( PeakDetectHorizontalIF::DEPTH_FROM_SETTING );
    latitudeFromSettingReader_ = newSettingReader( PeakDetectHorizontalIF::LATITUDE_FROM_SETTING );
    longitudeFromSettingReader_ = newSettingReader( PeakDetectHorizontalIF::LONGITUDE_FROM_SETTING );
    advectValuesSettingReader_ = newSettingReader( PeakDetectHorizontalIF::ADVECT_VALUES_SETTING );
    patchTrackingSettingReader_ = newSettingReader( PeakDetectHorizontalIF::PATCH_TRACKING_SETTING );
    detectTroughSettingReader_ = newSettingReader( PeakDetectHorizontalIF::DETECT_TROUGH_SETTING );
    beginThresholdSettingReader_ = newSettingReader( PeakDetectHorizontalIF::BEGIN_THRESHOLD_SETTING );
    offPeakFractionSettingReader_ = newSettingReader( PeakDetectHorizontalIF::OFF_PEAK_FRACTION_SETTING );
    filterWidthSettingReader_ = newSettingReader( PeakDetectHorizontalIF::FILTER_WIDTH_SETTING );
    numProfilesSlidingwindowSettingReader_ = newSettingReader( PeakDetectHorizontalIF::NUMPROFILES_SLIDINGWINDOW_SETTING );
    centerPeakSettingReader_ = newSettingReader( PeakDetectHorizontalIF::CENTER_PEAK_SETTING );
    detectTimeoutSettingReader_ = newSettingReader( PeakDetectHorizontalIF::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;
}

PeakDetectHorizontal::~PeakDetectHorizontal()
{}

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

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

    for( int i = 0; i < MAX_NUMPROFILES_SLIDINGWINDOW; i++ )
        filtDetect_[i].set( nanf( "" ) );

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

    //debug
    //printf("startTime_ = Timestamp::Now();\n\n");

    // 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( !patchTrackingSettingReader_->read( Units::BOOL, patchTrackingSetting_ ) )
    {
        patchTrackingSetting_ = false;
    }

    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( numProfilesSlidingwindowSettingReader_->isActive() && !numProfilesSlidingwindowSettingReader_->read( Units::COUNT, numProfilesSlidingwindowSetting_ ) )
    {
        logger_.syslog( "Could not read numProfilesSlidingwindow Setting", Syslog::CRITICAL );
        return;
    }
    numProfilesSlidingwindowSetting_ = AuvMath::Max( numProfilesSlidingwindowSetting_, 1 );

    if( patchTrackingSetting_ )
    {
        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() )
        {
            if( !detectTimeoutSettingReader_->read( Units::SECOND, detectTimeoutSetting_ ) )
            {
                logger_.syslog( "Could not read detectTimeout Setting", Syslog::CRITICAL );
                return;
            }
            detectTimeout_ = Timespan( detectTimeoutSetting_ );

            //debug
            //printf("detectTimeout_.asFloat() = %f\n\n", detectTimeout_.asFloat());
        }
    }

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

/// Read in the parameters for satisfied or runIfUnsatisfied: return true if OK.
bool PeakDetectHorizontal::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_ );

    //debug
    //logger_.syslog( "detect_, depth_, latitude_, longitude_, distance_ = " + Str( R2D( detect_), 4 ) + ", " + Str( R2D( depth_ ), 4 ) + ", " + Str(R2D(latitude_), 4 ) + ", " + Str( R2D( longitude_ ), 4 ) + ", " + Str(R2D(distance_),4) + ", ", Syslog::DEBUG );

    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 PeakDetectHorizontal::calcSatisfied()
{
    int i, k, m;

    if( historySize_ == 0 )
    {
        startTime_ = Timestamp::Now();

        //debug
        //printf("startTime_ = Timestamp::Now(); #2\n\n");
    }

    // 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( 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( historySize_ <= filterWidthSetting_ )
        return false;

    for( k = ( numProfilesSlidingwindowSetting_ - 2 ); k >= 0; k-- )
        filtDetect_[k + 1] = filtDetect_[k];

    filtDetect_[0].set( 0.0 );
    for( i = 0; i < filterWidthSetting_; ++i )
    {
        //debug
        //float tmp;
        //tmp = filtDetect_[0].getValue();

        filtDetect_[0] += *filterValues_.get( i );

        //debug
        //printf("Add: %e\n", filtDetect_[0].getValue()-tmp);
    }

    filtDetect_[0] /= filterWidthSetting_;

    //debug
    //filtDetect_[0].set(filtDetect_[0].getTime(), filtDetect_[0].getLatitude(), filtDetect_[0].getLongitude(), filtDetect_[0].getDistance(), filtDetect_[0].getDepth(), historySize_%10);

    //debug
    //printf("inPeak_, filterWidthSetting_= %d, %d\n\n", inPeak_, filterWidthSetting_);

    //printf("filtDetect_[0].getValue(), filtDetect_[1].getValue(), filtDetect_[2].getValue(), filtDetect_[3].getValue() = %e, %e, %e, %e\n\n", filtDetect_[0].getValue(), filtDetect_[1].getValue(), filtDetect_[2].getValue(), filtDetect_[3].getValue());

    //printf("filtDetect_[0].getLatitude(), filtDetect_[1].getLatitude(), filtDetect_[2].getLatitude(), filtDetect_[3].getLatitude() = %f, %f, %f, %f\n\n", filtDetect_[0].getLatitude(), filtDetect_[1].getLatitude(), filtDetect_[2].getLatitude(), filtDetect_[3].getLatitude());

    //printf("filtDetect_[0].getLongitude(), filtDetect_[1].getLongitude(), filtDetect_[2].getLongitude(), filtDetect_[3].getLongitude() = %f, %f, %f, %f\n\n", filtDetect_[0].getLongitude(), filtDetect_[1].getLongitude(), filtDetect_[2].getLongitude(), filtDetect_[3].getLongitude());

    //printf("filtDetect_[0].getDistance(), filtDetect_[1].getDistance(), filtDetect_[2].getDistance(), filtDetect_[3].getDistance() = %f, %f, %f, %f\n\n", filtDetect_[0].getDistance(), filtDetect_[1].getDistance(), filtDetect_[2].getDistance(), filtDetect_[3].getDistance());

    if( patchTrackingSetting_ )
    {
        if( !inPeak_ )
        {
            if( isnan( beginThresholdSetting_ )
                    || ( !detectTroughSetting_ && filtDetect_[0].getValue() >= beginThresholdSetting_ )
                    || ( detectTroughSetting_ && filtDetect_[0].getValue() <= beginThresholdSetting_ ) )
            {
                inPeak_ = true;
                startDetect_ = filtDetect_[0];
            }
            else if( ( detectTimeout_ != Timespan::INVALID_TIMESPAN ) && ( detectTimeout_.asFloat() > 0.0 ) && ( startTime_.elapsed() > detectTimeout_ ) )
            {
                //debug
                //printf("(startTime_.elapsed()).asFloat(), detectTimeout_.asFloat() = %e, %e\n", (startTime_.elapsed()).asFloat(), detectTimeout_.asFloat());

                startDetect_ = filtDetect_[0];
                endDetect_ = filtDetect_[0];
                return true;
            }

        }
        else
        {
            if( ( !detectTroughSetting_ && filtDetect_[0].getValue() <= ( peakDetect_.getValue() * offPeakFractionSetting_ ) ) ||
                    ( detectTroughSetting_ && peakDetect_.getValue() <= ( filtDetect_[0].getValue() * offPeakFractionSetting_ ) ) )
            {

                inPeak_ = false;
                endDetect_ = filtDetect_[0];

                /*
                printf( "peakDetect_.getTime(), peakDetect_.getLatitude(), peakDetect_.getLongitude(), peakDetect_.getDistance(), peakDetect_.getDepth(), peakDetect_.getValue() = %f, %f, %f, %f, %f, %e\n\n", peakDetect_.getTime(), peakDetect_.getLatitude() * 180.0 / 3.14159265, peakDetect_.getLongitude() * 180.0 / 3.14159265, peakDetect_.getDistance(), peakDetect_.getDepth(), peakDetect_.getValue() );

                float latitude( nanf( "" ) ), longitude( nanf( "" ) ), distance;
                latitudeReader_->read( Units::DEGREE, latitude );
                longitudeReader_->read( Units::DEGREE, longitude );
                distanceWrtSeaWaterReader_->read( Units::METER, distance );
                printf( "Latitude, longitude, distance of location of reporting = %f, %f, %f\n\n", latitude, longitude, distance );
                */

                return true;
            }
        }
    }
    //printf("inPeak_=%s, peakIndex_=%d at peakDetect_=%g, filtDetect / peakDetect_=%g\n",inPeak_?"true":"false",peakIndex_, peakDetect_, filtDetect / peakDetect_);

    //Correct the delay (except for value). Delay = filterWidthSetting_/2.
    if( historySize_ >= ( filterWidthSetting_ + ( filterWidthSetting_ >> 1 ) ) )
        peakDetect_.set( filtDetect_[filterWidthSetting_ >> 1].getTime(), filtDetect_[filterWidthSetting_ >> 1].getLatitude(), filtDetect_[filterWidthSetting_ >> 1].getLongitude(), filtDetect_[filterWidthSetting_ >> 1].getDistance(), filtDetect_[filterWidthSetting_ >> 1].getDepth(), filtDetect_[0].getValue() );
    else
    {
        for( m = ( filterWidthSetting_ >> 1 ); m >= 0; m-- )
        {
            if( !isnan( filtDetect_[m].getTime() ) )
            {
                peakDetect_.set( filtDetect_[m].getTime(), filtDetect_[m].getLatitude(), filtDetect_[m].getLongitude(), filtDetect_[m].getDistance(), filtDetect_[m].getDepth(), filtDetect_[0].getValue() );

                break;
            }
        }
    }

    for( i = 1; i <= ( numProfilesSlidingwindowSetting_ - 1 - ( ( filterWidthSetting_ >> 1 ) ) ); i++ )
    {
        if( ( !detectTroughSetting_ && ( peakDetect_.getValue() < filtDetect_[i].getValue() ) )
                || ( detectTroughSetting_ && ( peakDetect_.getValue() > filtDetect_[i].getValue() ) ) )
        {
            for( m = ( filterWidthSetting_ >> 1 ); m >= 0; m-- )
            {
                if( !isnan( filtDetect_[i + m].getTime() ) )
                {
                    peakDetect_.set( filtDetect_[i + m].getTime(), filtDetect_[i + m].getLatitude(), filtDetect_[i + m].getLongitude(), filtDetect_[i + m].getDistance(), filtDetect_[i + m].getDepth(), filtDetect_[i].getValue() );
                    break;
                }
            }
        }
    }

    reportPeak();

    return false;
}

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

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

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

    return calcSatisfied();
}

/// Do the run, and return true if envelope "satisfied"
bool PeakDetectHorizontal::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 PeakDetectHorizontal::uninitialize( void )
{
    logger_.syslog( "Uninitialize." );
    if( inPeak_ )
    {
        reportPeak();
        initialize();
    }
}

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

    // Report requested values

    //debug
    //printf("peakDetect_.getTime(), peakDetect_.getLatitude(), peakDetect_.getLongitude(), peakDetect_.getDistance(), peakDetect_.getDepth(), peakDetect_.getValue() = %f, %f, %f, %f, %f, %e\n\n", peakDetect_.getTime(), peakDetect_.getLatitude()*180.0/3.14159265, peakDetect_.getLongitude()*180.0/3.14159265, peakDetect_.getDistance(), peakDetect_.getDepth(), peakDetect_.getValue());

    //printf("latitude_, filtDetect_[0].getLatitude(), filtDetect_[1].getLatitude(), filtDetect_[2].getLatitude(), filtDetect_[3].getLatitude() = %f, %f, %f, %f, %f\n\n", latitude_*180.0/3.14159265, filtDetect_[0].getLatitude()*180.0/3.14159265, filtDetect_[1].getLatitude()*180.0/3.14159265, filtDetect_[2].getLatitude()*180.0/3.14159265, filtDetect_[3].getLatitude()*180.0/3.14159265);

    //printf("longitude_, filtDetect_[0].getLongitude(), filtDetect_[1].getLongitude(), filtDetect_[2].getLongitude(), filtDetect_[3].getLongitude() = %f, %f, %f, %f, %f\n\n", longitude_*180.0/3.14159265, filtDetect_[0].getLongitude()*180.0/3.14159265, filtDetect_[1].getLongitude()*180.0/3.14159265, filtDetect_[2].getLongitude()*180.0/3.14159265, filtDetect_[3].getLongitude()*180.0/3.14159265);

    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_ )
        {
            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 );

            /*
                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* PeakDetectHorizontal::CreateBehavior( const Str& prefix, const Module* module )
{
    return new PeakDetectHorizontal( prefix, module );
}