NemoInterface.cpp

#include "NemoInterface.h"

#include "QGCApplication.h"
#include "QGCLoggingCategory.h"
#include "QGCToolbox.h"
#include "SettingsFact.h"
#include "SettingsManager.h"
#include "WimaSettings.h"

#include <shared_mutex>

#include <QTimer>

#include "GenericSingelton.h"
#include "geometry/MeasurementArea.h"
#include "geometry/geometry.h"
#include "nemo_interface/FutureWatcher.h"
#include "nemo_interface/MeasurementTile.h"
#include "nemo_interface/QNemoHeartbeat.h"
#include "nemo_interface/TaskDispatcher.h"

#include "ros_bridge/include/messages/nemo_msgs/progress_array.h"

#include "ros_bridge/include/RosBridgeClient.h"
#include "ros_bridge/include/messages/geographic_msgs/geopoint.h"
#include "ros_bridge/include/messages/nemo_msgs/heartbeat.h"
#include "ros_bridge/include/messages/nemo_msgs/tile.h"
#include "ros_bridge/include/ros_bridge.h"
#include "ros_bridge/rapidjson/include/rapidjson/document.h"
#include "ros_bridge/rapidjson/include/rapidjson/ostreamwrapper.h"
#include "ros_bridge/rapidjson/include/rapidjson/writer.h"

QGC_LOGGING_CATEGORY(NemoInterfaceLog, "NemoInterfaceLog")

#define SYNC_INTERVAL 1000             // ms
#define NO_HEARTBEAT_TIMEOUT 10000     // ms
#define CONNECTION_TIMER_INTERVAL 1000 // ms
static constexpr auto maxResponseTime = std::chrono::milliseconds(10000);

using hrc = std::chrono::high_resolution_clock;
using ROSBridgePtr = std::shared_ptr<RosbridgeWsClient>;

typedef ros_bridge::messages::nemo_msgs::tile::GenericTile<QGeoCoordinate,
                                                           QList>
    Tile;
typedef std::map<std::int64_t, std::shared_ptr<Tile>> TileMap;
typedef std::map<std::int64_t, std::shared_ptr<const Tile>> TileMapConst;
typedef ros_bridge::messages::nemo_msgs::heartbeat::Heartbeat Heartbeat;
typedef nemo_interface::TaskDispatcher Dispatcher;
typedef nemo_interface::FutureWatcher<QVariant, std::shared_future>
    FutureWatcher;

class NemoInterface::Impl {
  enum class STATE {
    STOPPED,
    START_BRIDGE,
    WEBSOCKET_DETECTED,
    TRY_TOPIC_SERVICE_SETUP,
    SYNC_USER,
    SYNC_SYS,
    READY,
    WEBSOCKET_TIMEOUT,
    HEARTBEAT_TIMEOUT
  };

public:
  Impl(NemoInterface *p);
  ~Impl();

  void start();
  void stop();

  // Tile editing.
  // 	Functions that require communication to device.
  std::shared_future<QVariant> addTiles(const TilePtrArray &tileArray);
  std::shared_future<QVariant> removeTiles(const IDArray &idArray);
  std::shared_future<QVariant> clearTiles();

  // 	Functions that don't require communication to device.
  TileArray getTiles(const IDArray &idArray);
  TileArray getAllTiles();
  LogicalArray containsTiles(const IDArray &idArray);
  std::size_t size();
  bool empty();

  // Progress.
  ProgressArray getProgress();
  ProgressArray getProgress(const IDArray &idArray);

  NemoInterface::STATUS status();
  bool running(); // thread safe
  bool ready();   // thread safe

  const QString &infoString();
  const QString &warningString();

private:
  void _doTopicServiceSetup();
  void _doAction();
  void _trySynchronize();
  bool _isSynchronized();
  bool _userSync();                // thread safe
  bool _sysSync();                 // thread safe
  void _onFutureWatcherFinished(); // thread safe

  // called from dispatcher thread!
  QVariant _callAddTiles(
      std::shared_ptr<QVector<std::shared_ptr<const Tile>>> pTileArray);
  // called from dispatcher thread!
  QVariant _callRemoveTiles(std::shared_ptr<IDArray> pIdArray);
  // called from dispatcher thread!
  QVariant _callClearTiles();
  // called from dispatcher thread!
  QVariant _callGetProgress(std::shared_ptr<IDArray> pIdArray);

  void _addTilesRemote(
      std::shared_ptr<QVector<std::shared_ptr<const Tile>>> pTileArray);
  void
  _addTilesRemote(std::shared_ptr<QVector<std::shared_ptr<Tile>>> pTileArray);
  void _removeTilesRemote(std::shared_ptr<IDArray> idArray);
  void _clearTilesRemote();
  void _updateProgress(std::shared_ptr<ProgressArray> pArray);
  void _onHeartbeatReceived(const QNemoHeartbeat &hb);
  void _setInfoString(const QString &info);
  void _setWarningString(const QString &warning);

  bool _setState(STATE newState); // not thread safe
  static bool _ready(STATE s);
  static bool _userSync(STATE s);
  static bool _sysSync(STATE s);
  static bool _running(STATE s);
  static NemoInterface::STATUS _status(STATE state);
  static QString _toString(STATE s);
  static QString _toString(NemoInterface::STATUS s);

  std::atomic<STATE> _state;
  ROSBridgePtr _pRosBridge;
  TileMap _remoteTiles;
  TileMapConst _localTiles;
  NemoInterface *const _parent;
  Dispatcher _dispatcher;
  QString _infoString;
  QString _warningString;
  QTimer _timeoutTimer;
  QTimer _connectionTimer;
  QNemoHeartbeat _lastHeartbeat;
  FutureWatcher _futureWatcher;
};

using StatusMap = std::map<NemoInterface::STATUS, QString>;
static StatusMap statusMap{
    std::make_pair<NemoInterface::STATUS, QString>(
        NemoInterface::STATUS::NOT_CONNECTED, "Not Connected"),
    std::make_pair<NemoInterface::STATUS, QString>(NemoInterface::STATUS::SYNC,
                                                   "Synchronizing"),
    std::make_pair<NemoInterface::STATUS, QString>(NemoInterface::STATUS::READY,
                                                   "Ready"),
    std::make_pair<NemoInterface::STATUS, QString>(
        NemoInterface::STATUS::TIMEOUT, "Timeout"),
    std::make_pair<NemoInterface::STATUS, QString>(
        NemoInterface::STATUS::WEBSOCKET_DETECTED, "Websocket Detected")};

NemoInterface::Impl::Impl(NemoInterface *p)
    : _state(STATE::STOPPED), _parent(p) {

  // ROS Bridge.
  WimaSettings *wimaSettings =
      qgcApp()->toolbox()->settingsManager()->wimaSettings();
  auto connectionStringFact = wimaSettings->rosbridgeConnectionString();
  auto setConnectionString = [connectionStringFact, this] {
    auto connectionString = connectionStringFact->rawValue().toString();
    if (is_valid_port_path(connectionString.toLocal8Bit().data())) {
    } else {
      qgcApp()->warningMessageBoxOnMainThread(
          "Nemo Interface",
          "Websocket connection string possibly invalid: " + connectionString +
              ". Trying to connect anyways.");
    }

    if (this->_pRosBridge) {
      this->_pRosBridge->reset();
    }
    this->_pRosBridge = std::make_shared<RosbridgeWsClient>(
        connectionString.toLocal8Bit().data());
    this->_pRosBridge->reset();
  };
  connect(connectionStringFact, &SettingsFact::rawValueChanged,
          setConnectionString);
  setConnectionString();

  // Heartbeat timeout.
  connect(&this->_timeoutTimer, &QTimer::timeout, [this] {
    this->_setState(STATE::HEARTBEAT_TIMEOUT);
    this->_doAction();
  });

  // Connection timer (temporary workaround)
  connect(&this->_connectionTimer, &QTimer::timeout, [this] {
    if (this->_pRosBridge->connected()) {
      if (this->_state == STATE::START_BRIDGE ||
          this->_state == STATE::WEBSOCKET_TIMEOUT) {
        this->_setState(STATE::WEBSOCKET_DETECTED);
        this->_doAction();
      }
    } else {
      if (this->_state == STATE::TRY_TOPIC_SERVICE_SETUP ||
          this->_state == STATE::READY ||
          this->_state == STATE::WEBSOCKET_DETECTED ||
          this->_state == STATE::HEARTBEAT_TIMEOUT) {
        this->_setState(STATE::WEBSOCKET_TIMEOUT);
        this->_doAction();
      }
    }
  });

  connect(&this->_futureWatcher, &FutureWatcher::finished,
          [this] { this->_onFutureWatcherFinished(); });
}

NemoInterface::Impl::~Impl() {}

void NemoInterface::Impl::start() {
  if (!running()) {
    this->_setState(STATE::START_BRIDGE);
    this->_doAction();
  }
}

void NemoInterface::Impl::stop() {
  if (running()) {
    this->_setState(STATE::STOPPED);
    this->_doAction();
  }
}

std::shared_future<QVariant>
NemoInterface::Impl::addTiles(const TilePtrArray &tileArray) {
  using namespace nemo_interface;

  if (tileArray.size() > 0) {

    // copy unknown tiles
    auto pTileArray = std::make_shared<QVector<std::shared_ptr<const Tile>>>();
    for (const auto *pTile : tileArray) {
      auto id = pTile->id();
      const auto it = this->_localTiles.find(id);
      Q_ASSERT(it == _localTiles.end());
      if (Q_LIKELY(it == _localTiles.end())) {
        auto pTileCopy =
            std::make_shared<const Tile>(pTile->coordinateList(), 0.0, id);
        _localTiles.insert(std::make_pair(id, pTileCopy));
        pTileArray->push_back(pTileCopy);
      } else {
        qCDebug(NemoInterfaceLog)
            << "addTiles(): tile with id: " << pTile->id() << "already added.";
      }
    }

    // ready for send?
    if (pTileArray->size() > 0 && (this->ready() || this->_userSync())) {

      this->_setState(STATE::SYNC_USER);
      this->_doAction();

      // create command.
      auto sendTilesCommand = std::make_unique<Task>(
          std::bind(&Impl::_callAddTiles, this, pTileArray));

      // dispatch command and return.
      auto ret = _dispatcher.dispatch(std::move(sendTilesCommand));
      auto sfut = ret.share();
      _futureWatcher.setFuture(sfut);
      return sfut;
    }
  }

  std::promise<QVariant> p;
  p.set_value(QVariant(false));
  return p.get_future();
}

std::shared_future<QVariant>
NemoInterface::Impl::removeTiles(const IDArray &idArray) {
  using namespace nemo_interface;

  if (idArray.size() > 0) {

    // copy known ids
    auto pIdArray = std::make_shared<IDArray>();
    for (const auto id : idArray) {
      const auto it = this->_localTiles.find(id);
      Q_ASSERT(it != _localTiles.end());
      if (Q_LIKELY(it != _localTiles.end())) {
        _localTiles.erase(it);
        pIdArray->push_back(id);
      } else {
        qCDebug(NemoInterfaceLog) << "removeTiles(): unknown id: " << id << ".";
      }
    }

    // ready for send?
    if (pIdArray->size() > 0 && (this->ready() || this->_userSync())) {

      this->_setState(STATE::SYNC_USER);
      this->_doAction();

      // create command.
      auto cmd = std::make_unique<Task>(
          std::bind(&Impl::_callRemoveTiles, this, pIdArray));

      // dispatch command and return.
      auto ret = _dispatcher.dispatch(std::move(cmd));
      auto sfut = ret.share();
      _futureWatcher.setFuture(sfut);
      return sfut;
    }
  }

  std::promise<QVariant> p;
  p.set_value(QVariant(false));
  return p.get_future();
}

std::shared_future<QVariant> NemoInterface::Impl::clearTiles() {
  using namespace nemo_interface;

  // clear local tiles (_localTiles)
  this->_localTiles.clear();

  if (this->_localTiles.size() > 0 && (this->ready() || this->_userSync())) {

    this->_setState(STATE::SYNC_USER);
    this->_doAction();

    // create command.
    auto pTask =
        std::make_unique<Task>(std::bind(&Impl::_callClearTiles, this));

    // dispatch command and return.
    auto ret = _dispatcher.dispatch(std::move(pTask));
    auto sfut = ret.share();
    _futureWatcher.setFuture(sfut);
    return sfut;
  } else {
    std::promise<QVariant> p;
    p.set_value(QVariant(false));
    return p.get_future();
  }
}

TileArray NemoInterface::Impl::getTiles(const IDArray &idArray) {
  TileArray tileArray;

  for (const auto &id : idArray) {
    const auto it = _localTiles.find(id);
    if (it != _localTiles.end()) {
      MeasurementTile copy;
      copy.setId(it->second->id());
      copy.setProgress(it->second->progress());
      copy.setPath(it->second->tile());
      tileArray.append(std::move(copy));
    }
  }

  return tileArray;
}

TileArray NemoInterface::Impl::getAllTiles() {
  TileArray tileArray;

  for (const auto &entry : _localTiles) {
    auto pTile = entry.second;
    MeasurementTile copy;
    copy.setId(pTile->id());
    copy.setProgress(pTile->progress());
    copy.setPath(pTile->tile());
    tileArray.append(std::move(copy));
  }

  return tileArray;
}

LogicalArray NemoInterface::Impl::containsTiles(const IDArray &idArray) {
  LogicalArray logicalArray;

  for (const auto &id : idArray) {
    const auto &it = _localTiles.find(id);
    logicalArray.append(it != _localTiles.end());
  }

  return logicalArray;
}

std::size_t NemoInterface::Impl::size() { return _localTiles.size(); }

bool NemoInterface::Impl::empty() { return _localTiles.empty(); }

ProgressArray NemoInterface::Impl::getProgress() {
  ProgressArray progressArray;

  if (this->_isSynchronized()) {
    for (const auto &entry : _remoteTiles) {
      progressArray.append(
          LabeledProgress{entry.second->progress(), entry.second->id()});
    }
  } else {
    for (const auto &entry : _localTiles) {
      progressArray.append(
          LabeledProgress{entry.second->progress(), entry.second->id()});
    }
  }

  return progressArray;
}

ProgressArray NemoInterface::Impl::getProgress(const IDArray &idArray) {
  ProgressArray progressArray;

  if (this->_isSynchronized()) {
    for (const auto &id : idArray) {
      const auto it = _remoteTiles.find(id);
      if (it != _remoteTiles.end()) {
        progressArray.append(
            LabeledProgress{it->second->progress(), it->second->id()});
      }
    }
  } else {
    for (const auto &id : idArray) {
      const auto it = _localTiles.find(id);
      if (it != _localTiles.end()) {
        progressArray.append(
            LabeledProgress{it->second->progress(), it->second->id()});
      }
    }
  }

  return progressArray;
}

NemoInterface::STATUS NemoInterface::Impl::status() {
  return _status(this->_state);
}

bool NemoInterface::Impl::running() { return _running(this->_state); }

bool NemoInterface::Impl::ready() { return _ready(this->_state.load()); }

bool NemoInterface::Impl::_sysSync() { return _sysSync(this->_state); }

void NemoInterface::Impl::_onFutureWatcherFinished() {
  auto lastTransactionSuccessfull = _futureWatcher.result().toBool();
  if (!lastTransactionSuccessfull) {
    if (this->_userSync()) {
      _trySynchronize();
    } else if (this->_sysSync()) {
      QTimer::singleShot(1000, [this] { this->_trySynchronize(); });
    }
  } else {
    // fetch progress

    auto pIdArray = std::make_shared<IDArray>();
    for (const auto &pair : _remoteTiles) {
      pIdArray->push_back(pair.first);
    }
    auto pTask = std::make_unique<nemo_interface::Task>(
        std::bind(&Impl::_callGetProgress, this, pIdArray));

    // dispatch command.
    auto ret = _dispatcher.dispatch(std::move(pTask));
    Q_ASSERT(false);
    _futureWatcher.setFuture(ret.share());
  }
}

bool NemoInterface::Impl::_userSync() { return _userSync(this->_state); }

const QString &NemoInterface::Impl::infoString() { return _infoString; }

const QString &NemoInterface::Impl::warningString() { return _warningString; }

void NemoInterface::Impl::_updateProgress(
    std::shared_ptr<ProgressArray> pArray) {
  for (auto itLP = pArray->begin(); itLP != pArray->end();) {

    auto it = _remoteTiles.find(itLP->id());

    if (Q_LIKELY(it != _remoteTiles.end())) {
      it->second->setProgress(itLP->progress());
      ++itLP;
    } else {
      qCDebug(NemoInterfaceLog)
          << "_updateProgress(): tile with id " << itLP->id() << " not found.";
      itLP = pArray->erase(itLP);
    }
  }

  if (pArray->size() > 0) {
    emit _parent->progressChanged(*pArray);
  }
}

void NemoInterface::Impl::_onHeartbeatReceived(const QNemoHeartbeat &hb) {
  _lastHeartbeat = hb;
  this->_timeoutTimer.start(NO_HEARTBEAT_TIMEOUT);
  if (this->_state == STATE::TRY_TOPIC_SERVICE_SETUP) {
    this->_setState(STATE::READY);
    this->_doAction();
  } else if (this->_state == STATE::HEARTBEAT_TIMEOUT) {
    this->_setState(STATE::READY);
    this->_doAction();
  }
}

void NemoInterface::Impl::_setInfoString(const QString &info) {
  if (_infoString != info) {
    _infoString = info;
    emit this->_parent->infoStringChanged();
  }
}

void NemoInterface::Impl::_setWarningString(const QString &warning) {
  if (_warningString != warning) {
    _warningString = warning;
    emit this->_parent->warningStringChanged();
  }
}

void NemoInterface::Impl::_doTopicServiceSetup() {
  using namespace ros_bridge::messages;

  // Subscribe nemo progress.
  const char *progressClient = "client:/nemo/progress";
  this->_pRosBridge->addClient(progressClient);
  this->_pRosBridge->subscribe(
      progressClient, "/nemo/progress",
      [this](std::shared_ptr<WsClient::Connection>,
             std::shared_ptr<WsClient::InMessage> in_message) {
        auto msg = in_message->string();

        // parse in_message
        rapidjson::Document d;
        d.Parse(msg.c_str());
        if (!d.HasParseError()) {
          if (d.HasMember("msg") && d["msg"].IsObject()) {

            // create obj from json
            nemo_msgs::progress_array::ProgressArray progressArray;
            if (nemo_msgs::progress_array::fromJson(d["msg"], progressArray)) {

              // correct range errors of progress
              for (auto &lp : progressArray.progress_array()) {
                bool rangeError = false;
                if (lp.progress() < 0) {
                  lp.setProgress(0);
                  rangeError = true;
                }
                if (lp.progress() > 100) {
                  lp.setProgress(100);
                  rangeError = true;
                }

                if (rangeError) {
                  qCWarning(NemoInterfaceLog) << "/nemo/progress progress out "
                                                 "of range, value was set to: "
                                              << lp.progress();
                }
              }

              auto p = std::make_shared<ProgressArray>();
              *p = std::move(progressArray.progress_array());
              QMetaObject::invokeMethod(
                  this->_parent,
                  std::bind(&NemoInterface::Impl::_updateProgress, this, p));
            } else {
              qCWarning(NemoInterfaceLog) << "/nemo/progress not able to "
                                             "create ProgressArray form json: "
                                          << msg.c_str();
            }
          } else {
            qCWarning(NemoInterfaceLog)
                << "/nemo/progress no \"msg\" key or wrong type: "
                << msg.c_str();
          }
        } else {
          qCWarning(NemoInterfaceLog)
              << "/nemo/progress message parse error (" << d.GetParseError()
              << "): " << msg.c_str();
        }
      });
  // Subscribe heartbeat msg.
  const char *heartbeatClient = "client:/nemo/heartbeat";
  this->_pRosBridge->addClient(heartbeatClient);
  this->_pRosBridge->subscribe(
      heartbeatClient, "/nemo/heartbeat",
      [this](std::shared_ptr<WsClient::Connection>,
             std::shared_ptr<WsClient::InMessage> in_message) {
        auto msg = in_message->string();

        // parse in_message
        rapidjson::Document d;
        d.Parse(msg.c_str());
        if (!d.HasParseError()) {
          if (d.HasMember("msg") && d["msg"].IsObject()) {

            // create obj from json
            nemo_msgs::heartbeat::Heartbeat heartbeat;
            if (nemo_msgs::heartbeat::fromJson(d["msg"], heartbeat)) {
              QMetaObject::invokeMethod(
                  this->_parent,
                  std::bind(&NemoInterface::Impl::_onHeartbeatReceived, this,
                            heartbeat));
            } else {
              qCWarning(NemoInterfaceLog) << "/nemo/heartbeat not able to "
                                             "create Heartbeat form json: "
                                          << msg.c_str();
            }
          } else {
            qCWarning(NemoInterfaceLog)
                << "/nemo/heartbeat no \"msg\" key or wrong type: "
                << msg.c_str();
          }
        } else {
          qCWarning(NemoInterfaceLog)
              << "/nemo/heartbeat message parse error (" << d.GetParseError()
              << "): " << msg.c_str();
        }
      });
}

void NemoInterface::Impl::_trySynchronize() {
  if (!_isSynchronized()) {
    this->_setState(STATE::SYNC_SYS);
    this->_doAction();

    // create clear cmd.
    auto pTask = std::make_unique<nemo_interface::Task>(
        std::bind(&Impl::_callClearTiles, this));

    // dispatch command.
    qCritical() << "this assert is triggered sometimes! sdf92894";
    Q_ASSERT(_dispatcher.pendingTasks() == 0);
    auto ret = _dispatcher.dispatch(std::move(pTask));

    // create tile array.
    auto pTileArray = std::make_shared<QVector<std::shared_ptr<const Tile>>>();
    for (auto pair : _localTiles) {
      pTileArray->push_back(pair.second);
    }

    // create addTiles cmd.
    auto sendTilesCommand = std::make_unique<nemo_interface::Task>(
        std::bind(&Impl::_callAddTiles, this, pTileArray));

    // dispatch command.
    ret = _dispatcher.dispatch(std::move(sendTilesCommand));
    _futureWatcher.setFuture(ret.share());
  }
}

bool NemoInterface::Impl::_isSynchronized() {
  return _localTiles.size() > 0 && _remoteTiles.size() > 0 &&
         std::equal(
             _localTiles.begin(), _localTiles.end(), _remoteTiles.begin(),
             [](const auto &a, const auto &b) { return a.first == b.first; });
}

void NemoInterface::Impl::_doAction() {
  static bool resetDone = false;
  switch (this->_state) {
  case STATE::STOPPED:
    this->_connectionTimer.stop();
    this->_timeoutTimer.stop();
    this->_clearTilesRemote();
    if (this->_pRosBridge->running()) {
      this->_pRosBridge->reset();
    }
    break;

  case STATE::START_BRIDGE:
    this->_pRosBridge->run();
    this->_connectionTimer.start(CONNECTION_TIMER_INTERVAL);
    break;
    break;
  case STATE::WEBSOCKET_DETECTED:
    resetDone = false;
    this->_setState(STATE::TRY_TOPIC_SERVICE_SETUP);
    this->_doAction();
    break;
  case STATE::TRY_TOPIC_SERVICE_SETUP:
    this->_doTopicServiceSetup();
    this->_timeoutTimer.start(NO_HEARTBEAT_TIMEOUT);
    break;
  case STATE::READY:
    _trySynchronize();
    break;
  case STATE::SYNC_USER:
  case STATE::SYNC_SYS:
    break;
  case STATE::HEARTBEAT_TIMEOUT:
    this->_clearTilesRemote();
    break;
  case STATE::WEBSOCKET_TIMEOUT:
    if (!resetDone) {
      resetDone = true;
      this->_pRosBridge->reset();
      this->_pRosBridge->run();
    }
    this->_clearTilesRemote();
    break;
  };
}

QVariant NemoInterface::Impl::_callAddTiles(

    std::shared_ptr<QVector<std::shared_ptr<const Tile>>> pTileArray) {
  // create json object
  rapidjson::Document request(rapidjson::kObjectType);
  auto &allocator = request.GetAllocator();
  rapidjson::Value jsonTileArray(rapidjson::kArrayType);
  for (const auto &tile : *pTileArray) {
    using namespace ros_bridge::messages;
    rapidjson::Value jsonTile(rapidjson::kObjectType);
    if (!nemo_msgs::tile::toJson(*tile, jsonTile, allocator)) {
      qCDebug(NemoInterfaceLog)
          << "addTiles(): not able to create json object: tile id: "
          << tile->id() << " progress: " << tile->progress()
          << " points: " << tile->tile();
    }
    jsonTileArray.PushBack(jsonTile, allocator);
  } // for
  rapidjson::Value tileKey("in_tile_array");
  request.AddMember(tileKey, jsonTileArray, allocator);

  // create response handler.
  auto promise_response = std::make_shared<std::promise<bool>>();
  auto future_response = promise_response->get_future();
  auto responseHandler =
      [promise_response](
          std::shared_ptr<WsClient::Connection> connection,
          std::shared_ptr<WsClient::InMessage> in_message) mutable {
        // check if transaction was successfull
        auto msg = in_message->string();
        rapidjson::Document d;
        d.Parse(msg.c_str());
        if (!d.HasParseError()) {
          if (d.HasMember("values") && d["values"].IsObject()) {
            auto values = d["values"].GetObject();
            if (values.HasMember("success") && values["success"].IsBool()) {
              promise_response->set_value(values["success"].GetBool());
            } else {
              qCWarning(NemoInterfaceLog)
                  << "/nemo/add_tiles no \"success\" key or wrong type: "
                  << msg.c_str();
              promise_response->set_value(false);
            }
          } else {
            qCWarning(NemoInterfaceLog)
                << "/nemo/add_tiles no \"values\" key or wrong type: "
                << msg.c_str();
            promise_response->set_value(false);
          }
        } else {
          qCWarning(NemoInterfaceLog)
              << "/nemo/add_tiles message parse error (" << d.GetParseError()
              << "): " << msg.c_str();
          promise_response->set_value(false);
        }
        connection->send_close(1000);
      };

  // call service.
  this->_pRosBridge->callService("/nemo/add_tiles", responseHandler, request);

  // wait for response.
  auto tStart = hrc::now();
  bool abort = true;
  do {
    auto status = future_response.wait_for(std::chrono::milliseconds(100));
    if (status == std::future_status::ready) {
      abort = false;
      break;
    }
  } while (hrc::now() - tStart < maxResponseTime ||
           this->_dispatcher.isInterruptionRequested());

  if (abort) {
    qCWarning(NemoInterfaceLog)
        << "addTiles(): Websocket not responding to request.";
    return QVariant(false);
  }

  // transaction error?
  if (!future_response.get()) {
    return QVariant(false);
  }

  // add remote tiles (_remoteTiles)
  QMetaObject::invokeMethod(this->_parent /* context */, [this, pTileArray] {
    this->_addTilesRemote(pTileArray);
  });

  // return success
  return QVariant(true);
}

QVariant
NemoInterface::Impl::_callRemoveTiles(std::shared_ptr<IDArray> pIdArray) {
  // create json object
  rapidjson::Document request(rapidjson::kObjectType);
  auto &allocator = request.GetAllocator();
  rapidjson::Value jsonIdArray(rapidjson::kArrayType);
  for (const auto id : *pIdArray) {
    using namespace ros_bridge::messages;
    jsonIdArray.PushBack(rapidjson::Value(id), allocator);
  } // for
  rapidjson::Value tileKey("ids");
  request.AddMember(tileKey, jsonIdArray, allocator);

  // create response handler.
  auto promise_response = std::make_shared<std::promise<bool>>();
  auto future_response = promise_response->get_future();
  auto responseHandler =
      [promise_response](
          std::shared_ptr<WsClient::Connection> connection,
          std::shared_ptr<WsClient::InMessage> in_message) mutable {
        // check if transaction was successfull
        auto msg = in_message->string();
        rapidjson::Document d;
        d.Parse(msg.c_str());
        if (!d.HasParseError()) {
          if (d.HasMember("values") && d["values"].IsObject()) {
            auto values = d["values"].GetObject();
            if (values.HasMember("success") && values["success"].IsBool()) {
              promise_response->set_value(values["success"].GetBool());
            } else {
              qCWarning(NemoInterfaceLog)
                  << "/nemo/remove_tiles no \"success\" key or wrong type: "
                  << msg.c_str();
              promise_response->set_value(false);
            }
          } else {
            qCWarning(NemoInterfaceLog)
                << "/nemo/remove_tiles no \"values\" key or wrong type: "
                << msg.c_str();
            promise_response->set_value(false);
          }
        } else {
          qCWarning(NemoInterfaceLog)
              << "/nemo/remove_tiles message parse error (" << d.GetParseError()
              << "): " << msg.c_str();
          promise_response->set_value(false);
        }
        connection->send_close(1000);
      };

  // call service.
  this->_pRosBridge->callService("/nemo/remove_tiles", responseHandler,
                                 request);

  // wait for response.
  auto tStart = hrc::now();
  bool abort = true;
  do {
    auto status = future_response.wait_for(std::chrono::milliseconds(100));
    if (status == std::future_status::ready) {
      abort = false;
      break;
    }
  } while (hrc::now() - tStart < maxResponseTime ||
           this->_dispatcher.isInterruptionRequested());

  if (abort) {
    qCWarning(NemoInterfaceLog)
        << "remove_tiles(): Websocket not responding to request.";
    return QVariant(false);
  }

  // transaction error?
  if (!future_response.get()) {
    return QVariant(false);
  }

  // remove remote tiles (_remoteTiles)
  QMetaObject::invokeMethod(this->_parent /* context */, [this, pIdArray] {
    this->_removeTilesRemote(pIdArray);
  });

  // return success
  return QVariant(true);
}

QVariant NemoInterface::Impl::_callClearTiles() {
  // create response handler.
  auto promise_response = std::make_shared<std::promise<bool>>();
  auto future_response = promise_response->get_future();
  auto responseHandler =
      [promise_response](
          std::shared_ptr<WsClient::Connection> connection,
          std::shared_ptr<WsClient::InMessage> in_message) mutable {
        // check if transaction was successfull
        auto msg = in_message->string();
        rapidjson::Document d;
        d.Parse(msg.c_str());
        if (!d.HasParseError()) {
          if (d.HasMember("result") && d["result"].IsBool()) {
            promise_response->set_value(d["result"].GetBool());
          } else {
            qCWarning(NemoInterfaceLog)
                << "/nemo/clear_tiles no \"result\" key or wrong type: "
                << msg.c_str();
            promise_response->set_value(false);
          }
        } else {
          qCWarning(NemoInterfaceLog)
              << "/nemo/clear_tiles message parse error (" << d.GetParseError()
              << "): " << msg.c_str();
          promise_response->set_value(false);
        }
        connection->send_close(1000);
      };

  // call service.
  this->_pRosBridge->callService("/nemo/clear_tiles", responseHandler,
                                 rapidjson::Document(rapidjson::kObjectType));

  // wait for response.
  auto tStart = hrc::now();
  bool abort = true;
  do {
    auto status = future_response.wait_for(std::chrono::milliseconds(100));
    if (status == std::future_status::ready) {
      abort = false;
      break;
    }
  } while (hrc::now() - tStart < maxResponseTime ||
           this->_dispatcher.isInterruptionRequested());

  if (abort) {
    qCWarning(NemoInterfaceLog) << "Websocket not responding to request.";
    return QVariant(false);
  }

  // transaction failed?
  if (!future_response.get()) {
    return QVariant(false);
  }

  // clear remote tiles (_remoteTiles)
  QMetaObject::invokeMethod(this->_parent,
                            std::bind(&Impl::_clearTilesRemote, this));

  // return success
  return QVariant(true);
}

QVariant
NemoInterface::Impl::_callGetProgress(std::shared_ptr<IDArray> pIdArray) {
  // create json object
  rapidjson::Document request(rapidjson::kObjectType);
  auto &allocator = request.GetAllocator();
  rapidjson::Value jsonIdArray(rapidjson::kArrayType);
  for (const auto id : *pIdArray) {
    using namespace ros_bridge::messages;
    jsonIdArray.PushBack(rapidjson::Value(id), allocator);
  } // for
  rapidjson::Value tileKey("ids");
  request.AddMember(tileKey, jsonIdArray, allocator);

  // create response handler.
  typedef std::shared_ptr<ProgressArray> ResponseType;
  auto promise_response = std::make_shared<std::promise<ResponseType>>();
  auto future_response = promise_response->get_future();
  auto responseHandler =
      [promise_response](
          std::shared_ptr<WsClient::Connection> connection,
          std::shared_ptr<WsClient::InMessage> in_message) mutable {
        // check if transaction was successfull
        auto msg = in_message->string();
        rapidjson::Document d;
        d.Parse(msg.c_str());
        if (!d.HasParseError()) {
          if (d.HasMember("values") && d["values"].IsObject()) {
            auto values = d["values"].GetObject();
            ros_bridge::messages::nemo_msgs::progress_array::ProgressArray
                progressArrayMsg;
            if (ros_bridge::messages::nemo_msgs::progress_array::fromJson(
                    d["values"], progressArrayMsg)) {
              auto pArray = std::make_shared<ProgressArray>();
              *pArray = std::move(progressArrayMsg.progress_array());
              promise_response->set_value(pArray);
            } else {
              qCWarning(NemoInterfaceLog)
                  << "/nemo/get_progress error while creating ProgressArray "
                     "from json.";
              promise_response->set_value(nullptr);
            }
          } else {
            qCWarning(NemoInterfaceLog)
                << "/nemo/get_progress no \"values\" key or wrong type: "
                << msg.c_str();
            promise_response->set_value(nullptr);
          }
        } else {
          qCWarning(NemoInterfaceLog)
              << "/nemo/get_progress message parse error (" << d.GetParseError()
              << "): " << msg.c_str();
          promise_response->set_value(nullptr);
        }
        connection->send_close(1000);
      };

  // call service.
  this->_pRosBridge->callService("/nemo/get_progress", responseHandler,
                                 request);

  // wait for response.
  auto tStart = hrc::now();
  bool abort = true;
  do {
    auto status = future_response.wait_for(std::chrono::milliseconds(100));
    if (status == std::future_status::ready) {
      abort = false;
      break;
    }
  } while (hrc::now() - tStart < maxResponseTime ||
           this->_dispatcher.isInterruptionRequested());

  if (abort) {
    qCWarning(NemoInterfaceLog)
        << "remove_tiles(): Websocket not responding to request.";
    return QVariant(false);
  }

  // transaction error?
  auto pArray = future_response.get();
  if (pArray == nullptr) {
    return QVariant(false);
  }

  // remove remote tiles (_remoteTiles)
  QMetaObject::invokeMethod(this->_parent /* context */,
                            [this, pArray] { this->_updateProgress(pArray); });

  // return success
  return QVariant(true);
}

void NemoInterface::Impl::_addTilesRemote(
    std::shared_ptr<QVector<std::shared_ptr<const Tile>>> pTileArray) {
  auto pArrayDup = std::make_shared<QVector<std::shared_ptr<Tile>>>();
  for (auto pTile : *pTileArray) {
    pArrayDup->push_back(std::make_shared<Tile>(*pTile));
  }
  _addTilesRemote(pArrayDup);
}

void NemoInterface::Impl::_addTilesRemote(
    std::shared_ptr<QVector<std::shared_ptr<Tile>>> pTileArray) {

  bool anyChange = false;

  for (auto pTile : *pTileArray) {
    auto id = pTile->id();
    auto it = _remoteTiles.find(id);
    qCritical() << "this assert is triggered sometimes! 1212341242";
    Q_ASSERT(it == _remoteTiles.end());
    if (Q_LIKELY(it == _remoteTiles.end())) {
      auto ret = _remoteTiles.insert(std::make_pair(id, pTile));
      Q_ASSERT(ret.second == true);
      Q_UNUSED(ret);
      anyChange = true;
    } else {
      qCWarning(NemoInterfaceLog)
          << "_addTilesRemote: tile with id " << id << " already added.";
    }
  }

  if (anyChange) {
    if (this->_isSynchronized()) {
      this->_setState(STATE::READY);
      this->_doAction();
    }
    this->_parent->tilesChanged();
  }
}

void NemoInterface::Impl::_removeTilesRemote(std::shared_ptr<IDArray> idArray) {
  bool anyChange = false;

  for (const auto id : *idArray) {
    auto it = _remoteTiles.find(id);
    Q_ASSERT(it != _remoteTiles.end());
    if (Q_LIKELY(it != _remoteTiles.end())) {
      _remoteTiles.erase(it);
      anyChange = true;
    } else {
      qCWarning(NemoInterfaceLog)
          << "_removeTilesRemote: tile with unknown id " << id << ".";
    }
  }

  if (anyChange) {
    if (this->_isSynchronized()) {
      this->_setState(STATE::READY);
      this->_doAction();
    }
    this->_parent->tilesChanged();
  }
}

void NemoInterface::Impl::_clearTilesRemote() {
  if (_remoteTiles.size() > 0) {
    _remoteTiles.clear();
    if (this->_isSynchronized()) {
      this->_setState(STATE::READY);
      this->_doAction();
    }
    this->_parent->tilesChanged();
  }
}

bool NemoInterface::Impl::_setState(STATE newState) {
  if (newState != this->_state) {
    auto oldState = this->_state.load();
    this->_state = newState;

    qCDebug(NemoInterfaceLog)
        << "state: " << _toString(oldState) << " -> " << _toString(newState);
    auto oldStatus = _status(oldState);
    auto newStatus = _status(newState);
    if (oldStatus != newStatus) {
      qCDebug(NemoInterfaceLog) << "status: " << _toString(oldStatus) << " -> "
                                << _toString(newStatus);
      emit this->_parent->statusChanged();
    }

    if (_running(oldState) != _running(newState)) {
      emit this->_parent->runningChanged();
    }

    return true;
  } else {
    return false;
  }
}

bool NemoInterface::Impl::_ready(NemoInterface::Impl::STATE s) {
  return s == STATE::READY;
}

bool NemoInterface::Impl::_userSync(NemoInterface::Impl::STATE s) {
  return s == STATE::SYNC_USER;
}

bool NemoInterface::Impl::_sysSync(NemoInterface::Impl::STATE s) {
  return s == STATE::SYNC_SYS;
}

bool NemoInterface::Impl::_running(NemoInterface::Impl::STATE s) {
  return s != STATE::STOPPED;
}

NemoInterface::STATUS
NemoInterface::Impl::_status(NemoInterface::Impl::STATE state) {
  NemoInterface::STATUS status;
  switch (state) {
  case STATE::STOPPED:
    status = NemoInterface::STATUS::NOT_CONNECTED;
    break;
  case STATE::START_BRIDGE:
    status = NemoInterface::STATUS::NOT_CONNECTED;
    break;
  case STATE::WEBSOCKET_DETECTED:
    status = NemoInterface::STATUS::WEBSOCKET_DETECTED;
    break;
  case STATE::TRY_TOPIC_SERVICE_SETUP:
    status = NemoInterface::STATUS::WEBSOCKET_DETECTED;
    break;
  case STATE::READY:
    status = NemoInterface::STATUS::READY;
    break;
  case STATE::SYNC_USER:
  case STATE::SYNC_SYS:
    status = NemoInterface::STATUS::SYNC;
    break;
  case STATE::WEBSOCKET_TIMEOUT:
  case STATE::HEARTBEAT_TIMEOUT:
    status = NemoInterface::STATUS::TIMEOUT;
    break;
  }

  return status;
}

QString NemoInterface::Impl::_toString(NemoInterface::Impl::STATE s) {
  switch (s) {
  case STATE::STOPPED:
    return QString("STOPPED");
  case STATE::START_BRIDGE:
    return QString("START_BRIDGE");
  case STATE::WEBSOCKET_DETECTED:
    return QString("WEBSOCKET_DETECTED");
  case STATE::TRY_TOPIC_SERVICE_SETUP:
    return QString("TRY_TOPIC_SERVICE_SETUP");
  case STATE::READY:
    return QString("READY");
  case STATE::SYNC_USER:
    return QString("SYNC_USER");
  case STATE::SYNC_SYS:
    return QString("SYNC_SYS");
  case STATE::WEBSOCKET_TIMEOUT:
    return QString("WEBSOCKET_TIMEOUT");
  case STATE::HEARTBEAT_TIMEOUT:
    return QString("HEARTBEAT_TIMEOUT");
  }
  return "unknown state!";
}

QString NemoInterface::Impl::_toString(NemoInterface::STATUS s) {
  switch (s) {
  case NemoInterface::STATUS::NOT_CONNECTED:
    return QString("NOT_CONNECTED");
  case NemoInterface::STATUS::READY:
    return QString("READY");
  case NemoInterface::STATUS::TIMEOUT:
    return QString("TIMEOUT");
  case NemoInterface::STATUS::WEBSOCKET_DETECTED:
    return QString("WEBSOCKET_DETECTED");
  case NemoInterface::STATUS::SYNC:
    return QString("SYNC");
  }
  return "unknown state!";
}

// ===============================================================
// NemoInterface
NemoInterface::NemoInterface()
    : QObject(), pImpl(std::make_unique<NemoInterface::Impl>(this)) {}

NemoInterface *NemoInterface::createInstance() { return new NemoInterface(); }

NemoInterface *NemoInterface::instance() {
  return GenericSingelton<NemoInterface>::instance(
      NemoInterface::createInstance);
}

NemoInterface::~NemoInterface() {}

void NemoInterface::start() { this->pImpl->start(); }

void NemoInterface::stop() { this->pImpl->stop(); }

std::shared_future<QVariant>
NemoInterface::addTiles(const TileArray &tileArray) {
  TilePtrArray ptrArray;
  for (const auto &tile : tileArray) {
    ptrArray.push_back(const_cast<MeasurementTile *>(&tile));
  }
  return this->pImpl->addTiles(ptrArray);
}

std::shared_future<QVariant>
NemoInterface::addTiles(const TilePtrArray &tileArray) {
  return this->pImpl->addTiles(tileArray);
}

std::shared_future<QVariant>
NemoInterface::removeTiles(const IDArray &idArray) {
  return this->pImpl->removeTiles(idArray);
}

std::shared_future<QVariant> NemoInterface::clearTiles() {
  return this->pImpl->clearTiles();
}

TileArray NemoInterface::getTiles(const IDArray &idArray) {
  return this->pImpl->getTiles(idArray);
}

TileArray NemoInterface::getAllTiles() { return this->pImpl->getAllTiles(); }

LogicalArray NemoInterface::containsTiles(const IDArray &idArray) {
  return this->pImpl->containsTiles(idArray);
}

std::size_t NemoInterface::size() { return this->pImpl->size(); }

bool NemoInterface::empty() { return this->pImpl->empty(); }

ProgressArray NemoInterface::getProgress() {
  return this->pImpl->getProgress();
}

ProgressArray NemoInterface::getProgress(const IDArray &idArray) {
  return this->pImpl->getProgress(idArray);
}

NemoInterface::STATUS NemoInterface::status() const {
  return this->pImpl->status();
}

QString NemoInterface::statusString() const {
  return statusMap.at(this->pImpl->status());
}

QString NemoInterface::infoString() const { return this->pImpl->infoString(); }

QString NemoInterface::warningString() const {
  return this->pImpl->warningString();
}

QString NemoInterface::editorQml() {
  return QStringLiteral("NemoInterface.qml");
}

bool NemoInterface::running() { return this->pImpl->running(); }