MeasurementArea.cc

#include "MeasurementArea.h"
#include "QtConcurrentRun"
#include "geometry.h"
#include "nemo_interface/SnakeTile.h"
#include <ctime>

#include <boost/units/systems/si.hpp>

#include "JsonHelper.h"
#include "QGCLoggingCategory.h"

#include <QJsonArray>

#ifndef SNAKE_MAX_TILES
#define SNAKE_MAX_TILES 1000
#endif

using namespace geometry;
namespace trans = bg::strategy::transform;

// Aux function
bool getTiles(const FPolygon &area, Length tileHeight, Length tileWidth,
              Area minTileArea, std::vector<FPolygon> &tiles,
              BoundingBox &bbox);

QGC_LOGGING_CATEGORY(MeasurementAreaLog, "MeasurementAreaLog")

namespace {
const char *tileCenterPointsKey = "TileCenterPoints";
const char *tileArrayKey = "TileArray";
} // namespace

TileData::TileData() : tiles(this) {}

TileData::~TileData() { tiles.clearAndDeleteContents(); }

TileData &TileData::operator=(const TileData &other) {
  this->tiles.clearAndDeleteContents();
  for (std::size_t i = 0; i < std::size_t(other.tiles.count()); ++i) {
    const auto *obj = other.tiles[i];
    const auto *tile = qobject_cast<const SnakeTile *>(obj);
    if (tile != nullptr) {
      this->tiles.append(tile->clone(this));
    } else {
      qCWarning(MeasurementAreaLog) << "TileData::operator=: nullptr";
    }
  }
  this->tileCenterPoints = other.tileCenterPoints;
  return *this;
}

bool TileData::operator==(const TileData &other) const {
  if (this->tileCenterPoints == other.tileCenterPoints &&
      this->tiles.count() == other.tiles.count()) {
    for (int i = 0; i < other.tiles.count(); ++i) {
      if (this->tiles[i] != other.tiles[i]) {
        return false;
      }
    }
    return true;
  } else {
    return false;
  }
}

bool TileData::operator!=(const TileData &other) const {
  return !this->operator==(other);
}

void TileData::saveToJson(QJsonObject &json) {
  // save center points
  QJsonValue jsonCenterPoints;
  JsonHelper::saveGeoCoordinateArray(tileCenterPoints, false, jsonCenterPoints);
  json[tileCenterPointsKey] = std::move(jsonCenterPoints);

  // save tiles
  QJsonArray jsonTiles;
  for (int i = 0; i < tiles.count(); ++i) {
    auto tile = tiles.value<SnakeTile *>(i);
    if (tile != nullptr) {
      QJsonObject jsonTile;
      tile->saveToJson(jsonTile);
      jsonTiles.append(jsonTile);
    } else {
      qCritical() << "TileData::saveToJson(): Object other than SnakeTile "
                     "inside tiles (QmlObjectListModel))";
      Q_ASSERT(tile != nullptr);
    }
  }
  json[tileArrayKey] = std::move(jsonTiles);
}

bool TileData::loadFromJson(const QJsonObject &json, QString &errorString) {
  clear();

  // load tiles
  if (json.contains(tileArrayKey) && json[tileArrayKey].isArray()) {
    QString e;
    for (const auto &jsonTile : json[tileArrayKey].toArray()) {
      auto tile = new SnakeTile(this);
      if (tile->loadFromJson(jsonTile.toObject(), e)) {
        tiles.append(tile);
      } else {
        tile->deleteLater();
        errorString.append(e);
        return false;
      }
    }
  } else {
    errorString.append(tr("Not able to load tiles.\n"));
    qCWarning(MeasurementAreaLog)
        << "Not able to load tiles. tileArrayKey missing or wrong type.";
    if (json.contains(tileArrayKey)) {
      qCWarning(MeasurementAreaLog)
          << "tile array type: " << json[tileArrayKey].type();
    }
    return false;
  }

  // load center points
  if (json.contains(tileCenterPointsKey) &&
      json[tileCenterPointsKey].isArray()) {
    QString e;
    if (!JsonHelper::loadGeoCoordinateArray(json[tileCenterPointsKey], false,
                                            tileCenterPoints, e)) {
      errorString.append(e);
      errorString.append("\n");
      return false;
    }

  } else {
    errorString.append(tr("Not able to load center points.\n"));
    qCWarning(MeasurementAreaLog)
        << "Not able to load center points. tileCenterPointsKey missing or "
           "wrong type.";
    if (json.contains(tileCenterPointsKey)) {
      qCWarning(MeasurementAreaLog)
          << "center points type: " << json[tileCenterPointsKey].type();
    }
    return false;
  }

  return true;
}

void TileData::clear() {
  this->tiles.clearAndDeleteContents();
  this->tileCenterPoints.clear();
}

size_t TileData::size() const {
  if (tiles.count() == tileCenterPoints.size()) {
    return tiles.count();
  } else {
    return 0;
  }
}

const char *MeasurementArea::settingsGroup = "MeasurementArea";
const char *tileHeightKey = "TileHeight";
const char *tileWidthName = "TileWidth";
const char *minTileAreaKey = "MinTileAreaPercent";
const char *showTilesKey = "ShowTiles";
const char *progressKey = "Progress";
const char *tileKey = "Tiles";
const char *MeasurementArea::nameString = "Measurement Area";

MeasurementArea::MeasurementArea(QObject *parent)
    : GeoArea(parent),
      _metaDataMap(FactMetaData::createMapFromJsonFile(
          QStringLiteral(":/json/MeasurementArea.SettingsGroup.json"),
          this /* QObject parent */)),
      _tileHeight(SettingsFact(settingsGroup, _metaDataMap[tileHeightKey],
                               this /* QObject parent */)),
      _tileWidth(SettingsFact(settingsGroup, _metaDataMap[tileWidthName],
                              this /* QObject parent */)),
      _minTileAreaPercent(SettingsFact(settingsGroup,
                                       _metaDataMap[minTileAreaKey],
                                       this /* QObject parent */)),
      _showTiles(SettingsFact(settingsGroup, _metaDataMap[showTilesKey],
                              this /* QObject parent */)),
      _holdProgress(false), _state(STATE::IDLE) {
  init();
}

MeasurementArea::MeasurementArea(const MeasurementArea &other, QObject *parent)
    : GeoArea(other, parent),
      _metaDataMap(FactMetaData::createMapFromJsonFile(
          QStringLiteral(":/json/MeasurementArea.SettingsGroup.json"),
          this /* QObject parent */)),
      _tileHeight(SettingsFact(settingsGroup, _metaDataMap[tileHeightKey],
                               this /* QObject parent */)),
      _tileWidth(SettingsFact(settingsGroup, _metaDataMap[tileWidthName],
                              this /* QObject parent */)),
      _minTileAreaPercent(SettingsFact(settingsGroup,
                                       _metaDataMap[minTileAreaKey],
                                       this /* QObject parent */)),
      _showTiles(SettingsFact(settingsGroup, _metaDataMap[showTilesKey],
                              this /* QObject parent */)),
      _holdProgress(false), _state(STATE::IDLE) {
  init();
  disableUpdate();

  _tileHeight = other._tileHeight;
  _tileWidth = other._tileWidth;
  _minTileAreaPercent = other._minTileAreaPercent;
  _showTiles = other._showTiles;

  if (other.ready()) {
    _progress = other._progress;
    _tileData = other._tileData;
    enableUpdate();
  } else {
    enableUpdate();
    doUpdate();
  }
}

MeasurementArea &MeasurementArea::operator=(const MeasurementArea &other) {
  GeoArea::operator=(other);

  disableUpdate();
  _tileHeight = other._tileHeight;
  _tileWidth = other._tileWidth;
  _minTileAreaPercent = other._minTileAreaPercent;
  _showTiles = other._showTiles;

  if (other.ready()) {
    _progress = other._progress;
    _tileData = other._tileData;
    enableUpdate();
  } else {
    enableUpdate();
    doUpdate();
  }
  return *this;
}

MeasurementArea::~MeasurementArea() {}

QString MeasurementArea::mapVisualQML() const {
  return QStringLiteral("MeasurementAreaMapVisual.qml");
  // return QStringLiteral("");
}

QString MeasurementArea::editorQML() const {
  return QStringLiteral("MeasurementAreaEditor.qml");
}

MeasurementArea *MeasurementArea::clone(QObject *parent) const {
  return new MeasurementArea(*this, parent);
}

Fact *MeasurementArea::tileHeight() { return &_tileHeight; }

Fact *MeasurementArea::tileWidth() { return &_tileWidth; }

Fact *MeasurementArea::minTileArea() { return &_minTileAreaPercent; }

Fact *MeasurementArea::showTiles() { return &_showTiles; }

QmlObjectListModel *MeasurementArea::tiles() { return &this->_tileData.tiles; }

const QVector<int> &MeasurementArea::progress() const {
  return this->_progress;
}

QVector<int> MeasurementArea::progressQml() const { return this->_progress; }

const QmlObjectListModel *MeasurementArea::tiles() const {
  return &this->_tileData.tiles;
}

const QVariantList &MeasurementArea::tileCenterPoints() const {
  return this->_tileData.tileCenterPoints;
}

const TileData &MeasurementArea::tileData() const { return this->_tileData; }

int MeasurementArea::maxTiles() const { return SNAKE_MAX_TILES; }

bool MeasurementArea::ready() const { return this->_state == STATE::IDLE; }

bool MeasurementArea::measurementCompleted() const {
  if (ready()) {
    for (const auto &p : _progress) {
      if (p != 100) {
        return false;
      }
    }
    return true;
  } else {
    return false;
  }
}

bool MeasurementArea::saveToJson(QJsonObject &json) {
  if (ready()) {
    if (this->GeoArea::saveToJson(json)) {
      json[tileHeightKey] = _tileHeight.rawValue().toDouble();
      json[tileWidthName] = _tileWidth.rawValue().toDouble();
      json[minTileAreaKey] = _minTileAreaPercent.rawValue().toDouble();
      json[showTilesKey] = _showTiles.rawValue().toBool();
      json[areaTypeKey] = nameString;

      // save progess
      QJsonArray jsonProgess;
      for (const auto &p : _progress) {
        jsonProgess.append(p);
      }
      json[progressKey] = std::move(jsonProgess);

      // save tiles
      QJsonObject jsonTiles;
      _tileData.saveToJson(jsonTiles);
      json[tileKey] = std::move(jsonTiles);

      return true;
    } else {
      qCDebug(MeasurementAreaLog)
          << "saveToJson(): error inside GeoArea::saveToJson().";
    }
  } else {
    qCDebug(MeasurementAreaLog) << "saveToJson(): not ready().";
  }
  return false;
}

bool MeasurementArea::loadFromJson(const QJsonObject &json,
                                   QString &errorString) {
  if (this->GeoArea::loadFromJson(json, errorString)) {
    disableUpdate();
    bool retVal = true;

    // load parameters necessary for tile calculation.
    if (!json.contains(tileHeightKey) || !json[tileHeightKey].isDouble()) {
      errorString.append(tr("Could not load tile height!\n"));
      retVal = false;
    } else {
      _tileHeight.setRawValue(json[tileHeightKey].toDouble());
    }

    if (!json.contains(tileWidthName) || !json[tileWidthName].isDouble()) {
      errorString.append(tr("Could not load tile width!\n"));
      retVal = false;
    } else {
      _tileWidth.setRawValue(json[tileWidthName].toDouble());
    }

    if (!json.contains(minTileAreaKey) || !json[minTileAreaKey].isDouble()) {
      errorString.append(tr("Could not load minimal tile area!\n"));
      retVal = false;
    } else {
      _minTileAreaPercent.setRawValue(json[minTileAreaKey].toDouble());
    }

    // load less important parameters
    if (json.contains(showTilesKey) || !json[showTilesKey].isBool()) {
      _showTiles.setRawValue(json[showTilesKey].toBool());
    }

    // load tiles and progress
    bool tileError = false;
    if (json.contains(tileKey) && json[tileKey].isObject()) {
      QString e;
      if (!_tileData.loadFromJson(json[tileKey].toObject(), e)) {
        qCWarning(MeasurementAreaLog) << "TileData::loadFromJson(): " << e;
        tileError = true;
      } else {
        progressChanged();
      }
    } else {
      tileError = true;
    }

    bool progressError = false;
    QVector<int> prog;
    if (json.contains(progressKey) && json[progressKey].isArray()) {
      for (const auto &p : json[progressKey].toArray()) {
        if (p.isDouble()) {
          prog.append(p.toDouble());
        } else {
          progressError = true;
          break;
        }
      }

      // check if entries are in range
      if (!progressError) {
        for (const auto &p : prog) {
          if (p < 0 || p > 100) {
            progressError = true;
            break;
          }
        }
      }
    } else {
      progressError = true;
    }
    if (!progressError) {
      _progress.swap(prog);
      emit progressChanged();
    }

    // do update if error occurred.
    enableUpdate();
    if (progressError || tileError) {
      doUpdate();
    }

    return retVal;
  } else {
    return false;
  }
}

bool MeasurementArea::isCorrect() {
  if (GeoArea::isCorrect()) {
    if (ready()) {
      return true;
    } else {
      setErrorString(
          tr("Measurement Area tile calculation in progess. Please wait."));
    }
  }
  return false;
}

bool MeasurementArea::setProgress(const QVector<int> &p) {
  if (ready()) {
    if (!_holdProgress && p.size() == this->tiles()->count() &&
        this->_progress != p) {
      this->_progress = p;
      emit progressChanged();
      emit progressAccepted();
      return true;
    }
  }
  emit progressNotAccepted();
  return false;
}

void MeasurementArea::randomProgress() {
  if (ready()) {
    std::srand(std::time(nullptr));
    for (auto &p : _progress) {
      p += std::rand() % 125;
      if (p > 100) {
        p = 100;
      }
    }

    emit progressChanged();
  }
}

void MeasurementArea::resetProgress() {
  if (ready()) {
    for (auto &p : _progress) {
      p = 0;
    }

    emit progressChanged();
  }
}
//!
//! \brief MeasurementArea::doUpdate
//! \pre MeasurementArea::deferUpdate must be called first, don't call
//! this function directly!
void MeasurementArea::doUpdate() {
  using namespace geometry;
  using namespace boost::units;

  auto start = std::chrono::high_resolution_clock::now();

  if (this->_state != STATE::UPDATEING && this->_state != STATE::STOP) {
    const auto height = this->_tileHeight.rawValue().toDouble() * si::meter;
    const auto width = this->_tileWidth.rawValue().toDouble() * si::meter;
    const auto tileArea = width * height;
    const auto totalArea = this->area() * si::meter * si::meter;
    const auto estNumTiles = totalArea / tileArea;
    // Check some conditions.
    if (long(std::ceil(estNumTiles.value())) <= SNAKE_MAX_TILES &&
        this->GeoArea::isCorrect()) {
      setState(STATE::UPDATEING);

      auto polygon = this->coordinateList();
      for (auto &v : polygon) {
        v.setAltitude(0);
      }
      const auto minArea =
          this->_minTileAreaPercent.rawValue().toDouble() / 100 * tileArea;
      auto *th = this->thread();
      auto future = QtConcurrent::run([polygon, th, height, width, minArea] {
        auto start = std::chrono::high_resolution_clock::now();

        DataPtr pData(new TileData());
        // Convert to ENU system.
        QGeoCoordinate origin = polygon.first();
        FPolygon polygonENU;
        areaToEnu(origin, polygon, polygonENU);
        std::vector<FPolygon> tilesENU;
        BoundingBox bbox;
        // Generate tiles.
        if (getTiles(polygonENU, height, width, minArea, tilesENU, bbox)) {
          // Convert to geo system.
          for (const auto &t : tilesENU) {
            auto geoTile = new SnakeTile(pData.get());
            for (const auto &v : t.outer()) {
              QGeoCoordinate geoVertex;
              fromENU(origin, v, geoVertex);
              geoTile->push_back(geoVertex);
            }
            pData->tiles.append(geoTile);
            // Calculate center.
            geometry::FPoint center;
            geometry::polygonCenter(t, center);
            QGeoCoordinate geoCenter;
            fromENU(origin, center, geoCenter);
            pData->tileCenterPoints.append(QVariant::fromValue(geoCenter));
          }
        }
        pData->moveToThread(th);

        qCDebug(MeasurementAreaLog)
            << "doUpdate(): update time: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(
                   std::chrono::high_resolution_clock::now() - start)
                   .count()
            << " ms";

        return pData;
      }); // QtConcurrent::run()

      this->_watcher.setFuture(future);
    }
  }
  qCDebug(MeasurementAreaLog)
      << "doUpdate(): execution time: "
      << std::chrono::duration_cast<std::chrono::milliseconds>(
             std::chrono::high_resolution_clock::now() - start)
             .count()
      << " ms";
}

void MeasurementArea::deferUpdate() {
  if (this->_state == STATE::IDLE || this->_state == STATE::DEFERED) {
    qCDebug(MeasurementAreaLog) << "defereUpdate(): defer update.";
    if (this->_state == STATE::IDLE) {
      this->_progress.clear();
      this->_tileData.clear();
      emit this->progressChanged();
      emit this->tilesChanged();
    }
    this->setState(STATE::DEFERED);
    this->_timer.start(100);
  } else if (this->_state == STATE::UPDATEING) {
    qCDebug(MeasurementAreaLog) << "defereUpdate(): restart.";
    setState(STATE::RESTARTING);
  }
}

void MeasurementArea::storeTiles() {
  auto start = std::chrono::high_resolution_clock::now();

  if (this->_state == STATE::UPDATEING) {
    qCDebug(MeasurementAreaLog) << "storeTiles(): update.";

    this->_tileData = *this->_watcher.result();
    // This is expensive. Drawing tiles is expensive too.
    this->_progress = QVector<int>(this->_tileData.tiles.count(), 0);
    this->progressChanged();
    emit this->tilesChanged();
    setState(STATE::IDLE);
  } else if (this->_state == STATE::RESTARTING) {
    qCDebug(MeasurementAreaLog) << "storeTiles(): restart.";
    doUpdate();
  } else if (this->_state == STATE::STOP) {
    qCDebug(MeasurementAreaLog) << "storeTiles(): stop.";
  }
  qCDebug(MeasurementAreaLog)
      << "storeTiles() execution time: "
      << std::chrono::duration_cast<std::chrono::milliseconds>(
             std::chrono::high_resolution_clock::now() - start)
             .count()
      << " ms";
}

void MeasurementArea::disableUpdate() {
  setState(STATE::IDLE);
  this->_timer.stop();
}

void MeasurementArea::enableUpdate() {
  if (this->_state == STATE::STOP) {
    setState(STATE::IDLE);
  }
}

void MeasurementArea::init() {
  this->setObjectName(nameString);
  connect(&this->_tileHeight, &Fact::rawValueChanged, this,
          &MeasurementArea::deferUpdate);
  connect(&this->_tileWidth, &Fact::rawValueChanged, this,
          &MeasurementArea::deferUpdate);
  connect(&this->_minTileAreaPercent, &Fact::rawValueChanged, this,
          &MeasurementArea::deferUpdate);
  connect(this, &GeoArea::pathChanged, this, &MeasurementArea::deferUpdate);
  this->_timer.setSingleShot(true);
  connect(&this->_timer, &QTimer::timeout, this, &MeasurementArea::doUpdate);
  connect(&this->_watcher,
          &QFutureWatcher<std::unique_ptr<QmlObjectListModel>>::finished, this,
          &MeasurementArea::storeTiles);
}

void MeasurementArea::setState(MeasurementArea::STATE s) {
  if (this->_state != s) {
    auto oldState = this->_state;
    this->_state = s;
    if (s == STATE::IDLE || oldState == STATE::IDLE) {
      emit readyChanged();
    }
  }
}

bool MeasurementArea::holdProgress() const { return _holdProgress; }

void MeasurementArea::setHoldProgress(bool holdProgress) {
  if (_holdProgress != holdProgress) {
    _holdProgress = holdProgress;
    emit holdProgressChanged();
  }
}

bool getTiles(const FPolygon &area, Length tileHeight, Length tileWidth,
              Area minTileArea, std::vector<FPolygon> &tiles,
              BoundingBox &bbox) {
  if (area.outer().empty() || area.outer().size() < 4) {
    qCDebug(MeasurementAreaLog) << "Area has to few vertices.";
    return false;
  }

  if (tileWidth <= 0 * bu::si::meter || tileHeight <= 0 * bu::si::meter ||
      minTileArea < 0 * bu::si::meter * bu::si::meter) {
    std::stringstream ss;
    ss << "Parameters tileWidth (" << tileWidth << "), tileHeight ("
       << tileHeight << "), minTileArea (" << minTileArea
       << ") must be positive.";
    qCDebug(MeasurementAreaLog) << ss.str().c_str();
    return false;
  }

  if (bbox.corners.outer().size() != 5) {
    bbox.corners.clear();
    minimalBoundingBox(area, bbox);
  }

  if (bbox.corners.outer().size() < 5)
    return false;
  double bboxWidth = bbox.width;
  double bboxHeight = bbox.height;
  FPoint origin = bbox.corners.outer()[0];

  // cout << "Origin: " << origin[0] << " " << origin[1] << endl;
  // Transform _mArea polygon to bounding box coordinate system.
  trans::rotate_transformer<boost::geometry::degree, double, 2, 2> rotate(
      bbox.angle * 180 / M_PI);
  trans::translate_transformer<double, 2, 2> translate(-origin.get<0>(),
                                                       -origin.get<1>());
  FPolygon translated_polygon;
  FPolygon rotated_polygon;
  boost::geometry::transform(area, translated_polygon, translate);
  boost::geometry::transform(translated_polygon, rotated_polygon, rotate);
  bg::correct(rotated_polygon);
  // cout << bg::wkt<BoostPolygon2D>(rotated_polygon) << endl;

  size_t iMax = ceil(bboxWidth / tileWidth.value());
  size_t jMax = ceil(bboxHeight / tileHeight.value());

  if (iMax < 1 || jMax < 1) {
    std::stringstream ss;
    ss << "Tile width (" << tileWidth << ") or tile height (" << tileHeight
       << ") to large for measurement area.";
    qCDebug(MeasurementAreaLog) << ss.str().c_str();
    return false;
  }

  trans::rotate_transformer<boost::geometry::degree, double, 2, 2> rotate_back(
      -bbox.angle * 180 / M_PI);
  trans::translate_transformer<double, 2, 2> translate_back(origin.get<0>(),
                                                            origin.get<1>());
  for (size_t i = 0; i < iMax; ++i) {
    double x_min = tileWidth.value() * i;
    double x_max = x_min + tileWidth.value();
    for (size_t j = 0; j < jMax; ++j) {
      double y_min = tileHeight.value() * j;
      double y_max = y_min + tileHeight.value();

      FPolygon tile_unclipped;
      tile_unclipped.outer().push_back(FPoint{x_min, y_min});
      tile_unclipped.outer().push_back(FPoint{x_min, y_max});
      tile_unclipped.outer().push_back(FPoint{x_max, y_max});
      tile_unclipped.outer().push_back(FPoint{x_max, y_min});
      tile_unclipped.outer().push_back(FPoint{x_min, y_min});

      std::deque<FPolygon> boost_tiles;
      if (!boost::geometry::intersection(tile_unclipped, rotated_polygon,
                                         boost_tiles))
        continue;

      for (FPolygon t : boost_tiles) {
        if (bg::area(t) > minTileArea.value()) {
          // Transform boost_tile to world coordinate system.
          FPolygon rotated_tile;
          FPolygon translated_tile;
          boost::geometry::transform(t, rotated_tile, rotate_back);
          boost::geometry::transform(rotated_tile, translated_tile,
                                     translate_back);

          // Store tile and calculate center point.
          tiles.push_back(translated_tile);
        }
      }
    }
  }

  if (tiles.size() < 1) {
    std::stringstream ss;
    ss << "No tiles calculated. Is the minTileArea (" << minTileArea
       << ") parameter large enough?";
    qCDebug(MeasurementAreaLog) << ss.str().c_str();
    return false;
  }

  return true;
}