#include <string.h>
#include "LcmSlateWriter.h"
#include "units/Units.h"
#include "units/UnitRegistry.h"
#include "data/UniversalDataWriter.h"

LcmSlateWriter::LcmSlateWriter( Component *component, bool enableLcm ) :
    DataVectorsMgr()
{
    component_ = component;
    lcmEnabled_ = enableLcm;
    debug_ = false;
}

LcmSlateWriter::~LcmSlateWriter()
{
}


bool LcmSlateWriter::add( const DataURI *uri )
{
    char buf[128];

    component_->getLogger().syslog( Str( "LcmSlateWriter::add(DAtaURI): " ) + *uri );

    // Parse data name from DataURI
    const char *uriChar = uri->cStr();
    char *dataName = strchr( ( char * )uriChar, '.' );

    if( !dataName )
    {
        dataName = ( char * )uriChar;
    }
    else
    {
        // Data name starts after '.'
        dataName++;
    }
    sprintf( buf, "LcmSlateWriter::add(): dataName: %s", dataName );
    component_->getLogger().syslog( buf );

    // Get units from DataURI
    const Unit &unit = uri->getUnit();

    dataWriters_[dataName] =  component_->newDataWriter( *uri );

    // Assume float data type
    addFloatVector( dataName, unit.getName(), 1 );

    return true;
}



bool LcmSlateWriter::add( const UniversalURI *uri, const float accuracy,
                          const Unit *accuracyUnit,
                          Logger::TimePrecisionType timePrecision )
{


    component_->getLogger().syslog( Str( "LcmSlateWriter::add(): " ) + *uri );

    const char *dataName = uri->cStr();

    if( debug_ )
    {
        sprintf( buf_, "LcmsSlateWriter::add() new dataWriter %s", dataName );
        component_->getLogger().syslog( buf_, Syslog::DEBUG );
    }

    dataWriters_[dataName] =
        component_->newUniversalWriter( *uri,
                                        *accuracyUnit, accuracy,
                                        timePrecision );


    component_->getLogger().syslog( Str( "created writer for : " ) + *uri,
                                    Syslog::INFO );

    if( debug_ )
    {
        sprintf( buf_, "added writer, now dataWriters_.size()=%zu",
                 dataWriters_.size() );

        component_->getLogger().syslog( buf_, Syslog::DEBUG );
    }

    DataVectorsMgr::addFloatVector( dataName,
                                    uri->getUnit().getName(), 1 );

    return true;
}



void LcmSlateWriter::publish( const char *channelName, lcm::LCM *lcm,
                              int64_t epochMillisec )
{

    if( debug_ )
    {
        ///// TEST TEST TEST
        for( int i = 0; i < dataVectors_.nFloatVectors; i++ )
        {
            const char *name = dataVectors_.floatVector[i].name.c_str();
            sprintf( buf_, "float vector name: %s\n", name );
            component_->getLogger().syslog( buf_, Syslog::DEBUG );
        }

        sprintf( buf_, "dataWriters_.size(): %ld", dataWriters_.size() );
        component_->getLogger().syslog( buf_, Syslog::DEBUG );
    }

    if( lcmEnabled_ )
    {
        // Publish DataVectors to LCM
        if( lcm != NULL )
        {
            DataVectorsMgr::publish( channelName, lcm, epochMillisec );
        }
        else
        {
            component_->getLogger().syslog( Str( "LcmSlateWriter::publish() - "
                                                 "invalid lcm" ), Syslog::ERROR );

            component_->setFailure( FailureMode::SOFTWARE );
        }
    }

    Timestamp timestamp = Timestamp( epochMillisec / 1000 );
    DataWriter *writer;

    // Write each element to the slate
    for( int i = 0; i < dataVectors_.nIntVectors; i++ )
    {
        const Unit *unit =
            UnitRegistry::FindUnit( dataVectors_.intVector[i].unit.c_str() );

        const char *name = dataVectors_.intVector[i].name.c_str();

        if( dataVectors_.intVector[i].nVal == 1 &&
                ( writer = getDataWriter( name ) ) != NULL )
        {
            // Write scalar value to slate
            writer->write( *unit,
                           dataVectors_.intVector[i].val[0],
                           timestamp );
        }
        else
        {
            char itemName[256];
            // Write vector elements to slate items
            for( int j = 0; j < dataVectors_.intVector[i].nVal; j++ )
            {
                sprintf( itemName, "%s-%d", name, j );
                if( ( writer = getDataWriter( itemName ) ) != NULL )
                {
                    writer->write( *unit,
                                   dataVectors_.intVector[i].val[j],
                                   timestamp );
                }
            }
        }
    }

    for( int i = 0; i < dataVectors_.nFloatVectors; i++ )
    {
        const Unit *unit =
            UnitRegistry::FindUnit( dataVectors_.floatVector[i].unit.c_str() );

        const char *name = dataVectors_.floatVector[i].name.c_str();

        if( dataVectors_.floatVector[i].nVal == 1 &&
                ( writer = getDataWriter( name ) ) != NULL )
        {
            // Write scalar value to slate
            bool result = writer->write( *unit, dataVectors_.floatVector[i].val[0],
                                         timestamp );
            if( !result )
            {
                sprintf( buf_, "LcmsSlateWriter::publish() %s write to slate FAILED\n",
                         dataVectors_.floatVector[i].name.c_str() );
                component_->getLogger().syslog( buf_, Syslog::ERROR );
            }
            else
            {
                if( debug_ )
                {
                    sprintf( buf_, "LcmsSlateWriter::publish() %s write to slate OK\n",
                             dataVectors_.floatVector[i].name.c_str() );
                    component_->getLogger().syslog( buf_, Syslog::DEBUG );
                }
            }
        }
        else
        {
            char itemName[256];
            // Write vector elements to slate items
            for( int j = 0; j < dataVectors_.floatVector[i].nVal; j++ )
            {
                sprintf( itemName, "%s-%d", name, j );
                if( ( writer = getDataWriter( itemName ) ) != NULL )
                {
                    writer->write( *unit,
                                   dataVectors_.floatVector[i].val[j],
                                   timestamp );
                }
            }
        }
    }


    for( int i = 0; i < dataVectors_.nDoubleVectors; i++ )
    {
        const Unit *unit =
            UnitRegistry::FindUnit( dataVectors_.doubleVector[i].unit.c_str() );

        const char *name = dataVectors_.doubleVector[i].name.c_str();

        if( dataVectors_.doubleVector[i].nVal == 1 &&
                ( writer = getDataWriter( name ) ) != NULL )
        {
            // Write scalar value to slate
            writer->write( *unit,
                           dataVectors_.doubleVector[i].val[0],
                           timestamp );
        }
        else
        {
            char itemName[256];
            // Write vector elements to slate items
            for( int j = 0; j < dataVectors_.doubleVector[i].nVal; j++ )
            {
                sprintf( itemName, "%s-%d", name, j );
                if( ( writer = getDataWriter( itemName ) ) != NULL )
                {
                    writer->write( *unit,
                                   dataVectors_.doubleVector[i].val[j],
                                   timestamp );
                }
            }
        }
    }
}


bool LcmSlateWriter::isDataRequested()
{

    // Check all DataWriters for data requests
    std::map<const char *, DataWriter *>::iterator iterator;
    for( iterator = dataWriters_.begin();
            iterator != dataWriters_.end();
            iterator++ )
    {
        if( iterator->second->isDataRequested() )
        {
            return true;
        }
    }

    // No data requests
    return false;
}



void LcmSlateWriter::setWritersValid( bool valid )
{

    std::map<const char *, DataWriter *>::iterator iterator;
    for( iterator = dataWriters_.begin();
            iterator != dataWriters_.end();
            iterator++ )
    {

        iterator->second->setInvalid( !valid );

    }
}


bool LcmSlateWriter::setWriterValid( const char *dataName, bool valid )
{

    if( debug_ )
    {
        sprintf( buf_, "LcmsSlateWriter::setWriterInvalid() %s valid=%d",
                 dataName, valid );
        component_->getLogger().syslog( buf_, Syslog::DEBUG );
    }

    std::map<const char *, DataWriter *>::iterator iterator;
    for( iterator = dataWriters_.begin();
            iterator != dataWriters_.end();
            iterator++ )
    {

        //    char *writerDataName = (char *)iterator->second->getUri().cStr();
        char *writerDataName = ( char * )iterator->first;

        if( debug_ )
        {
            sprintf( buf_, "LcmsSlateWriter::setWriterInvalid() writerDataName %s",
                     writerDataName );

            component_->getLogger().syslog( buf_, Syslog::DEBUG );
        }

        if( writerDataName )
        {
            if( !strcmp( writerDataName, dataName ) )
            {
                iterator->second->setInvalid( !valid );
                return true;
            }
        }
    }

    // Data name not found
    sprintf( buf_, "LcmsSlateWriter::setWriterInvalid() %s not found", dataName );
    component_->getLogger().syslog( buf_, Syslog::ERROR );
    return false;
}


DataWriter *LcmSlateWriter::getDataWriter( const char *name )
{
    std::map<const char *, DataWriter *>::iterator iterator;
    for( iterator = dataWriters_.begin();
            iterator != dataWriters_.end();
            iterator++ )
    {

        if( !strcmp( name, iterator->first ) )
        {
            return iterator->second;
        }
    }

    return NULL;
}

bool LcmSlateWriter::isValid( const char *name, bool *nameFound )
{

    *nameFound = false;
    std::map<const char *, DataWriter *>::iterator iterator;
    for( iterator = dataWriters_.begin();
            iterator != dataWriters_.end();
            iterator++ )
    {

        if( !strcmp( name, iterator->first ) )
        {
            *nameFound = true;
            return !iterator->second->isInvalid();
        }
    }
    return false;
}