NemoInterface.cpp

#include "NemoInterface.h"

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

#include <mutex>

#include <QJsonArray>
#include <QJsonObject>
#include <QQmlEngine>
#include <QTimer>
#include <QUrl>

#include "GenericSingelton.h"
#include "geometry/MeasurementArea.h"
#include "geometry/geometry.h"
#include "nemo_interface/MeasurementTile.h"
#include "nemo_interface/QNemoHeartbeat.h"
#include "nemo_interface/TaskDispatcher.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/progress_array.h"
#include "ros_bridge/include/messages/nemo_msgs/tile.h"
#include "ros_bridge/include/messages/nemo_msgs/tile_array.h"
#include "rosbridge/rosbridge.h"

/*
 * Here some rules:
 * * not threas safe functions are marked with *NotSafe(...)
 * * If a function is not safe:
 *     * the call to it must be protected with a Lock.
 *     * not safe functions are not allowed to emit signals directly (danger of
 * deadlock).
 *     * if a not safe function needs to emit a signal, defere it with
 *       QTimer::singleShot().
 *     * not safe functions are allowed to call other not safe functions
 *     * it is a bad idea to wait inside a not safe function for a asynchronous
 * operation (potential deadlock)
 * * Functions that are not marked with *NotSafe(...) must be thread safe!
 */

#define INVM(context, fun)                                                     \
  {                                                                            \
    auto value = QMetaObject::invokeMethod(context, fun);                      \
    Q_ASSERT(value == true);                                                   \
    Q_UNUSED(value);                                                           \
  }

Q_DECLARE_METATYPE(ProgressArray)

QGC_LOGGING_CATEGORY(NemointerfaceLog, "NemointerfaceLog")

#define NO_HEARTBEAT_TIMEOUT 5000   // ms
#define RESTART_INTERVAl 600000     // ms == 10 min
#define RESTART_RETRY_INTERVAl 2000 // ms
#define SYNC_INTERVAL 10000         // ms
#define SYNC_RETRY_INTERVAL 2000    // ms

static constexpr auto maxResponseTime = std::chrono::milliseconds(20000);

static char const *progressTopic = "/nemo/progress";
static char const *heartbeatTopic = "/nemo/heartbeat";

static char const *addTilesService = "/nemo/add_tiles";
static char const *removeTilesService = "/nemo/remove_tiles";
static char const *clearTilesService = "/nemo/clear_tiles";
static char const *getAllTilesService = "/nemo/get_all_tiles";
// static char const *getTilesService = "/nemo/get_tiles";
// static char const *containsTilesService = "/nemo/contains_tiles";
// static char const *extractTilesService = "/nemo/extract_tiles";
// static char const *sizeService = "/nemo/size";
// static char const *emptyService = "/nemo/empty";
static char const *getProgressService = "/nemo/get_progress";
static char const *getAllProgressService = "/nemo/get_all_progress";
static char const *getVersionService = "/nemo/get_version";

static const std::vector<char const *> requiredServices{
    addTilesService,    removeTilesService,    clearTilesService,
    getAllTilesService, getAllProgressService, getProgressService,
    getVersionService};

static const std::vector<char const *> requiredTopics{progressTopic,
                                                      heartbeatTopic};

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

typedef ros_bridge::messages::nemo_msgs::tile::GenericTile<QGeoCoordinate,
                                                           QList>
    Tile;
typedef std::map<QString, std::shared_ptr<Tile>> TileMap;
typedef std::map<QString, std::shared_ptr<const Tile>> TileMapConst;
typedef ros_bridge::messages::nemo_msgs::heartbeat::Heartbeat Heartbeat;
typedef nemo_interface::TaskDispatcher Dispatcher;
typedef std::unique_lock<std::mutex> Lock;

class Nemointerface::Impl {
  enum class STATE {
    STOPPED,
    START_BRIDGE,
    WEBSOCKET_DETECTED,
    TRY_SETUP,
    USER_SYNC,
    SYS_SYNC,
    SYNC_ERROR,
    READY,
    WEBSOCKET_TIMEOUT,
    HEARTBEAT_TIMEOUT
  };

public:
  Impl(const QString &connectionString, 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) const;
  TileArray getAllTiles() const;
  LogicalArray containsTiles(const IDArray &idArray) const;
  std::size_t size() const;
  bool empty() const;

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

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

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

private:
  void _doSetup();
  void _doActionNotSafe();
  void _synchronize();
  void _tryRestart();
  bool _isSynchronizedNotSafe() const;
  bool _isSynchronized() const;
  void _onHeartbeatTimeout();
  void _onRosbridgeStateChanged();
  bool _updateProgress(const ProgressArray &pArray);
  void _onHeartbeatReceived(const QNemoHeartbeat &hb);
  void _setInfoStringNotSafe(const QString &info);
  void _setWarningStringNotSafe(const QString &warning);
  void _setInfoString(const QString &info);
  void _setWarningString(const QString &warning);

  // state suff
  bool _userSync() const;
  bool _sysSync() const;
  bool _setStateNotSafe(STATE newState);
  static bool _readyNotSafe(STATE s);
  static bool _userSyncNotSafe(STATE s);
  static bool _sysSyncNotSafe(STATE s);
  static bool _runningNotSafe(STATE s);
  static Nemointerface::STATUS _status(STATE state);
  static QString _toString(STATE s);
  static QString _toString(Nemointerface::STATUS s);

  // impl functions
  bool _addTilesImpl(
      std::shared_ptr<QVector<std::shared_ptr<const Tile>>> pTileArray,
      std::shared_ptr<const IDArray> pIdArray);
  bool _removeTilesImpl(std::shared_ptr<const IDArray> pIdArray);
  bool _clearTilesImpl();

  // Worker functions.
  static bool
  _callAddTiles(const QVector<std::shared_ptr<const Tile>> &tileArray,
                Dispatcher &d, Rosbridge &rb);
  static bool _callClearTiles(Dispatcher &d, Rosbridge &rb);
  static ProgressArray _callGetProgress(const IDArray &pIdArray, Dispatcher &d,
                                        Rosbridge &rb);
  static ProgressArray _callGetAllProgress(Dispatcher &d, Rosbridge &rb);
  static QVector<std::shared_ptr<Tile>> _callGetAllTiles(Dispatcher &d,
                                                         Rosbridge &rb);
  static QString _callGetVersion(Dispatcher &d, Rosbridge &rb);
  static bool _callRemoveTiles(const IDArray &pIdArray, Dispatcher &d,
                               Rosbridge &rb);

  // functions to manipulate _remoteTiles
  bool _addTilesRemote(const QVector<std::shared_ptr<const Tile>> tileArray);
  bool _addTilesRemote(const QVector<std::shared_ptr<Tile>> &tileArray);
  void _removeTilesRemote(const IDArray &idArray);
  void _clearTilesRemote();
  void _clearTilesRemoteNotSafe();

  // static and const members
  static const char *_localVersion;
  Nemointerface *const _parent;
  const QString _connectionString;

  // thread safe members
  Rosbridge _rosbridge;
  Dispatcher _dispatcher;

  // protected by mutex
  mutable std::mutex _m;
  STATE _state;
  TileMap _remoteTiles;
  TileMapConst _localTiles;
  QString _infoString;
  QString _warningString;

  // Members belonging to _parent->thread()
  QTimer _timeoutTimer;
  QTimer _syncTimer;
  QTimer _restartTimer;
};

const char *Nemointerface::Impl::_localVersion("V_1.0");

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::ERROR,
                                                   "ERROR"),
    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(const QString &connectionString, Nemointerface *p)
    : _parent(p), _connectionString(connectionString),
      _rosbridge(QUrl(QString("ws://") + connectionString)),
      _state(STATE::STOPPED) {

  // Heartbeat timeout.
  connect(&this->_timeoutTimer, &QTimer::timeout,
          std::bind(&Impl::_onHeartbeatTimeout, this));

  connect(&this->_rosbridge, &Rosbridge::stateChanged, this->_parent,
          [this] { this->_onRosbridgeStateChanged(); });

  connect(&this->_restartTimer, &QTimer::timeout, this->_parent,
          [this] { this->_tryRestart(); });

  connect(&this->_syncTimer, &QTimer::timeout, this->_parent,
          [this] { this->_synchronize(); });

  static std::once_flag flag;
  std::call_once(flag, [] { qRegisterMetaType<ProgressArray>(); });
}

Nemointerface::Impl::~Impl() { this->_rosbridge.stop(); }

void Nemointerface::Impl::start() {
  Lock lk(this->_m);
  if (!_runningNotSafe(this->_state)) {
    this->_setStateNotSafe(STATE::START_BRIDGE);
    this->_doActionNotSafe();
  }
}

void Nemointerface::Impl::stop() {
  Lock lk(this->_m);
  if (_runningNotSafe(this->_state)) {
    this->_setStateNotSafe(STATE::STOPPED);
    this->_doActionNotSafe();
  }
}

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

  // qDebug() << "addTiles called";

  if (tileArray.size() > 0) {

    // copy unknown tiles
    auto pTileArray = std::make_shared<QVector<std::shared_ptr<const Tile>>>();
    auto pIdArray = std::make_shared<IDArray>();

    Lock lk(this->_m);
    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);
        pIdArray->push_back(id);
      } else {
        qCDebug(NemointerfaceLog)
            << "addTiles(): tile with id: " << pTile->id() << "already added.";
      }
    }
    if (pTileArray->size() > 0) {
      lk.unlock();
      emit this->_parent->tilesChanged();
      lk.lock();
    }

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

      this->_setStateNotSafe(STATE::USER_SYNC);
      this->_doActionNotSafe();
      lk.unlock();

      // create task.
      auto pTask = std::make_unique<Task>([this, pTileArray, pIdArray] {
        auto ret = this->_addTilesImpl(pTileArray, pIdArray);

        if (ret) {
          Lock lk(this->_m);
          if (this->_isSynchronizedNotSafe()) {
            this->_setStateNotSafe(STATE::READY);
            this->_doActionNotSafe();
          }
        }
        return ret;
      });

      // dispatch command.
      auto ret = _dispatcher.dispatch(std::move(pTask));
      return ret.share();
    }
  }

  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;

  // qDebug() << "removeTiles called";

  if (idArray.size() > 0) {

    // copy known ids
    auto pIdArray = std::make_shared<IDArray>();

    Lock lk(this->_m);
    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 << ".";
      }
    }
    if (pIdArray->size() > 0) {
      lk.unlock();
      emit this->_parent->tilesChanged();
      lk.lock();
    }

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

      this->_setStateNotSafe(STATE::USER_SYNC);
      this->_doActionNotSafe();
      lk.unlock();

      // create command.
      auto cmd = std::make_unique<Task>([this, pIdArray] {
        auto ret = this->_removeTilesImpl(pIdArray);

        if (ret) {
          Lock lk(this->_m);
          if (this->_isSynchronizedNotSafe()) {
            this->_setStateNotSafe(STATE::READY);
            this->_doActionNotSafe();
          }
        }

        return ret;
      });

      // dispatch command and return.
      auto ret = _dispatcher.dispatch(std::move(cmd));
      auto sfut = ret.share();
      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;

  // qDebug() << "clearTiles called";

  // clear local tiles (_localTiles)
  Lock lk(this->_m);
  if (!_localTiles.empty()) {
    this->_localTiles.clear();
    lk.unlock();
    emit this->_parent->tilesChanged();
    lk.lock();
  }

  if (this->_readyNotSafe(this->_state) || this->_readyNotSafe(this->_state)) {

    this->_setStateNotSafe(STATE::USER_SYNC);
    this->_doActionNotSafe();
    lk.unlock();

    // create command.
    auto pTask = std::make_unique<Task>([this] {
      auto ret = this->_clearTilesImpl();

      if (ret) {
        Lock lk(this->_m);
        if (this->_isSynchronizedNotSafe()) {
          this->_setStateNotSafe(STATE::READY);
          this->_doActionNotSafe();
        }
      }

      return QVariant(ret);
    });

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

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

  Lock lk(this->_m);
  if (this->ready()) {
    for (const auto &id : idArray) {
      const auto it = _remoteTiles.find(id);
      if (it != _remoteTiles.end()) {
        MeasurementTile copy;
        copy.setId(it->second->id());
        copy.setProgress(it->second->progress());
        copy.setPath(it->second->tile());
        tileArray.append(std::move(copy));
      }
    }
  } else {
    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() const {
  TileArray tileArray;

  Lock lk(this->_m);
  if (this->ready()) {
    for (const auto &entry : _remoteTiles) {
      auto pTile = entry.second;
      MeasurementTile copy;
      copy.setId(pTile->id());
      copy.setProgress(pTile->progress());
      copy.setPath(pTile->tile());
      tileArray.append(std::move(copy));
    }

  } else {
    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) const {
  LogicalArray logicalArray;

  Lock lk(this->_m);
  for (const auto &id : idArray) {
    const auto &it = _localTiles.find(id);
    logicalArray.append(it != _localTiles.end());
  }

  return logicalArray;
}

std::size_t Nemointerface::Impl::size() const {

  Lock lk(this->_m);
  return _localTiles.size();
}

bool Nemointerface::Impl::empty() const {
  Lock lk(this->_m);
  return _localTiles.empty();
}

ProgressArray Nemointerface::Impl::getProgress() const {
  ProgressArray progressArray;

  Lock lk(this->_m);
  if (this->_isSynchronizedNotSafe()) {
    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) const {
  ProgressArray progressArray;

  Lock lk(this->_m);
  if (this->_isSynchronizedNotSafe()) {
    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() const {
  Lock lk(this->_m);
  return _status(this->_state);
}

bool Nemointerface::Impl::running() const {
  Lock lk1(this->_m);
  return _runningNotSafe(this->_state);
}

bool Nemointerface::Impl::ready() const {
  Lock lk1(this->_m);
  return _readyNotSafe(this->_state);
}

bool Nemointerface::Impl::_sysSync() const {
  Lock lk1(this->_m);
  return _sysSyncNotSafe(this->_state);
}

void Nemointerface::Impl::_onHeartbeatTimeout() {
  Lock lk1(this->_m);
  this->_setStateNotSafe(STATE::HEARTBEAT_TIMEOUT);
  this->_doActionNotSafe();
}

void Nemointerface::Impl::_onRosbridgeStateChanged() {
  auto rbState = this->_rosbridge.state();
  if (rbState == Rosbridge::STATE::CONNECTED) {
    Lock lk1(this->_m);
    if (this->_state == STATE::START_BRIDGE ||
        this->_state == STATE::WEBSOCKET_TIMEOUT) {
      this->_setStateNotSafe(STATE::WEBSOCKET_DETECTED);
      this->_doActionNotSafe();
    }
  } else if (rbState == Rosbridge::STATE::TIMEOUT) {
    Lock lk1(this->_m);
    if (this->_state == STATE::TRY_SETUP || this->_state == STATE::READY ||
        this->_state == STATE::WEBSOCKET_DETECTED ||
        this->_state == STATE::HEARTBEAT_TIMEOUT) {
      this->_setStateNotSafe(STATE::WEBSOCKET_TIMEOUT);
      this->_doActionNotSafe();
    }
  }
}

bool Nemointerface::Impl::_userSync() const {
  Lock lk1(this->_m);
  return _userSyncNotSafe(this->_state);
}

const QString &Nemointerface::Impl::infoString() const {
  Lock lk1(this->_m);
  return _infoString;
}

const QString &Nemointerface::Impl::warningString() const {
  Lock lk1(this->_m);
  return _warningString;
}

const QString &Nemointerface::Impl::connectionString() const {
  return this->_connectionString;
}

bool Nemointerface::Impl::_updateProgress(const ProgressArray &pArray) {

  ProgressArray copy;
  bool error = false;

  Lock lk1(this->_m);
  for (auto itLP = pArray.begin(); itLP != pArray.end(); ++itLP) {

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

    if (Q_LIKELY(it != _remoteTiles.end())) {
      it->second->setProgress(itLP->progress());
      copy.push_back(*itLP);
    } else {
      qCDebug(NemointerfaceLog)
          << "_updateProgress(): tile with id " << itLP->id() << " not found.";
      error = true;
    }
  }
  lk1.unlock();

  if (copy.size() > 0) {
    emit _parent->progressChanged(copy);
  }

  return !error;
}

void Nemointerface::Impl::_onHeartbeatReceived(const QNemoHeartbeat &) {
  INVM(this->_parent,
       ([this]() mutable { this->_timeoutTimer.start(NO_HEARTBEAT_TIMEOUT); }))

  Lock lk(this->_m);
  if (this->_state == STATE::TRY_SETUP ||
      this->_state == STATE::HEARTBEAT_TIMEOUT) {
    this->_setStateNotSafe(STATE::READY);
    this->_doActionNotSafe();
  }
}

void Nemointerface::Impl::_setInfoStringNotSafe(const QString &info) {
  if (_infoString != info) {
    _infoString = info;

    // emit signal later, can't emit while mutex locked!
    QTimer::singleShot(5, this->_parent,
                       [this] { emit this->_parent->infoStringChanged(); });
  }
}

void Nemointerface::Impl::_setWarningStringNotSafe(const QString &warning) {
  if (_warningString != warning) {
    _warningString = warning;

    // emit signal later, can't emit while mutex locked!
    QTimer::singleShot(5, this->_parent,
                       [this] { emit this->_parent->warningStringChanged(); });
  }
}

void Nemointerface::Impl::_setInfoString(const QString &info) {
  Lock lk(this->_m);
  _setInfoStringNotSafe(info);
}

void Nemointerface::Impl::_setWarningString(const QString &warning) {
  Lock lk(this->_m);
  _setWarningStringNotSafe(warning);
}

void Nemointerface::Impl::_doSetup() {
  // create task
  auto pTask = std::make_unique<nemo_interface::Task>([this] {
    auto &rb = this->_rosbridge;
    auto &disp = this->_dispatcher;

    // check if required services are available
    auto cond = [&disp] { return !disp.isInterruptionRequested(); };
    for (const auto &cc : requiredServices) {
      QString s(cc);

      this->_setInfoString("Waiting for required service " + s);

      auto available = rb.waitForService(cc, cond);
      if (!available) {
        return QVariant(false);
      }
    }

    // check if version is compatible{
    this->_setInfoString("Checking version.");
    auto version = _callGetVersion(disp, rb);
    if (version != _localVersion) {
      this->_setWarningString(
          "Version checking failed. Local protocol version (" +
          QString(_localVersion) + ") does not match remote version (" +
          version + ").");
      return QVariant(false);
    }

    // check if required topics are available
    for (const auto &cc : requiredTopics) {
      QString s(cc);
      this->_setInfoString("Waiting for required topic " + s);
      auto available = rb.waitForTopic(cc, cond);
      if (!available) {
        return QVariant(false);
      }
    }

    // subscribe topics
    rb.subscribeTopic(progressTopic, [this](const QJsonObject &o) {
      ros_bridge::messages::nemo_msgs::progress_array::ProgressArray
          progressArray;
      if (ros_bridge::messages::nemo_msgs::progress_array::fromJson(
              o, 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) << progressTopic
                                        << " progress out "
                                           "of range, value was set to: "
                                        << lp.progress();
          }
        }
        auto ret = this->_updateProgress(progressArray.progress_array());
        if (!ret)
          this->_setWarningString("Progress update failed.");
      } else {
        qCWarning(NemointerfaceLog) << progressTopic
                                    << " not able to "
                                       "create ProgressArray form json: "
                                    << o;
      }
    });

    using namespace ros_bridge::messages;
    rb.subscribeTopic(heartbeatTopic, [this](const QJsonObject &o) {
      nemo_msgs::heartbeat::Heartbeat heartbeat;
      if (nemo_msgs::heartbeat::fromJson(o, heartbeat)) {
        this->_onHeartbeatReceived(heartbeat);
      } else {
        qCWarning(NemointerfaceLog) << heartbeatTopic
                                    << " not able to "
                                       "create Heartbeat form json: "
                                    << o;
      }
    });

    // now ready
    INVM(this->_parent, ([this]() mutable {
           this->_timeoutTimer.start(NO_HEARTBEAT_TIMEOUT);
         }))
    _setInfoString("");
    _setWarningString("");
    return QVariant(true);
  });

  auto f = _dispatcher.dispatch(std::move(pTask));
  Q_UNUSED(f);
}

void Nemointerface::Impl::_synchronize() {
  Lock lk(this->_m);
  if (this->_state == STATE::READY || this->_state == STATE::SYNC_ERROR) {

    _setStateNotSafe(STATE::SYS_SYNC);
    _doActionNotSafe();

    // copy tiles.
    auto pTileArray = std::make_shared<QVector<std::shared_ptr<const Tile>>>();
    for (auto it = _localTiles.begin(); it != _localTiles.end(); ++it) {
      pTileArray->push_back(it->second);
    }
    lk.unlock();

    // create cmd.
    auto pTask = std::make_unique<nemo_interface::Task>([this, pTileArray] {
      auto &rb = this->_rosbridge;
      auto &disp = this->_dispatcher;
      // are _localTiles and _remoteTiles synced?

      auto remoteTiles = this->_callGetAllTiles(disp, rb);

      // create tile map;
      std::map<QString, std::shared_ptr<Tile>> tempMap;
      for (auto &&pTile : remoteTiles) {
        auto it = tempMap.find(pTile->id());
        if (Q_LIKELY(it == tempMap.end())) {
          tempMap.insert(std::make_pair(pTile->id(), pTile));
        } else {
          qCDebug(NemointerfaceLog)
              << "_synchronizeIfNeccessary(): remoteTiles contains "
                 "duplicate id";
        }
      }

      // compare with pTileArray
      bool synced = true;
      for (auto &&pTile : *pTileArray) {
        auto it = tempMap.find(pTile->id());
        if (it == tempMap.end() || it->second->tile() != pTile->tile()) {
          synced = false;
          break;
        }
      }

      if (!synced) {
        auto success = this->_clearTilesImpl();
        if (!success) {
          Lock lk(this->_m);
          _setStateNotSafe(STATE::SYNC_ERROR);
          _doActionNotSafe();
          return QVariant(false);
        }

        auto pIdArray = std::make_shared<IDArray>();
        for (auto &&pTile : *pTileArray) {
          pIdArray->push_back(pTile->id());
        }

        success = this->_addTilesImpl(pTileArray, pIdArray);

        if (!success) {
          Lock lk(this->_m);
          _setStateNotSafe(STATE::SYNC_ERROR);
          _doActionNotSafe();
          return QVariant(false);
        }
      } else {
        // update progress
        this->_clearTilesRemote();
        auto ret = this->_addTilesRemote(*pTileArray);

        if (ret) {
          ProgressArray progress;
          for (auto &&pTile : remoteTiles) {
            progress.push_back(LabeledProgress(pTile->progress(), pTile->id()));
          }
          ret = _updateProgress(progress);
        }

        if (!ret) {
          Lock lk(this->_m);
          this->_setStateNotSafe(STATE::SYNC_ERROR);
          this->_doActionNotSafe();
          lk.unlock();
          this->_setWarningString("Getting progress failed.");
          qCDebug(NemointerfaceLog)
              << "_addTilesImpl(): _updateProgress failed: unknown id.";
          return QVariant(false);
        }
      }

      // check if local versions are still synced (user could habe modified
      // _localTiles).
      Lock lk(this->_m);
      if (_isSynchronizedNotSafe()) {
        _setStateNotSafe(STATE::READY);
        _doActionNotSafe();
      } else {
        INVM(this->_parent, [this] { this->_synchronize(); })
      }
      lk.unlock();

      return QVariant(true);
    });

    // dispatch command.
    auto ret = _dispatcher.dispatch(std::move(pTask));
    Q_UNUSED(ret);
    INVM(this->_parent, [this] { this->_syncTimer.start(SYNC_INTERVAL); })
  } else {
    INVM(this->_parent, [this] { this->_syncTimer.start(SYNC_RETRY_INTERVAL); })
  }
}

void Nemointerface::Impl::_tryRestart() {
  if (this->running()) {
    if (_dispatcher.idle()) {
      qDebug() << "_tryRestart: restarting";
      this->stop();
      this->start();
      _restartTimer.start(RESTART_INTERVAl);
    } else {
      _restartTimer.start(RESTART_RETRY_INTERVAl);
    }
  }
}

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

bool Nemointerface::Impl::_isSynchronized() const {
  Lock lk(this->_m);
  return _isSynchronizedNotSafe();
}

void Nemointerface::Impl::_doActionNotSafe() {
  static bool resetDone = false;
  switch (this->_state) {
  case STATE::STOPPED: {
    std::promise<void> p;
    INVM(this->_parent, ([this]() mutable {
           this->_timeoutTimer.stop();
           this->_restartTimer.stop();
           this->_syncTimer.stop();
         }))

    if (this->_rosbridge.state() != Rosbridge::STATE::STOPPED) {
      this->_rosbridge.stop();
    }
    _dispatcher.stop();
    _dispatcher.clear();

    _setInfoStringNotSafe("");
    _setWarningStringNotSafe("");

  } break;

  case STATE::START_BRIDGE: {
    INVM(this->_parent,
         ([this]() mutable { this->_restartTimer.start(RESTART_INTERVAl); }))

    this->_rosbridge.start();
  } break;
  case STATE::WEBSOCKET_DETECTED:
    resetDone = false;
    this->_setStateNotSafe(STATE::TRY_SETUP);
    this->_doActionNotSafe();
    break;
  case STATE::TRY_SETUP:
    this->_doSetup();
    break;
  case STATE::READY: {
    INVM(this->_parent,
         ([this]() mutable { this->_syncTimer.start(SYNC_INTERVAL); }))

    if (!_isSynchronizedNotSafe()) {
      // can't call this here directly.
      QTimer::singleShot(100, this->_parent, [this] { this->_synchronize(); });
    }

    _setInfoStringNotSafe("");
    _setWarningStringNotSafe("");
  } break;
  case STATE::USER_SYNC:
  case STATE::SYS_SYNC:
  case STATE::SYNC_ERROR:
    break;
  case STATE::HEARTBEAT_TIMEOUT: {
    INVM(this->_parent, ([this]() mutable { this->_syncTimer.stop(); }))

  } break;
  case STATE::WEBSOCKET_TIMEOUT: {
    INVM(this->_parent, ([this]() mutable {
           this->_timeoutTimer.stop();
           this->_syncTimer.stop();
         }))

    if (!resetDone) {
      resetDone = true;
      this->_rosbridge.stop();
      this->_rosbridge.start();
    }
    _dispatcher.stop();
    _dispatcher.clear();

    _setInfoStringNotSafe("");
    _setWarningStringNotSafe("");
  } break;
  };
}

bool Nemointerface::Impl::_callAddTiles(
    const QVector<std::shared_ptr<const Tile>> &tileArray, Dispatcher &d,
    Rosbridge &rb) {

  // qDebug() << "_callAddTiles called";

  // create json object
  QJsonArray jsonTileArray;
  for (auto &&tile : tileArray) {
    using namespace ros_bridge::messages;
    QJsonObject jsonTile;
    if (!nemo_msgs::tile::toJson(*tile, jsonTile)) {
      qCDebug(NemointerfaceLog)
          << "addTiles(): not able to create json object: tile id: "
          << tile->id() << " progress: " << tile->progress()
          << " points: " << tile->tile();
    }
    jsonTileArray.append(std::move(jsonTile));
  } // for

  QJsonObject req;
  req["in_tile_array"] = std::move(jsonTileArray);

  // create response handler.
  auto promise_response = std::make_shared<std::promise<bool>>();
  auto future_response = promise_response->get_future();
  auto responseHandler = [promise_response](const QJsonObject &o) mutable {
    // check if transaction was successfull
    if (o.contains("success") && o["success"].isBool()) {
      promise_response->set_value(o["success"].toBool());
    } else {
      qCWarning(NemointerfaceLog)
          << addTilesService << " no \"success\" key or wrong type: " << o;
      promise_response->set_value(false);
    }
  };

  // call service.
  rb.callService(addTilesService, responseHandler, req);

  // 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 ||
           d.isInterruptionRequested());

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

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

  // return success
  return true;
}

bool Nemointerface::Impl::_callRemoveTiles(const IDArray &pIdArray,
                                           Dispatcher &disp, Rosbridge &rb) {
  // qDebug() << "_callRemoveTiles called";

  // create json object
  QJsonArray jsonIdArray;
  for (auto &&id : pIdArray) {
    using namespace ros_bridge::messages;
    jsonIdArray.append(id);
  } // for
  QJsonObject req;
  req["ids"] = std::move(jsonIdArray);

  // create response handler.
  auto promise_response = std::make_shared<std::promise<bool>>();
  auto future_response = promise_response->get_future();
  auto responseHandler = [promise_response](const QJsonObject &o) mutable {
    // check if transaction was successfull
    QString msg = QJsonDocument(o).toJson(QJsonDocument::JsonFormat::Compact);
    if (o.contains("success") && o["success"].isBool()) {
      promise_response->set_value(o["success"].toBool());
    } else {
      qCWarning(NemointerfaceLog)
          << removeTilesService << " no \"success\" key or wrong type: " << msg;
      promise_response->set_value(false);
    }
  };

  // call service.
  rb.callService(removeTilesService, responseHandler, req);

  // 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 ||
           disp.isInterruptionRequested());

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

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

  return true;
}

bool Nemointerface::Impl::_callClearTiles(Dispatcher &disp, Rosbridge &rb) {

  // create response handler.
  auto promise_response = std::make_shared<std::promise<bool>>();
  auto future_response = promise_response->get_future();
  auto responseHandler = [promise_response](const QJsonObject &) mutable {
    // check if transaction was successfull
    promise_response->set_value(true);
  };

  // call service.
  rb.callService(clearTilesService, responseHandler, QJsonObject());

  // 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 ||
           disp.isInterruptionRequested());

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

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

  return true;
}

ProgressArray Nemointerface::Impl::_callGetProgress(const IDArray &pIdArray,
                                                    Dispatcher &disp,
                                                    Rosbridge &rb) {
  // qDebug() << "_callGetProgress called";