/** \file
 *
 *  Contains the VerticalHomogeneityIndexCalculator class implementation.
 *
 *  Copyright (c) 2015 MBARI
 *  MBARI Proprietary Information.  All Rights Reserved
 */

#include "VerticalHomogeneityIndexCalculator.h"
#include "VerticalHomogeneityIndexCalculatorIF.h"

#include "data/ConfigReader.h"
#include "data/UniversalDataReader.h"
#include "utils/Timestamp.h"
#include "units/Units.h"

#include <cmath> // abs, pow, sqrt

VerticalHomogeneityIndexCalculator::VerticalHomogeneityIndexCalculator( const Module* module )
    : SyncDerivationComponent( VerticalHomogeneityIndexCalculatorIF::NAME, module ),
      verbosity_( 0 ),
      medianFilterLengthSaltSetting_( 5 ),
      dataTimestamp_( Timestamp::NOT_SET_TIME ),
      dz_( 0.5 ),
      meanTemperature_( nanf( "" ) ),
      meanSalinity_( nanf( "" ) ),
      vthi_( nanf( "" ) ),
      vshi_( nanf( "" ) ),
      sigmaTemperature_( nanf( "" ) ),
      sigmaSalinity_( nanf( "" ) )
{
    depth_[0] = 5;
    depth_[1] = 10;
    depth_[2] = 15;
    depth_[3] = 20;

    cntrSalt_ = 0;

    depthReader_ = newUniversalReader( UniversalURI::DEPTH );
    temperatureReader_ = newUniversalReader( UniversalURI::SEA_WATER_TEMPERATURE );
    salinityReader_ = newUniversalReader( UniversalURI::SEA_WATER_SALINITY );
    meanTemperatureWriter_ = newDataWriter( VerticalHomogeneityIndexCalculatorIF::MEAN_SEA_WATER_TEMPERATURE );
    meanSalinityWriter_ = newDataWriter( VerticalHomogeneityIndexCalculatorIF::MEAN_SEA_WATER_SALINITY );
    vthiWriter_ = newDataWriter( VerticalHomogeneityIndexCalculatorIF::VERTICAL_TEMPERATURE_HOMOGENEITY_INDEX );
    vshiWriter_ = newDataWriter( VerticalHomogeneityIndexCalculatorIF::VERTICAL_SALINITY_HOMOGENEITY_INDEX );
    sigmaTemperatureWriter_ = newDataWriter( VerticalHomogeneityIndexCalculatorIF::STANDARD_DEVIATION_SEA_WATER_TEMPERATURE );
    sigmaSalinityWriter_ = newDataWriter( VerticalHomogeneityIndexCalculatorIF::STANDARD_DEVIATION_SEA_WATER_SALINITY );
    verbosityCfgReader_ = newConfigReader( VerticalHomogeneityIndexCalculatorIF::VERBOSITY );
    depth1CfgReader_ = newConfigReader( VerticalHomogeneityIndexCalculatorIF::DEPTH_1 );
    depth2CfgReader_ = newConfigReader( VerticalHomogeneityIndexCalculatorIF::DEPTH_2 );
    depth3CfgReader_ = newConfigReader( VerticalHomogeneityIndexCalculatorIF::DEPTH_3 );
    depth4CfgReader_ = newConfigReader( VerticalHomogeneityIndexCalculatorIF::DEPTH_4 );
    depthWindowCfgReader_ = newConfigReader( VerticalHomogeneityIndexCalculatorIF::DEPTH_WINDOW );
    medianFilterLengthSaltCfgReader_ = newConfigReader( VerticalHomogeneityIndexCalculatorIF::MEDIAN_FILTER_LENGTH_SALT );

}

VerticalHomogeneityIndexCalculator::~VerticalHomogeneityIndexCalculator()
{}

void VerticalHomogeneityIndexCalculator::initialize( void )
{
    logger_.syslog( "(re)initializing", ( verbosity_ > 0 ) ? Syslog::INFO : Syslog::DEBUG );
    readConfig(); // possibly update depths and window
    for( int i = 0; i < 4; i++ )
    {
        temperature_[i] = nanf( "" );
        salinity_[i] = nanf( "" );
        n_[i] = 0;
    }
    meanTemperature_ = nanf( "" );
    meanSalinity_ = nanf( "" );
    vthi_ = nanf( "" );
    vshi_ = nanf( "" );
    sigmaTemperature_ = nanf( "" );
    sigmaSalinity_ = nanf( "" );
}

void VerticalHomogeneityIndexCalculator::readConfig( void )
{
    verbosityCfgReader_->read( Units::COUNT, verbosity_ );
    depth1CfgReader_->read( Units::METER, depth_[0] );
    depth2CfgReader_->read( Units::METER, depth_[1] );
    depth3CfgReader_->read( Units::METER, depth_[2] );
    depth4CfgReader_->read( Units::METER, depth_[3] );
    float depthWindowSetting( nanf( "" ) );
    if( depthWindowCfgReader_->read( Units::METER, depthWindowSetting ) && !isnan( depthWindowSetting ) )
    {
        dz_ = 0.5 * depthWindowSetting;
    }
    medianFilterLengthSaltCfgReader_->read( Units::METER, medianFilterLengthSaltSetting_ );
}

void VerticalHomogeneityIndexCalculator::run( void )
{
    if( !addSample() && ( ( !isnan( temperature_[0] ) && !isnan( temperature_[1] ) && !isnan( temperature_[2] ) && !isnan( temperature_[3] ) ) || ( !isnan( salinity_[0] ) && !isnan( salinity_[1] ) && !isnan( salinity_[2] ) && !isnan( salinity_[3] ) ) ) )
    {
        calculate();
        writeData();
        initialize();
    }
}

bool VerticalHomogeneityIndexCalculator::addSample( void )
{
    if( ( depthReader_->isActive() ) && ( ( temperatureReader_->isActive() ) || ( salinityReader_->isActive() ) ) )
    {
        float depth( nanf( "" ) ), temperature( nanf( "" ) ), salinity( nanf( "" ) ), saltMedian, tmp, array_sort_salt[MAX_MEDIAN_FILTER_LENGTH_SALT];
        int lenStorageWindowForMedianFilter, k, m, n;

        // Calculate median-filtered salinity.
        if( salinityReader_->read( Units::PRACTICAL_SALINITY_UNIT, salinity ) && !isnan( salinity ) )
        {
            if( cntrSalt_ < medianFilterLengthSaltSetting_ )
            {
                cntrSalt_++;
                lenStorageWindowForMedianFilter = cntrSalt_;
            }
            else
            {
                cntrSalt_ = medianFilterLengthSaltSetting_; // No need to continue to grow.
                lenStorageWindowForMedianFilter = medianFilterLengthSaltSetting_;
            }

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

            saltInWindow_[0] = salinity;

            if( ( cntrSalt_ >= medianFilterLengthSaltSetting_ ) && ( medianFilterLengthSaltSetting_ >= 2 ) )
            {
                for( k = 0; k <= ( medianFilterLengthSaltSetting_ - 1 ); k++ )
                    array_sort_salt[k] = saltInWindow_[k];

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

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

                saltMedian = array_sort_salt[( medianFilterLengthSaltSetting_ - 1 ) >> 1]; // Take the median value.

                //debug
                //printf("After. array_sort_salt[] = %f, %f, %f, %f, %f, saltMedian = %f\n", array_sort_salt[0], array_sort_salt[1], array_sort_salt[2], array_sort_salt[3], array_sort_salt[4], saltMedian);
            }
            else
                saltMedian = saltInWindow_[0];
        }
        else
            saltMedian = nanf( "" );

        if( depthReader_->read( Units::METER, depth ) && !isnan( depth ) )
        {
            for( int i = 0; i < 4; i++ )
            {
                if( std::abs( depth - depth_[i] ) < dz_ )
                {
                    if( temperatureReader_->read( Units::CELSIUS, temperature ) && !isnan( temperature ) )
                    {
                        if( isnan( temperature_[i] ) )
                        {
                            temperature_[i] = temperature;
                            n_[i] = 1;
                        }
                        else
                        {
                            temperature_[i] = ( n_[i] * temperature_[i] + temperature ) / ( n_[i] + 1 );
                            n_[i] += 1;
                        }
                        if( verbosity_ > 1 ) logger_.syslog( "temperature at depth " + Str( depth_[i] ) + " m updated to " + Str( temperature_[i] ) + " degC using sample " + Str( n_[i] ) + " at " + Str( temperature ) + " degC.", Syslog::INFO );
                    }

                    if( !isnan( saltMedian ) )
                    {
                        if( isnan( salinity_[i] ) )
                        {
                            salinity_[i] = saltMedian;
                            n_[i] = 1;
                        }
                        else
                        {
                            salinity_[i] = ( n_[i] * salinity_[i] + saltMedian ) / ( n_[i] + 1 );
                            n_[i] += 1;
                        }
                        if( verbosity_ > 1 ) logger_.syslog( "Salinity at depth " + Str( depth_[i] ) + " m updated to " + Str( salinity_[i] ) + " PSU using sample " + Str( n_[i] ) + " at " + Str( salinity ) + " PSU.", Syslog::INFO );
                    }

                    return true;
                }
            }
        }
    }
    return false;
}

void VerticalHomogeneityIndexCalculator::calculate( void )
{
    float varTemperature( nanf( "" ) ), varSalinity( nanf( "" ) );
    meanTemperature_ = 0.25 * ( temperature_[0] + temperature_[1] + temperature_[2] + temperature_[3] );
    meanSalinity_ = 0.25 * ( salinity_[0] + salinity_[1] + salinity_[2] + salinity_[3] );
    vthi_ = 0.25 * ( std::abs( temperature_[0] - meanTemperature_ ) + std::abs( temperature_[1] - meanTemperature_ ) + std::abs( temperature_[2] - meanTemperature_ ) + std::abs( temperature_[3] - meanTemperature_ ) );
    vshi_ = 0.25 * ( std::abs( salinity_[0] - meanSalinity_ ) + std::abs( salinity_[1] - meanSalinity_ ) + std::abs( salinity_[2] - meanSalinity_ ) + std::abs( salinity_[3] - meanSalinity_ ) );
    varTemperature = 0.25 * ( std::pow( temperature_[0] - meanTemperature_, 2 ) + std::pow( temperature_[1] - meanTemperature_, 2 ) + std::pow( temperature_[2] - meanTemperature_, 2 ) + std::pow( temperature_[3] - meanTemperature_, 2 ) );
    sigmaTemperature_ = std::sqrt( varTemperature );
    varSalinity = 0.25 * ( std::pow( salinity_[0] - meanSalinity_, 2 ) + std::pow( salinity_[1] - meanSalinity_, 2 ) + std::pow( salinity_[2] - meanSalinity_, 2 ) + std::pow( salinity_[3] - meanSalinity_, 2 ) );
    sigmaSalinity_ = std::sqrt( varSalinity );
    logger_.syslog( "Calculated VTHI " + Str( vthi_, 2 ) + " degC, mean " + Str( meanTemperature_, 2 ) + " degC, variance " + Str( varTemperature, 2 ) + " degC^2, standard deviation " + Str( sigmaTemperature_, 2 ) + " degC, from samples: (" + Str( depth_[0], 2 ) + " m, " + Str( temperature_[0], 2 ) + " degC), (" + Str( depth_[1], 2 ) + " m, " + Str( temperature_[1], 2 ) + " degC), (" + Str( depth_[2], 2 ) + " m, " + Str( temperature_[2], 2 ) + " degC), (" + Str( depth_[3], 2 ) + " m, " + Str( temperature_[3], 2 ) + " degC). " + "Calculated VSHI " + Str( vshi_, 3 ) + " PSU, mean " + Str( meanSalinity_, 2 ) + " PSU, variance " + Str( varSalinity, 4 ) + " PSU^2, standard deviation " + Str( sigmaSalinity_, 3 ) + " PSU, from samples: (" + Str( depth_[0], 2 ) + " m, " + Str( salinity_[0], 2 ) + " PSU), (" + Str( depth_[1], 2 ) + " m, " + Str( salinity_[1], 2 ) + " PSU), (" + Str( depth_[2], 2 ) + " m, " + Str( salinity_[2], 2 ) + " PSU), (" + Str( depth_[3], 2 ) + " m, " + Str( salinity_[3], 2 ) + " PSU). ", ( verbosity_ > 0 ) ? Syslog::INFO : Syslog::DEBUG );
}

void VerticalHomogeneityIndexCalculator::writeData( void )
{
    dataTimestamp_ = Timestamp::Now();
    meanTemperatureWriter_->write( Units::CELSIUS, meanTemperature_, dataTimestamp_ );
    vthiWriter_->write( Units::CELSIUS, vthi_, dataTimestamp_ );
    sigmaTemperatureWriter_->write( Units::CELSIUS, sigmaTemperature_, dataTimestamp_ );
    meanSalinityWriter_->write( Units::PRACTICAL_SALINITY_UNIT, meanSalinity_, dataTimestamp_ );
    vshiWriter_->write( Units::PRACTICAL_SALINITY_UNIT, vshi_, dataTimestamp_ );
    sigmaSalinityWriter_->write( Units::PRACTICAL_SALINITY_UNIT, sigmaSalinity_, dataTimestamp_ );
}