AreaData.cc

#include "AreaData.h"

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

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

QGC_LOGGING_CATEGORY(AreaDataLog, "AreaDataLog")

const char *originKey = "Origin";
const char *areaListKey = "AreaList";
const char *initializedKey = "Initialized";

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

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

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

  _initialized = true;
  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);
}

bool AreaData::load(const QJsonObject &obj, QString &errorString) {
  bool returnValue = true;

  // load initialized.
  {
    QString e;
    QList<JsonHelper::KeyValidateInfo> keyInfo = {
        {initializedKey, QJsonValue::Bool, true},
    };
    if (JsonHelper::validateKeys(obj, keyInfo, e)) {
      _initialized = obj[initializedKey].toBool();
    } else {
      returnValue = false;
      errorString.append(e);
      errorString.append("\n");
    }
  }

  // load areaList.
  {
    QString e;
    QList<JsonHelper::KeyValidateInfo> keyInfo = {
        {areaListKey, QJsonValue::Array, true},
    };
    if (JsonHelper::validateKeys(obj, keyInfo, e)) {
      this->clear();
      // iterate over json array
      for (const auto &jsonArea : obj[areaListKey].toArray()) {
        // check if area type key is present
        QList<JsonHelper::KeyValidateInfo> areaInfo = {
            {GeoArea::areaTypeKey, QJsonValue::String, true},
        };
        if (!JsonHelper::validateKeys(jsonArea, areaInfo, e)) {
          // load MeasurementArea
          if (jsonArea[GeoArea::areaTypeKey] == MeasurementArea::name) {
            auto area = getGeoArea<MeasurementArea *>(_areaList);
            if (area == nullptr) {
              auto area = new MeasurementArea(this);
              if (area->loadFromJson(jsonArea, e)) {
                this->insert(area);
              } else {
                returnValue = false;
                errorString.append(e);
                errorString.append("\n");
                area->deleteLater();
              }
            } else {
              returnValue = false;
              errorString.append(
                  tr("Multiple Measurement Areas detected. Area was ignored."));
            }
          }
          // load SafeArea
          else if (jsonArea[GeoArea::areaTypeKey] == SafeArea::name) {
            auto area = getGeoArea<SafeArea *>(_areaList);
            if (area == nullptr) {
              auto area = new SafeArea(this);
              if (area->loadFromJson(jsonArea, e)) {
                this->insert(area);
              } else {
                returnValue = false;
                errorString.append(e);
                errorString.append("\n");
                area->deleteLater();
              }
            } else {
              returnValue = false;
              errorString.append(
                  tr("Multiple Safe Areas detected. Area was ignored."));
            }
          }
          // unknown area
          else {
            returnValue = false;
            errorString.append(tr("Unknown area type: ") +
                               jsonArea[GeoArea::areaTypeKey]);
          }
        }
        // GeoArea::areaTypeKey missing
        else {
          returnValue = false;
          errorString.append(e);
          errorString.append("\n");
        }
      }
    }
    // AreaList missing
    else {
      returnValue = false;
      errorString.append(e);
      errorString.append("\n");
    }
  }

  // load origin
  {
    QString e;
    QList<JsonHelper::KeyValidateInfo> keyInfo = {
        {originKey, QJsonValue::Object, true},
    };
    if (JsonHelper::validateKeys(obj, keyInfo, e)) {
      QGeoCoordinate origin;
      if (JsonHelper::loadGeoCoordinate(obj[originKey], false, origin, e)) {
        _origin = origin;
      }
    }
  }

  // check if this is correct.
  if (!this->isCorrect()) {
    returnValue = false;
  }

  return returnValue;
}

bool AreaData::save(QJsonObject &obj) {
  QJsonObject temp;

  QJsonValue jsonOrigin;
  JsonHelper::saveGeoCoordinate(_origin, true, jsonOrigin);
  temp[originKey] = jsonOrigin;
  temp[initializedKey] = _initialized;

  QJsonArray jsonAreaList;
  for (int i = 0; i < _areaList.count(); ++i) {
    auto qobj = _areaList[i];
    auto area = qobject_cast<GeoArea *>(qobj);
    QJsonObject jsonArea;
    if (area->saveToJson(jsonArea)) {
      jsonAreaList.append(jsonArea);
    } else {
      qDebug(AreaDataLog) << "save(): not able to save area: "
                          << area->objectName();
      _processError(tr("Not able to save area: ") + area->objectName());
      return false;
    }
  }
  temp[areaListKey] = jsonAreaList;

  obj = std::move(temp);
  return true;
}

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