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

#include "LcmPublisher.h"

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

#include "data/ConfigReader.h"
#include "data/Slate.h"
#include "units/Units.h"
#include "utils/AuvMath.h"
#include "data/LcmInstance.h"
#include "LcmPublisherIF.h"

#define DEBUG_LOG_FILE "tmp/LcmPublisher.out"

LcmPublisher::LcmPublisher( const Module* module )
    : AsyncComponent( LcmPublisherIF::NAME, module, Timespan( 1.0 / 3.0 ) )
{
    debug_ = false;
    redirectStdout_ = true;

    logger_.syslog( "Construct LcmPublisher." );
    nChanCfgReader_ = newConfigReader( LcmPublisherIF::NCHAN_CFG );
    nDoubleItemsCfgReader_ = newConfigReader( LcmPublisherIF::NDOUBLEITEMS_CFG );
    loopHzCfgReader_ = newConfigReader( LcmPublisherIF::LOOP_HZ_CFG );

    publishPrefixCfgReader_ =
        newConfigReader( LcmPublisherIF::PUBLISH_PREFIX_CFG );


    // TESTING ONLY!!
    if( redirectStdout_ )
    {
        printf( "Redirecting output to %s\n", DEBUG_LOG_FILE );
        // Reopen stdout to a file
        if( !freopen( DEBUG_LOG_FILE, "w", stdout ) )
        {
            logger_.syslog( "freopen() failed dor  stdout", Syslog::ERROR );
        }
    }

}

LcmPublisher::~LcmPublisher()
{
}

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

    // Set default configuraton values
    loopHz_ = 20.;
    nChannels_ = 1;
    nDoubleItems_ = 25;
    channelPrefix_ = ( char * )"a";

    // Read configuration values from file
    if( !readConfig() )
    {
        logger_.syslog( "Error loading LcmPublisher configuration",
                        Syslog::CRITICAL );

        this->setFailure( FailureMode::DATA );
        return;
    }


    msg_.doubleItem.resize( nDoubleItems_ );
    msg_.nItems = nDoubleItems_;

    char itemName[256];
    // Fill in double items
    for( int i = 0; i < msg_.nItems; i++ )
    {
        sprintf( itemName, "item-%0d", i );
        msg_.doubleItem[i].name.assign( itemName );
        msg_.doubleItem[i].unit.assign( "no-units" );
    }
    dummyValue_ = 0.;
}


bool LcmPublisher::readConfig( void )
{
    // Read values from configuration - if not found in configuration,
    // don't override the defaults already set
    // Thus the boolean returned by read()
    // doesn't really matter.
    nChanCfgReader_->read( Units::COUNT, nChannels_ );
    nDoubleItemsCfgReader_->read( Units::COUNT, nDoubleItems_ );
    loopHzCfgReader_->read( Units::HERTZ, loopHz_ );
    if( loopHz_ <= 0. )
    {
        logger_.syslog( "Inalid loopHz", Syslog::ERROR );
        return false;
    }

    publishPrefixCfgReader_->read( channelPrefix_ );

    char buf[256];
    sprintf( buf, "nChan: %d, nDoubleItems: %d, loopHz: %.1f",
             nChannels_, nDoubleItems_, loopHz_ );

    if( debug_ ) logger_.syslog( Str( buf ), Syslog::INFO );


    // Always return 'true', since we have set defaults in the code
    return true;
}


/// The actual "payload" of the component; publish state message
void LcmPublisher::run()
{
    if( debug_ ) logger_.syslog( "LcmPublisher.run()", Syslog::INFO );
    msg_.seqNo = 0;

    double loopIntervalSec = 1. / loopHz_;
    long loopIntervalUsec = loopIntervalSec * 1e6;

    // Get current time
    struct timespec tval;
    if( clock_gettime( CLOCK_MONOTONIC, &tval ) == -1 )
    {
        perror( "error from outer clock_gettime()" );
    }

    double pubSec = tval.tv_sec + tval.tv_nsec / 1e9;

    while( LcmInstance::IsValid() )
    {

        publish( LcmInstance::GetInstance(), nChannels_, nDoubleItems_,
                 ( char * )channelPrefix_.asString().cStr() );

        // Next loop should start at this time:
        pubSec += loopIntervalSec;

        // Get current time
        if( clock_gettime( CLOCK_MONOTONIC, &tval ) == -1 )
        {
            perror( "error from inner clock_gettime()" );
        }
        double nowSec = tval.tv_sec + tval.tv_nsec / 1e9;

        // Calculate how long to sleep before next loop
        long sleepUsec = ( pubSec - nowSec ) * 1e6;

        if( sleepUsec > 0 && sleepUsec <= loopIntervalUsec )
        {
            usleep( ( useconds_t )sleepUsec );
        }
        else if( sleepUsec > loopIntervalUsec )
        {
            // This should not happen if clock is monotonically increasing
            logger_.syslog( "LcmPublisher - clock rollback?", Syslog::ERROR );
            printf( "nowUsec: %.6f, pubUsec: %.6f, sleep for %ld usec\n",
                    nowSec, pubSec, sleepUsec );
            usleep( 0 );
        }
        else
        {
            logger_.syslog( "LcmPublisher - missed deadline", Syslog::ERROR );
            usleep( 0 );
        }
    }

    if( debug_ ) logger_.syslog( "leaving LcmPublisher.run()", Syslog::INFO );
}



void LcmPublisher::publish( lcm::LCM *lcm, int nChan, int nItems,
                            char *pubPrefix )
{

    char channelName[256];

    msg_.seqNo++;
    msg_.epochMillisec = 0;

    // Fill in double items
    for( int i = 0; i < msg_.nItems; i++ )
    {
        // Increment dummy value for each item
        msg_.doubleItem[i].val = dummyValue_++;
    }

    // Publish message to each channel
    // printf("publish to %d channels\n", nChan);
    for( int i = 0; i < nChan; i++ )
    {

        // Publish message on this channel
        sprintf( channelName, "%s-%03d", pubPrefix, i );

        if( debug_ )
        {
            // printf("publish on channel %s\n", channelName);
            // printf("msg.nItems: %d, msg.seqNo: %ld\n", msg_.nItems, msg_.seqNo);
            // printf("msg values:\n");
            /* ***
            for (int i = 0; i < msg_.nItems; i++) {
            printf("val: %.1f\n", msg_.doubleItem[i].val);
                 }
                 *** */
        }

        if( lcm->publish( channelName, &msg_ ) != 0 )
        {
            logger_.syslog( "LCM publish failed", Syslog::ERROR );
        }
        else
        {
            // printf("lcm publish succeeded\n");
        }
    }
}

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