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

#include "PeakDetectVsDepth.h"
#include "PeakDetectVsDepthIF.h"

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

#include <cstdlib>

/// if no "windowLength" setting is provided, what to use.
const int PeakDetectVsDepth::DEFAULT_WINDOW_LENGTH( 20 );

/// if no "medianFilterLength" setting is provided, what to use.
const int PeakDetectVsDepth::DEFAULT_MEDIAN_FILTER_LENGTH( 5 );

/// if no "consecutiveDepths" setting is provided, what to use
const int PeakDetectVsDepth::DEFAULT_CONSECUTIVE_DEPTHS( 4 );

/// if no "depthChangeThresh" setting is provided, what to use
const float PeakDetectVsDepth::DEFAULT_DEPTH_CHANGE_THRESH( 2 );

PeakDetectVsDepth::PeakDetectVsDepth( const Str& prefix, const Module* module )
    : Behavior( prefix + PeakDetectVsDepthIF::NAME, module, true, true ),
      consecutiveDepthsSetting_( DEFAULT_CONSECUTIVE_DEPTHS ),
      depthChangeThreshSetting_( DEFAULT_DEPTH_CHANGE_THRESH ),
      windowLengthSetting_( DEFAULT_WINDOW_LENGTH ),
      medianFilterLengthSetting_( DEFAULT_MEDIAN_FILTER_LENGTH ),
      detectBaseUnit_( &Units::NONE ),
      depthBaseUnit_( &Units::NONE ),
      peakDetect_( nanf( "" ) ),
      troughDetect_( nanf( "" ) ),
      peakDepth_( nanf( "" ) ),
      peakTemperature_( nanf( "" ) ),
      peakLatitude_( nanf( "" ) ),
      peakLongitude_( nanf( "" ) ),
      peakShallowBound_( nanf( "" ) ),
      peakDeepBound_( nanf( "" ) ),
      timeWindowPeakReport_( Timespan::INVALID_TIMESPAN ),
      timeWindowPeakReportSetting_( nanf( "" ) )

{

    logger_.syslog( "Construct PeakDetectVsDepth." );

    // Slate input setting variables
    detectSettingReader_ = newSettingReader( PeakDetectVsDepthIF::DETECT_SETTING );
    depthSurrogateSettingReader_ = newSettingReader( PeakDetectVsDepthIF::DEPTH_SURROGATE_SETTING );
    consecutiveDepthsSettingReader_ = newSettingReader( PeakDetectVsDepthIF::CONSECUTIVE_DEPTHS_SETTING );
    depthChangeThreshSettingReader_ = newSettingReader( PeakDetectVsDepthIF::DEPTH_CHANGE_THRESH_SETTING );
    windowLengthSettingReader_ = newSettingReader( PeakDetectVsDepthIF::WINDOW_LENGTH_SETTING );
    medianFilterLengthSettingReader_ = newSettingReader( PeakDetectVsDepthIF::MEDIAN_FILTER_LENGTH_SETTING );
    shallowBoundSettingReader_ = newSettingReader( PeakDetectVsDepthIF::SHALLOW_BOUND_SETTING );
    deepBoundSettingReader_ = newSettingReader( PeakDetectVsDepthIF::DEEP_BOUND_SETTING );
    timeWindowPeakReportSettingReader_ = newSettingReader( PeakDetectVsDepthIF::TIME_WINDOW_PEAK_REPORT_SETTING );

    // Slate input measurements - must be defined at initialization
    detectReader_ = NULL;
    depthReader_ = NULL;
    temperatureReader_ = newUniversalReader( UniversalURI::SEA_WATER_TEMPERATURE );
    latitudeReader_ = newUniversalReader( UniversalURI::LATITUDE );
    longitudeReader_ = newUniversalReader( UniversalURI::LONGITUDE );

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

    int k;

    for( k = 0; k < MAX_COSEC_DEPTH; k++ )
        dep_[k] = 0.0;

    for( k = 0; k <= 1; k++ )
        stateDepth_[k] = 1;

    cntrVar_ = 0;
    depMax_ = 0.0;
    depMin_ = 0.0;
    peakDepth_ = 0.0;
    peakTemperature_ = 0.0;
    peakProfile_ = 0.0;
    troughProfile_ = 0.0;
    varLowPass_ = 0.0;
    depthLowPass_ = 0.0;
    tempLowPass_ = 0.0;
    latLowPass_ = 0.0;
    lonLowPass_ = 0.0;
}

PeakDetectVsDepth::~PeakDetectVsDepth()
{}

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

    // Let's initialize the detectSetting
    if( !detectSettingReader_->isActive() )
    {
        logger_.syslog( "Must specify setting: detect", Syslog::CRITICAL );
        return;
    }
    Str detectSetting( detectSettingReader_->asString( Units::NONE ) );
    if( NULL != detectReader_ && detectReader_->getUri() != detectSetting )
    {
        delete detectReader_;
        detectReader_ = NULL;
    }
    if( detectSetting.find( "." ) == Str::NO_POS )
    {
        UniversalURI* surrogateURI = UniversalURI::FindURI( detectSetting.cStr() );
        if( NULL != surrogateURI )
        {
            detectReader_ = Slate::NewUniversalReader( *surrogateURI, this, NULL );
        }
        else
        {
            logger_.syslog( "No UniversalURI called " + detectSetting, Syslog::CRITICAL );
        }
    }
    else
    {
        ElementURI* surrogateURI = ElementURI::FindURI( detectSetting.cStr() );
        if( NULL != surrogateURI )
        {
            detectReader_ = Slate::NewReader( *surrogateURI, this, NULL );
        }
        else
        {
            logger_.syslog( "No ElementURI called " + detectSetting, Syslog::CRITICAL );
        }
    }
    if( NULL == detectReader_ )
    {
        logger_.syslog( "Could not create Reader called " + detectSetting, Syslog::CRITICAL );
        return;
    }

    detectBaseUnit_ = detectReader_->getElement().getBaseUnit();

    // Let's initialize the independentSetting
    StrValue depthSurrogateSetting( "depth" );
    if( depthSurrogateSettingReader_->isActive() )
    {
        depthSurrogateSettingReader_->read( depthSurrogateSetting );
    }
    if( NULL != depthReader_ && depthReader_->getUri() != depthSurrogateSetting.asString() )
    {
        delete depthReader_;
        depthReader_ = NULL;
    }
    if( NULL == depthReader_ )
    {
        if( depthSurrogateSetting.asString().find( "." ) == Str::NO_POS )
        {
            UniversalURI* surrogateURI = UniversalURI::FindURI( depthSurrogateSetting.asString().cStr() );
            if( NULL != surrogateURI )
            {
                depthReader_ = Slate::NewUniversalReader( *surrogateURI, this, NULL );
            }
            else
            {
                logger_.syslog( "No UniversalURI called " + depthSurrogateSetting.asString(), Syslog::CRITICAL );
            }
        }
        else
        {
            ElementURI* surrogateURI = ElementURI::FindURI( depthSurrogateSetting.asString().cStr() );
            if( NULL != surrogateURI )
            {
                depthReader_ = Slate::NewReader( *surrogateURI, this, NULL );
            }
            else
            {
                logger_.syslog( "No ElementURI called " + depthSurrogateSetting.asString(), Syslog::CRITICAL );
            }
        }
        if( NULL == depthReader_ )
        {
            logger_.syslog( "Could not create Reader called " + depthSurrogateSetting.asString(), Syslog::CRITICAL );
            return;
        }
    }

    if( !consecutiveDepthsSettingReader_->read( Units::COUNT, consecutiveDepthsSetting_ ) || isnan( consecutiveDepthsSetting_ ) )
    {
        consecutiveDepthsSetting_ = DEFAULT_CONSECUTIVE_DEPTHS;
    }

    if( !depthChangeThreshSettingReader_->read( Units::METER, depthChangeThreshSetting_ ) || isnan( depthChangeThreshSetting_ ) )
    {
        depthChangeThreshSetting_ = DEFAULT_DEPTH_CHANGE_THRESH;
    }

    if( !windowLengthSettingReader_->read( Units::COUNT, windowLengthSetting_ ) || isnan( windowLengthSetting_ ) )
    {
        windowLengthSetting_ = DEFAULT_WINDOW_LENGTH;
    }

    if( !medianFilterLengthSettingReader_->read( Units::COUNT, medianFilterLengthSetting_ ) || isnan( medianFilterLengthSetting_ ) )
    {
        medianFilterLengthSetting_ = DEFAULT_MEDIAN_FILTER_LENGTH;
    }

    shallowBoundSettingReader_->read( Units::METER, peakShallowBound_ );
    deepBoundSettingReader_->read( Units::METER, peakDeepBound_ );

    timeWindowPeakReportSettingReader_->read( Units::SECOND, timeWindowPeakReportSetting_ );
    // Don't yet assign to timeWindowPeakReport_, because timeWindowPeakReportSetting_ is NaN yet. See the assignment in calcSatisfied()

    consecutiveDepthsSetting_ = AuvMath::Limit( consecutiveDepthsSetting_, 1, MAX_COSEC_DEPTH );
    windowLengthSetting_ = AuvMath::Limit( windowLengthSetting_, 1, MAX_WINDOW_LENGTH );
    medianFilterLengthSetting_ = AuvMath::Limit( medianFilterLengthSetting_, 1, MAX_MEDIAN_FILTER_LENGTH );

    depthBaseUnit_ = depthReader_->getElement().getBaseUnit();

    // Argument output variables
    peakDetectWriter_ = findArgWriter( PeakDetectVsDepthIF::PEAK_DETECT_OUTPUT );
    troughDetectWriter_ = findArgWriter( PeakDetectVsDepthIF::TROUGH_DETECT_OUTPUT );
    peakDepthWriter_ = findArgWriter( PeakDetectVsDepthIF::PEAK_DEPTH_OUTPUT );
    peakTemperatureWriter_ = findArgWriter( PeakDetectVsDepthIF::PEAK_TEMPERATURE_OUTPUT );
    peakLatitudeWriter_ = findArgWriter( PeakDetectVsDepthIF::PEAK_LATITUDE_OUTPUT );
    peakLongitudeWriter_ = findArgWriter( PeakDetectVsDepthIF::PEAK_LONGITUDE_OUTPUT );

    startTimeWindowPeakReport_ = Timestamp::Now();

}

/// Read in the parameters for satisfied or runIfUnsatisfied: return true if OK.
bool PeakDetectVsDepth::readParams( float& depth, float& temperature, float& detect, double& lat, double& lon )
{

    if( NULL == depthReader_ || !depthReader_->isActive()
            || NULL == detectReader_ || !detectReader_->isActive()
            || !depthReader_->wasTouchedSinceLastRun( this ) )
    {
        return false;
    }

    /* debug
    detectReader_->read( *detectBaseUnit_, detect );
    depthReader_->read( *depthBaseUnit_, depth );
    temperatureReader_->read( Units::CELSIUS, temperature );
    latitudeReader_->read( Units::RADIAN, lat);
    longitudeReader_->read( Units::RADIAN, lon);
    printf("depth, temperature, detect, lat, lon = %f, %f, %e, %10.8f, %10.8f\n", depth, temperature, detect, lat, lon);
    //printf("temp WasTouched: %d\n", temperatureReader_->wasTouchedSinceLastRun( this ));
    */

    if( NULL != temperatureReader_ && temperatureReader_->isActive() )
        temperatureReader_->read( Units::CELSIUS, temperature );

    // Let's read in all the relevant measurements
    return detectReader_->read( *detectBaseUnit_, detect )
           && depthReader_->read( *depthBaseUnit_, depth )
           && latitudeReader_->read( Units::RADIAN, lat )
           && longitudeReader_->read( Units::RADIAN, lon );
}

/// Perform the satisfied: return true if envelope "satisfied"
bool PeakDetectVsDepth::calcSatisfied( const float depth, const float temperature, const float varProfile, const double lat, const double lon )
{
    int k, m, n, lenWindow, lenStorageWindowForMedianFilter;
    float varProfileMedian = 0.0, tmp, array_sort[MAX_MEDIAN_FILTER_LENGTH];
    //float varprofiletmp;
    bool condConsecutiveDiving, condConsecutiveClimbing;

    //printf("depth=%f, varProfile=%11.8f\n", depth, varProfile);

    // Read these each time in case they are set

    shallowBoundSettingReader_->read( Units::METER, peakShallowBound_ );
    deepBoundSettingReader_->read( Units::METER, peakDeepBound_ );

    if( !consecutiveDepthsSettingReader_->read( Units::COUNT, consecutiveDepthsSetting_ ) || isnan( consecutiveDepthsSetting_ ) )
    {
        consecutiveDepthsSetting_ = DEFAULT_CONSECUTIVE_DEPTHS;
    }

    if( !depthChangeThreshSettingReader_->read( Units::METER, depthChangeThreshSetting_ ) || isnan( depthChangeThreshSetting_ ) )
    {
        depthChangeThreshSetting_ = DEFAULT_DEPTH_CHANGE_THRESH;
    }

    if( !windowLengthSettingReader_->read( Units::COUNT, windowLengthSetting_ ) || isnan( windowLengthSetting_ ) )
    {
        windowLengthSetting_ = DEFAULT_WINDOW_LENGTH;
    }

    if( !medianFilterLengthSettingReader_->read( Units::COUNT, medianFilterLengthSetting_ ) || isnan( medianFilterLengthSetting_ ) )
    {
        medianFilterLengthSetting_ = DEFAULT_MEDIAN_FILTER_LENGTH;
    }

    timeWindowPeakReportSettingReader_->read( Units::SECOND, timeWindowPeakReportSetting_ );
    if( !isnan( timeWindowPeakReportSetting_ ) )
        timeWindowPeakReport_ = Timespan( timeWindowPeakReportSetting_ );

    if( isnan( depth ) || isnan( varProfile ) || isnan( peakShallowBound_ ) || isnan( peakDeepBound_ ) )
        return false;

    // if the shallow/deep bounds are valid numbers and depth is not inside them, return false
    if( ( !isnan( peakShallowBound_ ) && depth < peakShallowBound_ ) || ( !isnan( peakDeepBound_ ) && depth > peakDeepBound_ ) )
    {
        return false;
    }

    if( cntrVar_ < windowLengthSetting_ )
    {
        cntrVar_++;
        lenWindow = cntrVar_;
    }
    else
    {
        if( cntrVar_ <= ( windowLengthSetting_ + medianFilterLengthSetting_ ) )
            cntrVar_++;
        else
            cntrVar_ = windowLengthSetting_ + medianFilterLengthSetting_ + 1; // No need to continue to grow."+1" to cover the special case medianFilterLengthSetting_ == 1.

        lenWindow = windowLengthSetting_;
    }

    if( cntrVar_ < medianFilterLengthSetting_ )
        lenStorageWindowForMedianFilter = cntrVar_;
    else
        lenStorageWindowForMedianFilter = medianFilterLengthSetting_;

    if( lenStorageWindowForMedianFilter >= 2 )
    {
        for( k = ( lenStorageWindowForMedianFilter - 2 ); k >= 0; k-- )
            varProfileInWindow_[k + 1] = varProfileInWindow_[k];
    }

    varProfileInWindow_[0] = varProfile;

    //debug
    //varprofiletmp = cntrVar_;
    //varProfileInWindow_[0] = varprofiletmp;

    //debug
    //printf("lenStorageWindowForMedianFilter, lenWindow, cntrVar_ = %d, %d, %d\n", lenStorageWindowForMedianFilter, lenWindow, cntrVar_);

    //debug
    //printf("varProfileInWindow_[0], varProfileInWindow_[1], varProfileInWindow_[2], varProfileInWindow_[3], varProfileInWindow_[4] = %f, %f, %f, %f, %f\n", varProfileInWindow_[0], varProfileInWindow_[1], varProfileInWindow_[2], varProfileInWindow_[3], varProfileInWindow_[4]);

    // Calculate the median-filtered values.

    if( cntrVar_ >= medianFilterLengthSetting_ )
    {

        if( medianFilterLengthSetting_ >= 2 )
        {
            for( k = 0; k <= ( medianFilterLengthSetting_ - 1 ); k++ )
                array_sort[k] = varProfileInWindow_[k];

            //debug
            //printf("Before. array_sort[] = %f, %f, %f, %f, %f\n", array_sort[0], array_sort[1], array_sort[2], array_sort[3], array_sort[4]);

            for( m = 0; m <= ( medianFilterLengthSetting_ - 2 ); m++ ) // Bubble sorting
            {
                for( n = ( medianFilterLengthSetting_ - 1 ); n >= ( m + 1 ); n-- )
                {
                    if( array_sort[n - 1] > array_sort[n] )
                    {
                        tmp = array_sort[n];
                        array_sort[n] = array_sort[n - 1];
                        array_sort[n - 1] = tmp;
                    }
                }
            }

            varProfileMedian = array_sort[( medianFilterLengthSetting_ - 1 ) >> 1]; // Take the median value.

            //debug
            //printf("After. array_sort[] = %f, %f, %f, %f, %f, varProfileMedian = %f\n", array_sort[0], array_sort[1], array_sort[2], array_sort[3], array_sort[4], varProfileMedian);
        }
        else
        {
            varProfileMedian = varProfileInWindow_[0];

            //debug
            //printf("varProfile, varProfileMedian = %f, %f\n", varProfile, varProfileMedian);
        }

        // Calculate low-pass filtered values of varProfileMedian.
        if( cntrVar_ < ( windowLengthSetting_ + medianFilterLengthSetting_ ) )
            varLowPass_ = ( ( varLowPass_ * ( cntrVar_ - medianFilterLengthSetting_ ) ) + varProfileMedian ) / ( cntrVar_ - medianFilterLengthSetting_ + 1 );
        else
            varLowPass_ = ( ( varLowPass_ * windowLengthSetting_ ) - varProfileMedianInWindow_[windowLengthSetting_ - 1] + varProfileMedian ) / windowLengthSetting_;

        //debug
        //printf("varLowPass_ = %f\n", varLowPass_);

        if( lenWindow >= 2 )
        {
            for( k = ( lenWindow - 2 ); k >= 0; k-- )
            {
                varProfileMedianInWindow_[k + 1] = varProfileMedianInWindow_[k];
            }
        }

        varProfileMedianInWindow_[0] = varProfileMedian;
    }

    // Calculate low-pass filtered values of depth, temp, lat, and lon.

    if( cntrVar_ >= ( ( ( medianFilterLengthSetting_ - 1 ) >> 1 ) + 2 ) )
    {
        if( cntrVar_ <= ( windowLengthSetting_ + ( ( medianFilterLengthSetting_ - 1 ) >> 1 ) + 1 ) )
        {
            depthLowPass_ = ( ( depthLowPass_ * ( cntrVar_ - ( ( medianFilterLengthSetting_ - 1 ) >> 1 ) - 2 ) ) + depthInWindow_[( medianFilterLengthSetting_ - 1 ) >> 1] ) / ( cntrVar_ - ( ( medianFilterLengthSetting_ - 1 ) >> 1 ) - 1 );
            tempLowPass_ = ( ( tempLowPass_ * ( cntrVar_ - ( ( medianFilterLengthSetting_ - 1 ) >> 1 ) - 2 ) ) + tempInWindow_[( medianFilterLengthSetting_ - 1 ) >> 1] ) / ( cntrVar_ - ( ( medianFilterLengthSetting_ - 1 ) >> 1 ) - 1 );
            latLowPass_ = ( ( latLowPass_ * ( cntrVar_ - ( ( medianFilterLengthSetting_ - 1 ) >> 1 ) - 2 ) ) + latInWindow_[( medianFilterLengthSetting_ - 1 ) >> 1] ) / ( cntrVar_ - ( ( medianFilterLengthSetting_ - 1 ) >> 1 ) - 1 );
            lonLowPass_ = ( ( lonLowPass_ * ( cntrVar_ - ( ( medianFilterLengthSetting_ - 1 ) >> 1 ) - 2 ) ) + lonInWindow_[( medianFilterLengthSetting_ - 1 ) >> 1] ) / ( cntrVar_ - ( ( medianFilterLengthSetting_ - 1 ) >> 1 ) - 1 );
        }
        else
        {
            depthLowPass_ = ( ( depthLowPass_ * lenWindow ) - depthInWindow_[( lenWindow - 1 ) + ( ( medianFilterLengthSetting_ - 1 ) >> 1 ) + 1] + depthInWindow_[( medianFilterLengthSetting_ - 1 ) >> 1] ) / lenWindow; // Also corrects the delay induced by the median filter. Same for temp, lat, and lon below.
            tempLowPass_ = ( ( tempLowPass_ * lenWindow ) - tempInWindow_[( lenWindow - 1 ) + ( ( medianFilterLengthSetting_ - 1 ) >> 1 ) + 1] + tempInWindow_[( medianFilterLengthSetting_ - 1 ) >> 1] ) / lenWindow;
            latLowPass_ = ( ( latLowPass_ * lenWindow ) - latInWindow_[( lenWindow - 1 ) + ( ( medianFilterLengthSetting_ - 1 ) >> 1 ) + 1] + latInWindow_[( medianFilterLengthSetting_ - 1 ) >> 1] ) / lenWindow;
            lonLowPass_ = ( ( lonLowPass_ * lenWindow ) - lonInWindow_[( lenWindow - 1 ) + ( ( medianFilterLengthSetting_ - 1 ) >> 1 ) + 1] + lonInWindow_[( medianFilterLengthSetting_ - 1 ) >> 1] ) / lenWindow;
        }
    }

    /*debug
      printf("cntrVar_, lenWindow, windowLengthSetting_ = %d, %d, %d\n", cntrVar_, lenWindow, windowLengthSetting_);
      printf("varProfile, varProfileMedian = %f, %f\n", varProfile, varProfileMedian);
      printf("depthLowPass_, depth, depthInWindow_[0], depthInWindow_[1], depthInWindow_[2], depthInWindow_[3], depthInWindow_[4] = %f, %f, %f, %f, %f, %f, %f\n", depthLowPass_, depth, depthInWindow_[0], depthInWindow_[1], depthInWindow_[2], depthInWindow_[3], depthInWindow_[4]);
      printf("tempLowPass_, temperature, tempInWindow_[0], tempInWindow_[1], tempInWindow_[2], tempInWindow_[3], tempInWindow_[4] = %f, %f, %f, %f, %f, %f, %f\n", tempLowPass_, temperature, tempInWindow_[0], tempInWindow_[1], tempInWindow_[2], tempInWindow_[3], tempInWindow_[4]);
      printf("latLowPass_, lat, latInWindow_[0], latInWindow_[1], latInWindow_[2], latInWindow_[3], latInWindow_[4] = %10.8f, %10.8f, %10.8f, %10.8f, %10.8f, %10.8f, %10.8f\n", latLowPass_, lat, latInWindow_[0], latInWindow_[1], latInWindow_[2], latInWindow_[3], latInWindow_[4]);
      printf("lonLowPass_, lon, lonInWindow_[0], lonInWindow_[1], lonInWindow_[2], lonInWindow_[3], lonInWindow_[4] = %10.8f, %10.8f, %10.8f, %10.8f, %10.8f, %10.8f, %10.8f\n\n\n", lonLowPass_, lon, lonInWindow_[0], lonInWindow_[1], lonInWindow_[2], lonInWindow_[3], lonInWindow_[4]);
    */

    if( ( lenWindow + ( ( medianFilterLengthSetting_ - 1 ) >> 1 ) ) >= 2 )
    {
        for( k = ( lenWindow + ( ( medianFilterLengthSetting_ - 1 ) >> 1 ) ); k >= 0; k-- )
        {
            depthInWindow_[k + 1] = depthInWindow_[k];
            tempInWindow_[k + 1] = tempInWindow_[k];
            latInWindow_[k + 1] = latInWindow_[k];
            lonInWindow_[k + 1] = lonInWindow_[k];
        }
    }

    depthInWindow_[0] = depth;
    tempInWindow_[0] = temperature;
    latInWindow_[0] = lat;
    lonInWindow_[0] = lon;

    if( cntrVar_ < medianFilterLengthSetting_ )
        return false; // Because varProfileMedian is not produced yet.

    for( k = consecutiveDepthsSetting_; k >= 1; k-- )
        dep_[k] = dep_[k - 1];

    dep_[0] = depth;

    condConsecutiveDiving = ( dep_[0] >= dep_[1] );
    for( k = 1; k <= ( consecutiveDepthsSetting_ - 1 ); k++ )
        condConsecutiveDiving = condConsecutiveDiving && ( dep_[k] >= dep_[k + 1] );

    condConsecutiveClimbing = ( dep_[0] <= dep_[1] );
    for( k = 1; k <= ( consecutiveDepthsSetting_ - 1 ); k++ )
        condConsecutiveClimbing = condConsecutiveClimbing && ( dep_[k] <= dep_[k + 1] );

    stateDepth_[1] = stateDepth_[0];

    if( stateDepth_[0] == 1 )
    {
        if( depth > depMax_ )
            depMax_ = depth;
        else
        {
            if( ( ( depth - depMax_ ) <= -depthChangeThreshSetting_ ) && condConsecutiveClimbing )
            {
                stateDepth_[0] = 0;
                depMin_ = depth;
            }
        }
    }
    else
    {
        if( depth < depMin_ )
            depMin_ = depth;
        else
        {
            if( ( ( depth - depMin_ ) >= depthChangeThreshSetting_ ) && condConsecutiveDiving )
            {
                stateDepth_[0] = 1;
                depMax_ = depth;
            }
        }
    }

    if( ( !( timeWindowPeakReport_.asFloat() > 0.0 ) && ( stateDepth_[1] == ( 1 - stateDepth_[0] ) ) ) ||
            ( ( timeWindowPeakReport_.asFloat() > 0.0 ) && ( startTimeWindowPeakReport_.elapsed() >= timeWindowPeakReport_ ) ) ) // When the vehicle flips attitude or reaches the end of a peak-reporting time window.
    {
        //debug
        //printf("dep_[0], dep_[1], dep_[2], dep_[3], condConsecutiveDiving, condConsecutiveClimbing, depthChangeThreshSetting_, consecutiveDepthsSetting_, windowLengthSetting_, medianFilterLengthSetting_ = %f, %f, %f, %f, %i, %i, %f, %d, %d, %d\n", dep_[0], dep_[1], dep_[2], dep_[3], condConsecutiveDiving, condConsecutiveClimbing, depthChangeThreshSetting_, consecutiveDepthsSetting_, windowLengthSetting_, medianFilterLengthSetting_);

        //printf( "(startTimeWindowPeakReport_.elapsed()).asFloat(), timeWindowPeakReport_.asFloat() = %f, %f\n\n", (startTimeWindowPeakReport_.elapsed()).asFloat(), timeWindowPeakReport_.asFloat() );

        //printf( "stateDepth_[0], stateDepth_[1] = %i, %i\n", stateDepth_[0], stateDepth_[1] );

        //printf( "\n\npeakDetect_, peakIndependent_ = %11.8f, %5.2f m\n\n\n",
        //        peakDetect_, peakIndependent_ );

        peakProfile_ = nanf( "" ); // Reset.
        troughProfile_ = nanf( "" );

        startTimeWindowPeakReport_ = Timestamp::Now();

        // If peakDetect_ is NaN, return false
        if( isnan( peakDetect_ ) )
        {
            return false;
        }

        //debug
        //printf("depth, peakDetect_ = %f, %f\n", depth, peakDetect_);

        return true;
    }
    else
    {
        if( isnan( peakProfile_ ) || varLowPass_ > peakProfile_ )
        {
            peakDetect_ = peakProfile_ = varLowPass_;
            peakDepth_ = depthLowPass_;
            peakTemperature_ = tempLowPass_;
            peakLatitude_ = latLowPass_;
            peakLongitude_ = lonLowPass_;
        }
        if( isnan( troughProfile_ ) || varLowPass_ < troughProfile_ )
        {
            troughDetect_ = troughProfile_ = varLowPass_;
        }
        return false;
    }
}

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

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

    float depth, temperature, detect;
    double lat, lon;

    if( !readParams( depth, temperature, detect, lat, lon ) )
    {
        return false;
    }

    return calcSatisfied( depth, temperature, detect, lat, lon );
}

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

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

    float depth, temperature, detect;
    double lat, lon;

    if( readParams( depth, temperature, detect, lat, lon ) )
    {
        if( calcSatisfied( depth, temperature, detect, lat, lon ) )
        {
            Timestamp now( Timestamp::Now() );

            // Report requested values
            if( NULL != peakDetectWriter_ )
            {
                peakDetectWriter_->write( *detectBaseUnit_, peakDetect_, now );
            }
            if( NULL != peakDepthWriter_ )
            {
                peakDepthWriter_->write( *depthBaseUnit_, peakDepth_, now );
            }
            if( NULL != peakTemperatureWriter_ )
            {
                peakTemperatureWriter_->write( Units::CELSIUS, peakTemperature_, now );
            }
            if( NULL != peakLatitudeWriter_ )
            {
                peakLatitudeWriter_->write( Units::RADIAN, peakLatitude_, now );
            }
            if( NULL != peakLongitudeWriter_ )
            {
                peakLongitudeWriter_->write( Units::RADIAN, peakLongitude_, now );
            }

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

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

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