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

#include "SetNav.h"
#include "SetNavIF.h"

#include "data/UniversalDataWriter.h"

const float SetNav::SETNAV_ACCURACY( 0.00001 );

SetNav::SetNav( const Str& prefix, const Module* module )
    : Behavior( prefix + SetNavIF::NAME, module, true, true ),
      latitudeSetting_( nanf( "" ) ),
      longitudeSetting_( nanf( "" ) ),
      timeFix_( Timestamp::NOT_SET_TIME ),
      SetNav_( false )
{
    logger_.syslog( "Construct SetNav." );

    // Slate input setting variables
    latitudeSettingReader_ = newSettingReader( SetNavIF::LATITUDE_SETTING );
    longitudeSettingReader_ = newSettingReader( SetNavIF::LONGITUDE_SETTING );

    // Universal Slate output variables
    latitudeFixWriter_ = newDataWriter( SetNavIF::LATITUDE_FIX );
    longitudeFixWriter_ = newDataWriter( SetNavIF::LONGITUDE_FIX );
    timeFixWriter_ = newDataWriter( SetNavIF::TIME_FIX );
}

SetNav::~SetNav()
{}

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

    latitudeSetting_ = nanf( "" );
    longitudeSetting_ = nanf( "" );
    timeFix_ = Timestamp::NOT_SET_TIME;
    SetNav_ = false;
}

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

/// The actual "payload" of the component
bool SetNav::runIfUnsatisfied()
{
    bool ok( true );
    if( readSettings() )
    {
        timeFixWriter_->setInvalid( false );
        ok &= timeFixWriter_->write( Units::EPOCH_SECOND, timeFix_.asDouble(), timeFix_ );
        latitudeFixWriter_->setInvalid( false );
        ok &= latitudeFixWriter_->write( Units::DEGREE, latitudeSetting_, timeFix_ );
        longitudeFixWriter_->setInvalid( false );
        ok &= longitudeFixWriter_->write( Units::DEGREE, longitudeSetting_, timeFix_ );

        SetNav_ = ok;
    }

    if( ok ) logger_.syslog( "SetNav fix at " + timeFix_.toSmallString() + ": (" + Str( latitudeSetting_ ) + ", " + Str( longitudeSetting_ ) + ")", Syslog::INFO );

    return SetNav_;
}

bool SetNav::isSatisfied()
{
    return SetNav_;
}

bool SetNav::readSettings( )
{
    bool ok( true );

    if( !latitudeSettingReader_->read( Units::DEGREE, latitudeSetting_ )
            && latitudeSetting_ != latitudeSetting_ )
    {
        ok = false;
        logger_.syslog( "Failed to read latitude setting.", Syslog::CRITICAL );
    }

    if( !longitudeSettingReader_->read( Units::DEGREE, longitudeSetting_ )
            && longitudeSetting_ != longitudeSetting_ )
    {
        ok = false;
        logger_.syslog( "Failed to read longitude setting.", Syslog::CRITICAL );
    }

    if( ok )
    {
        timeFix_ = Timestamp::Now();
        SetNav_ = false;
    }

    return ok;
}

/// Uninit function
void SetNav::uninitialize( void )
{
    logger_.syslog( "Uninitialize SetNavComponent." );
}

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