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

#include "LcmDataWriter.h"
#include "LcmInstance.h"

Mutex LcmDataWriter::Mutex_;

LcmDataWriter::LcmDataWriter( Component* owner,
                              DataElement& element,
                              UniversalDataElement* universal,
                              Logger::TimePrecisionType timePrecisionType,
                              Str source,
                              Str channel,
                              LcmPolicy *policy
                            )
    : DataWriter( owner, element, universal, timePrecisionType ), logger_( "LcmDataWriter." + owner->getName() )
{
    // set LCM channel source
    source_ = source; //owner->getName();
    // set LCM channel name
    channel_ = channel; //element.getUri();
    // set policy (caller owns, should destroy)
    policy_ = policy;
    // initialize LCM message
    memset( &message_, 0, sizeof( LcmWriterMessage ) );
}

LcmDataWriter::~LcmDataWriter()
{
    // owner must delete policy instance
}

void LcmDataWriter::setPolicy( LcmPolicy *policy )
{
    // won't publish to LCM if set to NULL;
    policy_ = policy;
}

bool LcmDataWriter::write( const Unit& unit, const int value, const Timestamp& timestamp )
{
    // write to slate
    bool retval = DataWriter::write( unit, value, timestamp );

    // create and publish a message
    // TODO: mutex lock the message?

    // set message timestamp
    message_.imsg_.epoch_usec = timestamp.asMicros();
    // set message size
    message_.imsg_.size = element_.getTypeSize();
    // set message data
    message_.imsg_.data = value;

    retval = ( retval && publish( message_, LcmDataWriter::I32 ) );

    // return true if both writes succeed
    return retval;
}

bool LcmDataWriter::write( const Unit& unit, const double value, const Timestamp& timestamp )
{
    // write to slate
    bool retval = DataWriter::write( unit, value, timestamp );

    // create and publish a message
    // TODO: mutex lock the message?

    // set message timestamp
    message_.dmsg_.epoch_usec = timestamp.asMicros();
    // set message size
    message_.dmsg_.size = element_.getTypeSize();
    // set message data
    message_.dmsg_.data = value;

    retval = ( retval && publish( message_, LcmDataWriter::DOUBLE ) );

    // return true if both writes succeed
    return retval;
}

bool LcmDataWriter::write( const Unit& unit, const unsigned char value, const Timestamp& timestamp )
{
    // write to slate
    bool retval = DataWriter::write( unit, value, timestamp );

    // create and publish a message
    // TODO: mutex lock the message?

    // set message timestamp
    message_.umsg_.epoch_usec = timestamp.asMicros();
    // set message size
    message_.umsg_.size = element_.getTypeSize();
    // set message data
    message_.umsg_.data = value;

    retval = ( retval && publish( message_, LcmDataWriter::U8 ) );

    // return true if both writes succeed
    return retval;
}

bool LcmDataWriter::write( const DataValue& value, const Timestamp& timestamp )
{
    // write to slate
    bool retval = DataWriter::write( value, timestamp );

    logger_.syslog( "LcmDataWriter::write - DataValue LCM publish not implementd\n", Syslog::IMPORTANT );
    // create and publish a message
    // [currently, the DataValue class does not expose the underlying raw data]
//    retval = (retval && publish((uint8_t *)&value,timestamp, element_.getTypeSize()));

    // return true if both writes succeed
    return retval;
}

bool LcmDataWriter::publish( LcmWriterMessage message, LcmDataWriter::LcmMsgType type )
{
    bool retval = false;
    bool debug = false;
    // check for NULL policy_
    if( policy_ )
    {
        // check policy (LCM) permissions
        if( policy_->pubAllowed( channel_, ( LcmPolicy::OFLAGS )LcmPolicy::LCM ) )
        {

            // lock mutex [is this necessary if setLCM contains lock?]
            MutexLocker lock( LcmDataWriter::Mutex_ );

            // check lcm status
            // Do after other checks, since it only
            // matters if LCM is allowed
            if( LcmInstance::IsValid() )
            {
                // check type of message and publish accordingly
                switch( type )
                {
                case LcmDataWriter::I32:
                    retval = ( LcmInstance::GetInstance()->publish( ( const char * )( channel_.cStr() ), &message.imsg_ ) == 0 );
                    break;
                case LcmDataWriter::DOUBLE:
                    retval = ( LcmInstance::GetInstance()->publish( ( const char * )( channel_.cStr() ), &message.dmsg_ ) == 0 );
                    break;
                case LcmDataWriter::U8:
                    retval = ( LcmInstance::GetInstance()->publish( ( const char * )( channel_.cStr() ), &message.umsg_ ) == 0 );
                    break;
                default:
                    logger_.syslog( "publish - Unsupported message type [" + Str( ( int )type ) + "]", Syslog::ERROR );
                    break;
                }

                if( debug )
                {
                    //                    fprintf(stderr,"\nPUB(%s) returned %s\n",channel_.cStr(),retval?"OK":"ERR");
                    logger_.syslog( "publish - ch[" + channel_ + "] returned %s\n" + Str( ( retval ? "OK" : "ERR" ) ) );
                }
                if( !retval )
                {
                    logger_.syslog( "publish - pub to src[" + source_ + "] ch[" + channel_ + "] returned FALSE", Syslog::ERROR );
                }
            }
            else
            {
                // else NULL lcm or message
                if( debug )
                {
                    //                    fprintf(stderr,"\nlcm.good() returned FALSE\n");
                    logger_.syslog( "publish - lcm instance is invalid.", Syslog::ERROR );
                }
            }
        }
        else
        {
            if( debug )
            {
                //                fprintf(stderr,"\npublish - pub not allowed src[%s] ch[%s]\n",source_.cStr(), channel_.cStr());
                logger_.syslog( "publish - pub not allowed src[" + source_ + "] ch[" + channel_ + "]", Syslog::IMPORTANT );
            }
            retval = true;
        }
    }
    else
    {
        // policy is NULL (OK, indicates publishing disabled)
        if( debug )
        {
            logger_.syslog( "publish - Policy is NULL", Syslog::IMPORTANT );
        }
    }
    return retval;
}