/** \file
 *
 *  Contains the StratificationFrontDetector class implementation.
 *
 *  Copyright (c) 2013, 2014 MBARI
 *  MBARI Proprietary Information.  All Rights Reserved
 */

#include "StratificationFrontDetector.h"
#include "StratificationFrontDetectorIF.h"
#include "derivationModule/VerticalHomogeneityIndexCalculatorIF.h"
#include "guidanceModule/FrontTrackingIF.h"

#include "data/ConfigReader.h" // TODO: Shouldn't have to explicitly include this in each component.

StratificationFrontDetector::StratificationFrontDetector( const Module* module )
    : SyncEstimationComponent( StratificationFrontDetectorIF::NAME, module ),
      temperatureFront_( false ),
      salinityFront_( false ),
      homogeneousTemperature_( false ),
      homogeneousSalinity_( false ),
      frontFlagReset_( false ),
      frontDetectionEnabled_( false ),
      verbosity_( 0 ),
      dataTimestamp_( Timestamp::NOT_SET_TIME ),
      VTHI_( nanf( "" ) ),
      VSHI_( nanf( "" ) ),
      hitFrontTrackingWall_( false ),
      thresholdVTHI_( 0.4 ),
      thresholdVSHI_( 0.1 ),
      highWaterMark_( 5 ),
      countSameWaterType_( 0 ),
      countOppositeWaterType_( 0 ),
      temperatureFrontConfigReader_( newConfigReader( StratificationFrontDetectorIF::TEMPERATURE_FRONT ) ),
      salinityFrontConfigReader_( newConfigReader( StratificationFrontDetectorIF::SALINITY_FRONT ) ),
      verbosityLevelConfigReader_( newConfigReader( StratificationFrontDetectorIF::VERBOSITY ) ),
      thresholdVTHIConfigReader_( newConfigReader( StratificationFrontDetectorIF::THRESHOLD_VTHI ) ),
      thresholdVSHIConfigReader_( newConfigReader( StratificationFrontDetectorIF::THRESHOLD_VSHI ) ),
      highWaterMarkConfigReader_( newConfigReader( StratificationFrontDetectorIF::HIGHWATERMARK ) ),
      VTHIReader_( newDataReader( VerticalHomogeneityIndexCalculatorIF::VERTICAL_TEMPERATURE_HOMOGENEITY_INDEX ) ),
      VSHIReader_( newDataReader( VerticalHomogeneityIndexCalculatorIF::VERTICAL_SALINITY_HOMOGENEITY_INDEX ) ),
      hitWallReader_( newDataReader( FrontTrackingIF::HIT_WALL ) ),
      levelWriter_( newDataWriter( StratificationFrontDetectorIF::LEVEL ) ),
      frontWriter_( newDataWriter( StratificationFrontDetectorIF::FRONT ) ),
      stratifiedWriter_( newDataWriter( StratificationFrontDetectorIF::STRATIFIED ) ),
      homogeneousWriter_( newDataWriter( StratificationFrontDetectorIF::HOMOGENEOUS ) )
{
    readConfig();
}

StratificationFrontDetector::~StratificationFrontDetector()
{}

void StratificationFrontDetector::initialize( void )
{
    if( countOppositeWaterType_ != -1 ) logger_.syslog( "(re)initializing", ( verbosity_ > 0 ) ? Syslog::INFO : Syslog::DEBUG );
    if( readData() )
    {
        countOppositeWaterType_ = 0;

        if( temperatureFront_ )
        {
            homogeneousTemperature_ = ( VTHI_ <= thresholdVTHI_ ); // figure out whether starting in vertically homogeneous water.
            logger_.syslog( "Initial vertical homogeneity condition (0 = stratified, 1 = homogeneous), VTHI_, thresholdVTHI_ = " + Str( homogeneousTemperature_ ) + ", " + Str( VTHI_ ) + ", " + Str( thresholdVTHI_ ), ( verbosity_ > 0 ) ? Syslog::IMPORTANT : Syslog::INFO );
        }

        if( salinityFront_ )
        {
            homogeneousSalinity_ = ( VSHI_ <= thresholdVSHI_ ); // figure out whether starting in vertically homogeneous water.
            logger_.syslog( "Initial vertical homogeneity condition (0 = stratified, 1 = homogeneous), VSHI_, thresholdVSHI_ = " + Str( homogeneousSalinity_ ) + ", " + Str( VSHI_ ) + ", " + Str( thresholdVSHI_ ), ( verbosity_ > 0 ) ? Syslog::IMPORTANT : Syslog::INFO );
        }
    }
    else
    {
        countOppositeWaterType_ = -1; // keep track so we can try again...
    }
}

void StratificationFrontDetector::readConfig( void )
{
    temperatureFrontConfigReader_->read( Units::BOOL, temperatureFront_ );
    salinityFrontConfigReader_->read( Units::BOOL, salinityFront_ );
    verbosityLevelConfigReader_->read( Units::COUNT, verbosity_ );
    highWaterMarkConfigReader_->read( Units::COUNT, highWaterMark_ );

    float thresholdVTHI( thresholdVTHI_ ), thresholdVSHI( thresholdVSHI_ );
    thresholdVTHIConfigReader_->read( Units::CELSIUS, thresholdVTHI_ );
    thresholdVSHIConfigReader_->read( Units::PRACTICAL_SALINITY_UNIT, thresholdVSHI_ );
    if( thresholdVTHI != thresholdVTHI_ )
    {
        logger_.syslog( "thresholdVTHI set to: " + Str( thresholdVTHI_ ) + " degC", ( verbosity_ > 0 ) ? Syslog::IMPORTANT : Syslog::INFO );
        initialize();
    }

    if( thresholdVSHI != thresholdVSHI_ )
    {
        logger_.syslog( "thresholdVSHI set to: " + Str( thresholdVSHI_ ) + " PSU", ( verbosity_ > 0 ) ? Syslog::IMPORTANT : Syslog::INFO );
        initialize();
    }
}

void StratificationFrontDetector::run( void )
{
    readConfig();
    if( countOppositeWaterType_ < 0 ) initialize();

    if( ( countOppositeWaterType_ == 0 ) && ( !frontFlagReset_ ) )
    {
        //debug
        //printf("Here 3. countOppositeWaterType_ = %d\n\n", countOppositeWaterType_);

        frontWriter_->write( Units::BOOL, false, dataTimestamp_ );
        frontFlagReset_ = true;
    }

    if( readData() )
    {
        if( ( temperatureFront_ && ( ( homogeneousTemperature_ && VTHI_ > thresholdVTHI_ ) || ( !homogeneousTemperature_ && VTHI_ <= thresholdVTHI_ ) ) ) || ( salinityFront_ && ( ( homogeneousSalinity_ && VSHI_ > thresholdVSHI_ ) || ( !homogeneousSalinity_ && VSHI_ <= thresholdVSHI_ ) ) ) )
        {
            //debug
            //printf("Here 1. VSHI_, thresholdVSHI_, homogeneousSalinity_, countOppositeWaterType_ = %f, %f, %d, %d\n\n", VSHI_, thresholdVSHI_, homogeneousSalinity_, countOppositeWaterType_);

            countOppositeWaterType_++;

            countSameWaterType_ = 0;
        }
        else
        {
            //debug
            //printf("Here 2. VSHI_, thresholdVSHI_, homogeneousSalinity_, countOppositeWaterType_ = %f, %f, %d, %d\n\n", VSHI_, thresholdVSHI_, homogeneousSalinity_, countOppositeWaterType_);

            countSameWaterType_++;

            countOppositeWaterType_ = 0;

            if( temperatureFront_ && ( verbosity_ > 1 ) )
                logger_.syslog( "Did not detect transition conditions: VTHI = " + Str( VTHI_ ) + ", thresholdVTHI = " + Str( thresholdVTHI_ ) + ".", Syslog::INFO );

            if( salinityFront_ && ( verbosity_ > 1 ) )
                logger_.syslog( "Did not detect transition conditions: VSHI = " + Str( VSHI_ ) + ", thresholdVSHI = " + Str( thresholdVSHI_ ) + ".", Syslog::INFO );
        }

        if( ( !frontDetectionEnabled_ ) && ( ( countSameWaterType_ >= highWaterMark_ ) || ( countOppositeWaterType_ >= highWaterMark_ ) ) )
        {
            if( countOppositeWaterType_ >= highWaterMark_ )
            {
                if( temperatureFront_ )
                    homogeneousTemperature_ = !homogeneousTemperature_;

                if( salinityFront_ )
                    homogeneousSalinity_ = !homogeneousSalinity_;
            }

            frontDetectionEnabled_ = true;

            if( temperatureFront_ )
                logger_.syslog( "Front detection is now enabled. Present vertical homogeneity condition (0 = stratified, 1 = homogeneous), VTHI, thresholdVTHI_, countSameWaterType_, countOppositeWaterType_ = " + Str( homogeneousTemperature_ ) + ", " + Str( VTHI_ ) + ", " + Str( thresholdVTHI_ ) + ", " + Str( countSameWaterType_ ) + ", " + Str( countOppositeWaterType_ ), ( verbosity_ > 0 ) ? Syslog::IMPORTANT : Syslog::INFO );

            if( salinityFront_ )
                logger_.syslog( "Front detection is now enabled. Present vertical homogeneity condition (0 = stratified, 1 = homogeneous), VSHI, thresholdVSHI_, countSameWaterType_, countOppositeWaterType_ = " + Str( homogeneousSalinity_ ) + ", " + Str( VSHI_ ) + ", " + Str( thresholdVSHI_ ) + ", " + Str( countSameWaterType_ ) + ", " + Str( countOppositeWaterType_ ), ( verbosity_ > 0 ) ? Syslog::IMPORTANT : Syslog::INFO );

        }

        writeData();

        if( frontDetectionEnabled_ && ( countOppositeWaterType_ >= highWaterMark_ ) )
        {

            logger_.syslog( "Detected front!", ( verbosity_ > 0 ) ? Syslog::IMPORTANT : Syslog::INFO );

            if( temperatureFront_ )
                homogeneousTemperature_ = !homogeneousTemperature_;

            if( salinityFront_ )
                homogeneousSalinity_ = !homogeneousSalinity_;

            countOppositeWaterType_ = 0;

            countSameWaterType_ = 0;

            frontFlagReset_ = false;

        }
    }

    if( readHitWall() ) // When vehicle hits any wall, flip homogeneousTemperature_ or homogeneousSalinity_.
    {
        if( hitFrontTrackingWall_ )
        {
            if( temperatureFront_ )
                homogeneousTemperature_ = !homogeneousTemperature_;

            if( salinityFront_ )
                homogeneousSalinity_ = !homogeneousSalinity_;

            //debug
            //printf("Flipping homogeneousTemperature_ or homogeneousSalinity_.\n\n");
        }
    }
}

bool StratificationFrontDetector::readData( void )
{
    if( temperatureFront_ && VTHIReader_->isActive() && VTHIReader_->wasTouchedSinceLastRun( this ) )
    {
        return VTHIReader_->read( Units::CELSIUS, VTHI_ );
    }

    if( salinityFront_ && VSHIReader_->isActive() && VSHIReader_->wasTouchedSinceLastRun( this ) )
    {
        return VSHIReader_->read( Units::PRACTICAL_SALINITY_UNIT, VSHI_ );
    }

    return false;
}

bool StratificationFrontDetector::readHitWall( void )
{
    if( hitWallReader_->isActive() && hitWallReader_->wasTouchedSinceLastRun( this ) )
    {
        return hitWallReader_->read( Units::BOOL, hitFrontTrackingWall_ );
    }

    return false;
}


void StratificationFrontDetector::writeData( void )
{
    levelWriter_->write( Units::COUNT, countOppositeWaterType_, dataTimestamp_ );

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

    frontWriter_->write( Units::BOOL, ( frontDetectionEnabled_ && ( countOppositeWaterType_ >= highWaterMark_ ) ) );

    if( temperatureFront_ )
    {
        stratifiedWriter_->write( Units::BOOL, !homogeneousTemperature_, dataTimestamp_ );
        homogeneousWriter_->write( Units::BOOL, homogeneousTemperature_, dataTimestamp_ );
    }

    if( salinityFront_ )
    {
        stratifiedWriter_->write( Units::BOOL, !homogeneousSalinity_, dataTimestamp_ );
        homogeneousWriter_->write( Units::BOOL, homogeneousSalinity_, dataTimestamp_ );
    }
}