#include "ros_bridge/include/ROSBridge.h"

ROSBridge::ROSBridge::ROSBridge() :
    _stopped(std::make_shared<std::atomic_bool>(true))
  ,  _casePacker(&_typeFactory, &_jsonFactory)
  , _rbc("localhost:9090")
  , _topicPublisher(_casePacker, _rbc)
  , _topicSubscriber(_casePacker, _rbc)
  , _server(_casePacker, _rbc)
{

}

void ROSBridge::ROSBridge::publish(ROSBridge::ROSBridge::JsonDocUPtr doc)
{
    _topicPublisher.publish(std::move(doc));
}

void ROSBridge::ROSBridge::subscribe(const char *topic, const std::function<void(JsonDocUPtr)> &callBack)
{
    _topicSubscriber.subscribe(topic, callBack);
}

void ROSBridge::ROSBridge::advertiseService(const std::string &service,
                                            const std::string &type,
                                            const std::function<JsonDocUPtr(JsonDocUPtr)> &callback)
{
    _server.advertiseService(service, type, callback);
}

const ROSBridge::CasePacker *ROSBridge::ROSBridge::casePacker() const
{
    return &_casePacker;
}

void ROSBridge::ROSBridge::start()
{
    _topicPublisher.start();
    _topicSubscriber.start();
    _server.start();
    _stopped->store(false);
}

void ROSBridge::ROSBridge::reset()
{
    _topicPublisher.reset();
    _topicSubscriber.reset();
    _server.reset();
    _stopped->store(true);
}

bool ROSBridge::ROSBridge::ping()
{
    return _rbc.connected();
}

bool ROSBridge::ROSBridge::isRunning()
{
    return !_stopped->load();
}