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 <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::SnakeImpl {
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),
          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 {
    SnakeTiles tiles;
    QmlObjectListModel tilesQml;
    QVariantList tileCenterPoints;
    SnakeTilesLocal tilesENU;
    QVector<QPointF> tileCenterPointsENU;

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

    QString errorMessage;

    std::atomic_bool calcInProgress;

    mutable std::shared_timed_mutex mutex;
  };

  SnakeImpl(SnakeDataManager *p);

  bool precondition() const;
  bool doTopicServiceSetup();

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

  Input input;
  // Output
  Output output;
};

SnakeDataManager::SnakeImpl::SnakeImpl(SnakeDataManager *p)
    : rosBridgeEnabeled(false), topicServiceSetupDone(false), parent(p) {}

bool SnakeDataManager::SnakeImpl::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<SnakeImpl>(this))

{}

// SnakeDataManager::~SnakeDataManager() {}

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

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

const QmlObjectListModel *SnakeDataManager::tiles() const {
  SharedLock lk(this->pImpl->output.mutex);
  return &this->pImpl->output.tilesQml;
}

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

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

int SnakeDataManager::nemoStatus() const {
  SharedLock lk(this->pImpl->output.mutex);
  return this->pImpl->output.heartbeat.status();
}

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->mutex);
  return this->pImpl->lineDistance;
}

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

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

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

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

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

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

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

void SnakeDataManager::enableRosBridge() {
  this->pImpl->rosBridgeEnabeled = true;
}

void SnakeDataManager::disableRosBride() {
  this->pImpl->rosBridgeEnabeled = false;
}

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

  this->pImpl->calcInProgress.store(true);
  emit calcInProgressChanged(this->pImpl->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->calcInProgress.store(false);
    emit calcInProgressChanged(this->pImpl->calcInProgress.load());
  };
  CommandRAII<decltype(onExit)> onExitRAII(onExit);

  // Check preconditions.
  SharedLock sLock(this->pImpl->mutex);

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

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

  // Update Scenario if necessary
  if (this->pImpl->scenarioChanged) {
    // Set Origin
    this->pImpl->ENUOrigin = this->pImpl->mArea.front();
    auto &origin = this->pImpl->ENUOrigin;

    // Update measurement area.
    auto &mAreaEnu = this->pImpl->scenario.measurementArea();
    auto &mArea = this->pImpl->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->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->corridor;
    corridor.clear();
    for (auto geoVertex : corridor) {
      snake::BoostPoint p;
      snake::toENU(origin, geoVertex, p);
      corridorEnu.outer().push_back(p);
    }

    if (!this->pImpl->scenario.update()) {
      this->pImpl->errorMessage = this->pImpl->scenario.errorString.c_str();
      return;
    }
  }
  sLock.unlock;

  bool storeProgress = false;
  bool storeRoute = false;
  if (this->pImpl->scenarioChanged || this->pImpl->routeParametersChanged) {
    storeRoute = true;
    // Sample data
    SharedLock lk(this->pImpl->mutex);
    size_t nTiles = this->pImpl->tiles.polygons().size();
    if (this->pImpl->progress.size() != nTiles) {
    }
    const auto scenario = this->pImpl->scenario;
    std::vector<int> progress;
    size_t nTiles = this->pImpl->tiles.polygons().size();
    if (this->pImpl->progress.size() != nTiles) {
      progress.reserve(nTiles);
      for (size_t i = 0; i < nTiles; ++i) {
        progress.push_back(0);
      }
      storeProgress = true;
    } else {
      progress = this->pImpl->progress;
    }
    const auto lineDistance = this->pImpl->lineDistance;
    const auto minTransectLength = this->pImpl->minTransectLength;
    lk.unlock();

    // Create transects.
    std::string errorString;
    snake::Angle alpha(-scenario.mAreaBoundingBox().angle * degree::degree);
    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->mutex);
      this->pImpl->errorMessage = errorString.c_str();
      storeRoute = false;
    }

    // Route transects
    if (transects.size() > 1) {
      snake::flight_plan::Transects transectsRouted;
      snake::flight_plan::Route route;
      value = snake::flight_plan::route(scenario.joinedArea(), transects,
                                        transectsRouted, route, errorString);
      if (!value) {
        this->pImpl->errorMessage = errorString.c_str();
        return;
      }

      // 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->arrivalPathENU.push_back(enuVertex);
        this->pImpl->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->returnPathENU.push_back(enuVertex);
        this->pImpl->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->waypointsENU.push_back(enuVertex);
        this->pImpl->waypoints.push_back(geoVertex);
      }
    }
  }

  UniqueLock uLock(this->pImpl->mutex);
  // Store scenario.
  if (this->pImpl->scenarioChanged) {
    // Clear some output data.
    this->pImpl->tiles.polygons().clear();
    this->pImpl->tilesQml.clearAndDeleteContents();
    this->pImpl->tileCenterPoints.clear();
    this->pImpl->tilesENU.polygons().clear();
    this->pImpl->tileCenterPointsENU.clear();
    // 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->tiles.polygons().push_back(geoTile);
      auto *geoTileCopy = new SnakeTile(geoTile);
      geoTileCopy->moveToThread(this->pImpl->tilesQml.thread());
      this->pImpl->tilesQml.append(geoTileCopy);
      this->pImpl->tileCenterPoints.push_back(QVariant::fromValue(geoVertex));
      this->pImpl->tilesENU.polygons().push_back(enuTile);
      this->pImpl->tileCenterPointsENU.push_back(enuVertex);
    }
  }
  // Store route etc.
  if (this->pImpl->scenarioChanged || this->pImpl->routeParametersChanged) {
  }
  uLock.unlock;
  this->pImpl->scenarioChanged = false;
  this->pImpl->routeParametersChanged = false;
}