WimaController.cc

#include "WimaController.h"

#include "utilities.h"

#include "MissionController.h"
#include "MissionSettingsItem.h"
#include "PlanMasterController.h"
#include "QGCApplication.h"
#include "QGCLoggingCategory.h"
#include "SettingsManager.h"
#include "SimpleMissionItem.h"

#include "WimaBridge.h"
#include "WimaPlanData.h"
#include "WimaSettings.h"

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

#include "QVector3D"
#include <QScopedPointer>

#define CLIPPER_SCALE 1000000
#include "clipper/clipper.hpp"

#include <memory>

QGC_LOGGING_CATEGORY(WimaControllerLog, "WimaControllerLog")

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::flightSpeedName = "FlightSpeed";
const char *WimaController::altitudeName = "Altitude";

WimaController::WimaController(QObject *parent)
    : QObject(parent), _joinedArea(), _measurementArea(), _serviceArea(),
      _corridor(), _planDataValid(false),
      _areaInterface(&_measurementArea, &_serviceArea, &_corridor,
                     &_joinedArea),
      _WMSettings(), _emptyWM(_WMSettings, _areaInterface),
      _rtlWM(_WMSettings, _areaInterface),
      _currentWM(&_emptyWM), _WMList{&_emptyWM, &_rtlWM},
      _metaDataMap(FactMetaData::createMapFromJsonFile(
          QStringLiteral(":/json/WimaController.SettingsGroup.json"), this)),
      _enableWimaController(settingsGroup,
                            _metaDataMap[enableWimaControllerName]),
      _flightSpeed(settingsGroup, _metaDataMap[flightSpeedName]),
      _altitude(settingsGroup, _metaDataMap[altitudeName]),
      _lowBatteryHandlingTriggered(false),
      _batteryLevelTicker(EVENT_TIMER_INTERVAL, 1000 /*ms*/) {

  // Set up facts for waypoint manager.
  connect(&_flightSpeed, &Fact::rawValueChanged, this,
          &WimaController::_updateflightSpeed);
  connect(&_altitude, &Fact::rawValueChanged, this,
          &WimaController::_updateAltitude);

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

  // PlanData and Progress.
  connect(WimaBridge::instance(), &WimaBridge::planDataChanged, this,
          &WimaController::planDataChangedHandler);
  connect(WimaBridge::instance(), &WimaBridge::progressChanged, this,
          &WimaController::progressChangedHandler);
}

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

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

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

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

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

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

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

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

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::requestSmartRTL() {
  qCWarning(WimaControllerLog) << "requestSmartRTL() called";
  QString errorString("Smart RTL requested.");
  if (!_SRTLPrecondition(errorString)) {
    qgcApp()->showMessage(errorString);
    return;
  }
  emit smartRTLRequestConfirm();
}

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

bool WimaController::forceUpload() {
  auto &currentMissionItems = _currentWM->currentMissionItems();
  if (currentMissionItems.count() < 1 || !_missionController ||
      !_masterController) {
    qCWarning(WimaControllerLog)
        << "forceUpload(): error:"
        << "currentMissionItems.count(): " << currentMissionItems.count()
        << "_missionController: " << _missionController
        << "_masterController: " << _masterController;
    return false;
  } else {
    _missionController->removeAll();
    // Set homeposition of settingsItem.
    QmlObjectListModel *visuals = _missionController->visualItems();
    MissionSettingsItem *settingsItem =
        visuals->value<MissionSettingsItem *>(0);
    if (settingsItem == nullptr) {
      Q_ASSERT(false);
      qCWarning(WimaControllerLog) << "updateCurrentMissionItems(): nullptr";
      return false;
    } else {
      settingsItem->setCoordinate(_WMSettings.homePosition());

      // Copy mission items to _missionController and send them.
      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() {
  if (_masterController && _missionController) {
    _masterController->removeAllFromVehicle();
    _missionController->removeAll();
  }
}

void WimaController::executeSmartRTL() {
  qCWarning(WimaControllerLog) << "executeSmartRTL() called";
  if (_masterController && _masterController->managerVehicle()) {
    forceUpload();
    _masterController->managerVehicle()->startMission();
  }
}

void WimaController::initSmartRTL() {
  qCWarning(WimaControllerLog) << "initSmartRTL() called";
  _initSmartRTL();
}

void WimaController::removeVehicleTrajectoryHistory() {
  if (_masterController && _masterController->managerVehicle()) {
    _masterController->managerVehicle()
        ->trajectoryPoints()
        ->clearAndDeleteContents();
  }
}

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;
}

void WimaController::planDataChangedHandler() {

  // reset visual items
  _areas.clear();
  _measurementArea = WimaMeasurementAreaData();
  _serviceArea = WimaServiceAreaData();
  _corridor = WimaCorridorData();
  _joinedArea = WimaJoinedAreaData();

  emit visualItemsChanged();
  emit missionItemsChanged();
  emit waypointPathChanged();

  _planDataValid = false;

  auto planData = WimaBridge::instance()->planData();
  // 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;
  }

  emit visualItemsChanged();

  _WMSettings.setHomePosition(QGeoCoordinate(
      _serviceArea.depot().latitude(), _serviceArea.depot().longitude(), 0));

  _planDataValid = true;
}

void WimaController::progressChangedHandler() {
  _measurementArea.setProgress(WimaBridge::instance()->progress());
}

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

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

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

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

      if (battery1percentRemaining->rawValue().toDouble() < threshold &&
          battery2percentRemaining->rawValue().toDouble() < threshold) {
        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 && _missionController) { // vehicle has landed
    _switchWaypointManager(_emptyWM);
    _missionController->removeAll();
    disconnect(masterController()->managerVehicle(), &Vehicle::flyingChanged,
               this, &WimaController::_smartRTLCleanUp);
  }
}

bool WimaController::_SRTLPrecondition(QString &errorString) {
  if (!_planDataValid) {
    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;
    emit missionItemsChanged();
    emit waypointPathChanged();

    qCWarning(WimaControllerLog) << "_switchWaypointManager: statistics update "
                                    "missing.";
  }
}

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

  if (_SRTLPrecondition(errorString)) {
    if (_missionController && _masterController &&
        _masterController->managerVehicle()) {
      _masterController->managerVehicle()->pauseVehicle();
      connect(_masterController->managerVehicle(), &Vehicle::flyingChanged,
              this, &WimaController::_smartRTLCleanUp);
      if (_rtlWM.update()) { // Calculate return path.
        _switchWaypointManager(_rtlWM);
        removeFromVehicle();
        attemptCounter = 0;
        emit smartRTLPathConfirm();
      }
    }
  } 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);
  }
}