#include "ros_bridge/include/ros_bridge.h"


ros_bridge::ROSBridge::ROSBridge(const char *connectionString) :
    _stopped(std::make_shared<std::atomic_bool>(true))
  , _rbc(connectionString, false /*run*/)
  , _topicPublisher(_rbc)
  , _topicSubscriber(_rbc)
  , _server(_rbc)
{
}

ros_bridge::ROSBridge::ROSBridge() :
    ros_bridge::ROSBridge::ROSBridge("localhost:9090")
{
}

void ros_bridge::ROSBridge::publish(ros_bridge::ROSBridge::JsonDocUPtr doc, const char* topic)
{
    _topicPublisher.publish(std::move(doc), topic);
}

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

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

void ros_bridge::ROSBridge::advertiseTopic(const char *topic, const char *type)
{
    _topicPublisher.advertise(topic, type);
}

void ros_bridge::ROSBridge::start()
{
    if ( !_stopped->load() )
        return;
    _stopped->store(false);
    _rbc.run();
    _topicPublisher.start();
    _topicSubscriber.start();
    _server.start();
}

void ros_bridge::ROSBridge::reset()
{
    auto start = std::chrono::high_resolution_clock::now();
    if ( _stopped->load() )
        return;
    _stopped->store(true);
    _topicPublisher.reset();
    _topicSubscriber.reset();
    _server.reset();
    _rbc.reset();
    auto end = std::chrono::high_resolution_clock::now();
    auto delta = std::chrono::duration_cast<std::chrono::milliseconds>(end-start).count();
    std::cout << "ros_bridge reset duration: " << delta << " ms" << std::endl;
}

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

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


bool ros_bridge::isValidConnectionString(const char *connectionString)
{
    return is_valid_port_path(connectionString);
}