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


SimACommsMessage::SimACommsMessage( int destination, int source, int msgID )
    : state_( ACOMMS_QUERY_READY ),
      destination_( destination ),
      source_( source ),
      txTime_( Timestamp::NOT_SET_TIME ),
      rxTime_( Timestamp::NOT_SET_TIME ),
      msgID_( msgID )
{}

const Timespan SimACommsMessage::txElapsedTime()
{
    /// Return tx/rx time if query was answered.
    if( rxTime_ != Timestamp::NOT_SET_TIME )
    {
        return rxTime_ - txTime_;
    }

    return txTime_.elapsed();
}

bool SimACommsMessage::isTimedOut( void )
{
    if( txTime_ == Timestamp::NOT_SET_TIME )
    {
        return false;
    }
    return txElapsedTime() > acTimeout_;
}

void SimACommsMessage::clear()
{
    state_  = ACOMMS_QUERY_READY;
    txTime_ = Timestamp::NOT_SET_TIME;
    rxTime_ = Timestamp::NOT_SET_TIME;
}

/// AComms Data Message ///////////////////////////////////////////
SimACommsDataMessage::SimACommsDataMessage( int destination, int source, const Str& message, float maxDistance, float delay, float mediumSpeed )
    : SimACommsMessage( destination, source ),
      message_( message ),
      maxDistance_( maxDistance ),
      delay_( delay ),
      mediumSpeed_( mediumSpeed )
{}

SimACommsDataMessage::SimACommsDataMessage( int destination, int source, const Str& message )
    : SimACommsMessage( destination, source ),
      message_( message )
{}

void SimACommsDataMessage::clear( void )
{
    SimACommsMessage::clear();
    message_ = Str::EMPTY_STR;
}

void SimACommsDataMessage::read( int& destination, int& source, Str& message, float& maxDistance, float& delay, float& mediumSpeed )
{
    read( destination, source, message );
    maxDistance = maxDistance_;
    delay       = delay_;
    mediumSpeed = mediumSpeed_;
}

void SimACommsDataMessage::read( int& destination, int& source, Str& message )
{
    destination = destination_;
    source = source_;
    message = message_;
}


/// AComms Range Message ///////////////////////////////////////////
SimACommsRangeMessage::SimACommsRangeMessage()
    : SimACommsMessage(),
      rangeResponses_( true ),
      numPings_( 1 )
{
    clear();
}

SimACommsRangeMessage::SimACommsRangeMessage( int destination, int source, int msgID, int numPings )
    : SimACommsMessage( destination, source, msgID ),
      rangeResponses_( true ),
      numPings_( numPings )
{
    clear();
}

void SimACommsRangeMessage::clear( void )
{
    SimACommsMessage::clear();
    rangeResponses_.clear();
    numRecieved_ = 0;
}

void SimACommsRangeMessage::writeOutgoing( int destination, int source, int numPings )
{
    destination_ = destination;
    source_      = source;
    numPings_    = numPings;
}

void SimACommsRangeMessage::readOutgoing( int& destination, int& source, int& numPings, int& msgID )
{
    destination = destination_;
    source      = source_;
    numPings    = numPings_;
    msgID       = msgID_;
}

void SimACommsRangeMessage::writeIncoming( float range, float azimuth, float elevation, const Timestamp& rxTime )
{
    // Log the response and push it to the queue
    RangeResponse* rangeResponse = new RangeResponse( range, azimuth, elevation, rxTime );
    rangeResponses_.push( rangeResponse );
    rxTime_ = rxTime;

    // Update internal state to reflect data received
    setState( ACOMMS_QUERY_RECIEVED );
    numRecieved_ += 1;
}

void SimACommsRangeMessage::readIncoming( float& range, float& azimuth, float& elevation, Timestamp& rxTime )
{
    // First out
    RangeResponse* rangeResponse = rangeResponses_.pop( 0 );

    if( rangeResponse != NULL )
    {
        rangeResponse->read( range, azimuth, elevation, rxTime );
        delete rangeResponse;

        if( rangeResponses_.isEmpty() )
        {
            // Update internal state to mark the read
            if( numRecieved_ < numPings_ )
            {
                setState( ACOMMS_QUERY_SENT );
                return;
            }

            setState( ACOMMS_QUERY_DONE );
        }
    }
}