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

#include "LoopControl.h"
#include "LoopControlIF.h"

#include <limits.h>
#include <stdlib.h>

#include "component/ComponentRegistry.h"
#include "data/UniversalURI.h"
#include "data/StrValue.h"
#include "units/Units.h"
#include "utils/AuvMath.h"

LoopControl::LoopControl( const Module* module )
    : SyncControlComponent( LoopControlIF::NAME, module )
{
    logger_.syslog( "Construct LoopControl." );

    // Slate input settings
    periodCmdReader_ = newDataReader( LoopControlIF::PERIOD_CMD );//, this, Units::SECOND( LoopControlIF::LOOP_SLOW ) );
}

LoopControl::~LoopControl()
{}

// Initialize function
void LoopControl::initialize( void )
{
    logger_.syslog( "Initialize LoopControlComponent." );
}

/// The actual "payload" of the component
void LoopControl::run()
{
    Timespan periodCmd( LoopControlIF::LOOP_FAST );
    float periodCmdSec( nanf( "" ) );
    if( periodCmdReader_->wasTouchedSinceLastRun( this )
            && periodCmdReader_->read( Units::SECOND, periodCmdSec )
            && !isnan( periodCmdSec ) )
    {
        periodCmd = AuvMath::Limit( periodCmdSec,
                                    LoopControlIF::LOOP_FAST,
                                    LoopControlIF::LOOP_DRIFT );
    }
    ComponentRegistry::SetControlThreadPeriod( periodCmd );

}

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