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

#include "TempGradientCalculator.h"
#include "TempGradientCalculatorIF.h"

#include "data/ConfigReader.h"
#include "data/Slate.h"
#include "data/UniversalDataReader.h"
#include "data/UniversalDataWriter.h"
#include "units/Units.h"
#include "utils/AuvMath.h"
#include "utils/Timestamp.h"
#include <stdlib.h>

TempGradientCalculator::TempGradientCalculator( const Module* module )
    : SyncDerivationComponent( TempGradientCalculatorIF::NAME, module ),
      numAvg_( 0 ), binCnt_( 0 ), numConsecutiveDepths_( -1 )
{
    int k;

    // accessors
    depthReader_ = newUniversalReader( UniversalURI::DEPTH );
    tempReader_ = newUniversalReader( UniversalURI::SEA_WATER_TEMPERATURE );
    tempGradientWriter_ = newUniversalWriter( UniversalURI::UPWARD_DERIVATIVE_OF_SEA_WATER_TEMPERATURE );
    thermoclineDepthWriter_ = newDataWriter( TempGradientCalculatorIF::THERMOCLINE_DEPTH );
    targetDepthWriter_ = newDataWriter( TempGradientCalculatorIF::TARGET_DEPTH );
    tempHoriGradWriter_ = newDataWriter( TempGradientCalculatorIF::TEMP_HORI_GRAD );

    binsizeDepCfgReader_ = newConfigReader( TempGradientCalculatorIF::BINSIZE_DEP );
    numConsecutiveDepthsCfgReader_ = newConfigReader( TempGradientCalculatorIF::NUM_CONSEC_DEPTHS );
    threshDepChangeAbsCfgReader_ = newConfigReader( TempGradientCalculatorIF::THRESH_DEP_CHANGE_ABS );
    extensionDepCfgReader_ = newConfigReader( TempGradientCalculatorIF::EXTENSION_DEPTH );
    depShallowBndForAvgCfgReader_ = newConfigReader( TempGradientCalculatorIF::DEP_SHALLOW_BND_FOR_AVG );
    depDeepBndForAvgCfgReader_ = newConfigReader( TempGradientCalculatorIF::DEP_DEEP_BND_FOR_AVG );
    numProfilesLPCfgReader_ = newConfigReader( TempGradientCalculatorIF::NUM_PROFILES_LP );
    numProfilesGapCfgReader_ = newConfigReader( TempGradientCalculatorIF::NUM_PROFILES_GAP );

    for( k = 0; k <= 2; k++ )
    {
        tempDepBin_[k] = 0.0;
        depBinEnd_[k] = 0.0;
    }

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

    depMax_ = 0.0;
    depMin_ = 0.0;
    depTarget_ = 0.0;
    lastDepTarget_ = 0.0;
    depTempGradPk_ = 0.0;
    lastDepTempGradPk_ = 0.0;
    tempGradMaxabs_ = 0.0;

    cntProfileAvgLayer_ = 0;
    numAvgLayer_ = 0;
    tempAvgLayer_ = 0.0;
    for( k = 0; k <= ( MAX_PROFILES - 1 ); k++ ) // tempAvgLayerArray_[0] saves the oldest.
    {
        tempAvgLayerArray_[k] = 0.0;
    }
    flagDepthShallow_ = 0;
    flagDepthDeep_ = 0;
    tempHoriGrad_ = 0.0;

    //debug
    numProfilesLP_ = 2;
    numProfilesGap_ = 3;

    for( k = 0; k <= ( numProfilesLP_ + numProfilesGap_ ) && k <= ( MAX_PROFILES - 1 ); k++ )
        tempAvgLayerLP_[k] = 0.0;

}

TempGradientCalculator::~TempGradientCalculator()
{}

void TempGradientCalculator::initialize( void )
{
    logger_.syslog( "Initializing TempGradientCalculator." );
    readConfig();
}

void TempGradientCalculator::readConfig( void )
{
    binsizeDepCfgReader_->read( Units::METER, binsizeDep_ );
    threshDepChangeAbsCfgReader_->read( Units::METER, threshDepChangeAbs_ );
    int oldNumConsecDepths = numConsecutiveDepths_;
    numConsecutiveDepthsCfgReader_->read( Units::COUNT, numConsecutiveDepths_ );
    extensionDepCfgReader_->read( Units::METER, extensionDep_ );
    depShallowBndForAvgCfgReader_->read( Units::METER, depShallowBndForAvg_ );
    depDeepBndForAvgCfgReader_->read( Units::METER, depDeepBndForAvg_ );
    numProfilesLPCfgReader_->read( Units::COUNT, numProfilesLP_ );
    numProfilesGapCfgReader_->read( Units::COUNT, numProfilesGap_ );

    if( oldNumConsecDepths != numConsecutiveDepths_ )
    {
        for( int k = 0; k <= numConsecutiveDepths_ && k < MAX_CONSEC_DEPTHS; k++ )
        {
            dep_[k] = 0.0;
        }
    }

}

void TempGradientCalculator::run( void )
{
    if( ( depthReader_->isActive() ) && ( tempReader_->isActive() ) )
    {
        float depth( nan( "" ) ), temp( nan( "" ) );
        depthReader_->read( Units::METER, depth );
        tempReader_->read( Units::CELSIUS, temp );

        if( ( depth >= 0.0 ) && ( !isnan( temp ) ) )
        {
            float tempAccuracy( tempReader_->getAccuracy( Units::CELSIUS ) );
            float tempGradAccuracy = tempAccuracy / binsizeDep_;

            captureTempGradPeakCalcTempHoriGrad( temp, depth );

            tempGradientWriter_->writeWithAccuracy( Units::CELSIUS_PER_METER, tempGrad_, tempGradAccuracy );
        }
    }
    else
    {
        //logger_.syslog( "Depth and/or temperature measurement is not active", Syslog::ERROR );
    }
    return;
}

void TempGradientCalculator::captureTempGradPeakCalcTempHoriGrad( float temp, float depth )
{
    int k, m, m_max, len_window_LP;
    bool condConsecutiveDiving, condConsecutiveClimbing;

    if( isnan( temp ) || isnan( depth ) )
        return;

    for( k = AuvMath::Min( numConsecutiveDepths_, MAX_CONSEC_DEPTHS - 1 ); k >= 1; k-- )
        dep_[k] = dep_[k - 1];

    dep_[0] = depth;

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

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

//    printf("dep_[0], dep[1], condConsecutiveDiving, condConsecutiveClimbing = %f, %f, %i, %i\n", dep_[0], dep_[1], condConsecutiveDiving,
//    		condConsecutiveClimbing);

    stateDepth_[1] = stateDepth_[0];

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

    if( stateDepth_[1] == ( 1 - stateDepth_[0] ) )  // When the vehicle flips attitude.
    {
        //printf( "stateDepth_[0], stateDepth_[1] = %i, %i\n", stateDepth_[0], stateDepth_[1] );

        if( ( flagDepthShallow_ == 1 ) && ( flagDepthDeep_ == 1 ) )
        {
            cntProfileAvgLayer_++;

            //debug
            //printf("cntProfileAvgLayer_, tempAvgLayer_, depShallowBndForAvg_, depDeepBndForAvg_, numProfilesLP_, numProfilesGap_ = %d, %f, %f, %f, %d, %d\n", cntProfileAvgLayer_, tempAvgLayer_, depShallowBndForAvg_, depDeepBndForAvg_, numProfilesLP_, numProfilesGap_);

            if( cntProfileAvgLayer_ <= MAX_PROFILES ) // Only saves tempAvgLayerArray_ up to MAX_PROFILES profiles.
            {
                tempAvgLayerArray_[cntProfileAvgLayer_ - 1] = tempAvgLayer_;
            }
            else
            {
                for( k = 0; k <= ( MAX_PROFILES - 2 ); k++ ) // tempAvgLayerArray_[0] saves the oldest.
                {
                    tempAvgLayerArray_[k] = tempAvgLayerArray_[k + 1];
                }
                tempAvgLayerArray_[MAX_PROFILES - 1] = tempAvgLayer_;
            }

            if( ( cntProfileAvgLayer_ - 1 ) <= ( numProfilesLP_ + numProfilesGap_ ) )
                m_max = cntProfileAvgLayer_ - 1;
            else
            {
                m_max = numProfilesLP_ + numProfilesGap_;

                for( m = 0; m <= ( m_max - 1 ); m++ ) // tempAvgLayerLP_[0] saves the oldest.
                    tempAvgLayerLP_[m] = tempAvgLayerLP_[m + 1];
            }

            // Compute low-pass filtered tempAvgLayer.
            if( cntProfileAvgLayer_ <= numProfilesLP_ )
                len_window_LP = cntProfileAvgLayer_;
            else
                len_window_LP = numProfilesLP_;

            if( cntProfileAvgLayer_ == 1 )
            {
                tempAvgLayerSumInWindow_ = tempAvgLayer_;
                tempAvgLayerLP_[m_max] = tempAvgLayer_; // tempAvgLayerLP_[0] saves the oldest.
            }
            else
            {
                if( cntProfileAvgLayer_ <= MAX_PROFILES )
                {
                    if( cntProfileAvgLayer_ <= len_window_LP )
                        tempAvgLayerSumInWindow_ = tempAvgLayerSumInWindow_ + tempAvgLayer_;
                    else
                        tempAvgLayerSumInWindow_ = tempAvgLayerSumInWindow_ - tempAvgLayerArray_[( cntProfileAvgLayer_ - 1 ) - numProfilesLP_] + tempAvgLayer_;
                }
                else
                    tempAvgLayerSumInWindow_ = tempAvgLayerSumInWindow_ - tempAvgLayerArray_[( MAX_PROFILES - 1 ) - numProfilesLP_] + tempAvgLayer_;

                tempAvgLayerLP_[m_max] = tempAvgLayerSumInWindow_ / len_window_LP;
            }

            if( ( cntProfileAvgLayer_ - 1 ) >= ( numProfilesLP_ + numProfilesGap_ ) )
            {
                tempHoriGrad_ = tempAvgLayerLP_[m_max] - tempAvgLayerLP_[0];
                tempHoriGradWriter_->write( Units::CELSIUS, tempHoriGrad_ );
            }

            //debug
            //printf("tempAvgLayerArray_[0], tempAvgLayerArray_[1], tempAvgLayerArray_[2], tempAvgLayerArray_[3], tempAvgLayerArray_[4], tempAvgLayerArray_[5], tempAvgLayerArray_[6], tempAvgLayerArray_[7], tempAvgLayerArray_[8], tempAvgLayerArray_[9]  = \n%f, %f, %f, %f, %f, %f, %f, %f, %f, %f\n\n", tempAvgLayerArray_[0], tempAvgLayerArray_[1], tempAvgLayerArray_[2], tempAvgLayerArray_[3], tempAvgLayerArray_[4], tempAvgLayerArray_[5], tempAvgLayerArray_[6], tempAvgLayerArray_[7], tempAvgLayerArray_[8], tempAvgLayerArray_[9]);

            //debug
            //printf("tempAvgLayerLP_[0], tempAvgLayerLP_[1], tempAvgLayerLP_[2], tempAvgLayerLP_[3], tempAvgLayerLP_[4], tempAvgLayerLP_[5], tempAvgLayerLP_[6], tempAvgLayerLP_[7], tempAvgLayerLP_[8], tempAvgLayerLP_[9], tempHoriGrad_ = \n%f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f\n\n", tempAvgLayerLP_[0], tempAvgLayerLP_[1], tempAvgLayerLP_[2], tempAvgLayerLP_[3], tempAvgLayerLP_[4], tempAvgLayerLP_[5], tempAvgLayerLP_[6], tempAvgLayerLP_[7], tempAvgLayerLP_[8], tempAvgLayerLP_[9], tempHoriGrad_);

        }

        //debug
        //printf("cntProfileAvgLayer_, tempAvgLayerArray_[0], tempAvgLayerArray_[1], = %d, %5.2f, %5.2f\n",
        //       cntProfileAvgLayer_, tempAvgLayerArray_[0], tempAvgLayerArray_[1]);

        tempAvgLayer_ = 0.0;
        numAvgLayer_ = 0;
        flagDepthShallow_ = 0;
        flagDepthDeep_ = 0;

        if( stateDepth_[0] == 1 )
            depTarget_ = depTempGradPk_ + extensionDep_;
        else
        {
            depTarget_ = depTempGradPk_ - extensionDep_;
            if( depTarget_ < 0 )
            {
                depTarget_ = 0;
            }
        }

        tempGradMaxabs_ = 0.0;

        if( lastDepTempGradPk_ != depTempGradPk_ )
        {
            lastDepTempGradPk_ = depTempGradPk_;
            thermoclineDepthWriter_->write( Units::METER, depTempGradPk_ );
        }
        if( lastDepTarget_ != depTarget_ )
        {
            lastDepTarget_ = depTarget_;
            targetDepthWriter_->write( Units::METER, depTarget_ );
        }

        //printf( "depTempGradPk_ = %5.2f m\n", depTempGradPk_ );
    }
    else // When the vehicle continues on a descent or ascent
    {

        if( ( depth <= depShallowBndForAvg_ ) && ( flagDepthShallow_ == 0 ) )
        {
            flagDepthShallow_ = 1;
        }

        if( ( depth >= depDeepBndForAvg_ ) && ( flagDepthDeep_ == 0 ) )
        {
            flagDepthDeep_ = 1;
        }

        if( ( depth >= depShallowBndForAvg_ ) && ( depth <= depDeepBndForAvg_ ) )
        {
            tempAvgLayer_ = ( ( tempAvgLayer_ * numAvgLayer_ ) + temp ) / ( numAvgLayer_ + 1 );

            numAvgLayer_++;

            //debug
            //printf("depth, temp, numAvgLayer_, tempAvgLayer_ = %5.2f, %5.2f, %d, %5.2f\n",
            //   depth, temp, numAvgLayer_, tempAvgLayer_);
        }
    }

    if( numAvg_ == 0 )
    {
        depBin_ = int( depth / binsizeDep_ ) * binsizeDep_;
        tempDepBin_[0] = temp;
        depBinEnd_[0] = depth;

        tempGrad_ = 0.0;
        depTempGrad_ = depth;

        numAvg_++;
    }
    else
    {
        if( ( ( depth - depBin_ ) < binsizeDep_ ) && ( ( depth - depBin_ ) > -binsizeDep_ ) )
        {
            tempDepBin_[0] = ( ( tempDepBin_[0] * numAvg_ ) + temp ) / ( numAvg_ + 1 );
            depBinEnd_[0] = depth;
            numAvg_++;
        }
        else
        {
            binCnt_++;

            tempDepBin_[2] = tempDepBin_[1];
            tempDepBin_[1] = tempDepBin_[0];
            tempDepBin_[0] = temp;

            depBinEnd_[2] = depBinEnd_[1];
            depBinEnd_[1] = depBinEnd_[0];
            depBinEnd_[0] = depth;

            numAvg_ = 1;

            if( depth > depBin_ )
            {
                depBin_ = depBin_ + ( int( ( depth - depBin_ ) / binsizeDep_ ) * binsizeDep_ );
            }
            else
            {
                depBin_ = depBin_ - ( int( ( depBin_ - depth ) / binsizeDep_ ) * binsizeDep_ );
            }

            if( binCnt_ == 1 )
            {
                tempGrad_ = 0.0;
                depTempGrad_ = depBinEnd_[1];
            }
            else
            {
                tempGrad_ = ( tempDepBin_[1] - tempDepBin_[2] ) / binsizeDep_;
                depTempGrad_ = depBinEnd_[2];

                //printf( "tempDepBin_[1], tempDepBin_[2], tempGrad_, depTempGrad_, tempGradMaxabs_ = %5.2f %5.2f %5.2f %5.2f %5.2f\n",
                //        tempDepBin_[1], tempDepBin_[2], tempGrad_, depTempGrad_, tempGradMaxabs_ );

                if( fabs( tempGrad_ ) > tempGradMaxabs_ )
                {
                    tempGradMaxabs_ = fabs( tempGrad_ );
                    tempGradPk_ = tempGrad_;
                    depTempGradPk_ = depTempGrad_;
                }
            }
        }
    }
}