/** \file
 *
 *  Contains the SimACommsDataHandler class implementation.
 *
 *  Copyright (c) 2007,2008,2009 MBARI
 *  MBARI Proprietary Information.  All Rights Reserved
 */
#include "SimACommsDataHandler.h"


FlexArray<SimACommsDataMessage*> SimACommsDataHandler::IncomingMessages_( true );

FlexArray<SimACommsDataMessage*> SimACommsDataHandler::OutgoingMessages_( true );

Mutex SimACommsDataHandler::SimACommsDataMutex_;

Timespan SimACommsDataHandler::acousticResponseTimeout_( 10 );

int SimACommsDataHandler::localAddress_( -1 );

#ifdef __GAZEBO
SimACommsDataGazebo SimACommsDataHandler::ignAComms_;
#endif


/// Accessor function to add outgoing data messages to the queue
void SimACommsDataHandler::WriteOutgoingMessage( int destination, int source, const Str& message, float maxDistance, float delay, float mediumSpeed )
{
    MutexLocker Lock( SimACommsDataMutex_ );

    SimACommsDataMessage* msg = new SimACommsDataMessage( destination, source, message, maxDistance, delay, mediumSpeed );

#ifndef __GAZEBO
    // Add to message queue for transport layer conponent to handle (e.g., ExternalSim)
    OutgoingMessages_.push( msg );
#else
    // Send now via Gazebo publisher
    ignAComms_.PublishDataMessage( *msg );
    delete msg;
#endif
}

/// Transport layer accessor function to read outgoing messages from the queue
bool SimACommsDataHandler::ReadOutgingMessage( int& destination, int& source, Str& message, float& maxDistance, float& delay, float& mediumSpeed )
{
    MutexLocker Lock( SimACommsDataMutex_ );

    if( OutgoingMessages_.size() > 0 )
    {
        SimACommsDataMessage * msg = OutgoingMessages_.pop( 0 );
        msg->read( destination, source, message, maxDistance, delay, mediumSpeed );
        delete msg;
        return true;
    }
    return false;
}

/// Transport layer accessor function to write incoming messages to the queue
void SimACommsDataHandler::WriteIncomingMessage( int destination, int source, const Str& message )
{
    MutexLocker Lock( SimACommsDataMutex_ );

    SimACommsDataMessage* msg = new SimACommsDataMessage( destination, source, message );
    IncomingMessages_.push( msg );
}

/// Transport layer accessor function to read incoming messages from the queue
bool SimACommsDataHandler::ReadIncomingMessage( int& destination, int& source, Str& message )
{
    MutexLocker Lock( SimACommsDataMutex_ );

    for( unsigned int i = 0; i < IncomingMessages_.size(); ++i )
    {
        SimACommsDataMessage* msg = IncomingMessages_.pop( i );
        if( msg != NULL )
        {
            msg->read( destination, source, message );
            delete msg;
            return true;
        }
    }
    return false;
}

/// Sets the acoustic response timeout
void SimACommsDataHandler::SetAcousticResponseTimeout( const Timespan& acousticResponseTimeout )
{
    acousticResponseTimeout_ = acousticResponseTimeout;
}

/// Sets the local address (triggers init Igntion transport topics)
void SimACommsDataHandler::SetLocalAddress( int localAddress )
{
#ifdef __GAZEBO
    if( localAddress != localAddress_ )
    {
        // Update Gazebo transport topic names with the new address.
        ignAComms_.Initialize( localAddress );
    }
#endif
    localAddress_ = localAddress;
}