WimaController.cc

#include "WimaController.h"

#include "utilities.h"

#include "Snake/QNemoHeartbeat.h"
#include "Snake/QNemoProgress.h"

#include "QVector3D"
#include <QScopedPointer>
#include <QtConcurrentRun>

#include <memory>

template <typename T>
constexpr typename std::underlying_type<T>::type integral(T value) {
  return static_cast<typename std::underlying_type<T>::type>(value);
}

#define SMART_RTL_MAX_ATTEMPTS 3       // times
#define SMART_RTL_ATTEMPT_INTERVAL 200 // ms
#define EVENT_TIMER_INTERVAL 50        // ms

const char *WimaController::areaItemsName = "AreaItems";
const char *WimaController::missionItemsName = "MissionItems";
const char *WimaController::settingsGroup = "WimaController";
const char *WimaController::enableWimaControllerName = "EnableWimaController";
const char *WimaController::overlapWaypointsName = "OverlapWaypoints";
const char *WimaController::maxWaypointsPerPhaseName = "MaxWaypointsPerPhase";
const char *WimaController::startWaypointIndexName = "StartWaypointIndex";
const char *WimaController::showAllMissionItemsName = "ShowAllMissionItems";
const char *WimaController::showCurrentMissionItemsName =
    "ShowCurrentMissionItems";
const char *WimaController::flightSpeedName = "FlightSpeed";
const char *WimaController::arrivalReturnSpeedName = "ArrivalReturnSpeed";
const char *WimaController::altitudeName = "Altitude";
const char *WimaController::snakeTileWidthName = "SnakeTileWidth";
const char *WimaController::snakeTileHeightName = "SnakeTileHeight";
const char *WimaController::snakeMinTileAreaName = "SnakeMinTileArea";
const char *WimaController::snakeLineDistanceName = "SnakeLineDistance";
const char *WimaController::snakeMinTransectLengthName =
    "SnakeMinTransectLength";

WimaController::StatusMap WimaController::_nemoStatusMap{
    std::make_pair<int, QString>(0, "No Heartbeat"),
    std::make_pair<int, QString>(1, "Connected"),
    std::make_pair<int, QString>(-1, "Timeout"),
    std::make_pair<int, QString>(-2, "Error")};

WimaController::WimaController(QObject *parent)
    : QObject(parent), _joinedArea(), _measurementArea(), _serviceArea(),
      _corridor(), _localPlanDataValid(false),
      _areaInterface(&_measurementArea, &_serviceArea, &_corridor,
                     &_joinedArea),
      _WMSettings(), _defaultWM(_WMSettings, _areaInterface),
      _snakeWM(_WMSettings, _areaInterface),
      _rtlWM(_WMSettings, _areaInterface),
      _currentWM(&_defaultWM), _WMList{&_defaultWM, &_snakeWM, &_rtlWM},
      _metaDataMap(FactMetaData::createMapFromJsonFile(
          QStringLiteral(":/json/WimaController.SettingsGroup.json"), this)),
      _enableWimaController(settingsGroup,
                            _metaDataMap[enableWimaControllerName]),
      _overlapWaypoints(settingsGroup, _metaDataMap[overlapWaypointsName]),
      _maxWaypointsPerPhase(settingsGroup,
                            _metaDataMap[maxWaypointsPerPhaseName]),
      _nextPhaseStartWaypointIndex(settingsGroup,
                                   _metaDataMap[startWaypointIndexName]),
      _showAllMissionItems(settingsGroup,
                           _metaDataMap[showAllMissionItemsName]),
      _showCurrentMissionItems(settingsGroup,
                               _metaDataMap[showCurrentMissionItemsName]),
      _flightSpeed(settingsGroup, _metaDataMap[flightSpeedName]),
      _arrivalReturnSpeed(settingsGroup, _metaDataMap[arrivalReturnSpeedName]),
      _altitude(settingsGroup, _metaDataMap[altitudeName]),
      _snakeTileWidth(settingsGroup, _metaDataMap[snakeTileWidthName]),
      _snakeTileHeight(settingsGroup, _metaDataMap[snakeTileHeightName]),
      _snakeMinTileArea(settingsGroup, _metaDataMap[snakeMinTileAreaName]),
      _snakeLineDistance(settingsGroup, _metaDataMap[snakeLineDistanceName]),
      _snakeMinTransectLength(settingsGroup,
                              _metaDataMap[snakeMinTransectLengthName]),
      _lowBatteryHandlingTriggered(false), _measurementPathLength(-1),
      _snakeThread(this), _emptyThread(this), _currentThread(&_emptyThread),
      _nemoInterface(this),
      _batteryLevelTicker(EVENT_TIMER_INTERVAL, 1000 /*ms*/) {

  // Set up facts for waypoint manager.
  _showAllMissionItems.setRawValue(true);
  _showCurrentMissionItems.setRawValue(true);
  connect(&_overlapWaypoints, &Fact::rawValueChanged, this,
          &WimaController::_updateOverlap);
  connect(&_maxWaypointsPerPhase, &Fact::rawValueChanged, this,
          &WimaController::_updateMaxWaypoints);
  connect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, this,
          &WimaController::_setStartIndex);
  connect(&_flightSpeed, &Fact::rawValueChanged, this,
          &WimaController::_updateflightSpeed);
  connect(&_arrivalReturnSpeed, &Fact::rawValueChanged, this,
          &WimaController::_updateArrivalReturnSpeed);
  connect(&_altitude, &Fact::rawValueChanged, this,
          &WimaController::_updateAltitude);

  // Init waypoint managers.
  bool value;
  size_t overlap = _overlapWaypoints.rawValue().toUInt(&value);
  Q_ASSERT(value);
  size_t N = _maxWaypointsPerPhase.rawValue().toUInt(&value);
  Q_ASSERT(value);
  size_t startIdx = _nextPhaseStartWaypointIndex.rawValue().toUInt(&value);
  Q_ASSERT(value);
  (void)value;
  for (auto manager : _WMList) {
    manager->setOverlap(overlap);
    manager->setN(N);
    manager->setStartIndex(startIdx);
  }

  // Periodic.
  connect(&_eventTimer, &QTimer::timeout, this,
          &WimaController::_eventTimerHandler);
  _eventTimer.start(EVENT_TIMER_INTERVAL);

  // SnakeThread.
  connect(_currentThread, &SnakeThread::finished, this,
          &WimaController::_threadFinishedHandler);
  connect(_currentThread, &SnakeThread::calcInProgressChanged, this,
          &WimaController::snakeCalcInProgressChanged);
  connect(this, &QObject::destroyed, &this->_snakeThread, &SnakeThread::quit);
  connect(this, &QObject::destroyed, &this->_emptyThread, &SnakeThread::quit);

  // NemoInterface.
  connect(&_nemoInterface, &NemoInterface::progressChanged, this,
          &WimaController::_progressChangedHandler);
  connect(&_nemoInterface, &NemoInterface::statusChanged, this,
          &WimaController::nemoStatusChanged);
  connect(&_nemoInterface, &NemoInterface::statusChanged, this,
          &WimaController::nemoStatusStringChanged);

  // Enable/disable snake.
  connect(&_enableSnake, &Fact::rawValueChanged, this,
          &WimaController::_enableSnakeChangedHandler);
  _enableSnakeChangedHandler();
  connect(&_enableWimaController, &Fact::rawValueChanged, [this] {
    if (!this->_enableWimaController.rawValue().toBool()) {
      this->_enableSnake.setCookedValue(QVariant(false));
    }
  });

  // Snake Waypoint Manager.
  connect(&_enableSnake, &Fact::rawValueChanged, this,
          &WimaController::_switchToSnakeWaypointManager);
  _switchToSnakeWaypointManager(_enableSnake.rawValue());
}

PlanMasterController *WimaController::masterController() {
  return _masterController;
}

MissionController *WimaController::missionController() {
  return _missionController;
}

QmlObjectListModel *WimaController::visualItems() { return &_areas; }

QmlObjectListModel *WimaController::missionItems() {
  return const_cast<QmlObjectListModel *>(&_currentWM->missionItems());
}

QmlObjectListModel *WimaController::currentMissionItems() {
  return const_cast<QmlObjectListModel *>(&_currentWM->currentMissionItems());
}

QVariantList WimaController::waypointPath() {
  return const_cast<QVariantList &>(_currentWM->waypointsVariant());
}

QVariantList WimaController::currentWaypointPath() {
  return const_cast<QVariantList &>(_currentWM->currentWaypointsVariant());
}

Fact *WimaController::enableWimaController() { return &_enableWimaController; }

Fact *WimaController::overlapWaypoints() { return &_overlapWaypoints; }

Fact *WimaController::maxWaypointsPerPhase() { return &_maxWaypointsPerPhase; }

Fact *WimaController::startWaypointIndex() {
  return &_nextPhaseStartWaypointIndex;
}

Fact *WimaController::showAllMissionItems() { return &_showAllMissionItems; }

Fact *WimaController::showCurrentMissionItems() {
  return &_showCurrentMissionItems;
}

Fact *WimaController::flightSpeed() { return &_flightSpeed; }

Fact *WimaController::arrivalReturnSpeed() { return &_arrivalReturnSpeed; }

Fact *WimaController::altitude() { return &_altitude; }

QmlObjectListModel *WimaController::snakeTiles() {
  static QmlObjectListModel list;
  return &list;
}

QVariantList WimaController::snakeTileCenterPoints() {
  return _currentThread->tileCenterPoints();
}

QVector<int> WimaController::nemoProgress() {
  return _currentThread->progress();
}

double WimaController::phaseDistance() const {
  qWarning() << "using phaseDistance dummy";
  return 0.0;
}

double WimaController::phaseDuration() const {
  qWarning() << "using phaseDuration dummy";
  return 0.0;
}

int WimaController::nemoStatus() const {
  return integral(_nemoInterface.status());
}

QString WimaController::nemoStatusString() const {
  return _nemoStatusMap.at(nemoStatus());
}

bool WimaController::snakeCalcInProgress() const {
  return _currentThread->calcInProgress();
}

void WimaController::setMasterController(PlanMasterController *masterC) {
  _masterController = masterC;
  _WMSettings.setMasterController(masterC);
  emit masterControllerChanged();
}

void WimaController::setMissionController(MissionController *missionC) {
  _missionController = missionC;
  _WMSettings.setMissionController(missionC);
  emit missionControllerChanged();
}

void WimaController::nextPhase() { _calcNextPhase(); }

void WimaController::previousPhase() {
  if (!_currentWM->previous()) {
    Q_ASSERT(false);
  }

  emit missionItemsChanged();
  emit currentMissionItemsChanged();
  emit currentWaypointPathChanged();
  emit waypointPathChanged();
}

void WimaController::resetPhase() {
  if (!_currentWM->reset()) {
    Q_ASSERT(false);
  }

  emit missionItemsChanged();
  emit currentMissionItemsChanged();
  emit currentWaypointPathChanged();
  emit waypointPathChanged();
}

void WimaController::requestSmartRTL() {
  QString errorString("Smart RTL requested. ");
  if (!_checkSmartRTLPreCondition(errorString)) {
    qgcApp()->showMessage(errorString);
    return;
  }
  emit smartRTLRequestConfirm();
}

bool WimaController::upload() {
  auto &currentMissionItems = _defaultWM.currentMissionItems();
  if (!_serviceArea.containsCoordinate(
          _masterController->managerVehicle()->coordinate()) &&
      currentMissionItems.count() > 0) {
    emit forceUploadConfirm();
    return false;
  }

  return forceUpload();
}

bool WimaController::forceUpload() {
  auto &currentMissionItems = _defaultWM.currentMissionItems();
  if (currentMissionItems.count() < 1)
    return false;

  _missionController->removeAll();
  // Set homeposition of settingsItem.
  QmlObjectListModel *visuals = _missionController->visualItems();
  MissionSettingsItem *settingsItem = visuals->value<MissionSettingsItem *>(0);
  if (settingsItem == nullptr) {
    Q_ASSERT(false);
    qWarning("WimaController::updateCurrentMissionItems(): nullptr");
    return false;
  }
  settingsItem->setCoordinate(_WMSettings.homePosition());

  // Copy mission items to _missionController.
  for (int i = 1; i < currentMissionItems.count(); i++) {
    auto *item = currentMissionItems.value<const SimpleMissionItem *>(i);
    _missionController->insertSimpleMissionItem(*item, visuals->count());
  }

  _masterController->sendToVehicle();

  return true;
}

void WimaController::removeFromVehicle() {
  _masterController->removeAllFromVehicle();
  _missionController->removeAll();
}

void WimaController::executeSmartRTL() {
  forceUpload();
  masterController()->managerVehicle()->startMission();
}

void WimaController::initSmartRTL() { _initSmartRTL(); }

void WimaController::removeVehicleTrajectoryHistory() {
  Vehicle *managerVehicle = masterController()->managerVehicle();
  managerVehicle->trajectoryPoints()->clear();
}

bool WimaController::_calcShortestPath(const QGeoCoordinate &start,
                                       const QGeoCoordinate &destination,
                                       QVector<QGeoCoordinate> &path) {
  using namespace GeoUtilities;
  using namespace PolygonCalculus;
  QPolygonF polygon2D;
  toCartesianList(_joinedArea.coordinateList(), /*origin*/ start, polygon2D);
  QPointF start2D(0, 0);
  QPointF end2D;
  toCartesian(destination, start, end2D);
  QVector<QPointF> path2D;

  bool retVal =
      PolygonCalculus::shortestPath(polygon2D, start2D, end2D, path2D);
  toGeoList(path2D, /*origin*/ start, path);

  return retVal;
}

bool WimaController::setWimaPlanData(QSharedPointer<WimaPlanData> planData) {
  // reset visual items
  _areas.clear();
  _defaultWM.clear();
  _snakeWM.clear();

  emit visualItemsChanged();
  emit missionItemsChanged();
  emit currentMissionItemsChanged();
  emit waypointPathChanged();
  emit currentWaypointPathChanged();

  _localPlanDataValid = false;

  // extract list with WimaAreas
  QList<const WimaAreaData *> areaList = planData->areaList();

  int areaCounter = 0;
  const int numAreas = 4; // extract only numAreas Areas, if there are more they
                          // are invalid and ignored
  for (int i = 0; i < areaList.size(); i++) {
    const WimaAreaData *areaData = areaList[i];

    if (areaData->type() ==
        WimaServiceAreaData::typeString) { // is it a service area?
      _serviceArea = *qobject_cast<const WimaServiceAreaData *>(areaData);
      areaCounter++;
      _areas.append(&_serviceArea);

      continue;
    }

    if (areaData->type() ==
        WimaMeasurementAreaData::typeString) { // is it a measurement area?
      _measurementArea =
          *qobject_cast<const WimaMeasurementAreaData *>(areaData);
      areaCounter++;
      _areas.append(&_measurementArea);

      continue;
    }

    if (areaData->type() == WimaCorridorData::typeString) { // is it a corridor?
      _corridor = *qobject_cast<const WimaCorridorData *>(areaData);
      areaCounter++;
      //_visualItems.append(&_corridor); // not needed

      continue;
    }

    if (areaData->type() ==
        WimaJoinedAreaData::typeString) { // is it a corridor?
      _joinedArea = *qobject_cast<const WimaJoinedAreaData *>(areaData);
      areaCounter++;
      _areas.append(&_joinedArea);

      continue;
    }

    if (areaCounter >= numAreas)
      break;
  }

  if (areaCounter != numAreas) {
    Q_ASSERT(false);
    return false;
  }

  emit visualItemsChanged();

  // extract mission items
  auto tempMissionItems = planData->missionItems();
  if (tempMissionItems.size() < 1) {
    qWarning("WimaController: Mission items from WimaPlaner empty!");
    return false;
  }

  qWarning() << "WimaController:";
  for (auto *item : tempMissionItems) {
    qWarning() << item->coordinate();
    _defaultWM.push_back(item->coordinate());
  }

  _WMSettings.setHomePosition(QGeoCoordinate(
      _serviceArea.depot().latitude(), _serviceArea.depot().longitude(), 0));
  qWarning() << "service area depot: " << _serviceArea.depot();

  if (!_defaultWM.reset()) {
    qWarning() << "_defaultWM.reset() failed";
    return false;
  }

  emit missionItemsChanged();
  emit currentMissionItemsChanged();
  emit waypointPathChanged();
  emit currentWaypointPathChanged();

  // Update Snake Data Manager
  _snakeThread.setMeasurementArea(_measurementArea.coordinateList());
  _snakeThread.setServiceArea(_serviceArea.coordinateList());
  _snakeThread.setCorridor(_corridor.coordinateList());
  _currentThread->start();

  _localPlanDataValid = true;
  return true;
}

WimaController *WimaController::thisPointer() { return this; }

bool WimaController::_calcNextPhase() {
  if (!_currentWM->next()) {
    Q_ASSERT(false);
    return false;
  }

  emit missionItemsChanged();
  emit currentMissionItemsChanged();
  emit currentWaypointPathChanged();
  emit waypointPathChanged();

  return true;
}

bool WimaController::_setStartIndex() {
  bool value;
  _currentWM->setStartIndex(
      _nextPhaseStartWaypointIndex.rawValue().toUInt(&value) - 1);
  Q_ASSERT(value);
  (void)value;

  if (!_currentWM->update()) {
    Q_ASSERT(false);
    return false;
  }

  emit missionItemsChanged();
  emit currentMissionItemsChanged();
  emit currentWaypointPathChanged();
  emit waypointPathChanged();

  return true;
}

void WimaController::_recalcCurrentPhase() {
  if (!_currentWM->update()) {
    Q_ASSERT(false);
  }

  emit missionItemsChanged();
  emit currentMissionItemsChanged();
  emit currentWaypointPathChanged();
  emit waypointPathChanged();
}

void WimaController::_updateOverlap() {
  bool value;
  _currentWM->setOverlap(_overlapWaypoints.rawValue().toUInt(&value));
  Q_ASSERT(value);
  (void)value;

  if (!_currentWM->update()) {
    assert(false);
  }

  emit missionItemsChanged();
  emit currentMissionItemsChanged();
  emit currentWaypointPathChanged();
  emit waypointPathChanged();
}

void WimaController::_updateMaxWaypoints() {
  bool value;
  _currentWM->setN(_maxWaypointsPerPhase.rawValue().toUInt(&value));
  Q_ASSERT(value);
  (void)value;

  if (!_currentWM->update()) {
    Q_ASSERT(false);
  }

  emit missionItemsChanged();
  emit currentMissionItemsChanged();
  emit currentWaypointPathChanged();
  emit waypointPathChanged();
}

void WimaController::_updateflightSpeed() {
  bool value;
  _WMSettings.setFlightSpeed(_flightSpeed.rawValue().toDouble(&value));
  Q_ASSERT(value);
  (void)value;

  if (!_currentWM->update()) {
    Q_ASSERT(false);
  }

  emit missionItemsChanged();
  emit currentMissionItemsChanged();
  emit currentWaypointPathChanged();
  emit waypointPathChanged();
}

void WimaController::_updateArrivalReturnSpeed() {
  bool value;
  _WMSettings.setArrivalReturnSpeed(
      _arrivalReturnSpeed.rawValue().toDouble(&value));
  Q_ASSERT(value);
  (void)value;

  if (!_currentWM->update()) {
    Q_ASSERT(false);
  }

  emit missionItemsChanged();
  emit currentMissionItemsChanged();
  emit currentWaypointPathChanged();
  emit waypointPathChanged();
}

void WimaController::_updateAltitude() {
  bool value;
  _WMSettings.setAltitude(_altitude.rawValue().toDouble(&value));
  Q_ASSERT(value);
  (void)value;

  if (!_currentWM->update()) {
    Q_ASSERT(false);
  }

  emit missionItemsChanged();
  emit currentMissionItemsChanged();
  emit currentWaypointPathChanged();
  emit waypointPathChanged();
}

void WimaController::_checkBatteryLevel() {
  Vehicle *managerVehicle = masterController()->managerVehicle();
  WimaSettings *wimaSettings =
      qgcApp()->toolbox()->settingsManager()->wimaSettings();
  int batteryThreshold =
      wimaSettings->lowBatteryThreshold()->rawValue().toInt();
  bool enabled = _enableWimaController.rawValue().toBool();
  unsigned int minTime =
      wimaSettings->minimalRemainingMissionTime()->rawValue().toUInt();

  if (managerVehicle != nullptr && enabled == true) {
    Fact *battery1percentRemaining =
        managerVehicle->battery1FactGroup()->getFact(
            VehicleBatteryFactGroup::_percentRemainingFactName);
    Fact *battery2percentRemaining =
        managerVehicle->battery2FactGroup()->getFact(
            VehicleBatteryFactGroup::_percentRemainingFactName);

    if (battery1percentRemaining->rawValue().toDouble() < batteryThreshold &&
        battery2percentRemaining->rawValue().toDouble() < batteryThreshold) {
      if (!_lowBatteryHandlingTriggered) {
        _lowBatteryHandlingTriggered = true;
        if (!(_missionController->remainingTime() <= minTime)) {
          requestSmartRTL();
        }
      }
    } else {
      _lowBatteryHandlingTriggered = false;
    }
  }
}

void WimaController::_eventTimerHandler() {
  // Battery level check necessary?
  Fact *enableLowBatteryHandling = qgcApp()
                                       ->toolbox()
                                       ->settingsManager()
                                       ->wimaSettings()
                                       ->enableLowBatteryHandling();
  if (enableLowBatteryHandling->rawValue().toBool() &&
      _batteryLevelTicker.ready())
    _checkBatteryLevel();
}

void WimaController::_smartRTLCleanUp(bool flying) {
  if (!flying) { // vehicle has landed
    _switchWaypointManager(_defaultWM);
    _missionController->removeAllFromVehicle();
    _missionController->removeAll();
    disconnect(masterController()->managerVehicle(), &Vehicle::flyingChanged,
               this, &WimaController::_smartRTLCleanUp);
  }
}

void WimaController::_setPhaseDistance(double distance) {
  (void)distance;
  //    if (!qFuzzyCompare(distance, _phaseDistance)) {
  //        _phaseDistance = distance;

  //        emit phaseDistanceChanged();
  //    }
}

void WimaController::_setPhaseDuration(double duration) {
  (void)duration;
  //    if (!qFuzzyCompare(duration, _phaseDuration)) {
  //        _phaseDuration = duration;

  //        emit phaseDurationChanged();
  //    }
}

bool WimaController::_checkSmartRTLPreCondition(QString &errorString) {
  if (!_localPlanDataValid) {
    errorString.append(tr("No WiMA data available. Please define at least a "
                          "measurement and a service area."));
    return false;
  }

  return _rtlWM.checkPrecondition(errorString);
}

void WimaController::_switchWaypointManager(
    WaypointManager::ManagerBase &manager) {
  if (_currentWM != &manager) {
    _currentWM = &manager;

    disconnect(&_overlapWaypoints, &Fact::rawValueChanged, this,
               &WimaController::_updateOverlap);
    disconnect(&_maxWaypointsPerPhase, &Fact::rawValueChanged, this,
               &WimaController::_updateMaxWaypoints);
    disconnect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, this,
               &WimaController::_setStartIndex);

    _maxWaypointsPerPhase.setRawValue(_currentWM->N());
    _overlapWaypoints.setRawValue(_currentWM->overlap());
    _nextPhaseStartWaypointIndex.setRawValue(_currentWM->startIndex());

    connect(&_overlapWaypoints, &Fact::rawValueChanged, this,
            &WimaController::_updateOverlap);
    connect(&_maxWaypointsPerPhase, &Fact::rawValueChanged, this,
            &WimaController::_updateMaxWaypoints);
    connect(&_nextPhaseStartWaypointIndex, &Fact::rawValueChanged, this,
            &WimaController::_setStartIndex);

    emit missionItemsChanged();
    emit currentMissionItemsChanged();
    emit waypointPathChanged();
    emit currentWaypointPathChanged();

    qWarning()
        << "WimaController::_switchWaypointManager: statistics update missing.";
  }
}

void WimaController::_initSmartRTL() {
  QString errorString;
  static int attemptCounter = 0;
  attemptCounter++;

  if (_checkSmartRTLPreCondition(errorString)) {
    _masterController->managerVehicle()->pauseVehicle();
    connect(masterController()->managerVehicle(), &Vehicle::flyingChanged, this,
            &WimaController::_smartRTLCleanUp);
    if (_rtlWM.update()) { // Calculate return path.
      _switchWaypointManager(_rtlWM);
      attemptCounter = 0;
      emit smartRTLPathConfirm();
      return;
    }
  } else if (attemptCounter > SMART_RTL_MAX_ATTEMPTS) {
    errorString.append(
        tr("Smart RTL: No success after maximum number of attempts."));
    qgcApp()->showMessage(errorString);
    attemptCounter = 0;
  } else {
    _smartRTLTimer.singleShot(SMART_RTL_ATTEMPT_INTERVAL, this,
                              &WimaController::_initSmartRTL);
  }
}

void WimaController::_threadFinishedHandler() {
#ifdef SNAKE_SHOW_TIME
  auto start = std::chrono::high_resolution_clock::now();
#endif
  if (!_snakeThread.errorMessage().isEmpty()) {
    qDebug() << _snakeThread.errorMessage();
  }

  if (_snakeThread.tilesUpdated()) {
    this->tiles.clearAndDeleteContents();
    auto tls = _snakeThread.tiles().polygons();
    for (const auto &t : tls) {
      this->tiles.append(new SnakeTile(t, &tiles));
    }
    emit snakeTilesChanged();
    emit snakeTileCenterPointsChanged();

    _nemoInterface.setTilesENU(_snakeThread.tilesENU());
    _nemoInterface.setENUOrigin(_snakeThread.ENUOrigin());
  }
#ifdef SNAKE_SHOW_TIME
  auto end = std::chrono::high_resolution_clock::now();
  std::cout << "WimaController::_threadFinishedHandler(): tiles: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(end -
                                                                     start)
                   .count()
            << " ms" << std::endl;
  start = std::chrono::high_resolution_clock::now();
#endif
  if (_snakeThread.progressUpdated()) {
    emit nemoProgressChanged();
  }
#ifdef SNAKE_SHOW_TIME
  end = std::chrono::high_resolution_clock::now();
  std::cout << "WimaController::_threadFinishedHandler(): progress: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(end -
                                                                     start)
                   .count()
            << " ms" << std::endl;
  start = std::chrono::high_resolution_clock::now();
#endif
  if (_snakeThread.waypointsUpdated()) {
    // Copy waypoints to waypoint manager.
    _snakeWM.clear();
    auto waypoints = _snakeThread.waypoints();
    if (waypoints.size() < 1) {
      return;
    }
    for (auto &vertex : waypoints) {
      _snakeWM.push_back(vertex);
    }

    // Do update.
    this->_snakeWM.update(); // this can take a while (ca. 200ms)

    emit missionItemsChanged();
    emit currentMissionItemsChanged();
    emit currentWaypointPathChanged();
    emit waypointPathChanged();
  }
#ifdef SNAKE_SHOW_TIME
  end = std::chrono::high_resolution_clock::now();
  std::cout << "WimaController::_threadFinishedHandler(): waypoints: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(end -
                                                                     start)
                   .count()
            << " ms" << std::endl;
#endif
}

void WimaController::_switchToSnakeWaypointManager(QVariant variant) {
  if (variant.value<bool>()) {
    _switchWaypointManager(_snakeWM);
  } else {
    _switchWaypointManager(_defaultWM);
  }
}

void WimaController::_switchThreadObject(SnakeThread &thread) {
  if (_currentThread != &thread) {
    // SnakeThread.
    disconnect(_currentThread, &SnakeThread::finished, this,
               &WimaController::_threadFinishedHandler);
    disconnect(_currentThread, &SnakeThread::calcInProgressChanged, this,
               &WimaController::snakeCalcInProgressChanged);
    // NemoInterface.
    disconnect(&_nemoInterface, &NemoInterface::progressChanged, this,
               &WimaController::_progressChangedHandler);
    disconnect(&_nemoInterface, &NemoInterface::statusChanged, this,
               &WimaController::nemoStatusChanged);
    disconnect(&_nemoInterface, &NemoInterface::statusChanged, this,
               &WimaController::nemoStatusStringChanged);

    _currentThread = &thread;

    // SnakeThread.
    connect(_currentThread, &SnakeThread::finished, this,
            &WimaController::_threadFinishedHandler);
    connect(_currentThread, &SnakeThread::calcInProgressChanged, this,
            &WimaController::snakeCalcInProgressChanged);
    // NemoInterface.
    connect(&_nemoInterface, &NemoInterface::progressChanged, this,
            &WimaController::_progressChangedHandler);
    connect(&_nemoInterface, &NemoInterface::statusChanged, this,
            &WimaController::nemoStatusChanged);
    connect(&_nemoInterface, &NemoInterface::statusChanged, this,
            &WimaController::nemoStatusStringChanged);

    emit snakeCalcInProgressChanged();
    emit snakeTilesChanged();
    emit snakeTileCenterPointsChanged();
    emit nemoProgressChanged();
    emit nemoStatusChanged();
    emit nemoStatusStringChanged();
  }
}

void WimaController::_progressChangedHandler() {
  this->_snakeThread.setProgress(this->_nemoInterface.progress());
  this->_snakeThread.start();
}

void WimaController::_enableSnakeChangedHandler() {
  if (this->_enableSnake.rawValue().toBool()) {
    qDebug() << "WimaController: enabling snake.";
    _switchThreadObject(this->_snakeThread);
    this->_nemoInterface.start();
    this->_snakeThread.start();
  } else {
    qDebug() << "WimaController: disabling snake.";
    this->_nemoInterface.stop();
    this->_snakeThread.quit();
    _switchThreadObject(_emptyThread);
  }
}