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

#include "TerminalGuidance.h"
#include "TerminalGuidanceIF.h"

#include "controlModule/HorizontalControlIF.h"
#include "estimationModule/TrackAcousticContactIF.h"
#include "sensorModule/DDMIF.h"
#include "sensorModule/PowerOnlyIF.h"

#include "data/Location.h"
#include "data/Slate.h"
#include "data/UniversalURI.h"
#include "data/UniversalDataReader.h"
#include "units/Units.h"

#include <math.h>


TerminalGuidance::TerminalGuidance( const Str& prefix, const Module* module )
    : Behavior( prefix + TerminalGuidanceIF::NAME, module, true, true ),
      targetRangeSetting_( 40.0 ),  // Objective range for terminal guidace in meters
      headingToContact_( nanf( "" ) ),
      rangeToContact_( nanf( "" ) ),
      headingCmd_( nanf( "" ) ),
      initialized_( false ),
      debug_( false )
{
    logger_.syslog( "Construct." );

    // Slate input setting variables
    targetRangeSettingReader_ = newSettingReader( TerminalGuidanceIF::TARGET_RANGE_SETTING );

    // Slate input measurements
    orientationReader_ = newUniversalReader( UniversalURI::PLATFORM_ORIENTATION );
    headingToContactReader_ = newDataReader( TrackAcousticContactIF::HEADING_TO_CONTACT );
    rangeToContactReader_ = newDataReader( TrackAcousticContactIF::RANGE_TO_CONTACT );

    // Slate output variables
    horizontalModeWriter_ = newDataWriter( HorizontalControlIF::HORIZONTAL_MODE );
    headingCmdWriter_ = newDataWriter( HorizontalControlIF::HEADING_CMD );
}

TerminalGuidance::~TerminalGuidance()
{}

/// Initialize function
void TerminalGuidance::initialize()
{
    logger_.syslog( "Initialize." );

    orientationReader_->requestData( true );

    // Read Slate input setting variables, default to constructor values if unspecified
    float targetRange( nanf( "" ) );
    if( targetRangeSettingReader_->read( Units::METER, targetRange ) && !isnan( targetRange ) )
    {
        targetRangeSetting_ = fabs( targetRange );
    }

    // Initialize heading command with the vehicle's current heading
    orientationReader_->read( Units::RADIAN, headingCmd_ );

    initialized_ = true;
}

/// Read in the parameters for satisfied or runIfUnsatisfied: return true if OK.
bool TerminalGuidance::readParams()
{
    bool ok( true );

    if( !initialized_ ) initialize();

    ok &= headingToContactReader_->wasTouchedSinceLastRun( this );
    ok &= rangeToContactReader_->wasTouchedSinceLastRun( this );

    if( ok )
    {
        ok &= headingToContactReader_->read( Units::RADIAN, headingToContact_ ) && !isnan( headingToContact_ );
        ok &= rangeToContactReader_->read( Units::METER, rangeToContact_ ) && !isnan( rangeToContact_ );
    }

    return ok;
}

/// Perform the satisfied: return true if envelope "satisfied"
bool TerminalGuidance::calcSatisfied()
{
    bool statisfied( false );

    if( rangeToContact_ <= targetRangeSetting_ )
    {
        statisfied = true;
    }

    return statisfied;
}

/// Just do the run
void TerminalGuidance::run()
{
    runIfUnsatisfied();
}

/// Just do the satisfied: return true if envelope "satisfied"
bool TerminalGuidance::isSatisfied()
{
    bool satisfied( false );
    if( readParams() )
    {
        satisfied = calcSatisfied();
    }
    return satisfied;
}

/// Do the run, and return true if envelope "satisfied"
bool TerminalGuidance::runIfUnsatisfied()
{
    bool satisfied( false );
    if( readParams() )
    {
        satisfied = calcSatisfied();
        if( !satisfied )
        {
            // Update the heading command
            headingCmd_ = headingToContact_;

            // Update the vehicle's heading
            if( !isnan( headingCmd_ ) )
            {
                horizontalModeWriter_->write( Units::ENUM, HorizontalControlIF::HEADING );
                headingCmdWriter_->write( Units::RADIAN, headingCmd_ );
            }
        }
    }

    return satisfied;
}

/// Uninit function
void TerminalGuidance::uninitialize()
{
    orientationReader_->requestData( false );
}

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