MeasurementArea.cc

#include "MeasurementArea.h"
#include "HashFunctions.h"
#include "geometry.h"
#include "nemo_interface/MeasurementTile.h"

#include <ctime>

#include "QtConcurrentRun"
#include <QJsonArray>
#include <QQmlEngine>

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

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

#ifndef MAX_TILES
#define 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 *tileArrayKey = "TileArray";
} // namespace

const char *MeasurementArea::settingsGroup = "MeasurementArea";
const char *tileHeightKey = "TileHeight";
const char *tileWidthName = "TileWidth";
const char *minTileAreaKey = "MinTileAreaPercent";
const char *showTilesKey = "ShowTiles";
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 */)),
      _tiles(new QmlObjectListModel()), _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 */)),
      _tiles(new QmlObjectListModel()), _state(STATE::IDLE) {
  init();
  disableUpdate();

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

  if (other.ready()) {
    for (int i = 0; i < other._tiles->count(); ++i) {
      _tiles->append(
          qobject_cast<const MeasurementTile *>(other._tiles->operator[](i))
              ->clone(_tiles.get()));
    }
    _indexMap = other._indexMap;
    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()) {
    _tiles->clearAndDeleteContents();
    for (int i = 0; i < other._tiles->count(); ++i) {
      _tiles->append(
          qobject_cast<const MeasurementTile *>(other._tiles->operator[](i))
              ->clone(_tiles.get()));
    }
    _indexMap = other._indexMap;
    enableUpdate();
  } else {
    enableUpdate();
    doUpdate();
  }
  return *this;
}

MeasurementArea::~MeasurementArea() { _tiles->clearAndDeleteContents(); }

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 _tiles.get(); }

const QmlObjectListModel *MeasurementArea::tiles() const {
  return _tiles.get();
}

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

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

bool MeasurementArea::measurementCompleted() const {
  if (ready()) {
    for (int i = 0; i < _tiles->count(); ++i) {
      const auto tile = qobject_cast<const MeasurementTile *>(_tiles->get(i));
      if (!qFuzzyCompare(tile->progress(), 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 tiles
      QJsonArray jsonTileArray;
      for (int i = 0; i < _tiles->count(); ++i) {
        auto tile = qobject_cast<MeasurementTile *>(_tiles->get(i));
        QJsonObject jsonTile;
        tile->saveToJson(jsonTile);
        jsonTileArray.append(jsonTile);
      }
      json[tileArrayKey] = std::move(jsonTileArray);

      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
    bool tileError = false;
    if (json.contains(tileArrayKey) && json[tileArrayKey].isArray()) {

      QString e;
      _tiles->clearAndDeleteContents();

      for (auto &&jsonTile : json[tileArrayKey].toArray()) {

        auto tile = new MeasurementTile(this);

        if (tile->loadFromJson(jsonTile.toObject(), e)) {
          _tiles->append(tile);
        } else {
          tile->deleteLater();
          qCWarning(MeasurementAreaLog) << e;
          tileError = true;
          break;
        }
      }

      if (!tileError) {
        this->_indexMap.clear();
        for (int i = 0; i < _tiles->count(); ++i) {

          auto tile = qobject_cast<MeasurementTile *>(_tiles->get(i));
          auto it = _indexMap.find(tile->id());

          // find unique id
          if (it != _indexMap.end()) {
            auto newId = tile->id() + 1;
            constexpr long counterMax = 1e6;
            unsigned long counter = 0;
            for (; counter <= counterMax; ++counter) {
              it = _indexMap.find(newId);
              if (it == _indexMap.end()) {
                break;
              } else {
                ++newId;
              }
            }

            if (counter != counterMax) {
              tile->setId(newId);
              tile->setProgress(0.0);
            } else {
              qCritical() << "MeasurementArea::storeTiles(): not able to find "
                             "unique id!";
              continue;
            }
          }

          _indexMap.insert(std::make_pair(tile->id(), i));
        }
      }
    } else {
      qCWarning(MeasurementAreaLog)
          << "Not able to load tiles. tileArrayKey missing or wrong type.";
      if (json.contains(tileArrayKey)) {
        qCWarning(MeasurementAreaLog)
            << "tile array type: " << json[tileArrayKey].type();
      }
      tileError = true;
    }

    // do update if error occurred.
    enableUpdate();
    if (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;
}

void MeasurementArea::updateProgress(const ProgressArray &array) {
  if (ready() && array.size() > 0) {
    bool anyChanges = false;
    for (const auto &lp : array) {
      auto it = _indexMap.find(lp.id());
      if (it != _indexMap.end()) {
        int tileIndex = it->second;
        auto *tile = _tiles->value<MeasurementTile *>(tileIndex);
        if (!qFuzzyCompare(lp.progress(), tile->progress())) {
          tile->setProgress(lp.progress());
          anyChanges = true;
        }
      }
    }

    if (anyChanges) {
      emit progressChanged();
    }
  }
}

void MeasurementArea::randomProgress() {
  if (ready()) {

    std::srand(std::time(nullptr));

    ProgressArray progressArray;

    for (int i = 0; i < _tiles->count(); ++i) {

      auto tile = _tiles->value<MeasurementTile *>(i);
      Q_ASSERT(tile != nullptr);

      auto p = tile->progress();
      p += std::rand() % 125;
      if (p > 100) {
        p = 100;
      }

      progressArray.append(LabeledProgress(p, tile->id()));
    }

    updateProgress(progressArray);
  }
}

void MeasurementArea::resetProgress() {
  if (ready()) {
    bool anyChanges = false;

    for (int i = 0; i < _tiles->count(); ++i) {

      auto tile = _tiles->value<MeasurementTile *>(i);
      Q_ASSERT(tile != nullptr);

      if (!qFuzzyCompare(tile->progress(), 0)) {
        tile->setProgress(0);
        anyChanges = true;
      }
    }

    if (anyChanges) {
      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())) <= 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();

        TilePtr pData(new QmlObjectListModel());
        // 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 MeasurementTile(pData.get());
            std::size_t hashValue = 0;
            std::hash<QGeoCoordinate> hashFun;
            for (const auto &v : t.outer()) {
              QGeoCoordinate geoVertex;
              fromENU(origin, v, geoVertex);
              geoTile->push_back(geoVertex);
              hashValue ^= hashFun(geoVertex);
            }
            geoTile->setId(std::int64_t(hashValue));
            pData->append(geoTile);
          }
        }
        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->_indexMap.clear();
      this->_tiles->clearAndDeleteContents();
      emit tilesChanged();
      emit progressChanged();
    }
    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.";

    _tiles->clearAndDeleteContents();
    this->_tiles = this->_watcher.result();
    this->_watcher.result().reset();
    QQmlEngine::setObjectOwnership(this->_tiles.get(),
                                   QQmlEngine::CppOwnership);

    // update tileMap
    this->_indexMap.clear();
    for (int i = 0; i < _tiles->count(); ++i) {

      auto tile = qobject_cast<MeasurementTile *>(_tiles->get(i));
      auto it = _indexMap.find(tile->id());

      // find unique id
      if (it != _indexMap.end()) {
        auto newId = tile->id() + 1;
        constexpr long counterMax = 1e6;
        unsigned long counter = 0;
        for (; counter <= counterMax; ++counter) {
          it = _indexMap.find(newId);
          if (it == _indexMap.end()) {
            break;
          } else {
            ++newId;
          }
        }

        if (counter != counterMax) {
          tile->setId(newId);
          tile->setProgress(0.0);
        } else {
          qCritical()
              << "MeasurementArea::storeTiles(): not able to find unique id!";
          continue;
        }
      }

      _indexMap.insert(std::make_pair(tile->id(), i));
    }

    // This is expensive. Drawing tiles is expensive too.
    emit this->tilesChanged();
    emit progressChanged();
    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::STOP);
  this->_timer.stop();
}

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

void MeasurementArea::init() {
  this->setObjectName(nameString);

  QQmlEngine::setObjectOwnership(this->_tiles.get(), QQmlEngine::CppOwnership);

  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();
    }
  }
}

void MeasurementArea::updateIds(const QList<TileDiff> &array) {
  for (const auto &diff : array) {

    auto it = _indexMap.find(diff.oldTile.id());

    if (it != _indexMap.end()) {
      int tileIndex = it->second;
      auto *tile = _tiles->value<MeasurementTile *>(tileIndex);
      if (diff.oldTile.coordinateList() == tile->coordinateList()) {
        // Change id and update _tileMap.
        const auto newId = diff.newTile.id();
        tile->setId(newId);
        _indexMap.erase(it);
        auto ret = _indexMap.insert(std::make_pair(newId, tileIndex));
        Q_ASSERT(ret.second == true /*insert success?*/);
        Q_UNUSED(ret);
      }
    }
  }
}

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;
}