SnakeThread.cc

#include "SnakeThread.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"

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

  ~CommandRAII() { _fun(); }

private:
  Callable _fun;
};

using QVariantList = QList<QVariant>;

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

class SnakeThread::Impl {
public:
  struct Input {

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

    QList<QGeoCoordinate> mArea;
    QGeoCoordinate ENUOrigin;
    QList<QGeoCoordinate> sArea;
    QList<QGeoCoordinate> corridor;
    Length tileWidth;
    Length tileHeight;
    Area minTileArea;

    Length lineDistance;
    Length minTransectLength;

    QNemoProgress progress;

    std::atomic_bool scenarioChanged;
    std::atomic_bool progressChanged;
    std::atomic_bool routeParametersChanged;

    mutable std::shared_timed_mutex mutex;
  };

  struct Output {
    Output()
        : calcInProgress(false), tilesUpdated(false), waypointsUpdated(false),
          progressUpdated(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;
    bool tilesUpdated;
    bool waypointsUpdated;
    bool progressUpdated;

    mutable std::shared_timed_mutex mutex;
  };

  Impl(SnakeThread *p);

  bool precondition() const;
  bool doTopicServiceSetup();

  // Internal data.
  snake::BoostPolygon mAreaENU;
  snake::BoostPolygon sAreaENU;
  snake::BoostPolygon corridorENU;
  snake::BoostPolygon jAreaENU;
  std::vector<snake::BoostPolygon> tilesENU;
  QGeoCoordinate ENUOrigin;
  SnakeThread *parent;

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

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

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

SnakeThread::~SnakeThread() {}

void SnakeThread::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 SnakeThread::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 SnakeThread::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);
  }
}

void SnakeThread::setProgress(const QVector<int> &progress) {
  this->pImpl->input.progressChanged = true;
  UniqueLock lk(this->pImpl->input.mutex);
  this->pImpl->input.progress.progress() = progress;
}

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

SnakeTilesLocal SnakeThread::tilesENU() const {
  SharedLock lk(this->pImpl->output.mutex);
  return this->pImpl->output.tilesENU;
}

QGeoCoordinate SnakeThread::ENUOrigin() const {
  SharedLock lk(this->pImpl->output.mutex);
  return this->pImpl->output.ENUOrigin;
}

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

QVector<int> SnakeThread::progress() const {
  SharedLock lk(this->pImpl->output.mutex);
  return this->pImpl->input.progress.progress();
}

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

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

bool SnakeThread::tilesUpdated() {
  SharedLock lk(this->pImpl->output.mutex);
  return this->pImpl->output.tilesUpdated;
}

bool SnakeThread::waypointsUpdated() {
  SharedLock lk(this->pImpl->output.mutex);
  return this->pImpl->output.waypointsUpdated;
}

bool SnakeThread::progressUpdated() {
  SharedLock lk(this->pImpl->output.mutex);
  return this->pImpl->output.progressUpdated;
}

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

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

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

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

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

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

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

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

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

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

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

void SnakeThread::run() {
#ifndef NDEBUG
  auto startTime = std::chrono::high_resolution_clock::now();
#endif
  // Signal that calculation is in progress.
  this->pImpl->output.calcInProgress.store(true);
  emit calcInProgressChanged(this->pImpl->output.calcInProgress.load());
#ifndef NDEBUG
  auto onExit = [this, &startTime] {
#else
  auto onExit = [this] {
#endif
#ifndef NDEBUG
    qDebug() << "SnakeThread::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(false);
    emit calcInProgressChanged(this->pImpl->output.calcInProgress.load());
  };
  CommandRAII<decltype(onExit)> commandRAII(onExit);

  bool tilesValid = true;
  bool progressValid = false;
  snake::Length lineDistance;
  snake::Length minTransectLength;
  std::vector<int> progress;
  {
    // Check preconditions.
    SharedLock lk(this->pImpl->input.mutex);
    if (this->pImpl->input.mArea.size() < 3) {
      UniqueLock uLock(this->pImpl->output.mutex);
      this->pImpl->output.errorMessage = "Measurement area invalid: size < 3.";
      tilesValid = false;
    } else if (this->pImpl->input.sArea.size() < 3) {
      UniqueLock uLock(this->pImpl->output.mutex);
      this->pImpl->output.errorMessage = "Service area invalid: size < 3.";
      tilesValid = false;
    } else {
      // Update Scenario if necessary
      if (this->pImpl->input.scenarioChanged) {
        // Set Origin
        this->pImpl->ENUOrigin = this->pImpl->input.mArea.front();
        // Update measurement area.
        this->pImpl->mAreaENU.clear();
        snake::areaToEnu(this->pImpl->ENUOrigin, this->pImpl->input.mArea,
                         this->pImpl->mAreaENU);
        // Update service area.
        this->pImpl->sAreaENU.clear();
        snake::areaToEnu(this->pImpl->ENUOrigin, this->pImpl->input.sArea,
                         this->pImpl->sAreaENU);
        // Update corridor.
        this->pImpl->corridorENU.clear();
        snake::areaToEnu(this->pImpl->ENUOrigin, this->pImpl->input.corridor,
                         this->pImpl->corridorENU);
        // Fetch parametes.
        auto tileHeight = this->pImpl->input.tileHeight;
        auto tileWidth = this->pImpl->input.tileWidth;
        auto minTileArea = this->pImpl->input.minTileArea;
        // Update tiles.
        this->pImpl->tilesENU.clear();
        this->pImpl->jAreaENU.clear();
        std::string errorString;
        snake::BoundingBox bbox;
        if (!snake::joinedArea(this->pImpl->mAreaENU, this->pImpl->sAreaENU,
                               this->pImpl->corridorENU, this->pImpl->jAreaENU,
                               errorString) ||
            !snake::tiles(this->pImpl->mAreaENU, tileHeight, tileWidth,
                          minTileArea, this->pImpl->tilesENU, bbox,
                          errorString)) {
          UniqueLock uLock(this->pImpl->output.mutex);
          this->pImpl->output.errorMessage = errorString.c_str();
          tilesValid = false;
        }
      }
      if (tilesValid) {
        // Sample data
        lineDistance = this->pImpl->input.lineDistance;
        minTransectLength = this->pImpl->input.minTransectLength;
        // Verify progress.
        size_t nTiles = this->pImpl->tilesENU.size();
        if (size_t(this->pImpl->input.progress.progress().size()) != nTiles) {
          for (size_t i = 0; i < nTiles; ++i) {
            progress.push_back(0);
          }
        } else {
          for (size_t i = 0; i < nTiles; ++i) {
            progress.push_back(this->pImpl->input.progress.progress()[i]);
          }
        }
        progressValid = tilesValid;
      }
    }
  }

  bool waypointsValid = true;
  snake::Transects transects;
  snake::Transects transectsRouted;
  snake::Route route;
  bool needWaypointUpdate = this->pImpl->input.scenarioChanged ||
                            this->pImpl->input.routeParametersChanged ||
                            this->pImpl->input.progressChanged;
  if (tilesValid) {
    if (needWaypointUpdate) {
      // Data needed for transects.
      std::string errorString;
      snake::BoundingBox bbox;
      snake::minimalBoundingBox(this->pImpl->mAreaENU, bbox);
      snake::Angle alpha(bbox.angle * si::radian);
      snake::BoostPoint home;
      snake::polygonCenter(this->pImpl->sAreaENU, home);
      transects.push_back(bg::model::linestring<snake::BoostPoint>{home});
      // Create transects.
      bool success = snake::transectsFromScenario(
          lineDistance, minTransectLength, alpha, this->pImpl->mAreaENU,
          this->pImpl->tilesENU, progress, transects, errorString);
      if (!success) {
        UniqueLock lk(this->pImpl->output.mutex);
        this->pImpl->output.errorMessage = errorString.c_str();
        waypointsValid = false;
      } else if (transects.size() > 1) {
        // Route transects.
        success = snake::route(this->pImpl->jAreaENU, transects,
                               transectsRouted, route, errorString);
        if (!success) {
          UniqueLock lk(this->pImpl->output.mutex);
          this->pImpl->output.errorMessage = errorString.c_str();
          waypointsValid = false;
        }
      } else {
        // No transects
        waypointsValid = false;
      }
    }
  } else {
    waypointsValid = false;
  }

  UniqueLock lk(this->pImpl->output.mutex);
  // Store tiles.
  this->pImpl->output.tilesUpdated = false;
  if (!tilesValid) {
    // 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();
    this->pImpl->output.ENUOrigin = QGeoCoordinate(0.0, 0.0, 0.0);
    this->pImpl->output.tilesUpdated = true;
  } else if (this->pImpl->input.scenarioChanged) {
    this->pImpl->input.scenarioChanged = false;
    // 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 &tiles = this->pImpl->tilesENU;
    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) {
        const auto &p = tile.outer()[i];
        QPointF enuVertex{p.get<0>(), p.get<1>()};
        QGeoCoordinate geoVertex;
        snake::fromENU(this->pImpl->ENUOrigin, p, geoVertex);
        enuTile.polygon().points().push_back(enuVertex);
        geoTile.push_back(geoVertex);
      }
      snake::BoostPoint centerPoint;
      snake::polygonCenter(tile, centerPoint);
      QPointF enuVertex(centerPoint.get<0>(), centerPoint.get<1>());
      QGeoCoordinate geoVertex;
      snake::fromENU(this->pImpl->ENUOrigin, centerPoint, 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);
    }
    this->pImpl->output.tilesUpdated = true;
  }
  // Store progress.
  this->pImpl->output.progressUpdated = false;
  if (!progressValid) {
    this->pImpl->input.progress.progress().clear();
    this->pImpl->output.progressUpdated = true;
  } else if (this->pImpl->input.progressChanged) {
    if (progress.size() == this->pImpl->tilesENU.size()) {
      auto &qProgress = this->pImpl->input.progress;
      qProgress.progress().clear();
      for (const auto &p : progress) {
        qProgress.progress().push_back(p);
      }
    }
    this->pImpl->output.progressUpdated = true;
  }
  // Store waypoints.
  if (!waypointsValid) {
    // Clear waypoints.
    this->pImpl->output.arrivalPath.clear();
    this->pImpl->output.returnPath.clear();
    this->pImpl->output.arrivalPathENU.clear();
    this->pImpl->output.returnPathENU.clear();
    this->pImpl->output.waypoints.clear();
    this->pImpl->output.waypointsENU.clear();
    this->pImpl->output.waypointsUpdated = true;
  } else if (needWaypointUpdate) {
    // Clear waypoints.
    this->pImpl->output.arrivalPath.clear();
    this->pImpl->output.returnPath.clear();
    this->pImpl->output.arrivalPathENU.clear();
    this->pImpl->output.returnPathENU.clear();
    this->pImpl->output.waypoints.clear();
    this->pImpl->output.waypointsENU.clear();
    // Store arrival path.
    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(this->pImpl->ENUOrigin, 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(this->pImpl->ENUOrigin, 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(this->pImpl->ENUOrigin, boostVertex, geoVertex);
      this->pImpl->output.waypointsENU.push_back(enuVertex);
      this->pImpl->output.waypoints.push_back(geoVertex);
    }
    // Store waypoints.
    //    for (const auto &t : transects) {
    //      for (const auto &v : t) {
    //        QPointF enuVertex{v.get<0>(), v.get<1>()};
    //        QGeoCoordinate geoVertex;
    //        snake::fromENU(origin, v, geoVertex);
    //        this->pImpl->output.waypointsENU.push_back(enuVertex);
    //        this->pImpl->output.waypoints.push_back(geoVertex);
    //      }
    //    }
    this->pImpl->output.waypointsUpdated = true;
  }
}