CircularSurvey.cc

#include "CircularSurvey.h"

#include <QtConcurrentRun>

#include "JsonHelper.h"
#include "QGCApplication.h"

#include <chrono>

#include "clipper/clipper.hpp"
#include "snake.h"
#define CLIPPER_SCALE 10000

template <int k> ClipperLib::cInt get(ClipperLib::IntPoint &p);
#include "Geometry/GenericCircle.h"
#include "Geometry/GeoUtilities.h"
#include "Geometry/PlanimetryCalculus.h"
#include "Geometry/PolygonCalculus.h"

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

template <> ClipperLib::cInt get<0>(ClipperLib::IntPoint &p) { return p.X; }
template <> ClipperLib::cInt get<1>(ClipperLib::IntPoint &p) { return p.Y; }

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

private:
  Functor fun;
};

const char *CircularSurvey::settingsGroup = "CircularSurvey";
const char *CircularSurvey::deltaRName = "DeltaR";
const char *CircularSurvey::deltaAlphaName = "DeltaAlpha";
const char *CircularSurvey::transectMinLengthName = "TransectMinLength";
const char *CircularSurvey::reverseName = "Reverse";
const char *CircularSurvey::maxWaypointsName = "MaxWaypoints";
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)),
      _deltaR(settingsGroup, _metaDataMap[deltaRName]),
      _deltaAlpha(settingsGroup, _metaDataMap[deltaAlphaName]),
      _minLength(settingsGroup, _metaDataMap[transectMinLengthName]),
      _reverse(settingsGroup, _metaDataMap[reverseName]),
      _maxWaypoints(settingsGroup, _metaDataMap[maxWaypointsName]),
      _isInitialized(false), _calculating(false), _cancle(false) {
  Q_UNUSED(kmlOrShpFile)
  _editorQml = "qrc:/qml/CircularSurveyItemEditor.qml";

  // Defer update if facts or ref. changes.
  connect(&_deltaR, &Fact::valueChanged, this, &CircularSurvey::_deferUpdate);
  connect(&_deltaAlpha, &Fact::valueChanged, this,
          &CircularSurvey::_deferUpdate);
  connect(&_minLength, &Fact::valueChanged, this,
          &CircularSurvey::_deferUpdate);
  connect(&_maxWaypoints, &Fact::valueChanged, [this] {
    this->CircularSurvey::_deferUpdate();
    qWarning() << "max waypoints implementaion missing";
  });
  connect(&_reverse, &Fact::valueChanged, [this] {
    this->CircularSurvey::_deferUpdate();
    qWarning() << "reverse implementaion missing";
  });
  connect(this, &CircularSurvey::refPointChanged, this,
          &CircularSurvey::_deferUpdate);
  connect(&this->_surveyAreaPolygon, &QGCMapPolygon::pathChanged, this,
          &CircularSurvey::_deferUpdate);
  // Setup Timer.
  _timer.setSingleShot(true);
  connect(&_timer, &QTimer::timeout, [this] { this->_rebuildTransects(); });

  // Future watcher.
  connect(&_watcher, &Watcher::finished, [this] {
    this->_calculating = false;
    emit calculatingChanged();
    if (!_cancle) {
      this->_transectsDirty = false;
    } else {
      _cancle = false;
    }
    this->_rebuildTransects();
  });
}

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

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

    emit refPointChanged();
  }
}

void CircularSurvey::setIsInitialized(bool isInitialized) {
  if (isInitialized != _isInitialized) {
    _isInitialized = isInitialized;

    emit isInitializedChanged();
  }
}

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

Fact *CircularSurvey::deltaR() { return &_deltaR; }

Fact *CircularSurvey::deltaAlpha() { return &_deltaAlpha; }

bool CircularSurvey::isInitialized() { return _isInitialized; }

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},
      {deltaRName, QJsonValue::Double, true},
      {deltaAlphaName, QJsonValue::Double, true},
      {transectMinLengthName, QJsonValue::Double, true},
      {reverseName, QJsonValue::Bool, 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;
  }

  _deltaR.setRawValue(complexObject[deltaRName].toDouble());
  _deltaAlpha.setRawValue(complexObject[deltaAlphaName].toDouble());
  _minLength.setRawValue(complexObject[transectMinLengthName].toDouble());
  _referencePoint.setLongitude(complexObject[refPointLongitudeName].toDouble());
  _referencePoint.setLatitude(complexObject[refPointLatitudeName].toDouble());
  _referencePoint.setAltitude(complexObject[refPointAltitudeName].toDouble());
  _reverse.setRawValue(complexObject[reverseName].toBool());
  setIsInitialized(true);

  _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[deltaRName] = _deltaR.rawValue().toDouble();
  saveObject[deltaAlphaName] = _deltaAlpha.rawValue().toDouble();
  saveObject[transectMinLengthName] = _minLength.rawValue().toDouble();
  saveObject[reverseName] = _reverse.rawValue().toBool();
  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)
    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) {
  if (_calculating)
    return;
  if (!_transectsDirty) {
    auto pTransects = _watcher.result();
    if (pTransects) {
#ifdef DEBUG_CIRCULAR_SURVEY
      qWarning()
          << "CircularSurvey::_rebuildTransectsPhase1(): storing transects.";
#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 new transects;
      _transects = *pTransects;
    }
  } else {
    _transects.clear();
    // Check preconitions
#ifdef DEBUG_CIRCULAR_SURVEY
    qWarning()
        << "CircularSurvey::_rebuildTransectsPhase1(): checking preconditions.";
#endif
    if (_isInitialized && _surveyAreaPolygon.count() >= 3 &&
        _deltaAlpha.rawValue() <= _deltaAlpha.rawMax() &&
        _deltaAlpha.rawValue() >= _deltaAlpha.rawMin() &&
        _deltaR.rawValue() <= _deltaR.rawMax() &&
        _deltaR.rawValue() >= _deltaR.rawMin()) {
#ifdef DEBUG_CIRCULAR_SURVEY
      qWarning()
          << "CircularSurvey::_rebuildTransectsPhase1(): preconditions ok.";
#endif
      using namespace boost::units;
      _calculating = true;
      emit calculatingChanged();
      // Copy input.
      const auto &polygon = this->_surveyAreaPolygon.coordinateList();
      const auto &origin = this->_referencePoint;
      const snake::Length deltaR(this->_deltaR.rawValue().toDouble() *
                                 si::meter);
      const snake::Angle deltaAlpha(this->_deltaAlpha.rawValue().toDouble() *
                                    degree::degree);
      const snake::Length minLength(this->_minLength.rawValue().toDouble() *
                                    si::meter);
      const auto maxWaypoints(this->_maxWaypoints.rawValue().toUInt());
      auto future = QtConcurrent::run([polygon, origin, deltaAlpha, minLength,
                                       maxWaypoints, deltaR] {
#ifdef DEBUG_CIRCULAR_SURVEY
        qWarning() << "CircularSurvey::_rebuildTransectsPhase1(): calculation "
                      "started.";
#endif
#ifdef SHOW_CIRCULAR_SURVEY_TIME
        auto start = std::chrono::high_resolution_clock::now();
        auto onExit = [&start] {
          qWarning() << "CircularSurvey: concurrent update execution time: "
                     << std::chrono::duration_cast<std::chrono::milliseconds>(
                            std::chrono::high_resolution_clock::now() - start)
                            .count()
                     << " ms";
        };
        CommandRAII<decltype(onExit)> commandRAII(onExit);
#endif
        // Convert geo polygon to ENU polygon.
        snake::BoostPolygon polygonENU;
        snake::BoostPoint originENU{0, 0};
        snake::areaToEnu(origin, polygon, polygonENU);
        std::string error;
        // Check validity.
        if (!bg::is_valid(polygonENU, error)) {
#ifdef DEBUG_CIRCULAR_SURVEY
          qWarning() << "CircularSurvey::_rebuildTransectsPhase1(): "
                        "invalid polygon.";
          qWarning() << error.c_str();
#endif
          return PtrTransects();
        } 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());
          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>()))};

#ifdef SHOW_CIRCULAR_SURVEY_TIME
          auto s1 = std::chrono::high_resolution_clock::now();
#endif
          // 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() << "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;
          auto &outer = polygonENU.outer();
          polygonClipper.reserve(outer.size() - 1);
          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
          snake::Transects transectsENU;
          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));
            }
            if (bg::length(transect) >= minLength.value()) {
              transectsENU.push_back(transect);
            }
          }
          // Join sectors which where slit due to clipping.
          static_assert(false, "continue here.");
          for (std::size_t i = 0; i < transectsENU.size() - 1; ++i) {
            const auto &t1 = transectsENU[i];
            for (std::size_t j = i + 1; j < transectsENU.size(); ++j) {
              const auto &t2 = transectsENU[j];
            }
          }
#ifdef SHOW_CIRCULAR_SURVEY_TIME
          qWarning() << "CircularSurvey: concurrent update transect gen. time: "
                     << std::chrono::duration_cast<std::chrono::milliseconds>(
                            std::chrono::high_resolution_clock::now() - s1)
                            .count()
                     << " ms";
#endif

          if (transectsENU.size() == 0) {
#ifdef DEBUG_CIRCULAR_SURVEY
            qWarning() << "CircularSurvey::_rebuildTransectsPhase1(): "
                          "not able to generate transects.";
#endif
            return PtrTransects();
          }

          // Route transects;
          std::vector<snake::TransectInfo> transectsInfo;
          snake::Route route;
          std::string errorString;
          bool success = snake::route(polygonENU, transectsENU, transectsInfo,
                                      route, errorString);
          if (!success) {
#ifdef DEBUG_CIRCULAR_SURVEY
            qWarning() << "CircularSurvey::_rebuildTransectsPhase1(): "
                          "routing failed.";
#endif
            return PtrTransects();
          }

          // Remove return path.
          const auto &info = transectsInfo.back();
          const auto &lastTransect = transectsENU[info.index];
          const auto &lastWaypoint =
              info.reversed ? lastTransect.front() : lastTransect.back();
          auto &wp = route.back();
          while (wp != lastWaypoint) {
            route.pop_back();
            wp = route.back();
          }

          // Convert to geo coordinates.
          QList<CoordInfo_t> transectList;
          transectList.reserve(route.size());
          for (const auto &vertex : route) {
            QGeoCoordinate c;
            snake::fromENU(origin, vertex, c);
            CoordInfo_t coordinfo = {c, CoordTypeInterior};
            transectList.append(coordinfo);
          }
          PtrTransects pTransects(new Transects());
          pTransects->append(transectList);
#ifdef DEBUG_CIRCULAR_SURVEY
          qWarning() << "CircularSurvey::_rebuildTransectsPhase1(): "
                        "concurrent update success.";
#endif
          return pTransects;
        }
      }); // QtConcurrent::run()
      _watcher.setFuture(future);
    }
#ifdef DEBUG_CIRCULAR_SURVEY
    else {
      qWarning()
          << "CircularSurvey::_rebuildTransectsPhase1(): preconditions failed.";
    }
#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::_deferUpdate() {
  if (!_calculating) {
#ifdef DEBUG_CIRCULAR_SURVEY
    qWarning() << "CircularSurvey::_deferUpdate(): defer update.";
#endif
    _transectsDirty = true;
    if (_timer.isActive()) {
      _timer.stop();
    }
    _timer.start(100 /*ms*/);
  } else {
    _cancle = true;
  }
}

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

Fact *CircularSurvey::reverse() { return &_reverse; }

Fact *CircularSurvey::maxWaypoints() { return &_maxWaypoints; }

bool CircularSurvey::calculating() { return _calculating; }

/*!
    \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
*/