AreaData.cc

#include "AreaData.h"

#include "geometry/MeasurementArea.h"
#include "geometry/SafeArea.h"
#include "geometry/snake.h"

#include "QGCApplication.h"
#include "QGCLoggingCategory.h"
#include "QGCQGeoCoordinate.h"

QGC_LOGGING_CATEGORY(AreaDataLog, "AreaDataLog")

AreaData::AreaData(QObject *parent) : QObject(parent) {}

AreaData::~AreaData() {}

AreaData::AreaData(const AreaData &other, QObject *parent)
    : QObject(parent), _initialized(false), _showErrorMessages(true) {
  *this = other;
}

AreaData &AreaData::operator=(const AreaData &other) {
  this->clear();

  // Clone elements.
  for (int i = 0; i < other._areaList.count(); ++i) {
    auto obj = other._areaList[i];
    auto area = qobject_cast<const GeoArea *>(obj);
    this->insert(area->clone(this));
  }

  _origin = other._origin;
  _initialized = other._initialized;

  return *this;
}

bool AreaData::insert(GeoArea *areaData) {
  if (areaData != nullptr) {
    if (Q_LIKELY(!this->_areaList.contains(areaData))) {
      _areaList.append(areaData);
      emit areaList();

      auto *measurementArea = qobject_cast<MeasurementArea *>(areaData);
      if (measurementArea != nullptr) {
        connect(measurementArea, &MeasurementArea::centerChanged, this,
                &AreaData::_updateOrigin);
        _setOrigin(measurementArea->center());
      }
      return true;
    }
  }

  return false;
}

void AreaData::remove(GeoArea *areaData) {
  int index = _areaList.indexOf(areaData);
  if (index >= 0) {
    QObject *obj = _areaList.removeAt(index);

    auto *measurementArea = qobject_cast<MeasurementArea *>(areaData);
    if (measurementArea != nullptr) {
      disconnect(measurementArea, &MeasurementArea::centerChanged, this,
                 &AreaData::_updateOrigin);
      _setOrigin(QGeoCoordinate());
    }

    if (obj->parent() == nullptr || obj->parent() == this) {
      obj->deleteLater();
    }

    emit areaListChanged();
  }
}

void AreaData::clear() {
  if (_areaList.count() > 0) {
    while (_areaList.count() > 0) {
      remove(_areaList.value<GeoArea *>(0));
    }
    emit areaListChanged();
  }
  _errorString.clear();
}

QmlObjectListModel *AreaData::areaList() { return &_areaList; }

const QmlObjectListModel *AreaData::areaList() const { return &_areaList; }

const QGeoCoordinate &AreaData::origin() const { return _origin; }

bool AreaData::isCorrect() {
  if (!initialized()) {
    qCWarning(AreaDataLog) << "isCorrect(): not initialized";
    return false;
  }

  // Check if areas are correct
  if (!_areasCorrect()) {
    return false;
  }

  // Check if areas where added.
  MeasurementArea *measurementArea = nullptr;
  SafeArea *safeArea = nullptr;
  if (!_getAreas(&measurementArea, &safeArea)) {
    return false;
  }

  // Check if measurement area is covered by safe area.
  if (!_origin.isValid()) {
    qCWarning(AreaDataLog) << "isCorrect(): origin invalid";
    return false;
  }
  const auto &origin = this->origin();
  snake::FPolygon safeAreaENU;
  snake::areaToEnu(origin, safeArea->pathModel(), safeAreaENU);
  snake::FPolygon measurementAreaENU;
  snake::areaToEnu(origin, measurementArea->pathModel(), measurementAreaENU);
  //  qDebug() << "origin" << origin;
  //  std::stringstream ss;
  //  ss << "measurementAreaENU: " << bg::wkt(measurementAreaENU) << std::endl;
  //  ss << "safeAreaENU: " << bg::wkt(safeAreaENU) << std::endl;
  //  qDebug() << ss.str().c_str();
  if (!bg::covered_by(measurementAreaENU, safeAreaENU)) {
    _processError(tr("Measurement Area not inside Safe Area. Please adjust "
                     "the Measurement Area."));
    return false;
  }
  return true;
}

bool AreaData::initialize(const QGeoCoordinate &bottomLeft,
                          const QGeoCoordinate &topRight) {
  // bottomLeft and topRight define the bounding box.
  if (bottomLeft.isValid() && topRight.isValid() && bottomLeft != topRight) {
    auto *measurementArea = getGeoArea<MeasurementArea *>(_areaList);
    auto *safeArea = getGeoArea<SafeArea *>(_areaList);

    if (safeArea == nullptr) {
      safeArea = new SafeArea(this);
      if (!insert(safeArea)) {
        safeArea->deleteLater();
        qCCritical(AreaDataLog)
            << "initialize(): safeArea == nullptr, but insert() failed.";
        return false;
      }
    }

    if (measurementArea == nullptr) {
      measurementArea = new MeasurementArea(this);
      if (!insert(measurementArea)) {
        measurementArea->deleteLater();
        qCCritical(AreaDataLog) << "initialize(): measurementArea == nullptr, "
                                   "but insert() failed.";
        return false;
      }
    }

    // Fit safe area to bounding box.
    safeArea->clear();
    safeArea->appendVertex(bottomLeft);
    safeArea->appendVertex(
        QGeoCoordinate(topRight.latitude(), bottomLeft.longitude()));
    safeArea->appendVertex(topRight);
    safeArea->appendVertex(
        QGeoCoordinate(bottomLeft.latitude(), topRight.longitude()));

    // Put measurement area inside safeArea;
    measurementArea->clear();
    measurementArea->appendVertex(QGeoCoordinate(
        0.8 * bottomLeft.latitude() + 0.2 * topRight.latitude(),
        0.8 * bottomLeft.longitude() + 0.2 * topRight.longitude()));
    measurementArea->appendVertex(QGeoCoordinate(
        0.2 * bottomLeft.latitude() + 0.8 * topRight.latitude(),
        0.8 * bottomLeft.longitude() + 0.2 * topRight.longitude()));
    measurementArea->appendVertex(QGeoCoordinate(
        0.2 * bottomLeft.latitude() + 0.8 * topRight.latitude(),
        0.2 * bottomLeft.longitude() + 0.8 * topRight.longitude()));
    measurementArea->appendVertex(QGeoCoordinate(
        0.8 * bottomLeft.latitude() + 0.2 * topRight.latitude(),
        0.2 * bottomLeft.longitude() + 0.8 * topRight.longitude()));

    // Set depot
    safeArea->setDepot(QGeoCoordinate(
        safeArea->vertexCoordinate(0).latitude() * 0.5 +
            measurementArea->vertexCoordinate(0).latitude() * 0.5,
        safeArea->vertexCoordinate(0).longitude() * 0.5 +
            measurementArea->vertexCoordinate(0).longitude() * 0.5));

    _initialized = true;
    return true;
  } else {
    qCWarning(AreaDataLog)
        << "initialize(): bounding box invaldid (bottomLeft, topRight) "
        << bottomLeft << "," << topRight;
    return false;
  }
}

bool AreaData::initialized() { return _initialized; }

void AreaData::intersection() {
  if (initialized() && _areasCorrect()) {
    MeasurementArea *measurementArea = nullptr;
    SafeArea *safeArea = nullptr;
    if (_getAreas(&measurementArea, &safeArea)) {

      // convert to ENU
      const auto origin = this->origin();
      snake::FPolygon safeAreaENU;
      snake::areaToEnu(origin, safeArea->pathModel(), safeAreaENU);
      snake::FPolygon measurementAreaENU;
      snake::areaToEnu(origin, measurementArea->pathModel(),
                       measurementAreaENU);

      // do intersection
      std::deque<snake::FPolygon> outputENU;
      boost::geometry::intersection(measurementAreaENU, safeAreaENU, outputENU);

      if (outputENU.size() < 1 || outputENU[0].outer().size() < 4) {
        _processError(
            "Intersection did't deliver any result. Measurement Area and "
            "Safe Area must touch each other.");
        return;
      }

      if (outputENU[0].inners().size() > 0 || outputENU.size() > 1) {
        _processError(
            "Hint: Only simple polygons can be displayed. If Intersection"
            "produces polygons with holes or multi polygons, only "
            "partial information can be displayed.");
      }

      // Shrink the result if safeAreaENU doesn't cover it.
      auto large = std::move(outputENU[0]);
      snake::FPolygon small;
      while (!bg::covered_by(large, safeAreaENU)) {
        snake::offsetPolygon(large, small, -0.1);
        large = std::move(small);
        qDebug() << "intersection(): shrink";
      }

      // Convert.
      measurementArea->clear();
      for (auto it = large.outer().begin(); it != large.outer().end() - 1;
           ++it) {
        QGeoCoordinate c;
        snake::fromENU(origin, *it, c);
        measurementArea->appendVertex(c);
      }
    }
  }
}

bool AreaData::operator==(const AreaData &other) const {
  if (_areaList.count() == other._areaList.count()) {
    for (int i = 0; i < _areaList.count(); ++i) {
      if (_areaList[i] != other._areaList[i]) {
        return false;
      }
    }
    return true;
  } else {
    return false;
  }
}
bool AreaData::operator!=(const AreaData &other) const {
  return !(*this == other);
}

void AreaData::_setOrigin(const QGeoCoordinate &origin) {
  if (this->_origin != origin) {
    this->_origin = origin;
    emit originChanged();
  }
}

void AreaData::_processError(const QString &str) {
  this->_errorString = str;
  emit error();
  if (_showErrorMessages) {
    qgcApp()->informationMessageBoxOnMainThread(tr("Area Editor"),
                                                this->errorString());
  }
}

bool AreaData::_areasCorrect() {
  // Check if areas are correct.
  for (int i = 0; i < _areaList.count(); ++i) {
    auto *area = _areaList.value<GeoArea *>(0);
    if (!area->isCorrect()) {
      _processError(area->errorString());
      return false;
    }
  }

  return true;
}

bool AreaData::_getAreas(MeasurementArea **measurementArea,
                         SafeArea **safeArea) {
  *measurementArea = getGeoArea<MeasurementArea *>(_areaList);
  if (*measurementArea == nullptr) {
    _processError(
        tr("Measurement Area missing. Please define a measurement area."));
    return false;
  }
  *safeArea = getGeoArea<SafeArea *>(_areaList);
  if (*safeArea == nullptr) {
    _processError(tr("Safe Area missing. Please define a safe area."));
    return false;
  }

  return true;
}

void AreaData::setShowErrorMessages(bool showErrorMessages) {
  if (showErrorMessages != _showErrorMessages) {
    _showErrorMessages = showErrorMessages;
    emit showErrorMessagesChanged();
  }
}

void AreaData::_updateOrigin() {
  auto *measurementArea = getGeoArea<MeasurementArea *>(_areaList);
  if (measurementArea != nullptr) {
    _setOrigin(measurementArea->center());
  }
}

bool AreaData::showErrorMessages() const { return _showErrorMessages; }

QString AreaData::errorString() const { return _errorString; }