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

#include "BallastAndTrim.h"
#include "BallastAndTrimIF.h"

#include "data/Slate.h"
#include "data/UniversalDataReader.h"
#include "module/Config.h"
#include "units/Units.h"
#include "utils/AuvMath.h"

BallastAndTrim::BallastAndTrim( const Str& prefix, const Module* module )
    : Behavior( prefix, module, true, true, BallastAndTrimIF::ENABLE_BROADCAST ),
      startTime_( Timestamp::NOT_SET_TIME ),
      timeoutDuration_( Timespan::INVALID_TIMESPAN ),
      minDuration_( Timespan::INVALID_TIMESPAN ),
      initialized_( false ),
      massPos_( nanf( "" ) ),
      buoyPos_( nanf( "" ) ),
      sampleSize_( 0 ),
      satisfied_( false ),
      successful_( false ),
      targetConfidenceLevel_( 0.0 ),
      massTargetErrorBound_( 0.0 ),
      buoyTargetErrorBound_( 0.0 ),
      massPosMean_( 0.0 ),
      buoyPosMean_( 0.0 ),
      massPosVarianceAccumilator_( 0.0 ),
      buoyPosVarianceAccumilator_( 0.0 ),
      massPosVariance_( 0.0 ),
      buoyPosVariance_( 0.0 )
{

    logger_.syslog( "Construct BallastAndTrim." );

    // Inputs from the mission .xml file
    estimationTimeoutSettingReader_ = newSettingReader( BallastAndTrimIF::EST_TIMEOUT_SETTING );
    minEstimationTimeSettingReader_ = newSettingReader( BallastAndTrimIF::MIN_EST_TIME_SETTING );
    massConvergenceErrorSettingReader_ = newSettingReader( BallastAndTrimIF::MASS_CONVERGENCE_ERRORBOUND_SETTING );
    buoyConvergenceErrorSettingReader_ = newSettingReader( BallastAndTrimIF::BUOY_CONVERGENCE_ERRORBOUND_SETTING );
    confidenceSettingReader_ = newSettingReader( BallastAndTrimIF::CONFIDENCE_SETTING );

    // Inputs from the slate
    // Universals
    massPositionReader_ = newUniversalReader( UniversalURI::PLATFORM_MASS_POSITION );
    buoyancyPositionReader_ = newUniversalReader( UniversalURI::PLATFORM_BUOYANCY_POSITION );

    // Outputs to the slate
    massPositionMeanWriter_ = newDataWriter( BallastAndTrimIF::MASS_POSITION_ESTIMATED_MEAN );
    massPositionVarianceWriter_ = newDataWriter( BallastAndTrimIF::MASS_POSITION_ESTIMATED_VARIANCE );
    //massPositionConfidenceWriter_ = newDataWriter( BallastAndTrimIF::MASS_POSITION_CONFIDENCE_INTERVAL );
    buoyancyPositionMeanWriter_ = newDataWriter( BallastAndTrimIF::BUOYANCY_POSITION_ESTIMATED_MEAN );
    buoyancyPositionVarianceWriter_ = newDataWriter( BallastAndTrimIF::BUOYANCY_POSITION_ESTIMATED_VARIANCE );
    //buoyancyPositionConfidenceWriter_ = newDataWriter( BallastAndTrimIF::BUOYANCY_POSITION_CONFIDENCE_INTERVAL );

    // Set the broadcast channel name for all writers
    // Replaces the auto-generated channel name (component name) assigned to the behavior by the mission manager.
    setChannelName( BallastAndTrimIF::NAME );
}

BallastAndTrim::~BallastAndTrim()
{}

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

    timeoutDuration_ = Timespan::INVALID_TIMESPAN;
    minDuration_ = Timespan::INVALID_TIMESPAN;
    timeoutDurationSeconds_ =  nanf( "" );
    minDurationSeconds_ =  nanf( "" );
    massPos_ = nanf( "" );
    buoyPos_ = nanf( "" );
    sampleSize_ = 0;
    satisfied_ = false;
    targetConfidenceLevel_ = 0.0;
    massPosMean_ = 0.0;
    buoyPosMean_ = 0.0;
    massPosVarianceAccumilator_ = 0.0;
    buoyPosVarianceAccumilator_ = 0.0;
    massPosVariance_ = 0.0;
    buoyPosVariance_ = 0.0;

    initialized_ = true;
}

/// Uninit function
void BallastAndTrim::uninitialize( void )
{

    if( !satisfied_ && ( startTime_.elapsed() >= minDuration_ ) )
    {
        logger_.syslog( "BallastAndTrim aborted prematurely - reporting best estimate. Estimation time: " + Str( startTime_.elapsed().asFloat() / 60, 2 ) + " minutes.", Syslog::FAULT );
        logger_.syslog( "Estimated buoyancyNeutral: " + Str( buoyPosMean_ ) + " +/- " + Str( epsilon( buoyPosVariance_ ) ) + " cc (conf. level " + Str( targetConfidenceLevel_ * 100, 2 ) + "%, sigma: " +  Str( sqrt( buoyPosVariance_ ) ) + " cc).", Syslog::IMPORTANT );
        logger_.syslog( "Estimated massDefault: " + Str( massPosMean_ ) + " +/- " + Str( epsilon( massPosVariance_ ) ) + " mm (conf. level " + Str( targetConfidenceLevel_ * 100, 2 ) + "%, sigma: " +  Str( sqrt( massPosVariance_ ) ) + " mm).", Syslog::IMPORTANT );
    }

    logger_.syslog( "Uninitializing BallastAndTrim.", Syslog::INFO );

    timeoutDuration_ = Timespan::INVALID_TIMESPAN;
    startTime_ = Timestamp::NOT_SET_TIME;

    initialized_ = false;
}

void BallastAndTrim::readSettings()
{

    if( estimationTimeoutSettingReader_->isActive() && estimationTimeoutSettingReader_->read( Units::SECOND, timeoutDurationSeconds_ ) )
    {
        if( timeoutDurationSeconds_ < 600 )
        {
            logger_.syslog( "Received request for short estimation period : " + Str( timeoutDurationSeconds_ / 60 ) + " minutes. Consider increasing the estimation period to improve the accuracy of B&T results.", Syslog::FAULT );
        }
        timeoutDuration_ = Timespan( timeoutDurationSeconds_ );
    }
    else
    {
        // TODO: consider replacing this statement with default time Timespan::Minutes( 25 );
        timeoutDuration_ = Timespan::INVALID_TIMESPAN;
    }

    if( minEstimationTimeSettingReader_->isActive() && minEstimationTimeSettingReader_->read( Units::SECOND, minDurationSeconds_ ) )
    {
        minDuration_ = Timespan( minDurationSeconds_ );
        if( minDuration_ > timeoutDuration_ )
        {
            logger_.syslog( "Received request for minEstimationTime that is longer than total estimation period. Setting minEstimationTime to estimationTimeout.", Syslog::FAULT );
            minDuration_ = timeoutDuration_;
        }
    }
    else
    {
        minDuration_ = Timespan::Minutes( 10 );
    }

    if( massConvergenceErrorSettingReader_->isActive() )
    {
        massConvergenceErrorSettingReader_->read( Units::MILLIMETER, massTargetErrorBound_ );
    }

    if( buoyConvergenceErrorSettingReader_->isActive() )
    {
        buoyConvergenceErrorSettingReader_->read( Units::CUBIC_CENTIMETER, buoyTargetErrorBound_ );
    }

    if( confidenceSettingReader_->isActive() )
    {
        confidenceSettingReader_->read( Units::PERCENT, targetConfidenceLevel_ );
        targetConfidenceLevel_ = targetConfidenceLevel_ / 100;
    }

}

void BallastAndTrim::run()
{
    runIfUnsatisfied();
}

bool BallastAndTrim::isSatisfied()
{
    if( !initialized_ )
    {
        initialize();
    }

    // Start timer if not set
    if( startTime_ == Timestamp::NOT_SET_TIME )
    {
        startTime_ = Timestamp::Now();
    }

    // Check settings
    if( timeoutDuration_ == Timespan::INVALID_TIMESPAN )
    {
        readSettings();
    }

    return calcSatisfied();
}

/// Run until B&T etimates have converged is reached or timer expires
bool BallastAndTrim::runIfUnsatisfied()
{

    if( !initialized_ )
    {
        initialize();
        return false;
    }

    // Start timer if not set
    if( startTime_ == Timestamp::NOT_SET_TIME )
    {
        startTime_ = Timestamp::Now();
    }

    // Check settings
    if( timeoutDuration_ == Timespan::INVALID_TIMESPAN )
    {
        readSettings();
    }

    buoyPos_ = nanf( "" );
    massPos_ = nanf( "" );

    if( readData() )
    {

        // New data in. Incrament sample size counter.
        ++sampleSize_;

        // Estimate the means and variances in light of new data.
        estimate( buoyPos_, buoyPosMean_, buoyPosVarianceAccumilator_, buoyPosVariance_ ); // BUOYANCY
        estimate( massPos_, massPosMean_, massPosVarianceAccumilator_, massPosVariance_ ); // MASS

        // Write the updated estimates to the Slate.
        writeData();

        // Check if the estimates satisfy the user's error bound requirments or if timer expired.
        if( calcSatisfied() )
        {
            if( successful_ )
            {
                // If we're within bounding requirements write the configuration variables
                logger_.syslog( "Changing persisted config values to: " + Str( buoyPosMean_ ) + " cc buoyancy neutral and " + Str( massPosMean_ ) + " mm mass default", Syslog::IMPORTANT );
                Config::Persist( "VerticalControl.buoyancyNeutral", ( "VerticalControl.buoyancyNeutral=" + Str( buoyPosMean_ ) + " cubic_centimeter;\n" ).cStr(), logger_ );
                Config::Persist( "VerticalControl.massDefault", ( "VerticalControl.massDefault=" + Str( massPosMean_ ) + " millimeter;\n" ).cStr(), logger_ );

                // Run configSet list to display to the user
                Config::LoadPersistedConfigSets( logger_, false, true );
            }

            return true;
        }
    }

    return false;
}

// Welford algorithm for online mean and varinace (see Knuth TAOCP vol 2, 3rd edition, page 232)
void BallastAndTrim::estimate( const double pos, double& mean,  double& varAccum, double& var )
{
    double delta, delta2;

    delta = pos - mean;
    mean += delta / sampleSize_;
    delta2 = pos - mean;
    varAccum += delta * delta2;

    // unbiased estimator of sample variance
    var = ( sampleSize_ > 1 ) ? varAccum / ( sampleSize_ - 1 ) : 0.0;
}

// Compute error bounds for the estimator
double BallastAndTrim::epsilon( double& var )
{
    // Chebyshev's inequality
    double eps = ( sampleSize_ > 1 ) ? var / ( sampleSize_ * ( 1 - targetConfidenceLevel_ ) ) : 0.0;
    return ( eps >= 0 ? sqrt( eps ) : nanf( "" ) );
}

// Return true if target error bound is reached or if timer expires
bool BallastAndTrim::calcSatisfied()
{
    satisfied_ = false;
    if( startTime_.elapsed() >= minDuration_ )
    {
        // Compute error bounds for the estimators
        float buoyErrorBound = epsilon( buoyPosVariance_ );
        float massErrorBound = epsilon( massPosVariance_ );

        // Check if the error bounds satisfy the user's requirment
        if( ( buoyErrorBound <= buoyTargetErrorBound_ ) && ( massErrorBound <= massTargetErrorBound_ ) )
        {
            logger_.syslog( "BallastAndTrim completed. Estimation time: " + Str( startTime_.elapsed().asFloat() / 60, 2 ) + " minutes.", Syslog::IMPORTANT );
            logger_.syslog( "Estimated buoyancyNeutral: " + Str( buoyPosMean_ ) + " +/- " + Str( buoyErrorBound ) + " cc (conf. level " + Str( targetConfidenceLevel_ * 100, 2 ) + "%, sigma: " +  Str( sqrt( buoyPosVariance_ ) ) + " cc).", Syslog::IMPORTANT );
            logger_.syslog( "Estimated massDefault: " + Str( massPosMean_ ) + " +/- " + Str( massErrorBound ) + " mm (conf. level " + Str( targetConfidenceLevel_ * 100, 2 ) + "%, sigma: " +  Str( sqrt( massPosVariance_ ) ) + " mm).", Syslog::IMPORTANT );

            satisfied_ = true; // This satisfies the behavior
            successful_ = true; // This tells us the result is good
        }
        // Now check for a timeout
        else if( startTime_.elapsed() >= timeoutDuration_ )  // Check timer, report if expired
        {
            logger_.syslog( "BallastAndTrim completed. Values did not meet error bound requirements requested by user. Will NOT write estimation values to config! Estimation time: " + Str( startTime_.elapsed().asFloat() / 60, 2 ) + " minutes (time-out limit reached).", Syslog::FAULT );
            logger_.syslog( "Estimated buoyancyNeutral: " + Str( buoyPosMean_ ) + " +/- " + Str( buoyErrorBound ) + " cc (conf. level " + Str( targetConfidenceLevel_ * 100, 2 ) + "%, sigma: " +  Str( sqrt( buoyPosVariance_ ) ) + " cc).", Syslog::IMPORTANT );
            logger_.syslog( "Estimated massDefault: " + Str( massPosMean_ ) + " +/- " + Str( massErrorBound ) + " mm (conf. level " + Str( targetConfidenceLevel_ * 100, 2 ) + "%, sigma: " +  Str( sqrt( massPosVariance_ ) ) + " mm).", Syslog::IMPORTANT );

            satisfied_ = true;
            successful_ = false; // This tells us the result did not meet the error bounds requested by the user.
        }
    }

    return satisfied_;
}

// Read in the actuator positions for satisfied or runIfUnsatisfied: return true if OK.
bool BallastAndTrim::readData()
{
    bool gotMassPos( false ), gotBuoyPos( false );

    if( massPositionReader_->isActive() )
    {
        gotMassPos = massPositionReader_->read( Units::MILLIMETER, massPos_ ) && !isnan( massPos_ );
    }

    if( buoyancyPositionReader_->isActive() )
    {
        gotBuoyPos = buoyancyPositionReader_->read( Units::CUBIC_CENTIMETER, buoyPos_ ) && !isnan( buoyPos_ );
    }

    return gotMassPos && gotBuoyPos;
}

void BallastAndTrim::writeData()
{
    // Grab data timestamp
    dataTimestamp_ = Timestamp::Now();

    // Write etimated mean and variance for default mass position
    massPositionMeanWriter_->write( Units::MILLIMETER, massPosMean_, dataTimestamp_ );
    massPositionVarianceWriter_->write( Units::MILLIMETER, massPosVariance_, dataTimestamp_ );

    // Write etimated mean and variance for nuetral buoyancy position
    buoyancyPositionMeanWriter_->write( Units::CUBIC_CENTIMETER, buoyPosMean_, dataTimestamp_ );
    buoyancyPositionVarianceWriter_->write( Units::CUBIC_CENTIMETER, buoyPosVariance_, dataTimestamp_ );
}

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