LinearGenerator.cpp

#include "LinearGenerator.h"

#include "QGCLoggingCategory.h"
QGC_LOGGING_CATEGORY(LinearGeneratorLog, "LinearGeneratorLog")

#define CLIPPER_SCALE 1000000
#include "geometry/MeasurementArea.h"
#include "geometry/SafeArea.h"
#include "geometry/clipper/clipper.hpp"

#include "RoutingThread.h"
#include "nemo_interface/SnakeTile.h"

namespace routing {

bool linearTransects(const snake::FPolygon &polygon,
                     const std::vector<snake::FPolygon> &tiles,
                     snake::Length distance, snake::Angle angle,
                     snake::Length minLength, snake::Transects &transects);

const char *LinearGenerator::settingsGroup = "LinearGenerator";
const char *LinearGenerator::distanceKey = "TransectDistance";
const char *LinearGenerator::alphaKey = "Alpha";
const char *LinearGenerator::minLenghKey = "MinLength";

LinearGenerator::LinearGenerator(QObject *parent)
    : LinearGenerator(nullptr, parent) {}

LinearGenerator::LinearGenerator(GeneratorBase::Data d, QObject *parent)
    : GeneratorBase(d, parent),
      _metaDataMap(FactMetaData::createMapFromJsonFile(
          QStringLiteral(":/json/LinearGenerator.SettingsGroup.json"), this)),
      _distance(settingsGroup, _metaDataMap[distanceKey]),
      _alpha(settingsGroup, _metaDataMap[alphaKey]),
      _minLength(settingsGroup, _metaDataMap[minLenghKey ),
      _measurementArea(nullptr), _safeArea(nullptr) {
  connect(this->distance(), &Fact::rawValueChanged, this,
          &GeneratorBase::generatorChanged);
  connect(this->alpha(), &Fact::rawValueChanged, this,
          &GeneratorBase::generatorChanged);
  connect(this->minLength(), &Fact::rawValueChanged, this,
          &GeneratorBase::generatorChanged);
  connect(this->_d, &AreaData::areaListChanged, this,
          &LinearGenerator::onAreaListChanged);
}

QString LinearGenerator::editorQml() {
  return QStringLiteral("LinearGeneratorEditor.qml");
}

QString LinearGenerator::mapVisualQml() { return QStringLiteral(""); }

QString LinearGenerator::name() { return QStringLiteral("Linear Generator"); }

QString LinearGenerator::abbreviation() { return QStringLiteral("L. Gen."); }

bool LinearGenerator::get(Generator &generator) {
  if (_d) {
    if (this->_d->isCorrect()) {
      // Prepare data.
      auto origin = this->_d->origin();
      origin.setAltitude(0);
      if (!origin.isValid()) {
        qCDebug(LinearGeneratorLog) << "get(): origin invalid." << origin;
      }

      auto measurementArea =
          getGeoArea<const MeasurementArea *>(*this->_d->areaList());
      if (measurementArea == nullptr) {
        qCDebug(LinearGeneratorLog) << "get(): measurement area == nullptr";
        return false;
      }
      auto geoPolygon = measurementArea->coordinateList();
      for (auto &v : geoPolygon) {
        if (v.isValid()) {
          v.setAltitude(0);
        } else {
          qCDebug(LinearGeneratorLog) << "get(): measurement area invalid.";
          for (const auto &w : geoPolygon) {
            qCDebug(LinearGeneratorLog) << w;
          }
          return false;
        }
      }
      auto pPolygon = std::make_shared<snake::FPolygon>();
      snake::areaToEnu(origin, geoPolygon, *pPolygon);

      // Progress and tiles.
      const auto &progress = measurementArea->progress();
      const auto *tiles = measurementArea->tiles();
      auto pTiles = std::make_shared<std::vector<snake::FPolygon>>();
      if (progress.size() == tiles->count()) {
        for (int i = 0; i < tiles->count(); ++i) {
          if (progress[i] == 100) {
            const QObject *obj = (*tiles)[int(i)];
            const auto *tile = qobject_cast<const SnakeTile *>(obj);

            if (tile != nullptr) {
              snake::FPolygon tileENU;
              snake::areaToEnu(origin, tile->coordinateList(), tileENU);
              pTiles->push_back(std::move(tileENU));
            } else {
              qCDebug(LinearGeneratorLog) << "get(): tile == nullptr";
              return false;
            }
          }
        }
      } else {
        qCDebug(LinearGeneratorLog)
            << "get(): progress.size() != tiles->count().";
        return false;
      }

      auto serviceArea = getGeoArea<const SafeArea *>(*this->_d->areaList());
      if (serviceArea == nullptr) {
        qCDebug(LinearGeneratorLog) << "get(): service area == nullptr";
        return false;
      }
      auto geoDepot = serviceArea->depot();
      if (!geoDepot.isValid()) {
        qCDebug(LinearGeneratorLog) << "get(): depot invalid." << geoDepot;
        return false;
      }
      snake::FPoint depot;
      snake::toENU(origin, geoDepot, depot);

      // Fetch transect parameter.
      auto distance =
          snake::Length(this->_distance.rawValue().toDouble() * bu::si::meter);
      auto minLength =
          snake::Length(this->_minLength.rawValue().toDouble() * bu::si::meter);
      auto alpha =
          snake::Angle(this->_alpha.rawValue().toDouble() * bu::degree::degree);
      generator = [depot, pPolygon, pTiles, distance, alpha,
                   minLength](snake::Transects &transects) -> bool {
        bool value = linearTransects(*pPolygon, *pTiles, distance, alpha,
                                     minLength, transects);
        transects.insert(transects.begin(), snake::FLineString{depot});
        return value;
      };
      return true;
    } else {
      qCDebug(LinearGeneratorLog) << "get(): data invalid.";
      return false;
    }
  } else {
    qCDebug(LinearGeneratorLog) << "get(): data member not set.";
    return false;
  }
}

bool LinearGenerator::save(QJsonObject &obj) const {
  QJsonObject temp;

  bool ok = false;
  auto variant = _distance.rawValue();
  auto val = variant.toDouble(&ok);
  if (!ok) {
    qCDebug(LinearGeneratorLog)
        << "save(): not able to save distance. Not a double: "
        << variant.typeName();
    return false;
  } else {
    temp[distanceKey] = val;
  }

  variant = _alpha.rawValue();
  val = variant.toDouble(&ok);
  if (!ok) {
    qCDebug(LinearGeneratorLog)
        << "save(): not able to save alpha. Not a double: "
        << variant.typeName();
    return false;
  } else {
    temp[alphaKey] = val;
  }

  variant = _minLength.rawValue();
  val = variant.toDouble(&ok);
  if (!ok) {
    qCDebug(LinearGeneratorLog)
        << "save(): not able to save minLength. Not a double: "
        << variant.typeName();
    return false;
  } else {
    temp[minLenghKey  = val;
  }

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

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

  // load distance
  {
    QString e;
    QList<JsonHelper::KeyValidateInfo> keyInfo = {
        {distanceKey, QJsonValue::Double, true},
    };
    if (JsonHelper::validateKeys(obj, keyInfo, e)) {
      _distance.setRawValue(obj[distanceKey]);
    } else {
      returnValue = false;
      errorString.append(e);
      errorString.append("\n");
    }
  }

  // load alpha
  {
    QString e;
    QList<JsonHelper::KeyValidateInfo> keyInfo = {
        {alphaKey, QJsonValue::Double, true},
    };
    if (JsonHelper::validateKeys(obj, keyInfo, e)) {
      _alpha.setRawValue(obj[alphaKey]);
    } else {
      returnValue = false;
      errorString.append(e);
      errorString.append("\n");
    }
  }

  // load distance
  {
    QString e;
    QList<JsonHelper::KeyValidateInfo> keyInfo = {
        {minLengthKey, QJsonValue::Double, true},
    };
    if (JsonHelper::validateKeys(obj, keyInfo, e)) {
      _minLength.setRawValue(obj[minLengthKey]);
    } else {
      returnValue = false;
      errorString.append(e);
      errorString.append("\n");
    }
  }

  return returnValue;
}

Fact *LinearGenerator::distance() { return &_distance; }

Fact *LinearGenerator::alpha() { return &_alpha; }

Fact *LinearGenerator::minLength() { return &_minLength; }

void LinearGenerator::onAreaListChanged() {
  auto *measurementArea = getGeoArea<MeasurementArea *>(*this->_d->areaList());
  setMeasurementArea(measurementArea);
}

void LinearGenerator::setMeasurementArea(MeasurementArea *area) {
  if (_measurementArea != area) {

    if (_measurementArea != nullptr) {
      disconnect(_measurementArea, &MeasurementArea::progressChanged, this,
                 &GeneratorBase::generatorChanged);
      disconnect(_measurementArea, &MeasurementArea::tilesChanged, this,
                 &GeneratorBase::generatorChanged);
      disconnect(_measurementArea, &MeasurementArea::pathChanged, this,
                 &GeneratorBase::generatorChanged);
    }

    _measurementArea = area;

    if (_measurementArea != nullptr) {
      connect(_measurementArea, &MeasurementArea::progressChanged, this,
              &GeneratorBase::generatorChanged);
      connect(_measurementArea, &MeasurementArea::tilesChanged, this,
              &GeneratorBase::generatorChanged);
      connect(_measurementArea, &MeasurementArea::pathChanged, this,
              &GeneratorBase::generatorChanged);
    }

    emit generatorChanged();
  }
}

bool linearTransects(const snake::FPolygon &polygon,
                     const std::vector<snake::FPolygon> &tiles,
                     snake::Length distance, snake::Angle angle,
                     snake::Length minLength, snake::Transects &transects) {
  namespace tr = bg::strategy::transform;
  auto s1 = std::chrono::high_resolution_clock::now();

  // Check preconitions
  if (polygon.outer().size() >= 3) {
    // Convert to ENU system.
    std::string error;
    // Check validity.
    if (!bg::is_valid(polygon, error)) {
      std::stringstream ss;
      ss << bg::wkt(polygon);

      qCDebug(LinearGeneratorLog) << "linearTransects(): "
                                     "invalid polygon. "
                                  << error.c_str() << ss.str().c_str();
    } else {
      tr::rotate_transformer<bg::degree, double, 2, 2> rotate(angle.value() *
                                                              180 / M_PI);
      // Rotate polygon by angle and calculate bounding box.
      snake::FPolygon polygonENURotated;
      bg::transform(polygon.outer(), polygonENURotated.outer(), rotate);
      snake::FBox box;
      boost::geometry::envelope(polygonENURotated, box);
      double x0 = box.min_corner().get<0>();
      double y0 = box.min_corner().get<1>();
      double x1 = box.max_corner().get<0>();
      double y1 = box.max_corner().get<1>();

      // Generate transects and convert them to clipper path.
      size_t num_t = ceil((y1 - y0) / distance.value()); // number of transects
      vector<ClipperLib::Path> transectsClipper;
      transectsClipper.reserve(num_t);
      for (size_t i = 0; i < num_t; ++i) {
        // calculate transect
        snake::FPoint v1{x0, y0 + i * distance.value()};
        snake::FPoint v2{x1, y0 + i * distance.value()};
        snake::FLineString transect;
        transect.push_back(v1);
        transect.push_back(v2);
        // transform back
        snake::FLineString temp_transect;
        tr::rotate_transformer<bg::degree, double, 2, 2> rotate_back(
            -angle.value() * 180 / M_PI);
        bg::transform(transect, temp_transect, rotate_back);
        // to clipper
        ClipperLib::IntPoint c1{static_cast<ClipperLib::cInt>(
                                    temp_transect[0].get<0>() * CLIPPER_SCALE),
                                static_cast<ClipperLib::cInt>(
                                    temp_transect[0].get<1>() * CLIPPER_SCALE)};
        ClipperLib::IntPoint c2{static_cast<ClipperLib::cInt>(
                                    temp_transect[1].get<0>() * CLIPPER_SCALE),
                                static_cast<ClipperLib::cInt>(
                                    temp_transect[1].get<1>() * CLIPPER_SCALE)};
        ClipperLib::Path path{c1, c2};
        transectsClipper.push_back(path);
      }

      if (transectsClipper.size() == 0) {
        std::stringstream ss;
        ss << "Not able to generate transects. Parameter: distance = "
           << distance << std::endl;
        qCDebug(LinearGeneratorLog)
            << "linearTransects(): " << ss.str().c_str();
        return false;
      }

      // Convert measurement area to clipper path.
      snake::FPolygon shrinked;
      snake::offsetPolygon(polygon, shrinked, -0.2);
      auto &outer = shrinked.outer();
      ClipperLib::Path polygonClipper;
      for (auto vertex : outer) {
        polygonClipper.push_back(ClipperLib::IntPoint{
            static_cast<ClipperLib::cInt>(vertex.get<0>() * CLIPPER_SCALE),
            static_cast<ClipperLib::cInt>(vertex.get<1>() * CLIPPER_SCALE)});
      }

      // Perform clipping.
      // Clip transects to measurement area.
      ClipperLib::Clipper clipper;
      clipper.AddPath(polygonClipper, ClipperLib::ptClip, true);
      clipper.AddPaths(transectsClipper, ClipperLib::ptSubject, false);
      ClipperLib::PolyTree clippedTransecs;
      clipper.Execute(ClipperLib::ctIntersection, clippedTransecs,
                      ClipperLib::pftNonZero, ClipperLib::pftNonZero);

      // Subtract holes.
      if (tiles.size() > 0) {
        vector<ClipperLib::Path> processedTiles;
        for (const auto &tile : tiles) {
          ClipperLib::Path path;
          for (const auto &v : tile.outer()) {
            path.push_back(ClipperLib::IntPoint{
                static_cast<ClipperLib::cInt>(v.get<0>() * CLIPPER_SCALE),
                static_cast<ClipperLib::cInt>(v.get<1>() * CLIPPER_SCALE)});
          }
          processedTiles.push_back(std::move(path));
        }

        clipper.Clear();
        for (const auto &child : clippedTransecs.Childs) {
          clipper.AddPath(child->Contour, ClipperLib::ptSubject, false);
        }
        clipper.AddPaths(processedTiles, ClipperLib::ptClip, true);
        clippedTransecs.Clear();
        clipper.Execute(ClipperLib::ctDifference, clippedTransecs,
                        ClipperLib::pftNonZero, ClipperLib::pftNonZero);
      }

      // Extract transects from  PolyTree and convert them to BoostLineString
      for (const auto &child : clippedTransecs.Childs) {
        const auto &clipperTransect = child->Contour;
        snake::FPoint v1{
            static_cast<double>(clipperTransect[0].X) / CLIPPER_SCALE,
            static_cast<double>(clipperTransect[0].Y) / CLIPPER_SCALE};
        snake::FPoint v2{
            static_cast<double>(clipperTransect[1].X) / CLIPPER_SCALE,
            static_cast<double>(clipperTransect[1].Y) / CLIPPER_SCALE};

        snake::FLineString transect{v1, v2};
        if (bg::length(transect) >= minLength.value()) {
          transects.push_back(transect);
        }
      }

      if (transects.size() == 0) {
        std::stringstream ss;
        ss << "Not able to  generatetransects. Parameter: minLength = "
           << minLength << std::endl;
        qCDebug(LinearGeneratorLog)
            << "linearTransects(): " << ss.str().c_str();
        return false;
      }

      qCDebug(LinearGeneratorLog)
          << "linearTransects(): time: "
          << std::chrono::duration_cast<std::chrono::milliseconds>(
                 std::chrono::high_resolution_clock::now() - s1)
                 .count()
          << " ms";
      return true;
    }
  }
  return false;
}
} // namespace routing