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

#include "AbortDrift.h"
#include "AbortDriftIF.h"

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


AbortDrift::AbortDrift( const Str& prefix,  const Module* module )
    : Behavior( prefix + AbortDriftIF::NAME, module, true, true ),
      abortTimer_( Timestamp::NOT_SET_TIME ),
      timeoutDuration_( nanf( "" ) ),
      abortDriftTimeoutReached_( false ),
      receivedValidAcousticTime_( false ),
      receivedValidGpsTimeFix_( false ),
      acousticReceiveTime_( nanf( "" ) ),
      gpsTimeFix_( nanf( "" ) ),
      initialized_( false )
{
    logger_.syslog( "Construct AbortDrift." );

    // Inputs from the mission .xml file
    acousticTimeoutSettingReader_ = newSettingReader( AbortDriftIF::ACOUSTIC_TIMEOUT_SETTING );

    // Inputs from the slate
    // Universals
    acousticReceiveTimeReader_ = newUniversalReader( UniversalURI::ACOUSTIC_RECEIVE_TIME );
    gpsTimeFixReader_ = newUniversalReader( UniversalURI::TIME_FIX );

}

AbortDrift::~AbortDrift()
{}

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

    // Read mission acoustic timeout setting
    if( readSettings() )
    {
        logger_.syslog( "Acoustic timeout set to: " + Str( timeoutDuration_, 2 ) + " hours.", Syslog::IMPORTANT );
        initialized_ = true;
    }
}

/// Uninit function
void AbortDrift::uninitialize()
{
    logger_.syslog( "Uninitializing AbortDrift.", Syslog::INFO );

    timeoutDuration_ = nanf( "" );
    abortTimer_ = Timestamp::NOT_SET_TIME;
    abortDriftTimeoutReached_ =  false;

    initialized_ = false;
}


bool AbortDrift::readSettings()
{
    if( acousticTimeoutSettingReader_->isActive() )
    {
        if( acousticTimeoutSettingReader_->read( Units::HOUR, timeoutDuration_ ) && !isnan( timeoutDuration_ ) )
        {
            return true;
        }
    }

    return false;
}

bool AbortDrift::readParams()
{

    bool receivedValidSettings = readSettings();

    if( gpsTimeFixReader_->isActive() && gpsTimeFixReader_->wasTouchedSinceLastRun( this ) )
    {
        receivedValidGpsTimeFix_ =  gpsTimeFixReader_->read( Units::SECOND, gpsTimeFix_ ) && !isnan( gpsTimeFix_ );
    }

    if( acousticReceiveTimeReader_->isActive() && acousticReceiveTimeReader_->wasTouchedSinceLastRun( this ) )
    {
        receivedValidAcousticTime_ =  acousticReceiveTimeReader_->read( Units::SECOND, acousticReceiveTime_ ) && !isnan( acousticReceiveTime_ );
    }

    return ( receivedValidSettings && ( receivedValidGpsTimeFix_ || receivedValidAcousticTime_ ) );
}

/// The actual "payload" of the component
void AbortDrift::run()
{
    runIfUnsatisfied();
}

/// Runs and returns true if acoustic timer has expired, otherwise returns false
bool AbortDrift::runIfUnsatisfied()
{
    if( !initialized_ )
    {
        initialize();
        return false;
    }

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

    if( !abortDriftTimeoutReached_ )
    {
        receivedValidGpsTimeFix_ = false;
        receivedValidAcousticTime_ = false;

        gpsTimeFix_ = nanf( "" );
        acousticReceiveTime_ = nanf( "" );

        // Read GPS time-fix and acoustic receive time and check if values are valid
        if( readParams() )
        {
            if( receivedValidGpsTimeFix_ ) logger_.syslog( "Received valid gps time fix: " + Str( gpsTimeFix_, 2 ) + ". Resetting abort timer.", Syslog::INFO );
            if( receivedValidAcousticTime_ ) logger_.syslog( "Received valid acoustic ping at time: " + Str( acousticReceiveTime_, 2 ) + ". Resetting abort timer.", Syslog::INFO );
            // We got a valid GPS time-fix or an acoustic ping, reset the abort timer
            abortTimer_ = Timestamp::Now();
        }


        if( ( abortTimer_.elapsed().asFloat() / 3600 ) >= timeoutDuration_ )
        {
            abortDriftTimeoutReached_ = true;
            logger_.syslog( "Acoustic timeout " + Str( timeoutDuration_, 2 ) + " hours exceeded. Elapsed time since last timer reset: " + Str( abortTimer_.elapsed().asFloat() / 3600, 2 ) + " hours. Aborting drift by executing stop command.", Syslog::IMPORTANT );
            return true;
        }

        //logger_.syslog( "Acoustic abort timer running: " + Str( (abortTimer_.elapsed().asFloat() / 3600) ) + " hours elapsed.", Syslog::IMPORTANT );
    }

    return false;
}

/// isSatisfied will return false to prevent inserting multipule stop commands
bool AbortDrift::isSatisfied()
{
    if( !initialized_ )
    {
        initialize();
    }

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

    return false;
}

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