#include "rosbridge.h"
#include "rosbridgeimpl.h"

#include <QJsonArray>
#include <QJsonDocument>
#include <QJsonObject>
#include <QThread>

Q_DECLARE_METATYPE(Rosbridge::CallBack)
Q_DECLARE_METATYPE(Rosbridge::CallBackWReturn)

static const char *topicsKey = "topics";
static const char *servicesKey = "services";
static const char *rosServices = "/rosapi/services";
static const char *rosTopics = "/rosapi/topics";

static constexpr auto pollIntervall = std::chrono::milliseconds(200);

Rosbridge::STATE translate(RosbridgeImpl::STATE s);

Rosbridge::Rosbridge(const QUrl url, QObject *parent)
    : QObject(parent), _url(url), _running(false) {
  static std::once_flag flag;
  std::call_once(flag, [] {
    qRegisterMetaType<Rosbridge::CallBack>();
    qRegisterMetaType<Rosbridge::CallBackWReturn>();
  });
}

Rosbridge::~Rosbridge() { stop(); }

void Rosbridge::start() {
  if (!_running) {
    _running = true;
    _t = new QThread();
    _impl = new RosbridgeImpl(_url);
    _impl->moveToThread(_t);

    connect(_impl, &RosbridgeImpl::stateChanged, this,
            &Rosbridge::_onImplStateChanged);

    connect(this, &Rosbridge::_start, _impl, &RosbridgeImpl::start);
    connect(this, &Rosbridge::_stop, _impl, &RosbridgeImpl::stop);

    connect(this, &Rosbridge::_advertiseTopic, _impl,
            &RosbridgeImpl::advertiseTopic);
    connect(this, &Rosbridge::_publish, _impl, &RosbridgeImpl::publish);
    connect(this, &Rosbridge::_unadvertiseTopic, _impl,
            &RosbridgeImpl::unadvertiseTopic);
    connect(this, &Rosbridge::_unadvertiseAllTopics, _impl,
            &RosbridgeImpl::unadvertiseAllTopics);

    connect(this, &Rosbridge::_subscribeTopic, _impl,
            &RosbridgeImpl::subscribeTopic);
    connect(this, &Rosbridge::_unsubscribeTopic, _impl,
            &RosbridgeImpl::unsubscribeTopic);
    connect(this, &Rosbridge::_unsubscribeAllTopics, _impl,
            &RosbridgeImpl::unsubscribeAllTopics);

    connect(this, &Rosbridge::_callService, _impl, &RosbridgeImpl::callService);
    connect(this, &Rosbridge::_advertiseService, _impl,
            &RosbridgeImpl::advertiseService);
    connect(this, &Rosbridge::_unadvertiseService, _impl,
            &RosbridgeImpl::unadvertiseService);
    connect(this, &Rosbridge::_unadvertiseAllServices, _impl,
            &RosbridgeImpl::unadvertiseAllServices);

    _t->start();
    emit _start();
  }
}

void Rosbridge::stop() {
  if (_running) {
    _running = false;
    _impl->deleteLater();
    _t->quit();
    _t->wait();
    _t->deleteLater();
  }
}

Rosbridge::STATE Rosbridge::state() {
  if (_running) {
    return translate(_impl->state());
  } else {
    return STATE::STOPPED;
  }
}

void Rosbridge::waitConnected() {
  while (this->state() != Rosbridge::STATE::CONNECTED) {
    std::this_thread::sleep_for(std::chrono::milliseconds(1));
  }
}

void Rosbridge::advertiseTopic(const QString &topic, const QString &type) {
  emit _advertiseTopic(topic, type);
}

void Rosbridge::publish(const QString &topic, const QJsonObject &msg) {
  emit _publish(topic, msg);
}

void Rosbridge::unadvertiseTopic(const QString &topic) {
  emit _unadvertiseTopic(topic);
}

void Rosbridge::unadvertiseAllTopics() { emit _unadvertiseAllTopics(); }

void Rosbridge::subscribeTopic(const QString &topic,
                               Rosbridge::CallBack callback) {
  emit _subscribeTopic(topic, callback);
}

std::future<bool> Rosbridge::topicAvailable(const QString &topic) {
  auto pPromise = std::make_shared<std::promise<bool>>();
  auto future = pPromise->get_future();
  if (this->state() == STATE::CONNECTED) {
    auto setBool = [pPromise,
                    topic](const QJsonObject &topics) mutable -> void {
      if (topics.contains(topicsKey) && topics[topicsKey].isArray()) {
        QJsonValue v(topic);
        pPromise->set_value(topics[topicsKey].toArray().contains(v));
      } else {
        pPromise->set_value(false);
      }
    };
    emit this->_callService(rosTopics, setBool, QJsonObject());
  } else {
    pPromise->set_value(false);
  }
  return future;
}

std::future<QJsonObject> Rosbridge::getAdvertisedTopics() {
  auto pPromise = std::make_shared<std::promise<QJsonObject>>();
  auto future = pPromise->get_future();
  if (this->state() == STATE::CONNECTED) {
    auto setString = [pPromise](const QJsonObject &topics) mutable {
      pPromise->set_value(topics);
    };
    emit this->_callService(rosTopics, setString, QJsonObject());
  } else {
    pPromise->set_value(QJsonObject());
  }
  return future;
}

void Rosbridge::unsubscribeTopic(const QString &topic) {
  emit _unsubscribeTopic(topic);
}

void Rosbridge::unsubscribeAllTopics() { emit _unsubscribeAllTopics(); }

void Rosbridge::waitForTopic(const QString &topic) {
  if (this->state() == STATE::CONNECTED) {
    std::future<bool> future;
    while (true) {
      future = topicAvailable(topic);
      auto available = future.get();
      if (available) {
        break;
      }
      std::this_thread::sleep_for(pollIntervall);
      if (this->state() != STATE::CONNECTED) {
        qDebug() << "waitForTopic: Rosbridge not connected.";
        break;
      }
    }
  } else {
    qDebug() << "waitForTopic: Rosbridge not connected.";
  }
}

void Rosbridge::advertiseService(const QString &service, const QString &type,
                                 CallBackWReturn callback) {
  emit _advertiseService(service, type, callback);
}

void Rosbridge::callService(const QString &service,
                            Rosbridge::CallBack callback,
                            const QJsonObject &args) {
  emit _callService(service, callback, args);
}

std::future<bool> Rosbridge::serviceAvailable(const QString &service) {
  auto pPromise = std::make_shared<std::promise<bool>>();
  auto future = pPromise->get_future();
  if (this->state() == STATE::CONNECTED) {
    auto setBool = [pPromise,
                    service](const QJsonObject &services) mutable -> void {
      if (services.contains(servicesKey) && services[servicesKey].isArray()) {
        QJsonValue v(service);
        pPromise->set_value(services[servicesKey].toArray().contains(v));
      } else {
        pPromise->set_value(false);
      }
    };
    emit this->_callService(rosServices, setBool, QJsonObject());
  } else {
    pPromise->set_value(false);
  }
  return future;
}

std::future<QJsonObject> Rosbridge::getAdvertisedServices() {
  auto pPromise = std::make_shared<std::promise<QJsonObject>>();
  auto future = pPromise->get_future();
  if (this->state() == STATE::CONNECTED) {
    auto setString = [pPromise](const QJsonObject &services) mutable {
      pPromise->set_value(services);
    };
    emit this->_callService(rosServices, setString, QJsonObject());
  } else {
    pPromise->set_value(QJsonObject());
  }
  return future;
}

void Rosbridge::unadvertiseService(const QString &service) {
  emit _unadvertiseService(service);
}

void Rosbridge::unadvertiseAllServices() { emit _unadvertiseAllServices(); }

void Rosbridge::waitForService(const QString &service) {
  if (this->state() == STATE::CONNECTED) {
    std::future<bool> future;
    while (true) {
      future = serviceAvailable(service);
      auto available = future.get();
      if (available) {
        break;
      }
      std::this_thread::sleep_for(pollIntervall);
      if (this->state() != STATE::CONNECTED) {
        qDebug() << "waitForService: Rosbridge not connected.";
        break;
      }
    }
  } else {
    qDebug() << "waitForService: Rosbridge not connected.";
  }
}

void Rosbridge::_onImplStateChanged() {
  static STATE oldState = STATE::STOPPED;
  auto newState = translate(_impl->state());

  if (oldState != newState) {
    emit stateChanged();
  }
  oldState = newState;
}

Rosbridge::STATE translate(RosbridgeImpl::STATE s) {
  switch (s) {
  case RosbridgeImpl::STATE::STOPPED:
  case RosbridgeImpl::STATE::STOPPING:
    return Rosbridge::STATE::STOPPED;
  case RosbridgeImpl::STATE::STARTING:
    return Rosbridge::STATE::STARTED;
  case RosbridgeImpl::STATE::CONNECTED:
    return Rosbridge::STATE::CONNECTED;
  case RosbridgeImpl::STATE::TIMEOUT:
    return Rosbridge::STATE::TIMEOUT;
    break;
  }
  qCritical() << "unreachable branch!";
  return Rosbridge::STATE::STOPPED;
}