WimaPlaner.cc

#include "WimaPlaner.h"

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

#include "Geometry/GeoUtilities.h"
#include "Geometry/PlanimetryCalculus.h"
#include "OptimisationTools.h"

#include "CircularSurvey.h"
#include "Geometry/WimaArea.h"
#include "Geometry/WimaAreaData.h"
#include "WimaBridge.h"

#include "StateMachine.h"
using namespace wima_planer_detail;

#include <functional>

QGC_LOGGING_CATEGORY(WimaPlanerLog, "WimaPlanerLog")

class CommandRAII {
  std::function<void(void)> f;

public:
  CommandRAII(const std::function<void(void)> &fun) : f(fun) {}
  ~CommandRAII() { f(); }
};

const char *WimaPlaner::wimaFileExtension = "wima";
const char *WimaPlaner::areaItemsName = "AreaItems";
const char *WimaPlaner::missionItemsName = "MissionItems";

WimaPlaner::WimaPlaner(QObject *parent)
    : QObject(parent), _masterController(nullptr), _missionController(nullptr),
      _currentAreaIndex(-1), _wimaBridge(nullptr), _copyMAreaToSurvey(true),
      _copySAreaToSurvey(true), _corridorChanged(true), _joinedArea(this),
      _arrivalPathLength(0), _returnPathLength(0), _survey(nullptr),
      _surveyChanged(true), _synchronized(false), _nemoInterface(this),
      _stateMachine(new StateMachine) {

  connect(this, &WimaPlaner::currentPolygonIndexChanged, this,
          &WimaPlaner::updatePolygonInteractivity);

  // Enable monitoring (state update)
  enableMonitoring();

#ifndef NDEBUG
  // for debugging and testing purpose, remove if not needed anymore
  connect(&_autoLoadTimer, &QTimer::timeout, this,
          &WimaPlaner::autoLoadMission);
  _autoLoadTimer.setSingleShot(true);
  _autoLoadTimer.start(300);
#endif

  // NemoInterface
  connect(&this->_nemoInterface, &NemoInterface::progressChanged, [this] {
    this->_measurementArea.setProgress(this->_nemoInterface.progress());
  });

  // StateMachine
  connect(this->_stateMachine.get(), &StateMachine::upToDateChanged, this,
          &WimaPlaner::needsUpdateChanged);
}

WimaPlaner::~WimaPlaner() {}

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

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

QmlObjectListModel *WimaPlaner::visualItems() { return &_visualItems; }

int WimaPlaner::currentPolygonIndex() const { return _currentAreaIndex; }

QString WimaPlaner::currentFile() const { return _currentFile; }

QStringList WimaPlaner::loadNameFilters() const {
  QStringList filters;

  filters << tr("Supported types (*.%1 *.%2)")
                 .arg(wimaFileExtension)
                 .arg(AppSettings::planFileExtension)
          << tr("All Files (*.*)");
  return filters;
}

QStringList WimaPlaner::saveNameFilters() const {
  QStringList filters;

  filters << tr("Supported types (*.%1 *.%2)")
                 .arg(wimaFileExtension)
                 .arg(AppSettings::planFileExtension);
  return filters;
}

QString WimaPlaner::fileExtension() const { return wimaFileExtension; }

QGeoCoordinate WimaPlaner::joinedAreaCenter() const {
  return _joinedArea.center();
}

WimaBridge *WimaPlaner::wimaBridge() { return _wimaBridge; }

NemoInterface *WimaPlaner::nemoInterface() { return &_nemoInterface; }

void WimaPlaner::setMasterController(PlanMasterController *masterC) {
  _masterController = masterC;
  emit masterControllerChanged();
}

void WimaPlaner::setMissionController(MissionController *missionC) {
  _missionController = missionC;
  emit missionControllerChanged();
}

void WimaPlaner::setCurrentPolygonIndex(int index) {
  if (index >= 0 && index < _visualItems.count() &&
      index != _currentAreaIndex) {
    _currentAreaIndex = index;

    emit currentPolygonIndexChanged(index);
  }
}

void WimaPlaner::setWimaBridge(WimaBridge *bridge) {
  if (bridge != nullptr) {
    _wimaBridge = bridge;
    emit wimaBridgeChanged();
  }
}

bool WimaPlaner::synchronized() { return _synchronized; }

bool WimaPlaner::needsUpdate() { return !this->_stateMachine->upToDate(); }

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

void WimaPlaner::removeArea(int index) {
  if (index >= 0 && index < _visualItems.count()) {
    WimaArea *area = qobject_cast<WimaArea *>(_visualItems.removeAt(index));

    if (area == nullptr) {
      qCWarning(WimaPlanerLog)
          << "removeArea(): nullptr catched, internal error.";
      return;
    }
    area->clear();
    area->borderPolygon()->clear();

    emit visualItemsChanged();

    if (_visualItems.count() == 0) {
      // this branch is reached if all items are removed
      // to guarentee proper behavior, _currentAreaIndex must be set to a
      // invalid value, as on constructor init.
      resetAllInteractive();
      _currentAreaIndex = -1;
      return;
    }

    if (_currentAreaIndex >= _visualItems.count()) {
      setCurrentPolygonIndex(_visualItems.count() - 1);
    } else {
      updatePolygonInteractivity(_currentAreaIndex);
    }
  } else {
    qCWarning(WimaPlanerLog) << "removeArea(): Index out of bounds!";
  }
}

bool WimaPlaner::addMeasurementArea() {
  if (!_visualItems.contains(&_measurementArea)) {
    _visualItems.append(&_measurementArea);

    int newIndex = _visualItems.count() - 1;
    setCurrentPolygonIndex(newIndex);

    emit visualItemsChanged();
    return true;
  } else {
    return false;
  }
}

bool WimaPlaner::addServiceArea() {
  if (!_visualItems.contains(&_serviceArea)) {
    _visualItems.append(&_serviceArea);

    int newIndex = _visualItems.count() - 1;
    setCurrentPolygonIndex(newIndex);

    emit visualItemsChanged();
    return true;
  } else {
    return false;
  }
}

bool WimaPlaner::addCorridor() {
  if (!_visualItems.contains(&_corridor)) {
    _visualItems.append(&_corridor);

    int newIndex = _visualItems.count() - 1;
    setCurrentPolygonIndex(newIndex);

    emit visualItemsChanged();
    return true;
  } else {
    return false;
  }
}

void WimaPlaner::removeAll() {
  bool changesApplied = false;
  while (_visualItems.count() > 0) {
    removeArea(0);
    changesApplied = true;
  }

  _missionController->removeAll();

  _currentFile = "";

  _survey = nullptr;

  emit currentFileChanged();
  if (changesApplied)
    emit visualItemsChanged();
}

void WimaPlaner::update() { this->_update(); }

void WimaPlaner::_update() {
  setSynchronized(false);

  switch (this->_stateMachine->state()) {
  case STATE::NEEDS_INIT: {
    this->_stateMachine->updateState(EVENT::INIT_DONE);
    this->_update();
  } break;

  case STATE::NEEDS_J_AREA_UPDATE: {
    // check if at least service area and measurement area are available
    if (_visualItems.indexOf(&_serviceArea) == -1 ||
        _visualItems.indexOf(&_measurementArea) == -1)
      return;

    // Check if polygons have at least three vertices
    if (_serviceArea.count() < 3) {
      qgcApp()->showMessage(tr("Service area has less than three vertices."));
      return;
    }

    if (_measurementArea.count() < 3) {
      qgcApp()->showMessage(
          tr("Measurement area has less than three vertices."));
      return;
    }

    // Check for simple polygons
    if (!_serviceArea.isSimplePolygon()) {
      qgcApp()->showMessage(tr("Service area is not a simple polygon. "
                               "Only simple polygons allowed.\n"));
      return;
    }

    if (!_corridor.isSimplePolygon() && _corridor.count() > 0) {
      qgcApp()->showMessage(tr("Corridor is not a simple polygon. Only "
                               "simple polygons allowed.\n"));
      return;
    }

    if (!_measurementArea.isSimplePolygon()) {
      qgcApp()->showMessage(tr("Measurement area is not a simple "
                               "polygon. Only simple polygons allowed.\n"));
      return;
    }

    if (!_serviceArea.containsCoordinate(_serviceArea.depot())) {
      qgcApp()->showMessage(tr("Depot not inside service area."));
      return;
    }

    // Join areas.
    _joinedArea.setPath(_serviceArea.path());
    if (_corridor.count() >= 3) {
      _joinedArea.join(_corridor);
    }
    if (!_joinedArea.join(_measurementArea)) {
      qgcApp()->showMessage(
          tr("Not able to join areas. Service and measurement area"
             " must be overlapping, or connected through a "
             "corridor."));
      return;
    }
    this->_stateMachine->updateState(EVENT::J_AREA_UPDATED);
    this->_update();
  } break; // STATE::NEEDS_J_AREA_UPDATE

  case STATE::NEEDS_SURVEY_UPDATE: {
    // Need to insert Survey?
    QmlObjectListModel *missionItems = _missionController->visualItems();
    int surveyIndex = missionItems->indexOf(_survey);
    // Create survey item if not yet present.
    if (surveyIndex < 0) {
      _missionController->insertComplexMissionItem(
          _missionController->circularSurveyComplexItemName(),
          _measurementArea.center(), missionItems->count());
      _survey = qobject_cast<CircularSurvey *>(
          missionItems->get(missionItems->count() - 1));

      if (_survey == nullptr) {
        qCWarning(WimaPlanerLog) << "_survey == nullptr";
        return;
      }

      // establish connections
      _survey->setRefPoint(_measurementArea.center());
      _survey->setHidePolygon(true);
      connect(_survey, &CircularSurvey::calculatingChanged, this,
              &WimaPlaner::CSCalculatingChangedHandler);
      connect(_survey, &CircularSurvey::missionItemReady, this,
              &WimaPlaner::CSMissionItemReadyHandler);
      connect(_survey, &CircularSurvey::destroyed, this,
              &WimaPlaner::CSDestroyedHandler);
    }

    // update survey area
    disconnect(_survey, &CircularSurvey::calculatingChanged, this,
               &WimaPlaner::CSCalculatingChangedHandler);
    _survey->setMeasurementArea(this->_measurementArea);
    _survey->setJoinedArea(this->_joinedArea);
    connect(_survey, &CircularSurvey::calculatingChanged, this,
            &WimaPlaner::CSCalculatingChangedHandler);

    // Folloing statement just for completeness.
    this->_stateMachine->updateState(EVENT::SURVEY_UPDATE_TRIGGERED);
  } break; // STATE::NEEDS_SURVEY_UPDATE

  case STATE::WAITING_FOR_SURVEY_UPDATE: {
  } break;

  case STATE::NEEDS_PATH_UPDATE: {
    // Check if survey is present.
    QmlObjectListModel *missionItems = _missionController->visualItems();
    int surveyIndex = missionItems->indexOf(_survey);
    if (surveyIndex < 0) {
      this->_stateMachine->updateState(EVENT::SURVEY_DESTROYED);
      this->_update();
    } else {

      // Remove old arrival and return path.
      int size = missionItems->count();
      for (int i = surveyIndex + 1; i < size; i++)
        _missionController->removeMissionItem(surveyIndex + 1);
      for (int i = surveyIndex - 1; i > 1; i--)
        _missionController->removeMissionItem(i);

      // set home position to serArea center
      MissionSettingsItem *settingsItem =
          qobject_cast<MissionSettingsItem *>(missionItems->get(0));
      if (settingsItem == nullptr) {
        qCWarning(WimaPlanerLog) << "update(): settingsItem == nullptr";
        return;
      }

      // set altitudes
      auto depot = _serviceArea.depot();
      depot.setAltitude(0);
      settingsItem->setCoordinate(depot);

      // set takeoff position
      bool setCommandNeeded = false;
      if (missionItems->count() < 3) {
        setCommandNeeded = true;
        _missionController->insertSimpleMissionItem(depot, 1);
      }
      SimpleMissionItem *takeOffItem =
          qobject_cast<SimpleMissionItem *>(missionItems->get(1));
      if (takeOffItem == nullptr) {
        qCWarning(WimaPlanerLog) << "update(): takeOffItem == nullptr";
        return;
      }
      if (setCommandNeeded)
        _missionController->setTakeoffCommand(*takeOffItem);
      takeOffItem->setCoordinate(depot);

      if (_survey->visualTransectPoints().size() == 0) {
        qCWarning(WimaPlanerLog) << "update(): survey no points";
        return;
      }

      // calculate path from take off to survey
      QGeoCoordinate start = depot;
      QGeoCoordinate end = _survey->coordinate();
      QVector<QGeoCoordinate> path;
      if (!shortestPath(start, end, path)) {
        qgcApp()->showMessage(
            QString(tr("Not able to calculate path from "
                       "takeoff position to measurement area."))
                .toLocal8Bit()
                .data());
        return;
      }
      _arrivalPathLength = path.size() - 1;
      int sequenceNumber = 0;
      for (int i = 1; i < path.count() - 1; i++) {
        sequenceNumber = _missionController->insertSimpleMissionItem(
            path[i], missionItems->count() - 1);
        _missionController->setCurrentPlanViewIndex(sequenceNumber, true);
      }

      // calculate return path
      start = _survey->exitCoordinate();
      end = depot;
      path.clear();
      if (!shortestPath(start, end, path)) {
        qgcApp()->showMessage(
            QString(tr("Not able to calculate the path from "
                       "measurement area to landing position."))
                .toLocal8Bit()
                .data());
        return;
      }
      _returnPathLength =
          path.size() - 1; // -1: fist item is last measurement point
      for (int i = 1; i < path.count() - 1; i++) {
        sequenceNumber = _missionController->insertSimpleMissionItem(
            path[i], missionItems->count());
        _missionController->setCurrentPlanViewIndex(sequenceNumber, true);
      }

      // create land position item
      sequenceNumber = _missionController->insertSimpleMissionItem(
          depot, missionItems->count());
      _missionController->setCurrentPlanViewIndex(sequenceNumber, true);
      SimpleMissionItem *landItem = qobject_cast<SimpleMissionItem *>(
          missionItems->get(missionItems->count() - 1));
      if (landItem == nullptr) {
        qCWarning(WimaPlanerLog) << "update(): landItem == nullptr";
        return;
      }
      if (!_missionController->setLandCommand(*landItem))
        return;

      this->_stateMachine->updateState(EVENT::PATH_UPDATED);
    }
  } break; // STATE::NEEDS_PATH_UPDATE

  case STATE::UP_TO_DATE: {
  } break; // STATE::UP_TO_DATE

  } // switch
}

void WimaPlaner::CSDestroyedHandler() {
  this->_stateMachine->updateState(EVENT::SURVEY_DESTROYED);
}

void WimaPlaner::CSMissionItemReadyHandler() {
  this->_stateMachine->updateState(EVENT::SURVEY_UPDATED);
  this->_update();
}

void WimaPlaner::CSCalculatingChangedHandler() {
  if (this->_survey->calculating()) {
    this->_stateMachine->updateState(EVENT::SURVEY_UPDATE_TRIGGERED);
  }
}

void WimaPlaner::mAreaPathChangedHandler() {
  this->_stateMachine->updateState(EVENT::M_AREA_PATH_CHANGED);
}

void WimaPlaner::mAreaTilesChangedHandler() {
  this->_nemoInterface.setTileData(this->_measurementArea.tileData());
  this->_stateMachine->updateState(EVENT::M_AREA_TILES_CHANGED);
}

void WimaPlaner::mAreaProgressChangedHandler() {
  this->_stateMachine->updateState(EVENT::M_AREA_PROGRESS_CHANGED);
}

void WimaPlaner::sAreaPathChangedHandler() {
  this->_stateMachine->updateState(EVENT::S_AREA_PATH_CHANGED);
}

void WimaPlaner::corridorPathChangedHandler() {
  this->_stateMachine->updateState(EVENT::CORRIDOR_PATH_CHANGED);
}

void WimaPlaner::depotChangedHandler() {
  this->_stateMachine->updateState(EVENT::DEPOT_CHANGED);
}

void WimaPlaner::saveToCurrent() { saveToFile(_currentFile); }

void WimaPlaner::saveToFile(const QString &filename) {
  if (filename.isEmpty()) {
    return;
  }

  QString planFilename = filename;
  if (!QFileInfo(filename).fileName().contains(".")) {
    planFilename += QString(".%1").arg(wimaFileExtension);
  }

  QFile file(planFilename);
  if (!file.open(QIODevice::WriteOnly | QIODevice::Text)) {
    qgcApp()->showMessage(
        tr("Plan save error %1 : %2").arg(filename).arg(file.errorString()));
    _currentFile.clear();
    emit currentFileChanged();
  } else {
    FileType fileType = FileType::WimaFile;
    if (planFilename.contains(QString(".%1").arg(wimaFileExtension))) {
      fileType = FileType::WimaFile;
    } else if (planFilename.contains(
                   QString(".%1").arg(AppSettings::planFileExtension))) {
      fileType = FileType::PlanFile;
    } else {
      if (planFilename.contains(".")) {
        qgcApp()->showMessage(tr("File format not supported"));
      } else {
        qgcApp()->showMessage(tr("File without file extension not accepted."));
        return;
      }
    }

    QJsonDocument saveDoc = saveToJson(fileType);
    file.write(saveDoc.toJson());
    if (_currentFile != planFilename) {
      _currentFile = planFilename;
      emit currentFileChanged();
    }
  }
}

bool WimaPlaner::loadFromCurrent() { return loadFromFile(_currentFile); }

bool WimaPlaner::loadFromFile(const QString &filename) {
  // Remove obsolete connections.
  disableMonitoring();
  CommandRAII onExit([this] { this->enableMonitoring(); });
  // disconnect old survey
  if (_survey != nullptr) {
    disconnect(_survey, &CircularSurvey::calculatingChanged, this,
               &WimaPlaner::CSCalculatingChangedHandler);
    disconnect(_survey, &CircularSurvey::missionItemReady, this,
               &WimaPlaner::CSMissionItemReadyHandler);
    disconnect(_survey, &CircularSurvey::destroyed, this,
               &WimaPlaner::CSDestroyedHandler);
  }

  setSynchronized(false);

  // Precondition.
  QString errorString;
  QString errorMessage =
      tr("Error loading Plan file (%1). %2").arg(filename).arg("%1");

  if (filename.isEmpty()) {
    return false;
  }

  QFileInfo fileInfo(filename);
  QFile file(filename);

  if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) {
    errorString = file.errorString() + QStringLiteral(" ") + filename;
    qgcApp()->showMessage(errorMessage.arg(errorString));
    return false;
  }

  if (fileInfo.suffix() == wimaFileExtension) {
    QJsonDocument jsonDoc;
    QByteArray bytes = file.readAll();

    if (!JsonHelper::isJsonFile(bytes, jsonDoc, errorString)) {
      qgcApp()->showMessage(errorMessage.arg(errorString));
      return false;
    }

    QJsonObject json = jsonDoc.object();
    // AreaItems
    QJsonArray areaArray = json[areaItemsName].toArray();
    _visualItems.clear();

    int validAreaCounter = 0;
    for (int i = 0; i < areaArray.size() && validAreaCounter < 3; i++) {
      QJsonObject jsonArea = areaArray[i].toObject();

      if (jsonArea.contains(WimaArea::areaTypeName) &&
          jsonArea[WimaArea::areaTypeName].isString()) {
        if (jsonArea[WimaArea::areaTypeName] ==
            WimaMeasurementArea::WimaMeasurementAreaName) {
          bool success = _measurementArea.loadFromJson(jsonArea, errorString);

          if (!success) {
            qgcApp()->showMessage(errorMessage.arg(errorString));
            return false;
          }

          validAreaCounter++;
          _visualItems.append(&_measurementArea);
          emit visualItemsChanged();
        } else if (jsonArea[WimaArea::areaTypeName] ==
                   WimaServiceArea::wimaServiceAreaName) {
          bool success = _serviceArea.loadFromJson(jsonArea, errorString);

          if (!success) {
            qgcApp()->showMessage(errorMessage.arg(errorString));
            return false;
          }

          validAreaCounter++;
          _visualItems.append(&_serviceArea);
          emit visualItemsChanged();
        } else if (jsonArea[WimaArea::areaTypeName] ==
                   WimaCorridor::WimaCorridorName) {
          bool success = _corridor.loadFromJson(jsonArea, errorString);

          if (!success) {
            qgcApp()->showMessage(errorMessage.arg(errorString));
            return false;
          }

          validAreaCounter++;
          _visualItems.append(&_corridor);
          emit visualItemsChanged();
        } else {
          errorString +=
              QString(tr("%s not supported.\n").arg(WimaArea::areaTypeName));
          qgcApp()->showMessage(errorMessage.arg(errorString));
          return false;
        }
      } else {
        errorString += QString(tr("Invalid or non existing entry for %s.\n")
                                   .arg(WimaArea::areaTypeName));
        return false;
      }
    }

    _currentFile.sprintf("%s/%s.%s", fileInfo.path().toLocal8Bit().data(),
                         fileInfo.completeBaseName().toLocal8Bit().data(),
                         wimaFileExtension);

    emit currentFileChanged();

    QJsonObject missionObject = json[missionItemsName].toObject();

    QJsonDocument missionJsonDoc = QJsonDocument(missionObject);
    // create temporary file with missionItems
    QFile temporaryFile;
    QString cropedFileName = filename.section("/", 0, -2);
    QString temporaryFileName;
    for (int i = 0;; i++) {
      temporaryFileName =
          cropedFileName +
          QString("/temp%1.%2").arg(i).arg(AppSettings::planFileExtension);

      if (!QFile::exists(temporaryFileName)) {
        temporaryFile.setFileName(temporaryFileName);
        if (temporaryFile.open(QIODevice::WriteOnly | QIODevice::Text)) {
          break;
        }
      }

      if (i > 1000) {
        qCWarning(WimaPlanerLog)
            << "loadFromFile(): not able to create temporary file.";
        return false;
      }
    }

    temporaryFile.write(missionJsonDoc.toJson());
    temporaryFile.close();

    // load from temporary file
    _masterController->loadFromFile(temporaryFileName);
    QmlObjectListModel *missionItems = _missionController->visualItems();
    _survey = nullptr;
    for (int i = 0; i < missionItems->count(); i++) {
      _survey = missionItems->value<CircularSurvey *>(i);
      if (_survey != nullptr) {
        _survey->setHidePolygon(true);
        connect(_survey, &CircularSurvey::calculatingChanged, this,
                &WimaPlaner::CSCalculatingChangedHandler);
        connect(_survey, &CircularSurvey::missionItemReady, this,
                &WimaPlaner::CSMissionItemReadyHandler);
        connect(_survey, &CircularSurvey::destroyed, this,
                &WimaPlaner::CSDestroyedHandler);
        break;
      }
    }

    // remove temporary file
    if (!temporaryFile.remove()) {
      qCWarning(WimaPlanerLog)
          << "WimaPlaner::loadFromFile(): not able to remove "
             "temporary file.";
    }
    return true;
  } else {
    errorString += QString(tr("File extension not supported.\n"));
    qgcApp()->showMessage(errorMessage.arg(errorString));
    return false;
  }
}

void WimaPlaner::updatePolygonInteractivity(int index) {
  if (index >= 0 && index < _visualItems.count()) {
    resetAllInteractive();
    WimaArea *interactivePoly =
        qobject_cast<WimaArea *>(_visualItems.get(index));
    if (interactivePoly != nullptr)
      interactivePoly->setWimaAreaInteractive(true);
  }
}

void WimaPlaner::synchronize() {
  if (_wimaBridge != nullptr) {
    if (!needsUpdate()) {
      auto planData = toPlanData();
      if (planData) {
        (void)_wimaBridge->setWimaPlanData(planData);
        setSynchronized(true);
      } else {
        qCWarning(WimaPlanerLog) << "error creating plan data.";
      }
    }
  } else {
    qCWarning(WimaPlanerLog) << "no container assigned.";
  }
}

bool WimaPlaner::shortestPath(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;
  QVector<QPointF> path2D;
  toCartesian(destination, start, end2D);
  bool retVal =
      PolygonCalculus::shortestPath(polygon2D, start2D, end2D, path2D);
  toGeoList(path2D, /*origin*/ start, path);

  return retVal;
}

void WimaPlaner::setSynchronized(bool s) {
  if (this->_synchronized != s) {
    this->_synchronized = s;
    emit this->synchronizedChanged();
  }
}

void WimaPlaner::enableMonitoring() {
  connect(&this->_measurementArea, &WimaArea::pathChanged, this,
          &WimaPlaner::mAreaPathChangedHandler);
  connect(&this->_measurementArea, &WimaMeasurementArea::tilesChanged, this,
          &WimaPlaner::mAreaTilesChangedHandler);
  connect(&this->_measurementArea, &WimaMeasurementArea::progressChanged, this,
          &WimaPlaner::mAreaProgressChangedHandler);
  connect(&this->_serviceArea, &WimaArea::pathChanged, this,
          &WimaPlaner::sAreaPathChangedHandler);
  connect(&this->_serviceArea, &WimaServiceArea::depotChanged, this,
          &WimaPlaner::depotChangedHandler);
  connect(&this->_corridor, &WimaArea::pathChanged, this,
          &WimaPlaner::corridorPathChangedHandler);
}

void WimaPlaner::disableMonitoring() {
  disconnect(&this->_measurementArea, &WimaArea::pathChanged, this,
             &WimaPlaner::mAreaPathChangedHandler);
  disconnect(&this->_measurementArea, &WimaMeasurementArea::tilesChanged, this,
             &WimaPlaner::mAreaTilesChangedHandler);
  disconnect(&this->_measurementArea, &WimaMeasurementArea::progressChanged,
             this, &WimaPlaner::mAreaProgressChangedHandler);
  disconnect(&this->_serviceArea, &WimaArea::pathChanged, this,
             &WimaPlaner::sAreaPathChangedHandler);
  disconnect(&this->_serviceArea, &WimaServiceArea::depotChanged, this,
             &WimaPlaner::depotChangedHandler);
  disconnect(&this->_corridor, &WimaArea::pathChanged, this,
             &WimaPlaner::corridorPathChangedHandler);
}

void WimaPlaner::resetAllInteractive() {
  // Marks all areas as inactive (area.interactive == false)
  int itemCount = _visualItems.count();
  if (itemCount > 0) {
    for (int i = 0; i < itemCount; i++) {
      WimaArea *iteratorPoly = qobject_cast<WimaArea *>(_visualItems.get(i));
      iteratorPoly->setWimaAreaInteractive(false);
    }
  }
}

void WimaPlaner::setInteractive() {
  updatePolygonInteractivity(_currentAreaIndex);
}

/*!
 * \fn WimaPlanData WimaPlaner::toPlanData()
 *
 * Returns a \c WimaPlanData object containing information about the current
 * mission. The \c WimaPlanData object holds only the data which is relevant
 * for the \c WimaController class. Should only be called if update() was
 * successful.
 *
 * \sa WimaController, WimaPlanData
 */
QSharedPointer<WimaPlanData> WimaPlaner::toPlanData() {
  QSharedPointer<WimaPlanData> planData(new WimaPlanData());

  // store areas
  planData->append(WimaMeasurementAreaData(_measurementArea));
  planData->append(WimaServiceAreaData(_serviceArea));
  planData->append(WimaCorridorData(_corridor));
  planData->append(WimaJoinedAreaData(_joinedArea));

  // convert mission items to mavlink commands
  if (_missionController && _missionController->visualItems()) {
    int surveyIndex = _missionController->visualItems()->indexOf(_survey);
    if (surveyIndex > 0) {
      QList<MissionItem *> missionItems;
      _survey->appendMissionItems(missionItems, nullptr);
      planData->append(missionItems);
      planData->setTransects(this->_survey->rawTransects());
      return planData;
    }
  }
  return QSharedPointer<WimaPlanData>();
}

#ifndef NDEBUG
void WimaPlaner::autoLoadMission() {
  loadFromFile("/home/valentin/Desktop/drones/qgroundcontrol/Paths/"
               "KlingenbachTest.wima");
  synchronize();
}
#endif

QJsonDocument WimaPlaner::saveToJson(FileType fileType) {
  /// This function save all areas (of WimaPlaner) and all mission items (of
  /// MissionController) to a QJsonDocument
  /// @param fileType is either WimaFile or PlanFile (enum), if fileType ==
  /// PlanFile only mission items are stored
  QJsonObject json;

  if (fileType == FileType::WimaFile) {
    QJsonArray jsonArray;

    for (int i = 0; i < _visualItems.count(); i++) {
      QJsonObject json;

      WimaArea *area = qobject_cast<WimaArea *>(_visualItems.get(i));

      if (area == nullptr) {
        qCWarning(WimaPlanerLog) << "saveing, area == nullptr!";
        return QJsonDocument();
      }

      // check the type of area, create and append the JsonObject to the
      // JsonArray once determined
      WimaMeasurementArea *opArea = qobject_cast<WimaMeasurementArea *>(area);
      if (opArea != nullptr) {
        opArea->saveToJson(json);
        jsonArray.append(json);
        continue;
      }

      WimaServiceArea *serArea = qobject_cast<WimaServiceArea *>(area);
      if (serArea != nullptr) {
        serArea->saveToJson(json);
        jsonArray.append(json);
        continue;
      }

      WimaCorridor *corridor = qobject_cast<WimaCorridor *>(area);
      if (corridor != nullptr) {
        corridor->saveToJson(json);
        jsonArray.append(json);
        continue;
      }

      // if non of the obove branches was trigger, type must be WimaArea
      area->saveToJson(json);
      jsonArray.append(json);
    }

    json[areaItemsName] = jsonArray;
    json[missionItemsName] = _masterController->saveToJson().object();

    return QJsonDocument(json);
  } else if (fileType == FileType::PlanFile) {
    return _masterController->saveToJson();
  }

  return QJsonDocument(json);
}