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

#include "LBL.h"
#include "LBLIF.h"

#include "LBLNavigationIF.h"

#include "data/BlobWriter.h"
#include "data/Location.h"
#include "data/Slate.h"
#include "data/UniversalDataReader.h"
#include "sensorModule/DATIF.h"
#include "sensorModule/MicromodemIF.h"
#include "units/Units.h"
#include "utils/AuvMath.h"

LBL::LBL( const Str& prefix, const Module* module )
    : Behavior( prefix + LBLIF::NAME, module, true, true ),
      initialized_( false ),
      value_( Units::DEGREE, BLOB_FLOAT64LE )
{

    logger_.syslog( "Construct LBL." );

    // Slate input setting variables
    for( int iTrans = 0; iTrans < LBLIF::NUM_TRANS && iTrans < LBLNavigationIF::NUM_PINGS; ++iTrans )
    {
        transponders_[iTrans].locationSettingReader_ = newSettingReader( *LBLIF::TRANS_LOCATION_SETTINGS[iTrans] );
        transponders_[iTrans].depthSettingReader_ = newSettingReader( *LBLIF::TRANS_DEPTH_SETTINGS[iTrans] );
        transponders_[iTrans].lblNavPositionWriter_ = newBlobWriter( *LBLNavigationIF::PING_POSITIONS[iTrans] );
        transponders_[iTrans].initialize();
    }

    pingsRequestedWriters_[0] = newDataWriter( MicromodemIF::PINGS_REQUESTED );
    pingsRequestedWriters_[1] = newDataWriter( DATIF::NUMBER_OF_PINGS_REQUESTED );

}

LBL::~LBL()
{}

void LBL::Transponder::initialize( void )
{
    locationSetting_.setLat( Units::RADIAN, nanf( "" ) );
    locationSetting_.setLon( Units::RADIAN, nanf( "" ) );
    lastLocationSetting_ = locationSetting_;
    depthSetting_ = nanf( "" );
    lastDepthSetting_ = depthSetting_;
}

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

    for( int iTrans = 0; iTrans < LBLIF::NUM_TRANS; ++iTrans )
    {
        transponders_[iTrans].initialize();
    }

    initialized_ = true;
}

bool LBL::isSatisfied()
{
    if( !initialized_ )
    {
        initialize();
    }
    if( !initialized_ )
    {
        return false;
    }

    return true;
}

bool LBL::runIfUnsatisfied()
{
    //logger_.syslog( "Running LBL." );

    run();

    return false;
}

/// The actual "payload" of the component
void LBL::run()
{
    int pingsRequested = 0;
    for( int iTrans = 0; iTrans < LBLIF::NUM_TRANS; ++iTrans )
    {
        Transponder& trans = transponders_[iTrans];
        if( trans.locationSettingReader_->isActive()
                && trans.depthSettingReader_->isActive() )
        {
            pingsRequested |= ( 1 << iTrans );
            if( trans.locationSettingReader_->read( value_ ) )
            {
                value_.copyTo( Units::RADIAN, trans.locationSetting_ );
            }
            trans.depthSettingReader_->read( Units::METER, trans.depthSetting_ );

            if( trans.locationSetting_ !=  trans.lastLocationSetting_ || trans.depthSetting_ != trans.lastDepthSetting_ )
            {
                double position[3] =
                {
                    trans.locationSetting_.getLat( Units::RADIAN ),
                    trans.locationSetting_.getLon( Units::RADIAN ),
                    trans.depthSetting_
                };
                trans.lblNavPositionWriter_->write1DPtr( Units::NONE, position, 3 );
                trans.lastLocationSetting_ = trans.locationSetting_;
                trans.lastDepthSetting_ = trans.depthSetting_;
            }
        }
    }
    for( int iModem = 0; pingsRequested && iModem < NUM_MODEM_VARIANTS; ++iModem )
    {
        if( pingsRequestedWriters_[iModem]->isImplemented() )
        {
            pingsRequestedWriters_[iModem]->write( Units::ENUM, pingsRequested );
        }
    }
}

/// Uninit function
void LBL::uninitialize( void )
{
    logger_.syslog( "Uninitialize LBLComponent." );
    initialized_ = false;
}

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