CircularSurvey.cc

#include "CircularSurvey.h"
#include "CSWorker.h"
// QGC
#include "JsonHelper.h"
#include "QGCApplication.h"
// Wima
#include "snake.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;
};

const char *CircularSurvey::settingsGroup = "CircularSurvey";
const char *CircularSurvey::deltaRName = "DeltaR";
const char *CircularSurvey::deltaAlphaName = "DeltaAlpha";
const char *CircularSurvey::transectMinLengthName = "TransectMinLength";
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]),
      _pWorker(std::make_unique<CSWorker>()), _needsStoring(false),
      _needsReversal(false) {
  Q_UNUSED(kmlOrShpFile)
  _editorQml = "qrc:/qml/CircularSurveyItemEditor.qml";

  // Connect facts.
  connect(&_deltaR, &Fact::valueChanged, this,
          &CircularSurvey::_rebuildTransects);
  connect(&_deltaAlpha, &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 worker.
  qRegisterMetaType<PtrRoute>("PtrRoute");
  connect(this->_pWorker.get(), &CSWorker::ready, this,
          &CircularSurvey::_setTransects);
  connect(this->_pWorker.get(), &CSWorker::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;

    emit refPointChanged();
  }
}

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

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

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

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

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

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

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

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

  _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[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 (this->_needsStoring) {
    // If the transects are getting rebuilt then any previously loaded
    // mission items are now invalid.
    if (_loadedMissionItemsParent) {
      _loadedMissionItems.clear();
      _loadedMissionItemsParent->deleteLater();
      _loadedMissionItemsParent = nullptr;
    }
    // Copy Transects.
    QList<CoordInfo_t> list;
    for (const auto c : *this->_pRoute) {
      list.append(CoordInfo_t{c, CoordTypeInterior});
    }
    this->_transects.append(list);
    // Mark transect as stored;
    this->_needsStoring = false;
  } 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;
  } else {
    this->_transects.clear();
    auto polygon = this->_surveyAreaPolygon.coordinateList();
    if (this->_depot.isValid() && this->_safeArea.size() >= 3) {
      this->_pWorker->update(
          this->_depot, this->_safeArea, polygon, this->_referencePoint,
          this->_deltaR.rawValue().toDouble() * bu::si::meter,
          this->_minLength.rawValue().toDouble() * bu::si::meter,
          snake::Angle(this->_deltaAlpha.rawValue().toDouble() *
                       bu::degree::degree));
    } else {
      this->_pWorker->update(
          polygon, this->_referencePoint,
          this->_deltaR.rawValue().toDouble() * bu::si::meter,
          this->_minLength.rawValue().toDouble() * bu::si::meter,
          snake::Angle(this->_deltaAlpha.rawValue().toDouble() *
                       bu::degree::degree));
    }
  }
}

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::PtrRoute pRoute) {
  this->_pRoute = pRoute;
  this->_needsStoring = true;
  this->_rebuildTransects();
}

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

bool CircularSurvey::calculating() const {
  return this->_pWorker->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
*/