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

#include "SimACommsRangeGazebo.h"
#include "data/SimSlate.h"
#include "utils/FlexArray.h"

#include "lrauv_gazebo_plugins/lrauv_acoustic_message.pb.h"
#include "lrauv_gazebo_plugins/lrauv_range_bearing_request.pb.h"
#include "lrauv_gazebo_plugins/lrauv_range_bearing_response.pb.h"

/// Constructor
SimACommsRangeGazebo::SimACommsRangeGazebo()
    : rangeMsgDestination_( -1 ),
      initialized_( false )
{}

/// Destructor
SimACommsRangeGazebo::~SimACommsRangeGazebo()
{}

/// Uninitialize the handler
void SimACommsRangeGazebo::uninitialize()
{
    if( initialized_ )
    {
        node_.Unsubscribe( ( topic_ + "responses" ).cStr() );
    }
    initialized_ = false;
}

/// Initialize the handler with vehicle name topics
void SimACommsRangeGazebo::Initialize( const Str& vehicleName )
{
    // Clear any previous subscriptions.
    uninitialize();

    // Set up new advertise/subscription topics
    topic_ = "/" + vehicleName + "/range_bearing/";  // requests/responses

    // Advertising topics now to allow Discovery to connect to subscribers before publish calls are invoked.
    rangePub_ = node_.Advertise<lrauv_gazebo_plugins::msgs::LRAUVRangeBearingRequest>( ( topic_ + "requests" ).cStr() );

    // Subscribe to range responses topic
    if( !node_.Subscribe( ( topic_ + "responses" ).cStr(), &SimACommsRangeGazebo::rangeResponseCallback, this ) )
    {
        printf( "Error subscribing to Gazebo topic %s.\n", topic_.cStr() );
        return;
    }

    initialized_ = true;
}

/// Publish range request to remote contact
bool SimACommsRangeGazebo::PublishRangeMessage( const SimACommsRangeMessage& rangeMessage )
{
    if( !initialized_ )
        Initialize( rangeMessage.source_ );

    rangeMsgDestination_ = rangeMessage.destination_; // TODO: tmp workaround until destination is added to return message

    // Now ship out the range request
    lrauv_gazebo_plugins::msgs::LRAUVRangeBearingRequest msg;
    msg.set_req_id( rangeMessage.msgID_ );
    msg.set_to( rangeMessage.destination_ );

    rangePub_.Publish( msg );
    return true;
}


/// Callback function for Gazebo LRAUVRangeBearingResponse messages
void SimACommsRangeGazebo::rangeResponseCallback( const lrauv_gazebo_plugins::msgs::LRAUVRangeBearingResponse& msg )
{
    // TODO: add source ID (from) to LRAUVRangeBearingResponse message type
    Timestamp rxTime = Timestamp::Now();
    // SimACommsRangeHandler::WriteIncomingMessage( int source, float range, float azimuth, float elevation, const Timestamp& rxTime, int msgId )
    SimACommsRangeHandler::WriteIncomingMessage( rangeMsgDestination_, msg.bearing().x(), msg.bearing().z(), msg.bearing().y(), rxTime, msg.req_id() );
    return;
}