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

#include <sched.h>

#include "Handler.h"

#include "component/ComponentRegistry.h"
#include "logger/LogEngine.h"
#include "process/PCaller.h"
#include "supervisor/Supervisor.h"

#ifdef __FASTSIM
bool Handler::AllowFastSim_( false );
#endif

const Timespan Handler::SMALL_INTERVAL = Timespan::FromMicros( 200 );

/// Constructor.
Handler::Handler( Component &inComponent, const Str& name, bool idleThread, bool controlThread )
    : component_( inComponent ),
      componentStartTime_( Timestamp::Now() ),
      initTime_( Timestamp::Now() ),
      logger_( name ),
      pCaller_( new PCaller( idleThread, logger_ ) ),
      running_( false ),
      firstLoop_( true ),
      controlLoopHandler_( controlThread ),
      logHandler_( false )
{
#ifdef __GAZEBO
    worldStatHandler_ = NULL;
    if( controlLoopHandler_ )
    {
        worldStatHandler_ = new WorldStatHandler( initTime_, logger_ );
        AllowFastSim_ = true;
    }
#endif

}

/// Destructor
Handler::~Handler()
{
#ifdef __GAZEBO
    if( NULL != worldStatHandler_ )
    {
        worldStatHandler_->uninitialize();
        delete worldStatHandler_;
        worldStatHandler_ = NULL;
    }
#endif
    delete pCaller_;
}

/// Initialize the component, run it, then un-initialize it
void Handler::run()
{
    running_ = true;

    // Pre-run stuff (timing, profiling)
    //component_.getLogger().syslog("Calling pInitialize on this component", Syslog::INFO);
    pCaller_->pInitialize( component_ );

    do
    {
        if( firstLoop_ == true )
        {
            runOnce();
            firstLoop_ = false;
        }
        else
        {
            runBetween();
        }

        pCaller_->pExecute( component_ );

        // Be a good neighbor...
        sched_yield();
    }
    while( continueLoop() );

    running_ = false;
    //    Post-run stuff
    logger_.syslog( "Uninitializing protected caller thread.", Syslog::INFO );
    pCaller_->pUninitialize( component_ );
}

/// Protected methods

void Handler::runOnce( void )
{
    //printf( "Handler::runOnce with thread at 0x%08X (%s)\n", ( unsigned int )pthread_self(), component_.getName().c_str() );fflush(stdout);
    componentStartTime_ = Timestamp::Now();
#ifdef __GAZEBO
    if( controlLoopHandler_ )
    {
        logger_.syslog( "Waiting for Gazebo time sync...", Syslog::INFO );
        worldStatHandler_->handleGazeboSync( componentStartTime_ );
        logger_.syslog( "Gazebo time sync received.", Syslog::INFO );
    }
#endif
}

void Handler::runBetween( void )
{
    if( component_.getState() == Component::BLOCK_PERIODIC
            || ( component_.getState() == Component::BLOCK_CONTINUOUS && component_.isFailed() ) )
    {
        if( component_.isFailed() )
        {
            componentStartTime_ = Timestamp::Now() + ComponentRegistry::GetControlThreadPeriod();
        }
        else
        {
            componentStartTime_ += component_.getPeriod();
        }

        double sleepTime = ( componentStartTime_ - Timestamp::Now() ).asDouble();
        if( sleepTime > 0 )
        {
            if( !fastSimSleepCase() )
            {
                componentStartTime_.sleepTill();
            }
        }
        componentStartTime_ = Timestamp::Now();
    }
}

bool Handler::fastSimSleepCase( void )
{
#ifdef __FASTSIM
    if( AllowFastSim_ && !logHandler_ && !Supervisor::IsRunningSimsInRealTime() )
    {
        if( controlLoopHandler_ )
        {
#ifndef __GAZEBO
            Timestamp::SetNow( componentStartTime_ );
#else
            worldStatHandler_->handleGazeboSync( componentStartTime_ );
#endif
        }
        else
        {
            while( Timestamp::Now() < componentStartTime_ )
            {
                SMALL_INTERVAL.sleepFor();
            }

        }
        return true;
    }
    else
    {
        return false;
    }
#endif
    // Not running on the host? (in theory, the compiler should optimize this function out when building for the target)
    return false;
}

bool Handler::continueLoop( void )
{
    return ( component_.isCompleted() == false );
}

void Handler::shutdown( void )
{
#ifdef __GAZEBO
    if( NULL != worldStatHandler_ )
        worldStatHandler_->uninitialize();
#endif
    component_.setEnabled( false );
}