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/CommandDispatcher.h"
#include "nemo_interface/MeasurementTile.h"
#include "nemo_interface/QNemoHeartbeat.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 EVENT_TIMER_INTERVAL 100  // ms
#define NO_HEARTBEAT_TIMEOUT 5000 // ms
auto constexpr static maxResponseTime = std::chrono::milliseconds(10000);
using hrc = std::chrono::high_resolution_clock;

using ROSBridgePtr = std::shared_ptr<RosbridgeWsClient>;
using UniqueLock = std::unique_lock<std::shared_timed_mutex>;
using SharedLock = std::shared_lock<std::shared_timed_mutex>;

class NemoInterface::Impl {
  enum class STATE {
    STOPPED,
    START_BRIDGE,
    WEBSOCKET_DETECTED,
    TRY_TOPIC_SERVICE_SETUP,
    READY,
    TIMEOUT
  };

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

  void start();
  void stop();

  // Tile editing.
  // 	Functions that require communication to device.
  std::future<QVariant> addTiles(const TileArray &tileArray);
  void removeTiles(const IDArray &idArray);
  void 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();

  void _addTilesLocal(const TileArray &tileArray);
  void _removeTilesLocal(const IDArray &idArray);
  void _clearTilesLocal();
  void _updateProgress(ProgressArray progressArray);
  void _setHeartbeat(const QNemoHeartbeat &hb);
  void _setInfoString(const QString &info);
  void _setWarningString(const QString &warning);

private:
  typedef std::chrono::time_point<std::chrono::high_resolution_clock> TimePoint;
  typedef std::map<long, MeasurementTile> TileMap;
  typedef ros_bridge::messages::nemo_msgs::heartbeat::Heartbeat Heartbeat;
  typedef nemo_interface::CommandDispatcher Dispatcher;

  void _doTopicServiceSetup();
  void _doAction(); // does action according to state

  bool _setState(STATE s); // not thread safe
  static bool _ready(STATE s);
  static bool _running(STATE s);
  static void _translate(STATE state, NemoInterface::STATUS &status);
  static void _translate(Heartbeat hb, STATE &state);

  std::atomic<STATE> _state;
  ROSBridgePtr _pRosBridge;
  TileMap _tileMap;
  NemoInterface *_parent;
  Dispatcher _dispatcher;
  QString _infoString;
  QString _warningString;
  QTimer _timeoutTimer;
  QTimer _connectionTimer;
  QNemoHeartbeat _lastHeartbeat;
};

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::READY,
                                                   "Ready"),
    std::make_pair<NemoInterface::STATUS, QString>(
        NemoInterface::STATUS::TIMEOUT, "Timeout"),
    std::make_pair<NemoInterface::STATUS, QString>(
        NemoInterface::STATUS::INVALID_HEARTBEAT, "Error"),
    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();
    qCritical() << "NemoInterface: add reset code here";
  };
  connect(connectionStringFact, &SettingsFact::rawValueChanged,
          setConnectionString);
  setConnectionString();

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

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

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->_connectionTimer.stop();
    this->_doAction();
  }
}

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

  if (this->ready()) {

    // create command.
    auto pRosBridge = this->_pRosBridge;
    auto pDispatcher = &this->_dispatcher;
    Task sendTilesCommand([pRosBridge, tileArray, pDispatcher,
                           this](std::promise<QVariant> promise) {
      // create json object
      rapidjson::Document request;
      auto &allocator = request.GetAllocator();
      rapidjson::Value jsonTileArray(rapidjson::kArrayType);
      for (const auto &tile : tileArray) {
        const auto it = _tileMap.find(tile.id());
        if (Q_LIKELY(it == _tileMap.end())) {

          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.path();
          }
          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<void>>();
      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 {
            qDebug() << "addTiles: in_message" << in_message->string().c_str();
            promise_response->set_value();
            connection->send_close(1000);
          };

      // call service.
      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 ||
               pDispatcher->interruptionPoint());

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

      qCritical() << "addTiles(): ToDo: add return value checking here.";

      // update local tiles
      QMetaObject::invokeMethod(
          this->_parent, std::bind(&Impl::_addTilesLocal, this, tileArray));

      // return success
      promise.set_value(QVariant(true));
      return;
    }); // sendTilesCommand

    // dispatch command and return.
    auto ret = _dispatcher.dispatch(sendTilesCommand);
    return ret;
  } 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 = _tileMap.find(id);
    if (it != _tileMap.end()) {
      tileArray.append(it->second);
    }
  }

  return tileArray;
}

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

  for (const auto &entry : _tileMap) {
    tileArray.append(entry.second);
  }

  return tileArray;
}

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

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

  return logicalArray;
}

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

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

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

  for (const auto &entry : _tileMap) {
    progressArray.append(TaggedProgress{entry.first, entry.second.progress()});
  }

  return progressArray;
}

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

  for (const auto &id : idArray) {
    const auto it = _tileMap.find(id);
    if (it != _tileMap.end()) {
      progressArray.append(TaggedProgress{it->first, it->second.progress()});
    }
  }

  return progressArray;
}

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

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

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

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

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

void NemoInterface::Impl::_addTilesLocal(const TileArray &tileArray) {
  bool anyChanges = false;

  for (const auto &tile : tileArray) {
    const auto id = tile.id();
    const auto it = _tileMap.find(id);
    if (Q_LIKELY(it == _tileMap.end())) {
      auto ret = _tileMap.insert(std::make_pair(id, tile));
      anyChanges = true;
      Q_ASSERT(ret.second == true);
      Q_UNUSED(ret);
    } else {
      qCDebug(NemoInterfaceLog)
          << "_addTilesLocal(): tile with id " << id << " already inserted.";
    }
  }

  if (anyChanges) {
    emit _parent->tilesChanged();
  }
}

void NemoInterface::Impl::_removeTilesLocal(const IDArray &idArray) {
  bool anyChanges = false;
  for (const auto &id : idArray) {
    const auto it = _tileMap.find(id);
    if (Q_LIKELY(it != _tileMap.end())) {
      _tileMap.erase(it);
      anyChanges = true;
    } else {
      qCDebug(NemoInterfaceLog)
          << "_removeTilesLocal(): tile with id " << id << " not found.";
    }
  }

  if (anyChanges) {
    emit _parent->tilesChanged();
  }
}

void NemoInterface::Impl::_clearTilesLocal() {
  if (!_tileMap.empty()) {
    _tileMap.clear();
    emit _parent->tilesChanged();
  }
}

void NemoInterface::Impl::_updateProgress(ProgressArray progressArray) {
  for (auto itPair = progressArray.begin(); itPair != progressArray.end();) {

    const auto &id = itPair->first;
    auto it = _tileMap.find(id);

    if (Q_LIKELY(it != _tileMap.end())) {
      const auto &progress = itPair->second;
      it->second.setProgress(progress);
      ++itPair;
    } else {
      qCDebug(NemoInterfaceLog)
          << "_updateProgress(): tile with id " << id << " not found.";
      itPair = progressArray.erase(itPair);
    }
  }

  emit _parent->progressChanged(progressArray);
}

void NemoInterface::Impl::_setHeartbeat(const QNemoHeartbeat &hb) {
  if (this->_lastHeartbeat != hb) {
    _lastHeartbeat = hb;
    if (ready()) {
      this->_timeoutTimer.stop();
      this->_timeoutTimer.start(NO_HEARTBEAT_TIMEOUT);
    }
  }
}

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> connection,
             std::shared_ptr<WsClient::InMessage> in_message) {
        qDebug() << "doTopicServiceSetup(): /nemo/progress: "
                 << in_message->string().c_str();
        qDebug() << "impl missing";
      });
  // 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> connection,
             std::shared_ptr<WsClient::InMessage> in_message) {
        qDebug() << "doTopicServiceSetup(): /nemo/heartbeat: "
                 << in_message->string().c_str();
        qDebug() << "impl missing";
      });
}

void NemoInterface::Impl::_doAction() {
  // Check ROS Bridge status and do setup if necessary.
  switch (this->_state) {
  case STATE::STOPPED:
    if (this->_pRosBridge->running()) {
      this->_pRosBridge->reset();
    }
    break;

  case STATE::START_BRIDGE:
  case STATE::TIMEOUT:
    this->_pRosBridge->reset();
    this->_pRosBridge->run();
    this->_connectionTimer.start(EVENT_TIMER_INTERVAL);
    break;
  case STATE::WEBSOCKET_DETECTED:
    this->_setState(STATE::TRY_TOPIC_SERVICE_SETUP);
    this->_doAction();
    break;
  case STATE::TRY_TOPIC_SERVICE_SETUP:
    this->_doTopicServiceSetup();
    this->_setState(STATE::READY);
    break;
  case STATE::READY:
    break;
  };
}

bool NemoInterface::Impl::_setState(STATE s) {
  if (s != this->_state) {
    this->_state = s;
    emit this->_parent->statusChanged();
    return true;
  } else {
    return false;
  }
}

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

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

// ===============================================================
// 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(); }

void NemoInterface::addTiles(const TileArray &tileArray) {
  this->pImpl->addTiles(tileArray);
}

void NemoInterface::removeTiles(const IDArray &idArray) {
  this->pImpl->removeTiles(idArray);
}

void NemoInterface::clearTiles() { 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(); }