#include "ros_bridge/include/ROSBridge.h"

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

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

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()
{
    _stopped->store(false);
    _rbc.run();
    _topicPublisher.start();
    _topicSubscriber.start();
    _server.start();
}

void ROSBridge::ROSBridge::reset()
{
    auto start = std::chrono::high_resolution_clock::now();
    _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 << "ROSBridge reset duration: " << delta << " ms" << std::endl;
}

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

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


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