#include "TopicSubscriber.h"


ROSBridge::ComPrivate::TopicSubscriber::TopicSubscriber(CasePacker &casePacker,
        RosbridgeWsClient &rbc) :
    _casePacker(casePacker)
  , _rbc(rbc)
  , _stopped(std::make_shared<std::atomic_bool>(true))
{

}

ROSBridge::ComPrivate::TopicSubscriber::~TopicSubscriber()
{
    this->reset();
}

void ROSBridge::ComPrivate::TopicSubscriber::start()
{
    _stopped->store(false);
}

void ROSBridge::ComPrivate::TopicSubscriber::reset()
{
    if ( !_stopped->load() ){
        _stopped->store(true);
        {
            for (auto &item : _topicMap){
                _rbc.unsubscribe(item.second);
                _rbc.removeClient(item.first);
            }
        }
        _topicMap.clear();
    }
}

void ROSBridge::ComPrivate::TopicSubscriber::subscribe(
        const char *topic,
        const std::function<void(JsonDocUPtr)> &callback)
{
    if ( _stopped->load() )
        return;

    std::string clientName = ROSBridge::ComPrivate::_topicSubscriberKey
                           + std::string(topic);
    auto it = _topicMap.find(clientName);
    if ( it != _topicMap.end() ){ // Topic already subscribed?
        return;
    }
    _topicMap.insert(std::make_pair(clientName, std::string(topic)));

    using namespace std::placeholders;
    auto callbackWrapper = [callback](
            std::shared_ptr<WsClient::Connection>,
            std::shared_ptr<WsClient::InMessage> in_message){
                // Parse document.
                JsonDoc docFull;
                docFull.Parse(in_message->string().c_str());
                if ( docFull.HasParseError() ) {
                    std::cout << "Json document has parse error: "
                              << in_message->string()
                              << std::endl;
                    return;
                } else if (!docFull.HasMember("msg")) {
                    std::cout << "Json document does not contain a message (\"msg\"): "
                              << in_message->string()
                              << std::endl;
                    return;
                }

                // Extract message and call callback.
                JsonDocUPtr pDoc(new JsonDoc());
                pDoc->CopyFrom(docFull["msg"].Move(), docFull.GetAllocator());
                callback(std::move(pDoc));
                return;
    };

    if ( !_rbc.topicAvailable(topic) ){
        // Wait until topic is available.
        std::thread t([this, clientName, topic, callbackWrapper]{
            this->_rbc.waitForTopic(topic, this->_stopped);
            if ( !this->_stopped->load() ){
                this->_rbc.addClient(clientName);
                this->_rbc.subscribe(clientName, topic, callbackWrapper);
            }
        });
        t.detach();
    } else {
        _rbc.addClient(clientName);
        _rbc.subscribe(clientName, topic, callbackWrapper);
    }
}


