CircularSurvey.cc

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

template <int k> ClipperLib::cInt get(ClipperLib::IntPoint &p);
template <> ClipperLib::cInt get<0>(ClipperLib::IntPoint &p) { return p.X; }
template <> ClipperLib::cInt get<1>(ClipperLib::IntPoint &p) { return p.Y; }
#include "Geometry/GenericCircle.h"
// boost
#include <boost/units/io.hpp>
#include <boost/units/systems/si.hpp>

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 QGeoCoordinate &ref, const QGeoCoordinate &depot,
                       bool useDepot, const QList<QGeoCoordinate> &polygon,
                       snake::Length deltaR, snake::Angle deltaAlpha,
                       snake::Length minLength, snake::Transects &transects);

bool linearTransects(const QGeoCoordinate &origin, const QGeoCoordinate &depot,
                     bool useDepot, const QList<QGeoCoordinate> &polygon,
                     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";

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]),
      _pWorker(std::make_unique<RoutingThread>()), _needsStoring(false),
      _needsReversal(false), _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, &CircularSurvey::safeAreaChanged, this,
          &CircularSurvey::_rebuildTransects);
  connect(&this->_type, &Fact::rawValueChanged, 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 = false;
}

CircularSurvey::~CircularSurvey() {}

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

void CircularSurvey::reverse() {
  this->_needsReversal = true;
  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; }

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

QList<QGeoCoordinate> CircularSurvey::safeArea() const {
  return this->_safeArea;
}

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::setDepot(const QGeoCoordinate &depot) {
  if (this->_depot != depot) {
    this->_depot = depot;
    this->_depot.setAltitude(0);
    emit depotChanged();
  }
}

void CircularSurvey::setSafeArea(const QList<QGeoCoordinate> &safeArea) {
  if (this->_safeArea != safeArea) {
    this->_safeArea = safeArea;
    emit safeAreaChanged();
  }
}

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},
      {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());
  _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[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::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) {
  // Store result of former calculation.
  if (this->_needsStoring) {
#ifdef SHOW_CIRCULAR_SURVEY_TIME
    auto start = std::chrono::high_resolution_clock::now();
#endif
    // 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 &transectsENU = this->_workerOutput->transects;
    const auto &ori = this->_referencePoint;
    std::size_t startIdx = 0;
    if (transectsENU.size() > 0 && transectsENU.front().size() == 1) {
      startIdx = 1;
    }
    for (std::size_t i = startIdx; i < transectsENU.size(); ++i) {
      const auto &t = transectsENU[i];
      QList<QGeoCoordinate> trGeo;
      for (auto &v : t) {
        QGeoCoordinate c;
        snake::fromENU(ori, v, c);
        trGeo.append(c);
      }
      this->_rawTransects.append(trGeo);
    }
    // Store route.
    const auto &transectsInfo = this->_workerOutput->transectsInfo;
    const auto &route = this->_workerOutput->route;
    // Find index of first waypoint.
    std::size_t idxFirst = 0;
    const auto &infoFirst = transectsInfo.front();
    const auto &firstTransect = transectsENU[infoFirst.index];
    const auto &firstWaypoint =
        infoFirst.reversed ? firstTransect.back() : firstTransect.front();
    double th = 0.001;
    for (std::size_t i = 0; i < route.size(); ++i) {
      auto dist = bg::distance(route[i], firstWaypoint);
      if (dist < th) {
        idxFirst = i;
        break;
      }
    }
    // Find index of last waypoint.
    std::size_t idxLast = route.size() - 1;
    const auto &infoLast = transectsInfo.back();
    const auto &lastTransect = transectsENU[infoLast.index];
    const auto &lastWaypoint =
        infoLast.reversed ? lastTransect.front() : lastTransect.back();
    for (long i = route.size() - 1; i >= 0; --i) {
      auto dist = bg::distance(route[i], lastWaypoint);
      if (dist < th) {
        idxLast = i;
        break;
      }
    }
    // Convert to geo coordinates.
    QList<CoordInfo_t> list;
    for (std::size_t i = idxFirst; i <= idxLast; ++i) {
      auto &vertex = route[i];
      QGeoCoordinate c;
      snake::fromENU(ori, vertex, c);
      list.append(CoordInfo_t{c, CoordTypeInterior});
    }
    this->_transects.append(list);
    // Mark transect as stored and ready.
    this->_needsStoring = false;
    this->_transectsDirty = false;
#ifdef SHOW_CIRCULAR_SURVEY_TIME
    qWarning() << "CS::rebuildTransectsPhase1(): store: "
               << std::chrono::duration_cast<std::chrono::milliseconds>(
                      std::chrono::high_resolution_clock::now() - start)
                      .count()
               << " ms";
#endif
  }
  // Reverse transects only.
  else if (this->_needsReversal) {
    if (this->_transects.size() > 0) {
      auto &t = this->_transects.front();
      QList<CoordInfo_t> list;
      list.reserve(t.size());
      for (auto it = t.end() - 1; it >= t.begin(); --it) {
        list.append(*it);
      }
      this->_transects.clear();
      this->_transects.append(list);
    }
    this->_needsReversal = false;
  }
  // Start calculation.
  else {
#ifdef SHOW_CIRCULAR_SURVEY_TIME
    auto start = std::chrono::high_resolution_clock::now();
#endif
    this->_transects.clear();
    this->_rawTransects.clear();
    // Prepare data.
    auto ref = this->_referencePoint;
    auto polygon = this->_surveyAreaPolygon.coordinateList();
    for (auto &v : polygon) {
      v.setAltitude(0);
    }
    auto safeArea = this->_safeArea;
    for (auto &v : safeArea) {
      v.setAltitude(0);
    }
    auto depot = this->_depot;
    snake::BoostPolygon safeAreaENU;
    bool useDepot = false;
    if (this->_depot.isValid() && this->_safeArea.size() >= 3) {
      useDepot = true;
      snake::areaToEnu(ref, safeArea, safeAreaENU);
    } else {
      snake::areaToEnu(ref, polygon, safeAreaENU);
    }
    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)) {
      if (alpha >= snake::Angle(0.3 * bu::degree::degree) &&
          alpha <= snake::Angle(45 * bu::degree::degree)) {
        auto generator = [ref, depot, useDepot, polygon, distance, alpha,
                          minLength](snake::Transects &transects) -> bool {
          return circularTransects(ref, depot, useDepot, polygon, distance,
                                   alpha, minLength, transects);
        };
        // Start routing worker.
        this->_pWorker->route(safeAreaENU, 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 = [ref, depot, useDepot, polygon, distance, alpha,
                        minLength](snake::Transects &transects) -> bool {
        return linearTransects(ref, depot, useDepot, polygon, distance, alpha,
                               minLength, transects);
      };
      // Start routing worker.
      this->_pWorker->route(safeAreaENU, generator);
    } else {
      qWarning()
          << "CircularSurvey::rebuildTransectsPhase1(): invalid survey type:"
          << this->_type.rawValue().toUInt();
    }
    // Mark transects as dirty.
    this->_transectsDirty = true;
#ifdef SHOW_CIRCULAR_SURVEY_TIME
    qWarning() << "CS::rebuildTransectsPhase1(): start: "
               << std::chrono::duration_cast<std::chrono::milliseconds>(
                      std::chrono::high_resolution_clock::now() - start)
                      .count()
               << " ms";
#endif
  }
}

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->_workerOutput = pRoute;
  this->_needsStoring = true;
  this->_rebuildTransects();
}

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

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

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

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

bool circularTransects(const QGeoCoordinate &ref, const QGeoCoordinate &depot,
                       bool useDepot, const QList<QGeoCoordinate> &polygon,
                       snake::Length deltaR, snake::Angle deltaAlpha,
                       snake::Length minLength, snake::Transects &transects) {
#ifdef SHOW_CIRCULAR_SURVEY_TIME
  auto s1 = std::chrono::high_resolution_clock::now();
#endif
  // Check preconitions
  if (polygon.size() >= 3) {
    using namespace boost::units;
    // Convert geo polygon to ENU polygon.
    snake::BoostPolygon polygonENU;
    snake::BoostPoint originENU{0, 0};
    snake::BoostPoint depotENU{0, 0};
    snake::areaToEnu(ref, polygon, polygonENU);
    snake::toENU(ref, ref, originENU);
    snake::toENU(ref, depot, depotENU);
    std::string error;
    // Check validity.
    if (!bg::is_valid(polygonENU, error)) {
#ifdef DEBUG_CIRCULAR_SURVEY
      qWarning() << "CS::circularTransects(): "
                    "invalid polygon.";
      qWarning() << error.c_str();
      std::stringstream ss;
      ss << bg::wkt(polygonENU);
      qWarning() << ss.str().c_str();
#endif
    } else {
      // Calculate polygon distances and angles.
      std::vector<snake::Length> distances;
      distances.reserve(polygonENU.outer().size());
      std::vector<snake::Angle> angles;
      angles.reserve(polygonENU.outer().size());
      //#ifdef DEBUG_CIRCULAR_SURVEY
      //      qWarning() << "CS::circularTransects():";
      //#endif
      for (const auto &p : polygonENU.outer()) {
        snake::Length distance = bg::distance(originENU, 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
        //        qWarning() << "distances, angles, coordinates:";
        //        qWarning() << to_string(distance).c_str();
        //        qWarning() << to_string(snake::Degree(alpha)).c_str();
        //        qWarning() << "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(originENU, polygonENU)) {
        rMin = bg::distance(originENU, polygonENU) * 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(originENU.get<0>())),
          ClipperLib::cInt(std::round(originENU.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
      //      qWarning() << "CS::circularTransects(): sector parameres:";
      //      qWarning() << "alpha1: " <<
      //      to_string(snake::Degree(alpha1)).c_str(); qWarning() << "alpha2: "
      //      << to_string(snake::Degree(alpha2)).c_str(); qWarning() << "n: "
      //      << to_string((alpha2 - alpha1) / deltaAlpha).c_str(); qWarning()
      //      << "nSectors: " << nSectors; qWarning() << "rMin: " <<
      //      to_string(rMin).c_str(); qWarning() << "rMax: " <<
      //      to_string(rMax).c_str(); qWarning() << "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::BoostPolygon shrinked;
      snake::offsetPolygon(polygonENU, 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);

      // Extract transects from  PolyTree and convert them to
      // BoostLineString
      if (useDepot) {
        transects.push_back(snake::BoostLineString{depotENU});
      }
      for (const auto &child : transectsClipper.Childs) {
        snake::BoostLineString 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::BoostPoint(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::BoostLineString 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::BoostLineString 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::BoostLineString 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::BoostLineString 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
      auto begin = useDepot ? transects.begin() + 1 : transects.begin();
      for (auto it = begin; it < transects.end();) {
        if (bg::length(*it) < minLength.value()) {
          it = transects.erase(it);
        } else {
          ++it;
        }
      }

      if (!useDepot) {
        // Move transect with min. distance to the front.
        auto minDist = std::numeric_limits<double>::max();
        auto minIt = transects.begin();
        bool reverse = false;
        for (auto it = transects.begin(); it < transects.end(); ++it) {
          auto distFront = bg::distance(originENU, it->front());
          auto distBack = bg::distance(originENU, it->back());
          if (distFront < minDist) {
            minDist = distFront;
            minIt = it;
            reverse = false;
          }
          if (distBack < minDist) {
            minDist = distBack;
            minIt = it;
            reverse = true;
          }
        }
        // Swap and reverse (if necessary).
        if (minIt != transects.begin()) {
          auto minTransect = *minIt;
          if (reverse) {
            snake::BoostLineString rev;
            for (auto it = minTransect.end() - 1; it >= minTransect.begin();
                 --it) {
              rev.push_back(*it);
            }
            minTransect = rev;
          }
          *minIt = *transects.begin();
          *transects.begin() = minTransect;
        }
      }
#ifdef SHOW_CIRCULAR_SURVEY_TIME
      qWarning() << "CS::circularTransects(): transect gen. time: "
                 << std::chrono::duration_cast<std::chrono::milliseconds>(
                        std::chrono::high_resolution_clock::now() - s1)
                        .count()
                 << " ms";
#endif
      return true;
    }
  }
  return false;
}

bool linearTransects(const QGeoCoordinate &origin, const QGeoCoordinate &depot,
                     bool useDepot, const QList<QGeoCoordinate> &polygon,
                     snake::Length distance, snake::Angle angle,
                     snake::Length minLength, snake::Transects &transects) {
  namespace tr = bg::strategy::transform;
#ifdef SHOW_CIRCULAR_SURVEY_TIME
  auto s1 = std::chrono::high_resolution_clock::now();
#endif
  // Check preconitions
  if (polygon.size() >= 3) {
    // Convert to ENU system.
    snake::BoostPolygon polygonENU;
    snake::areaToEnu(origin, polygon, polygonENU);
    std::string error;
    // Check validity.
    if (!bg::is_valid(polygonENU, error)) {
#ifdef DEBUG_CIRCULAR_SURVEY
      qWarning() << "CS::circularTransects(): "
                    "invalid polygon.";
      qWarning() << error.c_str();
      std::stringstream ss;
      ss << bg::wkt(polygonENU);
      qWarning() << ss.str().c_str();
#endif
    } else {
      snake::BoostPoint depotENU;
      snake::toENU(origin, depot, depotENU);
      tr::rotate_transformer<bg::degree, double, 2, 2> rotate(angle.value() *
                                                              180 / M_PI);
      // Rotate polygon by angle and calculate bounding box.
      snake::BoostPolygon polygonENURotated;
      bg::transform(polygonENU, polygonENURotated, rotate);
      snake::BoostBox 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::BoostPoint v1{x0, y0 + i * distance.value()};
        snake::BoostPoint v2{x1, y0 + i * distance.value()};
        snake::BoostLineString transect;
        transect.push_back(v1);
        transect.push_back(v2);
        // transform back
        snake::BoostLineString 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;
        qWarning() << "CircularSurvey::linearTransects(): " << ss.str().c_str();
        return false;
      }

      // Convert measurement area to clipper path.
      snake::BoostPolygon shrinked;
      snake::offsetPolygon(polygonENU, 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);

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

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

      if (transects.size() == 0) {
        std::stringstream ss;
        ss << "Not able to generate transects. Parameter: minLength = "
           << minLength << std::endl;
        qWarning() << "CircularSurvey::linearTransects(): " << ss.str().c_str();
        return false;
      }
#ifdef SHOW_CIRCULAR_SURVEY_TIME
      qWarning() << "CS::circularTransects(): transect gen. time: "
                 << std::chrono::duration_cast<std::chrono::milliseconds>(
                        std::chrono::high_resolution_clock::now() - s1)
                        .count()
                 << " ms";
#endif
      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
*/