#include "TopicPublisher.h"

#include <unordered_map>

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

}

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

void ROSBridge::ComPrivate::TopicPublisher::start()
{
    if ( !_stopped->load() )  // start called while thread running.
        return;
    _stopped->store(false);
    _pThread = std::make_unique<std::thread>([this]{
        // Init.
        std::unordered_map<std::string, std::string> topicMap;
        // Main Loop.
        while( !this->_stopped->load()){
            std::unique_lock<std::mutex> lk(this->_mutex);
            // Check if new data available, wait if not.
            if (this->_queue.empty()){
                this->_cv.wait(lk); // Wait for condition, spurious wakeups don't matter in this case.
                continue;
            }
            // Pop message from queue.
            JsonDocUPtr pJsonDoc(std::move(this->_queue.front()));
            this->_queue.pop_front();
            lk.unlock();

            // Get tag from Json message and remove it.
            Tag tag;
            bool ret = this->_casePacker.getTag(pJsonDoc, tag);
            assert(ret); // Json message does not contain a tag;
            (void)ret;
            this->_casePacker.removeTag(pJsonDoc);

            // Check if topic must be advertised.
            // Advertised topics are stored in advertisedTopicsHashList as
            // a hash.
            std::string clientName = ROSBridge::ComPrivate::_topicAdvertiserKey
                                   + tag.topic();
            auto it =  topicMap.find(clientName);
            if ( it ==  topicMap.end()) { // Need to advertise topic?
                 topicMap.insert(std::make_pair(clientName, tag.topic()));
                 this->_rbc.addClient(clientName);
                 this->_rbc.advertise(clientName,
                                   tag.topic(),
                                   tag.messageType() );
                 this->_rbc.waitForTopic(tag.topic(), this->_stopped); // Wait until topic is advertised.
            }

            // Publish Json message.
            if ( !this->_stopped->load() )
                this->_rbc.publish(tag.topic(), *pJsonDoc.get());
        } // while loop

        // Tidy up.
        for (auto& it :  topicMap){
            this->_rbc.unadvertise(it.second);
            this->_rbc.removeClient(it.first);
        }
    });
}

void ROSBridge::ComPrivate::TopicPublisher::reset()
{
    if ( _stopped->load() )  // stop called while thread not running.
        return;
    _stopped->store(true);
    _cv.notify_one(); // Wake publisher thread.
    if ( !_pThread )
        return;
    _pThread->join();
    _queue.clear();
}

