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

#include "LcmMsgWriter.h"
#include "LcmMessageWriter.h"
#include "data/StrValue.h"

#include "data/LcmInstance.h"


using namespace lrauv_lcm_tools;

LcmMsgWriter::LcmMsgWriter( const Str& channelName )
    : channelName_( channelName )
{}

LcmMsgWriter::~LcmMsgWriter()
{}

/// Creates a LrauvLcmMessage data array entry for a DataElement
bool LcmMsgWriter::add( const DataElement& element )
{
    const ElementURI& uri = element.getUri();
    const Unit& unit = uri.getUnit();

    // match lrauv-app BinaryDataType to LcmMessage DataType
    DataType type = mapElementType( element );

    // gather the elementUri's dimensions
    Dim shape( 0, 0 );
    mapDimensions( uri, shape );
    // TODO: if blob, adjust dimensions to match actual type byte size (e.g., in the case of short)

    // create a data array entry for the DataElement in the LrauvLcmMessage
    return msg_.addArray( type, uri.getCode(), uriName( uri ), unit.getName(), shape );
}

/// Populates the LCM message array with data.
bool LcmMsgWriter::setValue( const DataElement& dataElement )
{
    unsigned short uriCode = dataElement.getUri().getCode();
    const DataValue* dataVal = dataElement.getDataValue();

    if( dataVal != NULL && set( uriCode, *dataVal ) )
    {
        msg_.setValid( uriCode, !dataElement.isInvalid() );
        timestamp_ = dataElement.getTimestamp();
        return true;
    }

    return false;
}

/// Assigns DataValue data to LrauvLcmMessage array that matches the specified key.
bool LcmMsgWriter::set( unsigned short uriCode, const DataValue& dataVal )
{
    if( dataVal.getBinaryType() == MULTIVALUE )
    {
        // Multivalue, meaning the DataValue contains a string or blob.
        // Downcast to StrValue and do a raw byte copy:
        const Str& str = dynamic_cast<const StrValue&>( dataVal ).asString();
        return msg_.copyFromRaw( uriCode, str.cStr(), ( unsigned )str.length() );
    }
    else
    {
        ScalarVal val;
        // Handle typed scalar data:
        switch( mapDataType( dataVal.getBinaryType() ) )
        {
        case lrauv_lcm_tools::Int:
            dataVal.copyTo( dataVal.getUnit(), val.ival );
            return msg_.set( uriCode, val.ival );

        case lrauv_lcm_tools::Float:
            dataVal.copyTo( dataVal.getUnit(), val.fval );
            return msg_.set( uriCode, val.fval );

        case lrauv_lcm_tools::Double:
            dataVal.copyTo( dataVal.getUnit(), val.dval );
            return msg_.set( uriCode, val.dval );

        case lrauv_lcm_tools::Byte:
            dataVal.copyTo( dataVal.getUnit(), val.bval );
            return msg_.set( uriCode, val.bval );

        case lrauv_lcm_tools::Unknown:
        default:
            return false;
        }
    }

    return false;
}

/// Publishes the component's LCM messages
bool LcmMsgWriter::publish( void )
{
    if( LcmInstance::IsValid() )
    {
        return msg_.publish( *LcmInstance::GetInstance(), channelName_.cStr(), timestamp_.asMillis() );
    }
    return false;
}

/// Matches DataElement data type to LcmMessageWriter's DataType
DataType LcmMsgWriter::mapElementType( const DataElement& element )
{
    const BinaryDataType& type = element.isUniversal() ? element.getUri().getBinaryType() : element.getDataValue()->getBinaryType();

    if( type == MULTIVALUE )
        return mapBlobDataType( element.getUri().getBlobType() );

    return mapDataType( type );
}

/// Matches the lrauv-app's BinaryDataType to LcmMessageWriter's DataType
DataType LcmMsgWriter::mapDataType( const BinaryDataType& dataType )
{
    switch( dataType )
    {
    case SHORT2:
    case USHORT2:
    case INT4:
        return lrauv_lcm_tools::Int;

    case FLOAT2:
    case FLOAT3:
    case FLOAT4:
        return lrauv_lcm_tools::Float;

    case DOUBLE4:
    case DOUBLE6:
    case DOUBLE8:
        return lrauv_lcm_tools::Double;

    case UCHAR1:
        return lrauv_lcm_tools::Byte;

    case MULTIVALUE:
    default:
        return lrauv_lcm_tools::Unknown;
    }
}

/// Matches the lrauv-app's BlobType to LcmMessageWriter's DataType
DataType LcmMsgWriter::mapBlobDataType( const BlobType& blobType )
{
    switch( blobType )
    {
    case NOT_BLOB:
    case BLOB_INT8:
    case BLOB_UINT8:
        return lrauv_lcm_tools::Byte;

    case BLOB_INT32LE:
    case BLOB_INT32BE:
    case BLOB_UINT32LE:
    case BLOB_UINT32BE:
    case BLOB_TYPE_COUNT:
        return lrauv_lcm_tools::Int;

    case BLOB_FLOAT32LE:
    case BLOB_FLOAT32BE:
        return lrauv_lcm_tools::Float;

    case BLOB_INT64LE:
    case BLOB_INT64BE:
    case BLOB_UINT64LE:
    case BLOB_UINT64BE:
    case BLOB_FLOAT64LE:
    case BLOB_FLOAT64BE:
        return lrauv_lcm_tools::Double;

    // TODO: this class needs to account for the byte size of these types and convert to Int or treat as Bytes before these blob types can be supported.
    case BLOB_INT16LE:
    case BLOB_INT16BE:
    case BLOB_UINT16LE:
    case BLOB_UINT16BE:
    default:
        return lrauv_lcm_tools::Unknown;
    }

}

/// Matches ElementURI dimensions to LcmMessageWriter's dimensions
void LcmMsgWriter::mapDimensions( const ElementURI& uri, Dim& dim )
{
    if( uri.getDimension1() > 0 )
    {
        dim.push_back( uri.getDimension1() );
    }

    if( uri.getDimension2() > 0 )
    {
        dim.push_back( uri.getDimension2() );
    }

    if( uri.getDimension3() > 0 )
    {
        dim.push_back( uri.getDimension3() );
    }

}

/// Extracts the data variable's name from a URI string
const char *LcmMsgWriter::uriName( Str uri )
{
    char *dataName = strchr( ( char* )uri.cStr(), '.' );

    if( !dataName )
    {
        dataName = ( char* )uri.cStr();
    }
    else
    {
        // Data name starts after '.'
        dataName++;
    }

    return ( const char * )dataName;
}