SnakeDataManager.cc

#include "SnakeDataManager.h"

#include <QGeoCoordinate>
#include <QMutexLocker>

#include "QGCApplication.h"
#include "QGCToolbox.h"
#include "SettingsFact.h"
#include "SettingsManager.h"
#include "WimaSettings.h"

#include <iostream>
#include <memory>
#include <mutex>
#include <shared_mutex>

#include "Wima/Snake/SnakeTiles.h"
#include "Wima/Snake/SnakeTilesLocal.h"
#include "snake.h"

using QVariantList = QList<QVariant>;

using UniqueLock = std::unique_lock<shared_timed_mutex>;
using SharedLock = std::shared_lock<shared_timed_mutex>;

class SnakeDataManager::Impl {
public:
  struct Input {

    Input()
        : tileWidth(5 * si::meter), tileHeight(5 * si::meter),
          minTileArea(1 * si::meter * si::meter), scenarioChanged(true),
          lineDistance(2 * si::meter), minTransectLength(1 * si::meter),
          routeParametersChanged(true) {}

    QList<QGeoCoordinate> mArea;
    QGeoCoordinate ENUOrigin;
    QList<QGeoCoordinate> sArea;
    QList<QGeoCoordinate> corridor;
    Length tileWidth;
    Length tileHeight;
    Area minTileArea;
    std::atomic_bool scenarioChanged;

    Length lineDistance;
    Length minTransectLength;
    std::atomic_bool routeParametersChanged;

    mutable std::shared_timed_mutex mutex;
  };

  struct Output {
    Output() : calcInProgress(false) {}

    SnakeTiles tiles;
    QVariantList tileCenterPoints;
    SnakeTilesLocal tilesENU;
    QVector<QPointF> tileCenterPointsENU;
    QGeoCoordinate ENUOrigin;

    QVector<QGeoCoordinate> waypoints;
    QVector<QGeoCoordinate> arrivalPath;
    QVector<QGeoCoordinate> returnPath;
    QVector<QPointF> waypointsENU;
    QVector<QPointF> arrivalPathENU;
    QVector<QPointF> returnPathENU;

    QString errorMessage;

    std::atomic_bool calcInProgress;
    std::atomic_bool tilesUpdated;
    std::atomic_bool waypointsUpdated;
    std::atomic_bool progressUpdated;

    QNemoProgress progress;
    mutable std::shared_timed_mutex mutex;
  };

  Impl(SnakeDataManager *p);

  bool precondition() const;
  bool doTopicServiceSetup();

  // Internal data.
  snake::Scenario scenario;
  SnakeDataManager *parent;

  // Input
  Input input;
  // Output
  Output output;
};

SnakeDataManager::Impl::Impl(SnakeDataManager *p) : parent(p) {}

bool SnakeDataManager::Impl::precondition() const { return true; }

template <class Callable> class CommandRAII {
public:
  CommandRAII(Callable &fun) : _fun(fun) {}

  ~CommandRAII() { _fun(); }

private:
  Callable _fun;
};

SnakeDataManager::SnakeDataManager(QObject *parent)
    : QThread(parent), pImpl(std::make_unique<Impl>(this)) {}

void SnakeDataManager::setMeasurementArea(
    const QList<QGeoCoordinate> &measurementArea) {
  this->pImpl->input.scenarioChanged = true;
  UniqueLock lk(this->pImpl->input.mutex);
  this->pImpl->input.mArea = measurementArea;
  for (auto &vertex : this->pImpl->input.mArea) {
    vertex.setAltitude(0);
  }
}

void SnakeDataManager::setServiceArea(
    const QList<QGeoCoordinate> &serviceArea) {
  this->pImpl->input.scenarioChanged = true;
  UniqueLock lk(this->pImpl->input.mutex);
  this->pImpl->input.sArea = serviceArea;
  for (auto &vertex : this->pImpl->input.sArea) {
    vertex.setAltitude(0);
  }
}

void SnakeDataManager::setCorridor(const QList<QGeoCoordinate> &corridor) {
  this->pImpl->input.scenarioChanged = true;
  UniqueLock lk(this->pImpl->input.mutex);
  this->pImpl->input.corridor = corridor;
  for (auto &vertex : this->pImpl->input.corridor) {
    vertex.setAltitude(0);
  }
}

SnakeTiles SnakeDataManager::tiles() const {
  SharedLock lk(this->pImpl->output.mutex);
  return this->pImpl->output.tiles;
}

QVariantList SnakeDataManager::tileCenterPoints() const {
  SharedLock lk(this->pImpl->output.mutex);
  return this->pImpl->output.tileCenterPoints;
}

QNemoProgress SnakeDataManager::progress() const {
  SharedLock lk(this->pImpl->output.mutex);
  return this->pImpl->output.progress;
}

bool SnakeDataManager::calcInProgress() const {
  return this->pImpl->output.calcInProgress.load();
}

QString SnakeDataManager::errorMessage() const {
  SharedLock lk(this->pImpl->output.mutex);
  return this->pImpl->output.errorMessage;
}

bool SnakeDataManager::success() const {
  SharedLock lk(this->pImpl->output.mutex);
  return this->pImpl->output.errorMessage.isEmpty() ? true : false;
}

QVector<QGeoCoordinate> SnakeDataManager::waypoints() const {
  SharedLock lk(this->pImpl->output.mutex);
  return this->pImpl->output.waypoints;
}

QVector<QGeoCoordinate> SnakeDataManager::arrivalPath() const {
  SharedLock lk(this->pImpl->output.mutex);
  return this->pImpl->output.arrivalPath;
}

QVector<QGeoCoordinate> SnakeDataManager::returnPath() const {
  SharedLock lk(this->pImpl->output.mutex);
  return this->pImpl->output.returnPath;
}

Length SnakeDataManager::lineDistance() const {
  SharedLock lk(this->pImpl->input.mutex);
  return this->pImpl->input.lineDistance;
}

void SnakeDataManager::setLineDistance(Length lineDistance) {
  UniqueLock lk(this->pImpl->input.mutex);
  this->pImpl->input.routeParametersChanged = true;
  this->pImpl->input.lineDistance = lineDistance;
}

Area SnakeDataManager::minTileArea() const {
  SharedLock lk(this->pImpl->input.mutex);
  return this->pImpl->scenario.minTileArea();
}

void SnakeDataManager::setMinTileArea(Area minTileArea) {
  this->pImpl->input.scenarioChanged = true;
  UniqueLock lk(this->pImpl->input.mutex);
  this->pImpl->scenario.setMinTileArea(minTileArea);
}

Length SnakeDataManager::tileHeight() const {
  SharedLock lk(this->pImpl->input.mutex);
  return this->pImpl->scenario.tileHeight();
}

void SnakeDataManager::setTileHeight(Length tileHeight) {
  this->pImpl->input.scenarioChanged = true;
  UniqueLock lk(this->pImpl->input.mutex);
  this->pImpl->scenario.setTileHeight(tileHeight);
}

Length SnakeDataManager::tileWidth() const {
  SharedLock lk(this->pImpl->input.mutex);
  return this->pImpl->scenario.tileWidth();
}

void SnakeDataManager::setTileWidth(Length tileWidth) {
  UniqueLock lk(this->pImpl->input.mutex);
  this->pImpl->input.scenarioChanged = true;
  this->pImpl->scenario.setTileWidth(tileWidth);
}

void SnakeDataManager::run() {
#ifndef NDEBUG
  auto startTime = std::chrono::high_resolution_clock::now();
#endif

  this->pImpl->output.calcInProgress.store(true);
  emit calcInProgressChanged(this->pImpl->output.calcInProgress.load());
  auto onExit = [this, &startTime] {
#ifndef NDEBUG
    qDebug() << "SnakeDataManager::run() execution time: "
             << std::chrono::duration_cast<std::chrono::milliseconds>(
                    std::chrono::high_resolution_clock::now() - startTime)
                    .count()
             << " ms";
#endif
    this->pImpl->output.calcInProgress.store(true);
    emit calcInProgressChanged(this->pImpl->output.calcInProgress.load());
  };
  CommandRAII<decltype(onExit)> onExitRAII(onExit);

  this->pImpl->output.tilesUpdated = false;
  this->pImpl->output.waypointsUpdated = false;
  this->pImpl->output.progressUpdated = false;

  {
    // Check preconditions.
    SharedLock lk(this->pImpl->input.mutex);

    if (!this->pImpl->precondition())
      return;

    if (this->pImpl->input.mArea.size() < 3) {
      UniqueLock uLock(this->pImpl->output.mutex);
      this->pImpl->output.errorMessage = "Measurement area invalid: size < 3.";
      return;
    }
    if (this->pImpl->input.sArea.size() < 3) {
      UniqueLock uLock(this->pImpl->output.mutex);
      this->pImpl->output.errorMessage = "Service area invalid: size < 3.";
      return;
    }

    // Update Scenario if necessary
    if (this->pImpl->input.scenarioChanged) {
      // Set Origin
      QGeoCoordinate origin;
      {
        UniqueLock uLock(this->pImpl->output.mutex);
        this->pImpl->output.ENUOrigin = this->pImpl->input.mArea.front();
        origin = this->pImpl->output.ENUOrigin;
      }

      // Update measurement area.
      auto &mAreaEnu = this->pImpl->scenario.measurementArea();
      auto &mArea = this->pImpl->input.mArea;
      mAreaEnu.clear();
      for (auto geoVertex : mArea) {
        snake::BoostPoint p;
        snake::toENU(origin, geoVertex, p);
        mAreaEnu.outer().push_back(p);
      }

      // Update service area.
      auto &sAreaEnu = this->pImpl->scenario.serviceArea();
      auto &sArea = this->pImpl->input.sArea;
      sAreaEnu.clear();
      for (auto geoVertex : sArea) {
        snake::BoostPoint p;
        snake::toENU(origin, geoVertex, p);
        sAreaEnu.outer().push_back(p);
      }

      // Update corridor.
      auto &corridorEnu = this->pImpl->scenario.corridor();
      auto &corridor = this->pImpl->input.corridor;
      corridor.clear();
      for (auto geoVertex : corridor) {
        snake::BoostPoint p;
        snake::toENU(origin, geoVertex, p);
        corridorEnu.outer().push_back(p);
      }

      // Update parametes.
      this->pImpl->scenario.setTileHeight(this->pImpl->input.tileHeight);
      this->pImpl->scenario.setTileWidth(this->pImpl->input.tileWidth);
      this->pImpl->scenario.setMinTileArea(this->pImpl->input.minTileArea);

      this->pImpl->input.scenarioChanged = false;
      if (!this->pImpl->scenario.update()) {
        this->pImpl->output.errorMessage =
            this->pImpl->scenario.errorString.c_str();
        return;
      }

      this->pImpl->output.tilesUpdated = true;
    }
  }

  snake::flight_plan::Transects transectsRouted;
  snake::flight_plan::Route route;
  std::vector<int> progress;
  if (this->pImpl->input.scenarioChanged ||
      this->pImpl->input.routeParametersChanged) {
    // Sample data
    SharedLock inputLock(this->pImpl->input.mutex);
    SharedLock outputLock(this->pImpl->output.mutex);
    // Verify progress.
    size_t nTiles = this->pImpl->scenario.tiles().size();
    if (this->pImpl->output.progress.progress().size() != nTiles) {
      outputLock.unlock();
      for (size_t i = 0; i < nTiles; ++i) {
        progress.push_back(0);
      }
      this->pImpl->output.progressUpdated = true;
    }

    const auto &scenario = this->pImpl->scenario;
    const auto lineDistance = this->pImpl->input.lineDistance;
    const auto minTransectLength = this->pImpl->input.minTransectLength;
    inputLock.unlock();

    // Create transects.
    std::string errorString;
    snake::Angle alpha(-scenario.mAreaBoundingBox().angle * degree::degree);
    std::cout << "Transects angle: " << alpha << std::endl;
    snake::flight_plan::Transects transects;
    transects.push_back(
        bg::model::linestring<snake::BoostPoint>{scenario.homePositon()});

    bool value = snake::flight_plan::transectsFromScenario(
        lineDistance, minTransectLength, alpha, scenario, progress, transects,
        errorString);
    if (!value) {
      UniqueLock lk(this->pImpl->output.mutex);
      this->pImpl->output.errorMessage = errorString.c_str();
    } else if (transects.size() > 1) {
      value = snake::flight_plan::route(scenario.joinedArea(), transects,
                                        transectsRouted, route, errorString);
      if (!value) {
        UniqueLock lk(this->pImpl->output.mutex);
        this->pImpl->output.errorMessage = errorString.c_str();
      } else {
        this->pImpl->output.waypointsUpdated = true;
      }
    }
  }

  UniqueLock lk(this->pImpl->output.mutex);
  // Store tiles.
  if (this->pImpl->output.tilesUpdated) {
    // Clear some output data.
    this->pImpl->output.tiles.polygons().clear();
    this->pImpl->output.tileCenterPoints.clear();
    this->pImpl->output.tilesENU.polygons().clear();
    this->pImpl->output.tileCenterPointsENU.clear();
    // Convert and store scenario data.
    const auto &origin = this->pImpl->output.ENUOrigin;
    const auto &tiles = this->pImpl->scenario.tiles();
    const auto &centerPoints = this->pImpl->scenario.tileCenterPoints();
    for (unsigned int i = 0; i < tiles.size(); ++i) {
      const auto &tile = tiles[i];
      SnakeTile geoTile;
      SnakeTileLocal enuTile;
      for (size_t i = 0; i < tile.outer().size() - 1; ++i) {
        auto &p = tile.outer()[i];
        QPointF enuVertex(p.get<0>(), p.get<1>());
        QGeoCoordinate geoVertex;
        snake::fromENU(origin, p, geoVertex);
        enuTile.polygon().points().push_back(enuVertex);
        geoTile.push_back(geoVertex);
      }
      const auto &boostPoint = centerPoints[i];
      QPointF enuVertex(boostPoint.get<0>(), boostPoint.get<1>());
      QGeoCoordinate geoVertex;
      snake::fromENU(origin, boostPoint, geoVertex);
      geoTile.setCenter(geoVertex);
      this->pImpl->output.tiles.polygons().push_back(geoTile);
      this->pImpl->output.tileCenterPoints.push_back(
          QVariant::fromValue(geoVertex));
      this->pImpl->output.tilesENU.polygons().push_back(enuTile);
      this->pImpl->output.tileCenterPointsENU.push_back(enuVertex);
    }
  }
  // Store progress.
  if (this->pImpl->output.progressUpdated) {
    size_t nTiles = progress.size();
    this->pImpl->output.progress.progress().clear();
    this->pImpl->output.progress.progress().reserve(nTiles);
    for (const auto &p : progress) {
      this->pImpl->output.progress.progress().push_back(p);
    }
  }
  // Store waypoints.
  if (this->pImpl->output.waypointsUpdated) {
    // Store arrival path.
    const auto &origin = this->pImpl->output.ENUOrigin;
    const auto &firstWaypoint = transectsRouted.front().front();
    long startIdx = 0;
    for (long i = 0; i < long(route.size()); ++i) {
      const auto &boostVertex = route[i];
      if (boostVertex == firstWaypoint) {
        startIdx = i;
        break;
      }
      QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()};
      QGeoCoordinate geoVertex;
      snake::fromENU(origin, boostVertex, geoVertex);
      this->pImpl->output.arrivalPathENU.push_back(enuVertex);
      this->pImpl->output.arrivalPath.push_back(geoVertex);
    }
    // Store return path.
    long endIdx = 0;
    const auto &lastWaypoint = transectsRouted.back().back();
    for (long i = route.size() - 1; i >= 0; --i) {
      const auto &boostVertex = route[i];
      if (boostVertex == lastWaypoint) {
        endIdx = i;
        break;
      }
      QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()};
      QGeoCoordinate geoVertex;
      snake::fromENU(origin, boostVertex, geoVertex);
      this->pImpl->output.returnPathENU.push_back(enuVertex);
      this->pImpl->output.returnPath.push_back(geoVertex);
    }
    // Store waypoints.
    for (long i = startIdx; i <= endIdx; ++i) {
      const auto &boostVertex = route[i];
      QPointF enuVertex{boostVertex.get<0>(), boostVertex.get<1>()};
      QGeoCoordinate geoVertex;
      snake::fromENU(origin, boostVertex, geoVertex);
      this->pImpl->output.waypointsENU.push_back(enuVertex);
      this->pImpl->output.waypoints.push_back(geoVertex);
    }
  }
}