CircularSurvey.cc

#include "CircularSurvey.h"
#include "RoutingThread.h"
// QGC
#include "JsonHelper.h"
#include "QGCApplication.h"
#include "QGCLoggingCategory.h"
// Wima
#include "snake.h"
#define CLIPPER_SCALE 1000000
#include "clipper/clipper.hpp"

#include "Geometry/GenericCircle.h"
#include "Snake/SnakeTile.h"

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

QGC_LOGGING_CATEGORY(CircularSurveyLog, "CircularSurveyLog")

using namespace ClipperLib;
template <> auto get<0>(const IntPoint &p) { return p.X; }
template <> auto get<1>(const IntPoint &p) { return p.Y; }

template <class Functor> class CommandRAII {
public:
  CommandRAII(Functor f) : fun(f) {}
  ~CommandRAII() { fun(); }

private:
  Functor fun;
};

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

bool circularTransects(const snake::FPolygon &polygon,
                       const std::vector<snake::FPolygon> &tiles,
                       snake::Length deltaR, snake::Angle deltaAlpha,
                       snake::Length minLength, snake::Transects &transects);

bool linearTransects(const snake::FPolygon &polygon,
                     const std::vector<snake::FPolygon> &tiles,
                     snake::Length distance, snake::Angle angle,
                     snake::Length minLength, snake::Transects &transects);

const char *CircularSurvey::settingsGroup = "CircularSurvey";
const char *CircularSurvey::transectDistanceName = "TransectDistance";
const char *CircularSurvey::alphaName = "Alpha";
const char *CircularSurvey::minLengthName = "MinLength";
const char *CircularSurvey::typeName = "Type";
const char *CircularSurvey::CircularSurveyName = "CircularSurvey";
const char *CircularSurvey::refPointLatitudeName = "ReferencePointLat";
const char *CircularSurvey::refPointLongitudeName = "ReferencePointLong";
const char *CircularSurvey::refPointAltitudeName = "ReferencePointAlt";
const char *CircularSurvey::variantName = "Variant";
const char *CircularSurvey::numRunsName = "NumRuns";
const char *CircularSurvey::runName = "Run";

CircularSurvey::CircularSurvey(Vehicle *vehicle, bool flyView,
                               const QString &kmlOrShpFile, QObject *parent)
    : TransectStyleComplexItem(vehicle, flyView, settingsGroup, parent),
      _referencePoint(QGeoCoordinate(0, 0, 0)),
      _metaDataMap(FactMetaData::createMapFromJsonFile(
          QStringLiteral(":/json/CircularSurvey.SettingsGroup.json"), this)),
      _transectDistance(settingsGroup, _metaDataMap[transectDistanceName]),
      _alpha(settingsGroup, _metaDataMap[alphaName]),
      _minLength(settingsGroup, _metaDataMap[minLengthName]),
      _type(settingsGroup, _metaDataMap[typeName]),
      _variant(settingsGroup, _metaDataMap[variantName]),
      _numRuns(settingsGroup, _metaDataMap[numRunsName]),
      _run(settingsGroup, _metaDataMap[runName]),
      _pWorker(std::make_unique<RoutingThread>()), _state(STATE::DEFAULT),
      _hidePolygon(false) {
  Q_UNUSED(kmlOrShpFile)
  _editorQml = "qrc:/qml/CircularSurveyItemEditor.qml";

  // Connect facts.
  connect(&_transectDistance, &Fact::valueChanged, this,
          &CircularSurvey::_rebuildTransects);
  connect(&_alpha, &Fact::valueChanged, this,
          &CircularSurvey::_rebuildTransects);
  connect(&_minLength, &Fact::valueChanged, this,
          &CircularSurvey::_rebuildTransects);
  connect(this, &CircularSurvey::refPointChanged, this,
          &CircularSurvey::_rebuildTransects);
  connect(this, &CircularSurvey::depotChanged, this,
          &CircularSurvey::_rebuildTransects);
  connect(&this->_type, &Fact::rawValueChanged, this,
          &CircularSurvey::_rebuildTransects);
  connect(&this->_variant, &Fact::rawValueChanged, this,
          &CircularSurvey::_changeVariant);
  connect(&this->_run, &Fact::rawValueChanged, this,
          &CircularSurvey::_changeRun);
  connect(&this->_numRuns, &Fact::rawValueChanged, this,
          &CircularSurvey::_rebuildTransects);

  // Areas.
  connect(this, &CircularSurvey::measurementAreaChanged, this,
          &CircularSurvey::_rebuildTransects);
  connect(this, &CircularSurvey::joinedAreaChanged, this,
          &CircularSurvey::_rebuildTransects);

  // Connect worker.
  connect(this->_pWorker.get(), &RoutingThread::result, this,
          &CircularSurvey::_setTransects);
  connect(this->_pWorker.get(), &RoutingThread::calculatingChanged, this,
          &CircularSurvey::calculatingChanged);
  this->_transectsDirty = true;
}

CircularSurvey::~CircularSurvey() {}

void CircularSurvey::resetReference() { setRefPoint(_mArea.center()); }

void CircularSurvey::reverse() {
  this->_state = STATE::REVERSE;
  this->_rebuildTransects();
}

void CircularSurvey::setRefPoint(const QGeoCoordinate &refPt) {
  if (refPt != _referencePoint) {
    _referencePoint = refPt;
    _referencePoint.setAltitude(0);

    emit refPointChanged();
  }
}

QGeoCoordinate CircularSurvey::refPoint() const { return _referencePoint; }

Fact *CircularSurvey::transectDistance() { return &_transectDistance; }

Fact *CircularSurvey::alpha() { return &_alpha; }

bool CircularSurvey::hidePolygon() const { return _hidePolygon; }

QList<QString> CircularSurvey::variantNames() const { return _variantNames; }

QList<QString> CircularSurvey::runNames() const { return _runNames; }

QGeoCoordinate CircularSurvey::depot() const { return this->_depot; }

const QList<QList<QGeoCoordinate>> &CircularSurvey::rawTransects() const {
  return this->_rawTransects;
}

void CircularSurvey::setHidePolygon(bool hide) {
  if (this->_hidePolygon != hide) {
    this->_hidePolygon = hide;
    emit hidePolygonChanged();
  }
}

void CircularSurvey::setMeasurementArea(const WimaMeasurementAreaData &mArea) {
  if (this->_mArea != mArea) {
    this->_mArea = mArea;
    emit measurementAreaChanged();
  }
}

void CircularSurvey::setJoinedArea(const WimaJoinedAreaData &jArea) {
  if (this->_jArea != jArea) {
    this->_jArea = jArea;
    emit joinedAreaChanged();
  }
}

void CircularSurvey::setMeasurementArea(const WimaMeasurementArea &mArea) {
  if (this->_mArea != mArea) {
    this->_mArea = mArea;
    emit measurementAreaChanged();
  }
}

void CircularSurvey::setJoinedArea(const WimaJoinedArea &jArea) {
  if (this->_jArea != jArea) {
    this->_jArea = jArea;
    emit joinedAreaChanged();
  }
}

void CircularSurvey::setDepot(const QGeoCoordinate &depot) {
  if (this->_depot.latitude() != depot.latitude() ||
      this->_depot.longitude() != depot.longitude()) {
    this->_depot = depot;
    this->_depot.setAltitude(0);
    emit depotChanged();
  }
}

bool CircularSurvey::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},
      {transectDistanceName, QJsonValue::Double, true},
      {alphaName, QJsonValue::Double, true},
      {minLengthName, QJsonValue::Double, true},
      {typeName, QJsonValue::Double, true},
      {variantName, QJsonValue::Double, false},
      {numRunsName, QJsonValue::Double, false},
      {runName, QJsonValue::Double, false},
      {refPointLatitudeName, QJsonValue::Double, true},
      {refPointLongitudeName, QJsonValue::Double, true},
      {refPointAltitudeName, QJsonValue::Double, 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 != CircularSurveyName) {
    errorString = tr("%1 does not support loading this complex mission item "
                     "type: %2:%3")
                      .arg(qgcApp()->applicationName())
                      .arg(itemType)
                      .arg(complexType);
    return false;
  }

  _ignoreRecalc = true;

  setSequenceNumber(sequenceNumber);

  if (!_surveyAreaPolygon.loadFromJson(complexObject, true /* required */,
                                       errorString)) {
    _surveyAreaPolygon.clear();
    return false;
  }

  if (!_load(complexObject, errorString)) {
    _ignoreRecalc = false;
    return false;
  }

  _transectDistance.setRawValue(complexObject[transectDistanceName].toDouble());
  _alpha.setRawValue(complexObject[alphaName].toDouble());
  _minLength.setRawValue(complexObject[minLengthName].toDouble());
  _type.setRawValue(complexObject[typeName].toInt());
  _variant.setRawValue(complexObject[variantName].toInt());
  _numRuns.setRawValue(complexObject[numRunsName].toInt());
  _run.setRawValue(complexObject[runName].toInt());
  _referencePoint.setLongitude(complexObject[refPointLongitudeName].toDouble());
  _referencePoint.setLatitude(complexObject[refPointLatitudeName].toDouble());
  _referencePoint.setAltitude(complexObject[refPointAltitudeName].toDouble());

  _ignoreRecalc = false;

  _recalcComplexDistance();
  if (_cameraShots == 0) {
    // Shot count was possibly not available from plan file
    _recalcCameraShots();
  }

  return true;
}

QString CircularSurvey::mapVisualQML() const {
  return QStringLiteral("CircularSurveyMapVisual.qml");
}

void CircularSurvey::save(QJsonArray &planItems) {
  QJsonObject saveObject;

  _save(saveObject);

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

  saveObject[transectDistanceName] = _transectDistance.rawValue().toDouble();
  saveObject[alphaName] = _alpha.rawValue().toDouble();
  saveObject[minLengthName] = _minLength.rawValue().toDouble();
  saveObject[typeName] = double(_type.rawValue().toUInt());
  saveObject[variantName] = double(_variant.rawValue().toUInt());
  saveObject[numRunsName] = double(_numRuns.rawValue().toUInt());
  saveObject[runName] = double(_numRuns.rawValue().toUInt());
  saveObject[refPointLongitudeName] = _referencePoint.longitude();
  saveObject[refPointLatitudeName] = _referencePoint.latitude();
  saveObject[refPointAltitudeName] = _referencePoint.altitude();

  // Polygon shape
  _surveyAreaPolygon.saveToJson(saveObject);

  planItems.append(saveObject);
}

bool CircularSurvey::specifiesCoordinate() const { return true; }

void CircularSurvey::appendMissionItems(QList<MissionItem *> &items,
                                        QObject *missionItemParent) {
  if (_transectsDirty)
    return;
  if (_loadedMissionItems.count()) {
    // We have mission items from the loaded plan, use those
    _appendLoadedMissionItems(items, missionItemParent);
  } else {
    // Build the mission items on the fly
    _buildAndAppendMissionItems(items, missionItemParent);
  }
}

void CircularSurvey::_appendLoadedMissionItems(QList<MissionItem *> &items,
                                               QObject *missionItemParent) {
  if (_transectsDirty)
    return;
  int seqNum = _sequenceNumber;

  for (const MissionItem *loadedMissionItem : _loadedMissionItems) {
    MissionItem *item = new MissionItem(*loadedMissionItem, missionItemParent);
    item->setSequenceNumber(seqNum++);
    items.append(item);
  }
}

void CircularSurvey::_buildAndAppendMissionItems(QList<MissionItem *> &items,
                                                 QObject *missionItemParent) {
  if (_transectsDirty || _transects.count() == 0)
    return;

  MissionItem *item;
  int seqNum = _sequenceNumber;

  MAV_FRAME mavFrame =
      followTerrain() || !_cameraCalc.distanceToSurfaceRelative()
          ? MAV_FRAME_GLOBAL
          : MAV_FRAME_GLOBAL_RELATIVE_ALT;

  for (const QList<TransectStyleComplexItem::CoordInfo_t> &transect :
       _transects) {
    // bool transectEntry = true;

    for (const CoordInfo_t &transectCoordInfo : transect) {
      item = new MissionItem(
          seqNum++, MAV_CMD_NAV_WAYPOINT, mavFrame,
          0,   // Hold time (delay for hover and capture to settle vehicle
               // before image is taken)
          0.0, // No acceptance radius specified
          0.0, // Pass through waypoint
          std::numeric_limits<double>::quiet_NaN(), // Yaw unchanged
          transectCoordInfo.coord.latitude(),
          transectCoordInfo.coord.longitude(),
          transectCoordInfo.coord.altitude(),
          true,  // autoContinue
          false, // isCurrentItem
          missionItemParent);
      items.append(item);
    }
  }
}

void CircularSurvey::_changeVariant() {
  this->_state = STATE::VARIANT_CHANGE;
  this->_rebuildTransects();
}

void CircularSurvey::_changeRun() {
  this->_state = STATE::RUN_CHANGE;
  this->_rebuildTransects();
}

void CircularSurvey::_updateWorker() {
  // Mark transects as dirty.
  this->_transectsDirty = true;
  // Reset data.
  this->_transects.clear();
  this->_rawTransects.clear();
  this->_variantVector.clear();
  this->_variantNames.clear();
  this->_runNames.clear();
  emit variantNamesChanged();
  emit runNamesChanged();

  // Prepare data.
  auto ref = this->_referencePoint;
  auto geoPolygon = this->_mArea.coordinateList();
  for (auto &v : geoPolygon) {
    v.setAltitude(0);
  }
  auto pPolygon = std::make_shared<snake::FPolygon>();
  snake::areaToEnu(ref, geoPolygon, *pPolygon);

  // Progress and tiles.
  const auto &progress = this->_mArea.progress();
  const auto *tiles = this->_mArea.tiles();
  auto pTiles = std::make_shared<std::vector<snake::FPolygon>>();
  if (progress.size() == tiles->count()) {
    for (int i = 0; i < tiles->count(); ++i) {
      if (progress[i] == 100) {
        const auto *tile = tiles->value<const SnakeTile *>(i);
        if (tile != nullptr) {
          snake::FPolygon tileENU;
          snake::areaToEnu(ref, tile->coordinateList(), tileENU);
          pTiles->push_back(std::move(tileENU));
        } else {
          qCWarning(CircularSurveyLog)
              << "_updateWorker(): progress.size() != tiles->count().";
          return;
        }
      }
    }
  } else {
    qCWarning(CircularSurveyLog)
        << "_updateWorker(): progress.size() != tiles->count().";
    return;
  }

  // Convert safe area.
  auto geoDepot = this->_depot;
  auto geoSafeArea = this->_jArea.coordinateList();
  if (!geoDepot.isValid()) {
    qCWarning(CircularSurveyLog)
        << "_updateWorker(): depot invalid." << geoDepot;
    return;
  }
  if (!(geoSafeArea.size() >= 3)) {
    qCWarning(CircularSurveyLog)
        << "_updateWorker(): safe area invalid." << geoSafeArea;
    return;
  }
  for (auto &v : geoSafeArea) {
    v.setAltitude(0);
  }
  snake::FPoint depot;
  snake::toENU(ref, geoDepot, depot);

  // Routing par.
  RoutingParameter par;
  par.numSolutionsPerRun = 5;
  if (this->_numRuns.rawValue().toUInt() < 1) {
    disconnect(&this->_numRuns, &Fact::rawValueChanged, this,
               &CircularSurvey::_rebuildTransects);

    this->_numRuns.setCookedValue(QVariant(1));

    connect(&this->_numRuns, &Fact::rawValueChanged, this,
            &CircularSurvey::_rebuildTransects);
  }
  par.numRuns = this->_numRuns.rawValue().toUInt();

  auto &safeAreaENU = par.safeArea;
  snake::areaToEnu(ref, geoSafeArea, safeAreaENU);

  // Fetch transect parameter.
  auto distance = snake::Length(this->_transectDistance.rawValue().toDouble() *
                                bu::si::meter);
  auto minLength =
      snake::Length(this->_minLength.rawValue().toDouble() * bu::si::meter);
  auto alpha =
      snake::Angle(this->_alpha.rawValue().toDouble() * bu::degree::degree);

  // Select survey type.
  if (this->_type.rawValue().toUInt() == integral(Type::Circular)) {
    // Clip angle.
    if (alpha >= snake::Angle(0.3 * bu::degree::degree) &&
        alpha <= snake::Angle(45 * bu::degree::degree)) {
      auto generator = [depot, pPolygon, pTiles, distance, alpha,
                        minLength](snake::Transects &transects) -> bool {
        bool value = circularTransects(*pPolygon, *pTiles, distance, alpha,
                                       minLength, transects);
        transects.insert(transects.begin(), snake::FLineString{depot});
        return value;
      };
      // Start routing worker.
      this->_pWorker->route(par, generator);
    } else {
      if (alpha < snake::Angle(0.3 * bu::degree::degree)) {
        this->_alpha.setCookedValue(QVariant(0.3));
      } else {
        this->_alpha.setCookedValue(QVariant(45));
      }
    }
  } else if (this->_type.rawValue().toUInt() == integral(Type::Linear)) {
    auto generator = [depot, pPolygon, pTiles, distance, alpha,
                      minLength](snake::Transects &transects) -> bool {
      bool value = linearTransects(*pPolygon, *pTiles, distance, alpha,
                                   minLength, transects);
      transects.insert(transects.begin(), snake::FLineString{depot});
      return value;
    };
    // Start routing worker.
    this->_pWorker->route(par, generator);
  } else {
    qCWarning(CircularSurveyLog)
        << "CircularSurvey::rebuildTransectsPhase1(): invalid survey type:"
        << this->_type.rawValue().toUInt();
  }
}

void CircularSurvey::_changeVariantRunWorker() {
  auto variant = this->_variant.rawValue().toUInt();
  auto run = this->_run.rawValue().toUInt();

  // Find old variant and run. Old run corresponts with empty list.
  std::size_t old_variant = std::numeric_limits<std::size_t>::max();
  std::size_t old_run = std::numeric_limits<std::size_t>::max();
  for (std::size_t i = 0; i < std::size_t(this->_variantVector.size()); ++i) {
    const auto &solution = this->_variantVector.at(i);
    for (std::size_t j = 0; j < std::size_t(solution.size()); ++j) {
      const auto &r = solution[j];
      if (r.isEmpty()) {
        old_variant = i;
        old_run = j;
        // break
        i = std::numeric_limits<std::size_t>::max() - 1;
        j = std::numeric_limits<std::size_t>::max() - 1;
      }
    }
  }

  // Swap route.
  if (variant != old_variant || run != old_run) {
    // Swap in new variant, if condition.
    if (variant < std::size_t(this->_variantVector.size()) &&
        run < std::size_t(this->_variantVector.at(variant).size())) {
      if (old_variant != std::numeric_limits<std::size_t>::max()) {
        // this->_transects containes a route, swap it back to
        // this->_solutionVector
        auto &old_solution = this->_variantVector[old_variant];
        auto &old_route = old_solution[old_run];
        old_route.swap(this->_transects);
      }
      auto &solution = this->_variantVector[variant];
      auto &route = solution[run];
      this->_transects.swap(route);

      if (variant != old_variant) {
        // Add run names.
        this->_runNames.clear();
        for (std::size_t i = 1; i <= std::size_t(solution.size()); ++i) {
          this->_runNames.append(QString::number(i));
        }
        emit runNamesChanged();
      }

    } else { // error
      qCWarning(CircularSurveyLog)
          << "Variant or run out of bounds (variant = " << variant
          << ", run = " << run << ").";
      qCWarning(CircularSurveyLog) << "Resetting variant and run.";

      disconnect(&this->_variant, &Fact::rawValueChanged, this,
                 &CircularSurvey::_changeVariant);
      disconnect(&this->_run, &Fact::rawValueChanged, this,
                 &CircularSurvey::_changeRun);
      if (old_variant < std::size_t(this->_variantVector.size())) {
        this->_variant.setCookedValue(QVariant::fromValue(old_variant));
        auto &solution = this->_variantVector[old_variant];
        if (old_run < std::size_t(solution.size())) {
          this->_run.setCookedValue(QVariant::fromValue(old_run));
        } else {
          this->_run.setCookedValue(QVariant(0));
        }
      } else {
        this->_variant.setCookedValue(QVariant(0));
        this->_run.setCookedValue(QVariant(0));
      }
      connect(&this->_variant, &Fact::rawValueChanged, this,
              &CircularSurvey::_changeVariant);
      connect(&this->_run, &Fact::rawValueChanged, this,
              &CircularSurvey::_changeRun);
      if (this->_variantVector.size() > 0 &&
          this->_variantVector.front().size() > 0) {
        this->_changeVariantRunWorker();
      }
    }
  }
}

void CircularSurvey::_reverseWorker() {
  if (this->_transects.size() > 0) {
    auto &t = this->_transects.front();
    std::reverse(t.begin(), t.end());
  }
}

void CircularSurvey::_storeWorker() {
  // If the transects are getting rebuilt then any previously loaded
  // mission items are now invalid.
  if (_loadedMissionItemsParent) {
    _loadedMissionItems.clear();
    _loadedMissionItemsParent->deleteLater();
    _loadedMissionItemsParent = nullptr;
  }

  // Store raw transects.
  const auto &pRoutingData = this->_pRoutingData;
  const auto &ori = this->_referencePoint;
  const auto &transectsENU = pRoutingData->transects;
  QList<QList<QGeoCoordinate>> rawTransects;
  for (std::size_t i = 1; i < transectsENU.size(); ++i) {
    const auto &t = transectsENU[i];
    rawTransects.append(QList<QGeoCoordinate>());
    auto trGeo = rawTransects.back();
    for (auto &v : t) {
      QGeoCoordinate c;
      snake::fromENU(ori, v, c);
      trGeo.append(c);
    }
  }

  // Store solutions.
  QVector<Runs> solutionVector;
  const auto nSolutions = pRoutingData->solutionVector.size();
  for (std::size_t j = 0; j < nSolutions; ++j) {
    const auto &solution = pRoutingData->solutionVector.at(j);
    const auto nRuns = solution.size();
    // Store runs.
    Runs runs(nRuns, Transects{QList<CoordInfo_t>()});
    for (std::size_t k = 0; k < nRuns; ++k) {
      const auto &route = solution.at(k);
      const auto &path = route.path;
      const auto &info = route.info;
      if (info.size() > 1) {
        // Find index of first waypoint.
        std::size_t idxFirst = 0;
        const auto &infoFirst = info.at(1);
        const auto &firstTransect = transectsENU[infoFirst.index];
        if (firstTransect.size() > 0) {
          const auto &firstWaypoint =
              infoFirst.reversed ? firstTransect.back() : firstTransect.front();
          double th = 0.01;
          for (std::size_t i = 0; i < path.size(); ++i) {
            auto dist = bg::distance(path[i], firstWaypoint);
            if (dist < th) {
              idxFirst = i;
              break;
            }
          }

          // Find index of last waypoint.
          std::size_t idxLast = path.size() - 1;
          const auto &infoLast = info.at(info.size() - 2);
          const auto &lastTransect = transectsENU[infoLast.index];
          if (lastTransect.size() > 0) {
            const auto &lastWaypoint =
                infoLast.reversed ? lastTransect.front() : lastTransect.back();
            for (long i = path.size() - 1; i >= 0; --i) {
              auto dist = bg::distance(path[i], lastWaypoint);
              if (dist < th) {
                idxLast = i;
                break;
              }
            }

            // Convert to geo coordinates.
            auto &list = runs[k].front();
            for (std::size_t i = idxFirst; i <= idxLast; ++i) {
              auto &vertex = path[i];
              QGeoCoordinate c;
              snake::fromENU(ori, vertex, c);
              list.append(CoordInfo_t{c, CoordTypeInterior});
            }
          } else {
            qCWarning(CircularSurveyLog)
                << "_storeWorker(): lastTransect.size() == 0";
          }
        } else {
          qCWarning(CircularSurveyLog)
              << "_storeWorker(): firstTransect.size() == 0";
        }
      } else {
        qCWarning(CircularSurveyLog)
            << "_storeWorker(): transectsInfo.size() <= 1";
      }
    }
    // Remove empty runs.
    bool error = true;
    for (auto it = runs.begin(); it < runs.end();) {
      if (it->size() > 0 && it->front().size() > 0) {
        error = false;
        ++it;
      } else {
        it = runs.erase(it);
      }
    }
    if (!error) {
      solutionVector.push_back(std::move(runs));
    }
  }

  // Remove empty solutions.
  std::size_t nSol = 0;
  for (auto it = solutionVector.begin(); it < solutionVector.end();) {
    if (it->size() > 0 && it->front().size() > 0) {
      ++it;
      ++nSol;
    } else {
      it = solutionVector.erase(it);
    }
  }

  // Assign routes if no error occured.
  if (nSol > 0) {
    // Swap first route to _transects.
    this->_variantVector.swap(solutionVector);

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

    // Swap in rawTransects.
    this->_rawTransects.swap(rawTransects);

    disconnect(&this->_variant, &Fact::rawValueChanged, this,
               &CircularSurvey::_changeVariant);
    disconnect(&this->_run, &Fact::rawValueChanged, this,
               &CircularSurvey::_changeRun);
    this->_variant.setCookedValue(QVariant(0));
    this->_run.setCookedValue(QVariant(0));
    connect(&this->_variant, &Fact::rawValueChanged, this,
            &CircularSurvey::_changeVariant);
    connect(&this->_run, &Fact::rawValueChanged, this,
            &CircularSurvey::_changeRun);
    this->_changeVariantRunWorker();
    // Mark transect as stored and ready.
    this->_transectsDirty = false;
  }
}

void CircularSurvey::applyNewAltitude(double newAltitude) {
  _cameraCalc.valueSetIsDistance()->setRawValue(true);
  _cameraCalc.distanceToSurface()->setRawValue(newAltitude);
  _cameraCalc.setDistanceToSurfaceRelative(true);
}

double CircularSurvey::timeBetweenShots() { return 1; }

QString CircularSurvey::commandDescription() const {
  return tr("Circular Survey");
}

QString CircularSurvey::commandName() const { return tr("Circular Survey"); }

QString CircularSurvey::abbreviation() const { return tr("C.S."); }

bool CircularSurvey::readyForSave() const {
  return TransectStyleComplexItem::readyForSave() && !_transectsDirty;
}

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

void CircularSurvey::_rebuildTransectsPhase1(void) {
  auto start = std::chrono::high_resolution_clock::now();

  switch (this->_state) {
  case STATE::STORE:
    qCWarning(CircularSurveyLog) << "rebuildTransectsPhase1: store.";
    this->_storeWorker();
    break;
  case STATE::VARIANT_CHANGE:
    qCWarning(CircularSurveyLog) << "rebuildTransectsPhase1: variant change.";
    this->_changeVariantRunWorker();
    break;
  case STATE::RUN_CHANGE:
    qCWarning(CircularSurveyLog) << "rebuildTransectsPhase1: run change.";
    this->_changeVariantRunWorker();
    break;
  case STATE::REVERSE:
    qCWarning(CircularSurveyLog) << "rebuildTransectsPhase1: reverse.";
    this->_reverseWorker();
    break;
  case STATE::DEFAULT:
    qCWarning(CircularSurveyLog) << "rebuildTransectsPhase1: update.";
    this->_updateWorker();
    break;
  }
  this->_state = STATE::DEFAULT;

  qCWarning(CircularSurveyLog)
      << "rebuildTransectsPhase1(): "
      << std::chrono::duration_cast<std::chrono::milliseconds>(
             std::chrono::high_resolution_clock::now() - start)
             .count()
      << " ms";
}

void CircularSurvey::_recalcComplexDistance() {
  _complexDistance = 0;
  if (_transectsDirty)
    return;
  for (int i = 0; i < _visualTransectPoints.count() - 1; i++) {
    _complexDistance +=
        _visualTransectPoints[i].value<QGeoCoordinate>().distanceTo(
            _visualTransectPoints[i + 1].value<QGeoCoordinate>());
  }
  emit complexDistanceChanged();
}

// no cameraShots in Circular Survey, add if desired
void CircularSurvey::_recalcCameraShots() { _cameraShots = 0; }

void CircularSurvey::_setTransects(CircularSurvey::PtrRoutingData pRoute) {
  this->_pRoutingData = pRoute;
  this->_state = STATE::STORE;
  this->_rebuildTransects();
}

Fact *CircularSurvey::minLength() { return &_minLength; }

Fact *CircularSurvey::type() { return &_type; }

Fact *CircularSurvey::variant() { return &_variant; }

Fact *CircularSurvey::numRuns() { return &_numRuns; }

Fact *CircularSurvey::run() { return &_run; }

int CircularSurvey::typeCount() const { return int(integral(Type::Count)); }

bool CircularSurvey::calculating() const {
  return this->_pWorker->calculating();
}

bool circularTransects(const snake::FPolygon &polygon,
                       const std::vector<snake::FPolygon> &tiles,
                       snake::Length deltaR, snake::Angle deltaAlpha,
                       snake::Length minLength, snake::Transects &transects) {
  auto s1 = std::chrono::high_resolution_clock::now();

  // Check preconitions
  if (polygon.outer().size() >= 3) {
    using namespace boost::units;
    // Convert geo polygon to ENU polygon.
    snake::FPoint origin{0, 0};
    std::string error;
    // Check validity.
    if (!bg::is_valid(polygon, error)) {
      qCWarning(CircularSurveyLog) << "circularTransects(): "
                                      "invalid polygon.";
      qCWarning(CircularSurveyLog) << error.c_str();
      std::stringstream ss;
      ss << bg::wkt(polygon);
      qCWarning(CircularSurveyLog) << ss.str().c_str();
    } else {
      // Calculate polygon distances and angles.
      std::vector<snake::Length> distances;
      distances.reserve(polygon.outer().size());
      std::vector<snake::Angle> angles;
      angles.reserve(polygon.outer().size());
      //#ifdef DEBUG_CIRCULAR_SURVEY
      //      qCWarning(CircularSurveyLog) << "circularTransects():";
      //#endif
      for (const auto &p : polygon.outer()) {
        snake::Length distance = bg::distance(origin, p) * si::meter;
        distances.push_back(distance);
        snake::Angle alpha = (std::atan2(p.get<1>(), p.get<0>())) * si::radian;
        alpha = alpha < 0 * si::radian ? alpha + 2 * M_PI * si::radian : alpha;
        angles.push_back(alpha);
        //#ifdef DEBUG_CIRCULAR_SURVEY
        //        qCWarning(CircularSurveyLog) << "distances, angles,
        //        coordinates:"; qCWarning(CircularSurveyLog) <<
        //        to_string(distance).c_str(); qCWarning(CircularSurveyLog) <<
        //        to_string(snake::Degree(alpha)).c_str();
        //        qCWarning(CircularSurveyLog) << "x = " << p.get<0>() << "y = "
        //        << p.get<1>();
        //#endif
      }

      auto rMin = deltaR; // minimal circle radius
      snake::Angle alpha1(0 * degree::degree);
      snake::Angle alpha2(360 * degree::degree);
      // Determine r_min by successive approximation
      if (!bg::within(origin, polygon.outer())) {
        rMin = bg::distance(origin, polygon) * si::meter;
      }

      auto rMax = (*std::max_element(distances.begin(),
                                     distances.end())); // maximal circle radius

      // Scale parameters and coordinates.
      const auto rMinScaled =
          ClipperLib::cInt(std::round(rMin.value() * CLIPPER_SCALE));
      const auto deltaRScaled =
          ClipperLib::cInt(std::round(deltaR.value() * CLIPPER_SCALE));
      auto originScaled =
          ClipperLib::IntPoint{ClipperLib::cInt(std::round(origin.get<0>())),
                               ClipperLib::cInt(std::round(origin.get<1>()))};

      // Generate circle sectors.
      auto rScaled = rMinScaled;
      const auto nTran = long(std::ceil(((rMax - rMin) / deltaR).value()));
      vector<ClipperLib::Path> sectors(nTran, ClipperLib::Path());
      const auto nSectors =
          long(std::round(((alpha2 - alpha1) / deltaAlpha).value()));
      //#ifdef DEBUG_CIRCULAR_SURVEY
      //      qCWarning(CircularSurveyLog) << "circularTransects(): sector
      //      parameres:"; qCWarning(CircularSurveyLog) << "alpha1: " <<
      //      to_string(snake::Degree(alpha1)).c_str();
      //      qCWarning(CircularSurveyLog) << "alpha2:
      //      "
      //      << to_string(snake::Degree(alpha2)).c_str();
      //      qCWarning(CircularSurveyLog) << "n: "
      //      << to_string((alpha2 - alpha1) / deltaAlpha).c_str();
      //      qCWarning(CircularSurveyLog)
      //      << "nSectors: " << nSectors; qCWarning(CircularSurveyLog) <<
      //      "rMin: " << to_string(rMin).c_str(); qCWarning(CircularSurveyLog)
      //      << "rMax: " << to_string(rMax).c_str();
      //      qCWarning(CircularSurveyLog) << "nTran: " << nTran;
      //#endif
      using ClipperCircle =
          GenericCircle<ClipperLib::cInt, ClipperLib::IntPoint>;
      for (auto &sector : sectors) {
        ClipperCircle circle(rScaled, originScaled);
        approximate(circle, nSectors, sector);
        rScaled += deltaRScaled;
      }
      // Clip sectors to polygonENU.
      ClipperLib::Path polygonClipper;
      snake::FPolygon shrinked;
      snake::offsetPolygon(polygon, shrinked, -0.3);
      auto &outer = shrinked.outer();
      polygonClipper.reserve(outer.size());
      for (auto it = outer.begin(); it < outer.end() - 1; ++it) {
        auto x = ClipperLib::cInt(std::round(it->get<0>() * CLIPPER_SCALE));
        auto y = ClipperLib::cInt(std::round(it->get<1>() * CLIPPER_SCALE));
        polygonClipper.push_back(ClipperLib::IntPoint{x, y});
      }
      ClipperLib::Clipper clipper;
      clipper.AddPath(polygonClipper, ClipperLib::ptClip, true);
      clipper.AddPaths(sectors, ClipperLib::ptSubject, false);
      ClipperLib::PolyTree transectsClipper;
      clipper.Execute(ClipperLib::ctIntersection, transectsClipper,
                      ClipperLib::pftNonZero, ClipperLib::pftNonZero);

      // Subtract holes.
      if (tiles.size() > 0) {
        vector<ClipperLib::Path> processedTiles;
        for (const auto &tile : tiles) {
          ClipperLib::Path path;
          for (const auto &v : tile.outer()) {
            path.push_back(ClipperLib::IntPoint{
                static_cast<ClipperLib::cInt>(v.get<0>() * CLIPPER_SCALE),
                static_cast<ClipperLib::cInt>(v.get<1>() * CLIPPER_SCALE)});
          }
          processedTiles.push_back(std::move(path));
        }

        clipper.Clear();
        for (const auto &child : transectsClipper.Childs) {
          clipper.AddPath(child->Contour, ClipperLib::ptSubject, false);
        }
        clipper.AddPaths(processedTiles, ClipperLib::ptClip, true);
        transectsClipper.Clear();
        clipper.Execute(ClipperLib::ctDifference, transectsClipper,
                        ClipperLib::pftNonZero, ClipperLib::pftNonZero);
      }

      // Extract transects from  PolyTree and convert them to
      // BoostLineString
      for (const auto &child : transectsClipper.Childs) {
        snake::FLineString transect;
        transect.reserve(child->Contour.size());
        for (const auto &vertex : child->Contour) {
          auto x = static_cast<double>(vertex.X) / CLIPPER_SCALE;
          auto y = static_cast<double>(vertex.Y) / CLIPPER_SCALE;
          transect.push_back(snake::FPoint(x, y));
        }
        transects.push_back(transect);
      }

      // Join sectors which where slit due to clipping.
      const double th = 0.01;
      for (auto ito = transects.begin(); ito < transects.end(); ++ito) {
        for (auto iti = ito + 1; iti < transects.end(); ++iti) {
          auto dist1 = bg::distance(ito->front(), iti->front());
          if (dist1 < th) {
            snake::FLineString temp;
            for (auto it = iti->end() - 1; it >= iti->begin(); --it) {
              temp.push_back(*it);
            }
            temp.insert(temp.end(), ito->begin(), ito->end());
            *ito = temp;
            transects.erase(iti);
            break;
          }
          auto dist2 = bg::distance(ito->front(), iti->back());
          if (dist2 < th) {
            snake::FLineString temp;
            temp.insert(temp.end(), iti->begin(), iti->end());
            temp.insert(temp.end(), ito->begin(), ito->end());
            *ito = temp;
            transects.erase(iti);
            break;
          }
          auto dist3 = bg::distance(ito->back(), iti->front());
          if (dist3 < th) {
            snake::FLineString temp;
            temp.insert(temp.end(), ito->begin(), ito->end());
            temp.insert(temp.end(), iti->begin(), iti->end());
            *ito = temp;
            transects.erase(iti);
            break;
          }
          auto dist4 = bg::distance(ito->back(), iti->back());
          if (dist4 < th) {
            snake::FLineString temp;
            temp.insert(temp.end(), ito->begin(), ito->end());
            for (auto it = iti->end() - 1; it >= iti->begin(); --it) {
              temp.push_back(*it);
            }
            *ito = temp;
            transects.erase(iti);
            break;
          }
        }
      }

      // Remove short transects
      for (auto it = transects.begin(); it < transects.end();) {
        if (bg::length(*it) < minLength.value()) {
          it = transects.erase(it);
        } else {
          ++it;
        }
      }

      qCWarning(CircularSurveyLog)
          << "circularTransects(): transect gen. time: "
          << std::chrono::duration_cast<std::chrono::milliseconds>(
                 std::chrono::high_resolution_clock::now() - s1)
                 .count()
          << " ms";
      return true;
    }
  }
  return false;
}

bool linearTransects(const snake::FPolygon &polygon,
                     const std::vector<snake::FPolygon> &tiles,
                     snake::Length distance, snake::Angle angle,
                     snake::Length minLength, snake::Transects &transects) {
  namespace tr = bg::strategy::transform;
  auto s1 = std::chrono::high_resolution_clock::now();

  // Check preconitions
  if (polygon.outer().size() >= 3) {
    // Convert to ENU system.
    std::string error;
    // Check validity.
    if (!bg::is_valid(polygon, error)) {
      std::stringstream ss;
      ss << bg::wkt(polygon);

      qCWarning(CircularSurveyLog) << "linearTransects(): "
                                      "invalid polygon. "
                                   << error.c_str() << ss.str().c_str();
    } else {
      tr::rotate_transformer<bg::degree, double, 2, 2> rotate(angle.value() *
                                                              180 / M_PI);
      // Rotate polygon by angle and calculate bounding box.
      snake::FPolygon polygonENURotated;
      bg::transform(polygon.outer(), polygonENURotated.outer(), rotate);
      snake::FBox box;
      boost::geometry::envelope(polygonENURotated, box);
      double x0 = box.min_corner().get<0>();
      double y0 = box.min_corner().get<1>();
      double x1 = box.max_corner().get<0>();
      double y1 = box.max_corner().get<1>();

      // Generate transects and convert them to clipper path.
      size_t num_t = ceil((y1 - y0) / distance.value()); // number of transects
      vector<ClipperLib::Path> transectsClipper;
      transectsClipper.reserve(num_t);
      for (size_t i = 0; i < num_t; ++i) {
        // calculate transect
        snake::FPoint v1{x0, y0 + i * distance.value()};
        snake::FPoint v2{x1, y0 + i * distance.value()};
        snake::FLineString transect;
        transect.push_back(v1);
        transect.push_back(v2);
        // transform back
        snake::FLineString temp_transect;
        tr::rotate_transformer<bg::degree, double, 2, 2> rotate_back(
            -angle.value() * 180 / M_PI);
        bg::transform(transect, temp_transect, rotate_back);
        // to clipper
        ClipperLib::IntPoint c1{static_cast<ClipperLib::cInt>(
                                    temp_transect[0].get<0>() * CLIPPER_SCALE),
                                static_cast<ClipperLib::cInt>(
                                    temp_transect[0].get<1>() * CLIPPER_SCALE)};
        ClipperLib::IntPoint c2{static_cast<ClipperLib::cInt>(
                                    temp_transect[1].get<0>() * CLIPPER_SCALE),
                                static_cast<ClipperLib::cInt>(
                                    temp_transect[1].get<1>() * CLIPPER_SCALE)};
        ClipperLib::Path path{c1, c2};
        transectsClipper.push_back(path);
      }

      if (transectsClipper.size() == 0) {
        std::stringstream ss;
        ss << "Not able to generate transects. Parameter: distance = "
           << distance << std::endl;
        qCWarning(CircularSurveyLog)
            << "linearTransects(): " << ss.str().c_str();
        return false;
      }

      // Convert measurement area to clipper path.
      snake::FPolygon shrinked;
      snake::offsetPolygon(polygon, shrinked, -0.2);
      auto &outer = shrinked.outer();
      ClipperLib::Path polygonClipper;
      for (auto vertex : outer) {
        polygonClipper.push_back(ClipperLib::IntPoint{
            static_cast<ClipperLib::cInt>(vertex.get<0>() * CLIPPER_SCALE),
            static_cast<ClipperLib::cInt>(vertex.get<1>() * CLIPPER_SCALE)});
      }

      // Perform clipping.
      // Clip transects to measurement area.
      ClipperLib::Clipper clipper;
      clipper.AddPath(polygonClipper, ClipperLib::ptClip, true);
      clipper.AddPaths(transectsClipper, ClipperLib::ptSubject, false);
      ClipperLib::PolyTree clippedTransecs;
      clipper.Execute(ClipperLib::ctIntersection, clippedTransecs,
                      ClipperLib::pftNonZero, ClipperLib::pftNonZero);

      // Subtract holes.
      if (tiles.size() > 0) {
        vector<ClipperLib::Path> processedTiles;
        for (const auto &tile : tiles) {
          ClipperLib::Path path;
          for (const auto &v : tile.outer()) {
            path.push_back(ClipperLib::IntPoint{
                static_cast<ClipperLib::cInt>(v.get<0>() * CLIPPER_SCALE),
                static_cast<ClipperLib::cInt>(v.get<1>() * CLIPPER_SCALE)});
          }
          processedTiles.push_back(std::move(path));
        }

        clipper.Clear();
        for (const auto &child : clippedTransecs.Childs) {
          clipper.AddPath(child->Contour, ClipperLib::ptSubject, false);
        }
        clipper.AddPaths(processedTiles, ClipperLib::ptClip, true);
        clippedTransecs.Clear();
        clipper.Execute(ClipperLib::ctDifference, clippedTransecs,
                        ClipperLib::pftNonZero, ClipperLib::pftNonZero);
      }

      // Extract transects from  PolyTree and convert them to BoostLineString
      for (const auto &child : clippedTransecs.Childs) {
        const auto &clipperTransect = child->Contour;
        snake::FPoint v1{
            static_cast<double>(clipperTransect[0].X) / CLIPPER_SCALE,
            static_cast<double>(clipperTransect[0].Y) / CLIPPER_SCALE};
        snake::FPoint v2{
            static_cast<double>(clipperTransect[1].X) / CLIPPER_SCALE,
            static_cast<double>(clipperTransect[1].Y) / CLIPPER_SCALE};

        snake::FLineString transect{v1, v2};
        if (bg::length(transect) >= minLength.value()) {
          transects.push_back(transect);
        }
      }

      if (transects.size() == 0) {
        std::stringstream ss;
        ss << "Not able to  generatetransects. Parameter: minLength = "
           << minLength << std::endl;
        qCWarning(CircularSurveyLog)
            << "linearTransects(): " << ss.str().c_str();
        return false;
      }
      qCWarning(CircularSurveyLog)
          << "linearTransects(): transect gen. time: "
          << std::chrono::duration_cast<std::chrono::milliseconds>(
                 std::chrono::high_resolution_clock::now() - s1)
                 .count()
          << " ms";
      return true;
    }
  }
  return false;
}
/*!
    \class CircularSurveyComplexItem
    \inmodule Wima

    \brief The \c CircularSurveyComplexItem class provides a survey mission
   item with circular transects around a point of interest.

    CircularSurveyComplexItem class provides a survey mission item with
   circular transects around a point of interest. Within the \c Wima module
   it's used to scan a defined area with constant angle (circular transects)
   to the base station (point of interest).

    \sa WimaArea
*/