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

const float SimACommsRangeHandler::ONEWAY_MODE_PING_INTERVAL( 0.5f );

FastMap<const int, SimACommsRangeMessage*> SimACommsRangeHandler::RangeMessages_;

Mutex SimACommsRangeHandler::SimACommsRangeMutex_;

Timespan SimACommsRangeHandler::acousticResponseTimeout_( 10 );

int SimACommsRangeHandler::acRangeMessageID_( 0 );

Str SimACommsRangeHandler::vehicleName_( Str::EMPTY_STR );

#ifdef __GAZEBO
SimACommsRangeGazebo SimACommsRangeHandler::ignAComms_;
#endif


/// Accessor function to add outgoing range requests to the queue
bool SimACommsRangeHandler::WriteOutgoingMessage( int destination, int source, int numPings )
{
    MutexLocker Lock( SimACommsRangeMutex_ );

    SimACommsRangeMessage* acRangeMsg = RangeMessages_.get( destination );
    if( acRangeMsg != NULL )
    {
        if( !acRangeMsg->isTimedOut() )
        {
            // A valid range message to this destination already exists
            return false;
        }
        else
        {
            // A range message to this destination already exists, but it's timed out. Clear it.
            acRangeMsg = RangeMessages_.pop( destination );
            delete acRangeMsg;
            acRangeMsg = NULL;
        }
    }

    acRangeMessageID_ += 1;
    // Create a new range query message
    acRangeMsg = new SimACommsRangeMessage( destination, source, acRangeMessageID_, numPings );

    // Calculate the timeout for this range query to account for numPings > 1
    acRangeMsg->setAcTimeout( acousticResponseTimeout_.asFloat() + ( numPings - 1 ) * ONEWAY_MODE_PING_INTERVAL );
    acRangeMsg->setState( ACOMMS_QUERY_QUEUED );

#ifdef __GAZEBO
    // Send it via Gazebo transport!
    ignAComms_.PublishRangeMessage( *acRangeMsg );
    acRangeMsg->setTxTime( Timestamp::Now() );
#endif
    RangeMessages_.put( destination, acRangeMsg );
    return true;
}

/// Transport layer accessor function to read requests from the queue
bool SimACommsRangeHandler::ReadOutgoingMessage( int& destination, int& source, int& numPings, int& msgID )
{
    MutexLocker Lock( SimACommsRangeMutex_ );

    // Get the next queued up range query (i.e., range query that hasn't been sent yet)
    for( unsigned int i = 0; i < RangeMessages_.size(); ++i )
    {
        SimACommsRangeMessage* acRangeMsg = RangeMessages_.getIndexed( i );

        if( acRangeMsg != NULL && acRangeMsg->getState() == ACOMMS_QUERY_QUEUED )
        {
            acRangeMsg->readOutgoing( destination, source, numPings, msgID );
            acRangeMsg->setState( ACOMMS_QUERY_SENT );
            acRangeMsg->setTxTime( Timestamp::Now() );
            return true;
        }
    }

    // No range queries queued up at this time.
    return false;
}

/// Transport layer accessor function to write returning range request results
bool SimACommsRangeHandler::WriteIncomingMessage( int source, float range, float azimuth, float elevation, const Timestamp& rxTime, int msgId )
{
    MutexLocker Lock( SimACommsRangeMutex_ );

    SimACommsRangeMessage* acRangeMsg = RangeMessages_.get( source );
    if( acRangeMsg == NULL )
    {
        // Unknown range message.
        return false;
    }
    else if( acRangeMsg->getDestination() != source || acRangeMsg->getID() != msgId )
    {
        // This incoming range message is intended for someone else...
        return false;
    }
    if( acRangeMsg->isTimedOut() )
    {
        // This range query timed out. Clear it.
        acRangeMsg = RangeMessages_.pop( source );
        delete acRangeMsg;
        return false;
    }

    acRangeMsg->writeIncoming( range, azimuth, elevation, rxTime );
    return true;
}

/// Transport layer accessor function to write returning range request results
bool SimACommsRangeHandler::ReadIncomingMessage( int source, float& range, float& azimuth, float& elevation, Timestamp& rxTime )
{
    MutexLocker Lock( SimACommsRangeMutex_ );

    SimACommsRangeMessage* acRangeMsg = RangeMessages_.get( source );
    if( acRangeMsg == NULL )
    {
        // Unknown range message.
        return false;
    }
    else if( acRangeMsg->getState() == ACOMMS_QUERY_SENT && acRangeMsg->isTimedOut() )
    {
        // Range query timed out. Clear it.
        acRangeMsg = RangeMessages_.pop( source );
        delete acRangeMsg;
        return false;
    }
    else if( acRangeMsg->getState() != ACOMMS_QUERY_RECIEVED )
    {
        // Range query response not received.
        return false;
    }

    // Extract range query response
    acRangeMsg->readIncoming( range, azimuth, elevation, rxTime );

    // All done with this range query?
    if( acRangeMsg->getState() == ACOMMS_QUERY_DONE )
    {
        acRangeMsg = RangeMessages_.pop( source );
        delete acRangeMsg;
    }

    return true;
}

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

/// Sets the vheicle name (triggers init Igntion transport topics)
void SimACommsRangeHandler::SetLocalAddress( const Str& vehicleName )
{
#ifdef __GAZEBO
    if( vehicleName != vehicleName_ )
    {
        // Update Gazebo transport topic names with the new address.
        ignAComms_.Initialize( vehicleName );
    }
#endif
    vehicleName_ = vehicleName;
}