MeasurementComplexItem.cc

#include "MeasurementComplexItem.h"

#include "CircularGenerator.h"
#include "LinearGenerator.h"
#include "RoutingThread.h"
#include "geometry/GenericCircle.h"
#include "geometry/MeasurementArea.h"
#include "geometry/SafeArea.h"
#include "geometry/clipper/clipper.hpp"
#include "geometry/snake.h"
#include "nemo_interface/SnakeTile.h"

// QGC
#include "JsonHelper.h"
#include "PlanMasterController.h"
#include "QGCApplication.h"
#include "QGCLoggingCategory.h"

// boost
#include <boost/units/io.hpp>
#include <boost/units/systems/si.hpp>

QGC_LOGGING_CATEGORY(MeasurementComplexItemLog, "MeasurementComplexItemLog")

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

const char *MeasurementComplexItem::settingsGroup = "MeasurementComplexItem";
const char *MeasurementComplexItem::jsonComplexItemTypeValue =
    "MeasurementComplexItem";
const QString MeasurementComplexItem::name(tr("Measurement"));

namespace {
const char *variantIndexKey = "VariantIndex";
const char *altitudeKey = "Altitude";
const char *areaDataKey = "AreaData";
const char *variantNamesKey = "VariantNames";
const char *generatorArrayKey = "GeneratorArray";
const char *variantArrayKey = "VariantArray";
const char *generatorIndexKey = "GeneratorIndex";
} // namespace

MeasurementComplexItem::MeasurementComplexItem(
    PlanMasterController *masterController, bool flyView,
    const QString &kmlOrShpFile, QObject *parent)
    : ComplexMissionItem(masterController, flyView, parent), _sequenceNumber(0),
      _followTerrain(false), _state(STATE::IDLE),
      _metaDataMap(FactMetaData::createMapFromJsonFile(
          QStringLiteral(":/json/MeasurementComplexItem.SettingsGroup.json"),
          this)),
      _altitude(settingsGroup, _metaDataMap[altitudeKey]),
      _variantIndex(settingsGroup, _metaDataMap[variantIndexKey]),
      _pAreaData(new AreaData(this)), _pEditorData(new AreaData(this)),
      _pCurrentData(_pAreaData), _pGenerator(nullptr),
      _pWorker(new RoutingThread(this)) {

  // Setup altitude.
  _altitude.setRawValue(qgcApp()
                            ->toolbox()
                            ->settingsManager()
                            ->appSettings()
                            ->defaultMissionItemAltitude()
                            ->rawValue());
  connect(&_altitude, &SettingsFact::rawValueChanged, [this] {
    emit this->minAMSLAltitudeChanged(this->_altitude.rawValue().toDouble());
  });
  connect(&_altitude, &SettingsFact::rawValueChanged, [this] {
    emit this->maxAMSLAltitudeChanged(this->_altitude.rawValue().toDouble());
  });
  connect(&_altitude, &SettingsFact::rawValueChanged, [this] {
    emit this->amslEntryAltChanged(this->_altitude.rawValue().toDouble());
  });
  connect(&_altitude, &SettingsFact::rawValueChanged, [this] {
    emit this->amslExitAltChanged(this->_altitude.rawValue().toDouble());
  });
  connect(&_altitude, &SettingsFact::rawValueChanged, this,
          &MeasurementComplexItem::_onAltitudeChanged);

  Q_UNUSED(kmlOrShpFile)
  _editorQml = "qrc:/qml/MeasurementItemEditor.qml";

  // Connect facts.
  connect(&this->_variantIndex, &Fact::rawValueChanged, this,
          &MeasurementComplexItem::_changeVariantIndex);

  // Connect worker.
  connect(this->_pWorker, &RoutingThread::result, this,
          &MeasurementComplexItem::_storeRoutingData);

  // Connect coordinate and exitCoordinate.
  connect(this, &MeasurementComplexItem::routeChanged,
          [this] { emit this->coordinateChanged(this->coordinate()); });
  connect(this, &MeasurementComplexItem::routeChanged,
          [this] { emit this->exitCoordinateChanged(this->exitCoordinate()); });
  connect(this, &MeasurementComplexItem::routeChanged, [this] {
    emit this->exitCoordinateSameAsEntryChanged(
        this->exitCoordinateSameAsEntry());
  });

  // Connect isIncomplete.
  connect(this, &MeasurementComplexItem::idleChanged, [this] {
    if (this->idle()) {
      if (this->route().size() > 0 && this->_isIncomplete == true) {
        this->_isIncomplete = false;
        emit this->isIncompleteChanged();
      }
    } else {
      if (this->_isIncomplete == false) {
        this->_isIncomplete = true;
        emit this->isIncompleteChanged();
      }
    }
  });

  // Connect readyForSave
  connect(this, &MeasurementComplexItem::idleChanged, this,
          &MeasurementComplexItem::readyForSaveStateChanged);

  // Connect flightPathSegments
  connect(this, &MeasurementComplexItem::routeChanged, this,
          &MeasurementComplexItem::_updateFlightpathSegments);

  // Connect complexDistance.
  connect(this, &MeasurementComplexItem::routeChanged,
          [this] { emit this->complexDistanceChanged(); });

  resetGenerators();
  startEditing();
}

MeasurementComplexItem::~MeasurementComplexItem() {}

void MeasurementComplexItem::reverseRoute() { _reverseRoute(); }

const AreaData *MeasurementComplexItem::areaData() const {
  return this->_pCurrentData;
}

AreaData *MeasurementComplexItem::areaData() { return this->_pCurrentData; }

QVariantList MeasurementComplexItem::route() { return _route; }

QStringList MeasurementComplexItem::variantNames() const {
  return _variantNames;
}

bool MeasurementComplexItem::load(const QJsonObject &complexObject,
                                  int sequenceNumber, QString &errorString) {
  // We need to pull version first to determine what validation/conversion
  // needs to be performed
  QList<JsonHelper::KeyValidateInfo> versionKeyInfoList = {
      {JsonHelper::jsonVersionKey, QJsonValue::Double, true},
  };
  if (!JsonHelper::validateKeys(complexObject, versionKeyInfoList,
                                errorString)) {
    return false;
  }

  int version = complexObject[JsonHelper::jsonVersionKey].toInt();
  if (version != 1) {
    errorString = tr("Survey items do not support version %1").arg(version);
    return false;
  }

  {
    QList<JsonHelper::KeyValidateInfo> keyInfoList = {
        {VisualMissionItem::jsonTypeKey, QJsonValue::String, true},
        {ComplexMissionItem::jsonComplexItemTypeKey, QJsonValue::String, true},
    };

    if (!JsonHelper::validateKeys(complexObject, keyInfoList, errorString)) {
      return false;
    }

    QString itemType = complexObject[VisualMissionItem::jsonTypeKey].toString();
    QString complexType =
        complexObject[ComplexMissionItem::jsonComplexItemTypeKey].toString();
    if (itemType != VisualMissionItem::jsonTypeComplexItemValue ||
        complexType != jsonComplexItemTypeValue) {
      errorString = tr("%1 does not support loading this complex mission item "
                       "type: %2:%3")
                        .arg(qgcApp()->applicationName())
                        .arg(itemType)
                        .arg(complexType);
      return false;
    }
  }

  setSequenceNumber(sequenceNumber);
  startEditing();

  // load variant index
  if (complexObject.contains(variantIndexKey) &&
      complexObject[variantIndexKey].isDouble()) {
    _variantIndex.setRawValue(complexObject[variantIndexKey].toInt());
  }

  // load altitude
  if (complexObject.contains(altitudeKey) &&
      complexObject[altitudeKey].isDouble()) {
    _altitude.setRawValue(complexObject[altitudeKey].toDouble());
  } else {
    errorString.append(tr("No altitude found in file.\n"));
    abortEditing();
    return false;
  }

  // load AreaData.
  if (complexObject.contains(areaDataKey) &&
      complexObject[areaDataKey].isObject()) {
    QString e;
    if (_pCurrentData->load(complexObject[areaDataKey].toObject(), e)) {
      if (!_pCurrentData->isCorrect(false /*don't show gui message*/)) {
        errorString.append(_pCurrentData->errorString());
        abortEditing();
        return false;
      }

    } else {
      // this is critical, proceeding is not
      // reasonable.
      errorString.append(e);
      abortEditing();
      return false;
    }
  } else {
    // this is critical, if no area data present, proceeding is not reasonable.
    errorString.append(tr("No area data found in file. Abort loading.\n"));
    abortEditing();
    return false;
  }

  // load Generators.
  if (complexObject.contains(generatorArrayKey) &&
      complexObject[generatorArrayKey].isArray()) {

    QVector<PtrGenerator> generatorList;
    QObject parent;

    for (const auto valueRef : complexObject[generatorArrayKey].toArray()) {
      const auto jsonGen = valueRef.toObject();

      if (jsonGen.contains(routing::GeneratorBase::typeKey) &&
          jsonGen[routing::GeneratorBase::typeKey].isString()) {
        QString e;

        // create generator
        auto gen = routing::GeneratorFactory::instance()->create(
            jsonGen, e, &parent /*parent*/);

        if (gen != nullptr) {
          // remove generators of same type and insert this generator.
          for (int i = 0; i < _generatorList.size();) {
            auto otherGen = generator(i);
            if (gen->type() == otherGen->type()) {
              removeGenerator(i);
            } else {
              ++i;
            }
          }
          gen->setData(this->_pAreaData);
          generatorList.append(gen);
        } else {
          // error loading generator.
          errorString.append(
              tr("Error loading generator of type ") +
              jsonGen[routing::GeneratorBase::typeKey].toString() + ".\n");
          if (!routing::GeneratorFactory::instance()->registered(
                  jsonGen[routing::GeneratorBase::typeKey].toString())) {
            errorString.append(tr("This type is unknown.\n"));
            qCritical()
                << "MeasurementComplexItem::load(): generator of type :"
                << jsonGen[routing::GeneratorBase::typeKey]
                << " not registered with the GeneratorFactory. This can either "
                   "mean that the file contains a invalid entry or "
                   "that the generator was not registered. In the latter case "
                   "use the REGISTER_GENERATOR() for registration";
          }
          abortEditing();
          return false;
        }

      } else {
        errorString.append(tr("Can not determine type of generator.\n"));
        abortEditing();
        return false;
      }

      // insert generators
      for (const auto gen : generatorList) {
        gen->setParent(this);
        addGenerator(gen);
      }
    }
  } else {
    errorString.append(
        tr("No generators found in file. Leaving generators unchanged.\n"));
    abortEditing();
    return false;
  }

  // load generator index
  bool indexLoaded = false;
  if (complexObject.contains(generatorIndexKey) &&
      complexObject[generatorIndexKey].isDouble()) {
    int index = complexObject[generatorIndexKey].toDouble();
    if (index >= 0 && index < _generatorList.size()) {
      indexLoaded = true;
      switchToGenerator(index);
    }
  }
  if (!indexLoaded) {
    switchToGenerator(0);
  }

  // load Route Variants
  bool variantsSuccess = true;
  QVector<Variant> variantVector;
  if (complexObject.contains(variantArrayKey) &&
      complexObject[variantArrayKey].isArray()) {

    // load variants to variantVector for further processing.
    for (const auto valueRef : complexObject[variantArrayKey].toArray()) {
      if (valueRef.isArray()) {
        const auto jsonVariant = valueRef.toArray();
        Variant variant;
        QString e;
        if (JsonHelper::loadGeoCoordinateArray(jsonVariant, false, variant,
                                               e)) {
          if (variant.size() > 0) {
            variantVector.append(std::move(variant));
          } else {
            qCDebug(MeasurementComplexItemLog)
                << "Empty route variant skipped.\n"
                << valueRef.type();
          }
        } else {
          qCDebug(MeasurementComplexItemLog)
              << "Error loading route variant: " << e;
          variantsSuccess = false;
        }
      } else {
        qCDebug(MeasurementComplexItemLog)
            << "json variant is not an array but of type: " << valueRef.type();
        variantsSuccess = false;
      }
    }

    // Check if variantVector and variants are non empty
    if (variantVector.size() == 0) {
      variantsSuccess = false;
    }
    for (const auto &var : variantVector) {
      if (var.size() == 0) {
        variantsSuccess = false;
      }
    }

    // Check if variants are covered by safe area.
    if (variantsSuccess) {
      auto safeArea = _pCurrentData->safeArea();
      if (safeArea != nullptr) {
        QGeoCoordinate origin =
            safeArea->pathModel().value<QGCQGeoCoordinate *>(0)->coordinate();
        snake::FPolygon safeAreaENU;
        snake::areaToEnu(origin, safeArea->coordinateList(), safeAreaENU);
        for (const auto &variant : variantVector) {
          snake::FLineString varENU;
          for (const auto &vertex : variant) {
            snake::FPoint vertexENU;
            snake::toENU(origin, vertex.value<QGeoCoordinate>(), vertexENU);
            varENU.push_back(vertexENU);
          }

          if (!bg::covered_by(varENU, safeAreaENU)) {
            variantsSuccess = false;
            break;
          }
        }
      } else {
        variantsSuccess = false;
      }
    }
  } else {
    variantsSuccess = false;
  }

  if (variantsSuccess) {
    _variantVector.swap(variantVector);

    // load variant names
    bool variantNamesLoaded = true;
    if (complexObject.contains(variantNamesKey) &&
        complexObject[variantNamesKey].isArray()) {
      QStringList variantNames;

      for (const auto &name : complexObject[variantNamesKey].toArray()) {
        if (name.isString()) {
          variantNames.append(name.toString());
        } else {
          variantNamesLoaded = false;
          break;
        }
      }

      if (variantNames.size() != _variantVector.size()) {
        variantNamesLoaded = false;
      }

      if (variantNamesLoaded) {
        _variantNames.swap(variantNames);
        emit variantNamesChanged();
      }
    } else {
      qCWarning(MeasurementComplexItemLog)
          << "Not able to load variant names. variantNamesKey missing or wrong "
             "type";
      if (complexObject.contains(variantNamesKey)) {
        qCWarning(MeasurementComplexItemLog)
            << "variantNamesKey type: "
            << complexObject[variantNamesKey].type();
      }
    }

    // create std. variant names if loading failed
    if (!variantNamesLoaded) {
      qCWarning(MeasurementComplexItemLog) << "Creating std. variant names.";
      this->_variantNames.clear();
      for (std::size_t i = 1; i <= std::size_t(this->_variantVector.size());
           ++i) {
        this->_variantNames.append(QString::number(i));
      }
      emit variantNamesChanged();
    }

    stopEditing(
        false /*doUpdate*/); // does noting if editing was already stopped

    _changeVariantIndex();
  } else {
    stopEditing(); // stop editing and trigger update
  }

  return true;
}

double
MeasurementComplexItem::greatestDistanceTo(const QGeoCoordinate &other) const {
  double d = -1 * std::numeric_limits<double>::infinity();
  if (other.isValid()) {
    if (this->_route.size() > 0) {
      std::for_each(this->_route.cbegin(), this->_route.cend(),
                    [&d, &other](const QVariant &variant) {
                      auto vertex = variant.value<QGeoCoordinate>();
                      d = std::max(d, vertex.distanceTo(other));
                    });
    }
  } else {
    qCDebug(MeasurementComplexItemLog)
        << "greatestDistanceTo(): invalid QGeoCoordinate: " << other;
  }
  return d;
}

bool MeasurementComplexItem::dirty() const { return _dirty; }

bool MeasurementComplexItem::isSimpleItem() const { return false; }

bool MeasurementComplexItem::isStandaloneCoordinate() const { return false; }

QString MeasurementComplexItem::mapVisualQML() const {
  return QStringLiteral("MeasurementItemMapVisual.qml");
}

void MeasurementComplexItem::save(QJsonArray &planItems) {
  if (idle()) {
    QJsonObject saveObject;

    saveObject[JsonHelper::jsonVersionKey] = 1;
    saveObject[VisualMissionItem::jsonTypeKey] =
        VisualMissionItem::jsonTypeComplexItemValue;
    saveObject[ComplexMissionItem::jsonComplexItemTypeKey] =
        jsonComplexItemTypeValue;

    // Variant and altitude.
    saveObject[variantIndexKey] = double(_variantIndex.rawValue().toUInt());
    saveObject[altitudeKey] = double(_altitude.rawValue().toUInt());

    // Variant names.
    QJsonArray jsonVariantNames;
    for (auto const &name : _variantNames) {
      jsonVariantNames.append(name);
    }
    saveObject[variantNamesKey] = jsonVariantNames;

    // AreaData.
    QJsonObject jsonAreaData;
    if (!_pAreaData->save(jsonAreaData)) {
      qCDebug(MeasurementComplexItemLog)
          << "save(): not able to save area data";
      return;
    }
    saveObject[areaDataKey] = jsonAreaData;

    // Generators.
    QJsonArray generatorArray;
    for (int i = 0; i < _generatorList.size(); ++i) {
      auto const gen = _generatorList[i];
      QJsonObject obj;
      if (!gen->save(obj)) {
        qCDebug(MeasurementComplexItemLog)
            << "save(): not able to save generator with name: " << gen->name();
        return;
      } else {
        generatorArray.append(obj);
      }
    }
    saveObject[generatorArrayKey] = generatorArray;

    // generator index
    saveObject[generatorIndexKey] = generatorIndex();

    // Route Variants
    QJsonArray variantsArray;
    for (auto const &route : _variantVector) {
      QJsonValue variant;
      if (route.size() > 0) {
        JsonHelper::saveGeoCoordinateArray(route, false, variant);
      } else {
        JsonHelper::saveGeoCoordinateArray(_route, false, variant);
      }
      variantsArray.append(variant);
    }
    saveObject[variantArrayKey] = variantsArray;

    planItems.append(saveObject);
  } else {
    qCDebug(MeasurementComplexItemLog) << "save(): called while not idle.";
  }
}

double MeasurementComplexItem::amslEntryAlt() const {
  return _altitude.rawValue().toDouble() +
         this->_masterController->missionController()
             ->plannedHomePosition()
             .altitude();
}

double MeasurementComplexItem::amslExitAlt() const { return amslEntryAlt(); }

double MeasurementComplexItem::minAMSLAltitude() const {
  return amslEntryAlt();
}

double MeasurementComplexItem::maxAMSLAltitude() const {
  return amslEntryAlt();
}

QString MeasurementComplexItem::commandDescription() const {
  return QStringLiteral("Measurement");
}

QString MeasurementComplexItem::commandName() const {
  return QStringLiteral("Measurement");
}

QString MeasurementComplexItem::abbreviation() const {
  return QStringLiteral("M");
}

bool MeasurementComplexItem::specifiesCoordinate() const {
  return _route.count() > 0;
}

bool MeasurementComplexItem::specifiesAltitudeOnly() const { return false; }

QGeoCoordinate MeasurementComplexItem::coordinate() const {
  return this->_route.size() > 0 ? _route.first().value<QGeoCoordinate>()
                                 : QGeoCoordinate();
}

QGeoCoordinate MeasurementComplexItem::exitCoordinate() const {
  return this->_route.size() > 0 ? _route.last().value<QGeoCoordinate>()
                                 : QGeoCoordinate();
}

int MeasurementComplexItem::sequenceNumber() const { return _sequenceNumber; }

double MeasurementComplexItem::specifiedFlightSpeed() {
  return std::numeric_limits<double>::quiet_NaN();
}

double MeasurementComplexItem::specifiedGimbalYaw() {
  return std::numeric_limits<double>::quiet_NaN();
}

double MeasurementComplexItem::specifiedGimbalPitch() {
  return std::numeric_limits<double>::quiet_NaN();
}

void MeasurementComplexItem::appendMissionItems(QList<MissionItem *> &items,
                                                QObject *missionItemParent) {

  if (idle()) {
    qCDebug(MeasurementComplexItemLog) << "appendMissionItems()";

    int seqNum = this->_sequenceNumber;

    MAV_FRAME mavFrame =
        followTerrain() ? MAV_FRAME_GLOBAL : MAV_FRAME_GLOBAL_RELATIVE_ALT;

    for (const auto &variant : this->_route) {
      auto vertex = variant.value<QGeoCoordinate>();
      MissionItem *item = new MissionItem(
          seqNum++, MAV_CMD_NAV_WAYPOINT, mavFrame,
          0,   // hold time
          0.0, // No acceptance radius specified
          0.0, // Pass through waypoint
          std::numeric_limits<double>::quiet_NaN(), // Yaw unchanged
          vertex.latitude(), vertex.longitude(), vertex.altitude(),
          true,  // autoContinue
          false, // isCurrentItem
          missionItemParent);
      items.append(item);
    }
  } else {
    qCDebug(MeasurementComplexItemLog)
        << "appendMissionItems(): called while not ready().";
  }
}

void MeasurementComplexItem::setMissionFlightStatus(
    const MissionController::MissionFlightStatus_t &missionFlightStatus) {
  ComplexMissionItem::setMissionFlightStatus(missionFlightStatus);
}

void MeasurementComplexItem::applyNewAltitude(double newAltitude) {
  this->_altitude.setRawValue(newAltitude);
}

double MeasurementComplexItem::additionalTimeDelay() const { return 0; }

bool MeasurementComplexItem::_setGenerator(PtrGenerator newG) {
  if (this->_pGenerator != newG) {
    if (this->_pGenerator != nullptr) {
      disconnect(this->_pGenerator, &routing::GeneratorBase::generatorChanged,
                 this, &MeasurementComplexItem::_updateRoute);
    }

    this->_pGenerator = newG;

    if (this->_pGenerator != nullptr) {
      connect(this->_pGenerator, &routing::GeneratorBase::generatorChanged,
              this, &MeasurementComplexItem::_updateRoute);
    }

    emit generatorChanged();

    if (!editing()) {
      this->_setState(STATE::IDLE);
      _updateRoute();
    }

    return true;
  } else {
    return false;
  }
}

void MeasurementComplexItem::_setState(MeasurementComplexItem::STATE state) {
  if (this->_state != state) {
    auto oldState = this->_state;
    this->_state = state;

    if (_calculating(oldState) != _calculating(state)) {
      emit calculatingChanged();
    }

    if (_editing(oldState) != _editing(state)) {
      emit editingChanged();
    }

    if (_idle(oldState) != _idle(state)) {
      emit idleChanged();
    }
  }
}

bool MeasurementComplexItem::_calculating(MeasurementComplexItem::STATE state) {
  return state == STATE::ROUTING;
}

bool MeasurementComplexItem::_editing(MeasurementComplexItem::STATE state) {
  return state == STATE::EDITING;
}

bool MeasurementComplexItem::_idle(MeasurementComplexItem::STATE state) {
  return state == STATE::IDLE;
}

void MeasurementComplexItem::_updateFlightpathSegments() {
  bool hasCollisionOld = _cTerrainCollisionSegments > 0;
  _cTerrainCollisionSegments = 0;

  _flightPathSegments.beginReset();
  _flightPathSegments.clearAndDeleteContents();

  if (_route.size() > 2) {
    bool ok = false;
    double alt = _altitude.rawValue().toDouble(&ok) +
                 _masterController->missionController()
                     ->plannedHomePosition()
                     .altitude();
    if (ok) {
      auto prev = _route.cbegin();
      for (auto next = _route.cbegin() + 1; next != _route.end(); ++next) {
        auto v1 = prev->value<QGeoCoordinate>();
        auto v2 = next->value<QGeoCoordinate>();
        _appendFlightPathSegment(v1, alt, v2, alt);
        prev = next;
      }
    } else {
      qCCritical(MeasurementComplexItemLog) << "_altitude fact not ok.";
    }
  }

  _flightPathSegments.endReset();

  // Terrain collsision.
  bool hasCollision = _cTerrainCollisionSegments > 0;
  if (hasCollisionOld != hasCollision) {
    emit terrainCollisionChanged(hasCollision);
  }
  if (_pAreaData->measurementArea() != nullptr) {
    _pAreaData->measurementArea()->setShowAltColor(hasCollision);
  }

  _masterController->missionController()->recalcTerrainProfile();
}

void MeasurementComplexItem::_onAltitudeChanged() {
  // Apply altitude to variants and route.
  auto alt = _altitude.rawValue().toDouble();
  for (auto &var : _variantVector) {

    Variant *pVar;
    if (var.size() > 0) {
      pVar = &var;
    } else {
      pVar = &_route;
    }

    for (auto &qVariant : *pVar) {
      auto vertex = qVariant.value<QGeoCoordinate>();
      vertex.setAltitude(alt);
      qVariant = QVariant::fromValue(vertex);
    }
  }

  if (_route.size() > 0) {
    emit routeChanged();
  }
}
void MeasurementComplexItem::_setAreaData(
    MeasurementComplexItem::PtrAreaData data) {
  if (_pCurrentData != data) {
    _pCurrentData = data;
    emit areaDataChanged();
  }
}

void MeasurementComplexItem::_updateRoute() {
  if (!editing()) {
    // Reset data.
    this->_route.clear();
    emit routeChanged();
    this->_variantVector.clear();
    this->_variantNames.clear();
    emit variantNamesChanged();

    if (this->_pAreaData->isCorrect()) {

      // Prepare data.
      auto origin = this->_pAreaData->origin();
      origin.setAltitude(0);
      if (!origin.isValid()) {
        qCDebug(MeasurementComplexItemLog)
            << "_updateWorker(): origin invalid." << origin;
        return;
      }

      // Convert safe area.
      auto serviceArea =
          getGeoArea<const SafeArea *>(*this->_pAreaData->areaList());
      auto geoSafeArea = serviceArea->coordinateList();
      if (!(geoSafeArea.size() >= 3)) {
        qCDebug(MeasurementComplexItemLog)
            << "_updateWorker(): safe area invalid." << geoSafeArea;
        return;
      }
      for (auto &v : geoSafeArea) {
        if (v.isValid()) {
          v.setAltitude(0);
        } else {
          qCDebug(MeasurementComplexItemLog)
              << "_updateWorker(): safe area contains invalid coordinate."
              << geoSafeArea;
          return;
        }
      }

      // Routing par.
      RoutingParameter par;
      par.numSolutions = 5;
      auto &safeAreaENU = par.safeArea;
      snake::areaToEnu(origin, geoSafeArea, safeAreaENU);

      // Create generator.
      if (this->_pGenerator != nullptr) {
        routing::GeneratorBase::Generator g; // Transect generator.
        if (this->_pGenerator->get(g)) {
          // Start/Restart routing worker.
          this->_pWorker->route(par, g);
          _setState(STATE::ROUTING);
          return;
        } else {
          qCDebug(MeasurementComplexItemLog)
              << "_updateWorker(): generator creation failed.";
          return;
        }
      } else {
        qCDebug(MeasurementComplexItemLog)
            << "_updateWorker(): pGenerator == nullptr, number of registered "
               "generators: "
            << this->_generatorList.size();
        return;
      }
    } else {
      qCDebug(MeasurementComplexItemLog)
          << "_updateWorker(): area data invalid.";
      return;
    }
  }
}

void MeasurementComplexItem::_changeVariantIndex() {
  if (idle()) {
    auto variant = this->_variantIndex.rawValue().toUInt();

    // Find old variant. Old variant corresponts with empty list.
    std::size_t old_variant = std::numeric_limits<std::size_t>::max();
    for (std::size_t i = 0; i < std::size_t(this->_variantVector.size()); ++i) {
      const auto &variantCoordinates = this->_variantVector.at(i);
      if (variantCoordinates.isEmpty()) {
        old_variant = i;
        break;
      }
    }

    // Swap route.
    if (variant != old_variant) {
      // Swap in new variant.
      if (variant < std::size_t(this->_variantVector.size())) {
        if (old_variant != std::numeric_limits<std::size_t>::max()) {
          // this->_route containes a route, swap it back to
          // this->_solutionVector
          auto &oldRoute = this->_variantVector[old_variant];
          oldRoute.swap(this->_route);
        }
        auto &newRoute = this->_variantVector[variant];
        this->_route.swap(newRoute);
        emit routeChanged();
      } else { // error
        qCDebug(MeasurementComplexItemLog)
            << "Variant out of bounds (variant =" << variant << ").";
        qCDebug(MeasurementComplexItemLog) << "Resetting variant to zero.";

        disconnect(&this->_variantIndex, &Fact::rawValueChanged, this,
                   &MeasurementComplexItem::_changeVariantIndex);
        this->_variantIndex.setCookedValue(QVariant(0));
        connect(&this->_variantIndex, &Fact::rawValueChanged, this,
                &MeasurementComplexItem::_changeVariantIndex);

        if (this->_variantVector.size() > 0) {
          this->_changeVariantIndex();
        }
      }
    }
  }
}

void MeasurementComplexItem::_reverseRoute() {
  if (idle()) {
    if (this->_route.size() > 0) {
      auto &t = this->_route;
      std::reverse(t.begin(), t.end());
    }
    emit routeChanged();
  }
}

ComplexMissionItem::ReadyForSaveState
MeasurementComplexItem::readyForSaveState() const {
  if (idle()) {
    return ReadyForSaveState::ReadyForSave;
  } else {
    return ReadyForSaveState::NotReadyForSaveData;
  }
}

bool MeasurementComplexItem::exitCoordinateSameAsEntry() const {
  return this->_route.size() > 0 ? this->_route.first() == this->_route.last()
                                 : false;
}

void MeasurementComplexItem::setDirty(bool dirty) {
  if (this->_dirty != dirty) {
    this->_dirty = dirty;
    emit dirtyChanged(this->_dirty);
  }
}

void MeasurementComplexItem::setCoordinate(const QGeoCoordinate &coordinate) {
  Q_UNUSED(coordinate);
}

void MeasurementComplexItem::setSequenceNumber(int sequenceNumber) {
  if (this->_sequenceNumber != sequenceNumber) {
    this->_sequenceNumber = sequenceNumber;
    emit sequenceNumberChanged(this->_sequenceNumber);
  }
}

QString MeasurementComplexItem::patternName() const { return name; }

double MeasurementComplexItem::complexDistance() const {
  double d = 0;
  if (this->_route.size() > 1) {
    auto vertex = _route.first().value<QGeoCoordinate>();
    std::for_each(this->_route.cbegin() + 1, this->_route.cend(),
                  [&vertex, &d](const QVariant &variant) {
                    auto otherVertex = variant.value<QGeoCoordinate>();
                    d += vertex.distanceTo(otherVertex);
                    vertex = otherVertex;
                  });
  }
  return d;
}

int MeasurementComplexItem::lastSequenceNumber() const {
  return _sequenceNumber + std::max(0, this->_route.size() - 1);
}

bool MeasurementComplexItem::addGenerator(routing::GeneratorBase *g) {

  if (g == nullptr) {
    qCDebug(MeasurementComplexItemLog) << "addGenerator(): empty generator.";
    Q_ASSERT(g != nullptr);
    return false;
  }

  for (const auto &otherGenerator : _generatorList) {
    if (otherGenerator->name() == g->name()) {
      qCDebug(MeasurementComplexItemLog)
          << "addGenerator(): generator with name " << g->name()
          << " already added.";
      Q_ASSERT(otherGenerator->name() == g->name());
      return false;
    }
  }

  this->_generatorList.push_back(g);
  if (this->_generatorList.size() == 1) {
    _setGenerator(g);
  }

  emit generatorListChanged();
  return true;
}

bool MeasurementComplexItem::removeGenerator(const QString &name) {
  return removeGenerator(generatorIndex(name));
}

bool MeasurementComplexItem::removeGenerator(int index) {
  if (index >= 0 && index < this->_generatorList.size()) {
    // Is this the current generator?
    const auto &g = this->_generatorList.at(index);
    if (g == this->_pGenerator) {
      if (index > 0) {
        _setGenerator(this->_generatorList.at(index - 1));
      } else if (index + 1 < _generatorList.size()) {
        _setGenerator(this->_generatorList.at(index + 1));
      } else {
        _setGenerator(nullptr);
      }
    }

    auto gen = this->_generatorList.takeAt(index);

    // Should the generator be deleted?
    if (gen->parent() == this || gen->parent() == nullptr) {
      gen->deleteLater();
    }

    emit generatorListChanged();
    return true;
  } else {
    qCDebug(MeasurementComplexItemLog) << "removeGenerator(): index (" << index
                                       << ") out"
                                          "of bounds ( "
                                       << this->_generatorList.size() << " ).";
    return false;
  }
}

bool MeasurementComplexItem::switchToGenerator(const QString &name) {
  return switchToGenerator(generatorIndex(name));
}

bool MeasurementComplexItem::switchToGenerator(int index) {
  if (index >= 0 && index < _generatorList.size()) {
    _setGenerator(this->_generatorList.at(index));
    return true;
  } else {
    qCDebug(MeasurementComplexItemLog)
        << "switchToGenerator(): index (" << index
        << ") out"
           "of bounds ( "
        << this->_generatorList.size() << " ).";
    return false;
  }
}

void MeasurementComplexItem::resetGenerators() {
  while (_generatorList.size() > 0) {
    removeGenerator(0);
  }

  auto lg = routing::GeneratorFactory::instance()->create(
      routing::LinearGenerator::typeString, this);
  lg->setData(this->_pAreaData);
  addGenerator(lg);

  auto cg = routing::GeneratorFactory::instance()->create(
      routing::CircularGenerator::typeString, this);
  cg->setData(this->_pAreaData);
  addGenerator(cg);
}

QList<MeasurementComplexItem::PtrGenerator>
MeasurementComplexItem::generatorList() const {
  return _generatorList;
}

QStringList MeasurementComplexItem::generatorNameList() const {
  QStringList list;
  for (const auto gen : _generatorList) {
    list.append(gen->name());
  }
  return list;
}

routing::GeneratorBase *MeasurementComplexItem::generator() {
  return _pGenerator;
}

const routing::GeneratorBase *MeasurementComplexItem::generator() const {
  return _pGenerator;
}

const routing::GeneratorBase *
MeasurementComplexItem::generator(int index) const {
  if (index >= 0 && index < _generatorList.size()) {
    return _generatorList[index];
  } else {
    return nullptr;
  }
}

routing::GeneratorBase *MeasurementComplexItem::generator(int index) {
  if (index >= 0 && index < _generatorList.size()) {
    return _generatorList[index];
  } else {
    return nullptr;
  }
}

int MeasurementComplexItem::generatorIndex() const {
  return this->_generatorList.indexOf(this->_pGenerator);
}

int MeasurementComplexItem::generatorIndex(const QString &name) {
  int index = -1;
  for (int i = 0; i < _generatorList.size(); ++i) {
    const auto gen = _generatorList[i];
    if (gen->name() == name) {
      index = i;
      break;
    }
  }
  return index;
}

void MeasurementComplexItem::startEditing() {
  if (!editing()) {
    *_pEditorData = *_pAreaData;
    _setAreaData(_pEditorData);
    _setState(STATE::EDITING);
  }
}

bool MeasurementComplexItem::stopEditing(bool doUpdate) {
  if (editing()) {
    bool isDifferent = *_pEditorData != *_pAreaData;
    bool correct = _pEditorData->isCorrect();
    if (correct) {
      *_pAreaData = *_pEditorData;
    }
    _setAreaData(_pAreaData);
    _setState(STATE::IDLE);
    bool updated = false;
    if (doUpdate && correct && isDifferent) {
      updated = true;
      _updateRoute();
    }

    return updated;
  }
  return false;
}

void MeasurementComplexItem::abortEditing() {
  if (editing()) {
    _setAreaData(_pAreaData);
    _setState(STATE::IDLE);
  }
}

void MeasurementComplexItem::_storeRoutingData(
    MeasurementComplexItem::PtrRoutingData pRoute) {
  if (this->_state == STATE::ROUTING) {
    // Store solutions.
    auto ori = this->_pAreaData->origin();
    ori.setAltitude(0);
    QVector<Variant> variantVector;
    const auto nSolutions = pRoute->solutionVector.size();

    for (std::size_t j = 0; j < nSolutions; ++j) {
      Variant var;
      const auto &solution = pRoute->solutionVector.at(j);
      if (solution.size() > 0) {
        const auto &route = solution.at(0);
        const auto &path = route.path;

        // Convert to geo coordinates.

        for (const auto &vertex : path) {
          QGeoCoordinate c;
          snake::fromENU(ori, vertex, c);
          var.append(QVariant::fromValue(c));
        }
      } else {
        qCDebug(MeasurementComplexItemLog)
            << "_setTransects(): solution.size() == 0";
      }

      if (var.size() > 0) {
        variantVector.push_back(std::move(var));
      }
    }

    // Assign routes if no error occured.
    if (variantVector.size() > 0) {
      // Swap first route to _route.
      this->_variantVector.swap(variantVector);

      // Add route variant names.
      this->_variantNames.clear();
      for (std::size_t i = 1; i <= std::size_t(this->_variantVector.size());
           ++i) {
        this->_variantNames.append(QString::number(i));
      }
      emit variantNamesChanged();

      // Set variant to 0.
      disconnect(&this->_variantIndex, &Fact::rawValueChanged, this,
                 &MeasurementComplexItem::_changeVariantIndex);
      this->_variantIndex.setCookedValue(QVariant(0));
      connect(&this->_variantIndex, &Fact::rawValueChanged, this,
              &MeasurementComplexItem::_changeVariantIndex);

      // Select first variant as route.
      this->_route.swap(this->_variantVector.first());
      emit routeChanged();

      this->_setState(STATE::IDLE);
    } else {
      qCDebug(MeasurementComplexItemLog)
          << "_setTransects(): failed, variantVector empty.";
      this->_setState(STATE::IDLE);
    }
  }
}

Fact *MeasurementComplexItem::variantIndex() { return &_variantIndex; }

Fact *MeasurementComplexItem::altitude() { return &this->_altitude; }

bool MeasurementComplexItem::calculating() const {
  return this->_calculating(this->_state);
}

bool MeasurementComplexItem::editing() const { return _editing(this->_state); }

bool MeasurementComplexItem::idle() const { return _idle(this->_state); }

bool MeasurementComplexItem::followTerrain() const { return _followTerrain; }