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(1 * 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::Scenario scenario;
  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->scenario.minTileArea();
}

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

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

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

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

void SnakeThread::setTileWidth(Length tileWidth) {
  this->pImpl->input.scenarioChanged = true;
  UniqueLock lk(this->pImpl->input.mutex);
  this->pImpl->scenario.setTileWidth(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)> onExitRAII(onExit);

  bool tilesValid = true;
  QGeoCoordinate origin;
  {
    // 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 {
      // Set Origin
      origin = this->pImpl->input.mArea.front();
      // Update Scenario if necessary
      if (this->pImpl->input.scenarioChanged) {
        // 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;
        corridorEnu.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);

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

  bool waypointsValid = tilesValid;
  bool progressValid = tilesValid;
  snake::flight_plan::Transects transects;
  snake::flight_plan::Transects transectsRouted;
  snake::flight_plan::Route route;
  std::vector<int> progress;
  bool needWaypointUpdate = this->pImpl->input.scenarioChanged ||
                            this->pImpl->input.routeParametersChanged ||
                            this->pImpl->input.progressChanged;
  if (tilesValid) {
    if (needWaypointUpdate) {
      // Sample data
      SharedLock inputLock(this->pImpl->input.mutex);
      // Verify progress.
      size_t nTiles = this->pImpl->scenario.tiles().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]);
        }
      }
      // Copy remaining parameters and release lock.
      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 * si::radian);
      std::cout << "Transects angle: " << alpha << std::endl;
      transects.push_back(
          bg::model::linestring<snake::BoostPoint>{scenario.homePositon()});

      bool success = snake::flight_plan::transectsFromScenario(
          lineDistance, minTransectLength, alpha, scenario, progress, transects,
          errorString);
      if (!success) {
        UniqueLock lk(this->pImpl->output.mutex);
        this->pImpl->output.errorMessage = errorString.c_str();
        waypointsValid = false;
        progressValid = true;
      } else if (transects.size() > 1) {
        success =
            snake::flight_plan::route(scenario.joinedArea(), transects,
                                      transectsRouted, route, errorString);
        if (!success) {
          UniqueLock lk(this->pImpl->output.mutex);
          this->pImpl->output.errorMessage = errorString.c_str();
          waypointsValid = false;
          progressValid = true;
        } else {
          waypointsValid = true;
          progressValid = true;
        }
      } else {
        // No transects
        waypointsValid = false;
        progressValid = true;
      }
    } else {
      // No update necessary
      waypointsValid = true;
      progressValid = true;
    }
  }

  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();
    this->pImpl->output.ENUOrigin = origin;
    // Convert and store scenario data.
    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);
    }
    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->scenario.tiles().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(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);
    //    }
    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;
  }
}