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 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]){
  // 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->currentMissionItems());
}

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

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

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::_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();
  _planDataValid = false;

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

  // Extract areas.
  auto planData = WimaBridge::instance()->planData();

  // Measurement Area.
  if (planData.measurementArea().coordinateList().size() >= 3) {
    _measurementArea = planData.measurementArea();
    _areas.append(&_measurementArea);

    // Service Area.
    if (planData.serviceArea().coordinateList().size() >= 3) {
      _serviceArea = planData.serviceArea();
      _areas.append(&_serviceArea);

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

      // Joined Area.
      if (planData.joinedArea().coordinateList().size() >= 3) {
        _joinedArea = planData.joinedArea();
        _areas.append(&_joinedArea);

        _planDataValid = true;

        // Corridor.
        if (planData.corridor().coordinateList().size() >= 3) {
          _corridor = planData.corridor();
        }
      }
    }
  }

  if (_planDataValid) {
    emit visualItemsChanged();
  } else {
    _areas.clear();
    _measurementArea = WimaMeasurementAreaData();
    _serviceArea = WimaServiceAreaData();
    _corridor = WimaCorridorData();
    _joinedArea = WimaJoinedAreaData();
  }
}

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