CircularGenerator.cpp

#include "CircularGenerator.h"

#include "QGCLoggingCategory.h"
#include "SettingsFact.h"

#define CLIPPER_SCALE 1000000
#include "RoutingThread.h"
#include "geometry/GenericCircle.h"
#include "geometry/MeasurementArea.h"
#include "geometry/SafeArea.h"
#include "geometry/clipper/clipper.hpp"
#include "nemo_interface/SnakeTile.h"

QGC_LOGGING_CATEGORY(CircularGeneratorLog, "CircularGeneratorLog")

using namespace ClipperLib;
template <> inline auto get<0>(const IntPoint &p) { return p.X; }
template <> inline auto get<1>(const IntPoint &p) { return p.Y; }

namespace routing {

bool circularTransects(const snake::FPoint &reference,
                       const snake::FPolygon &polygon,
                       const std::vector<snake::FPolygon> &tiles,
                       snake::Length deltaR, snake::Angle deltaAlpha,
                       snake::Length minLength, snake::Transects &transects);

const char *CircularGenerator::settingsGroup = "CircularGenerator";
const char *CircularGenerator::distanceName = "TransectDistance";
const char *CircularGenerator::deltaAlphaName = "DeltaAlpha";
const char *CircularGenerator::minLengthName = "MinLength";
const char *CircularGenerator::refPointLatitudeName = "ReferencePointLat";
const char *CircularGenerator::refPointLongitudeName = "ReferencePointLong";
const char *CircularGenerator::refPointAltitudeName = "ReferencePointAlt";

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

CircularGenerator::CircularGenerator(GeneratorBase::Data d, QObject *parent)
    : GeneratorBase(d, parent), _connectionsEstablished(false),
      _metaDataMap(FactMetaData::createMapFromJsonFile(
          QStringLiteral(":/json/CircularGenerator.SettingsGroup.json"), this)),
      _distance(settingsGroup, _metaDataMap[distanceName]),
      _deltaAlpha(settingsGroup, _metaDataMap[deltaAlphaName]),
      _minLength(settingsGroup, _metaDataMap[minLengthName]) {
  establishConnections();
}

QString CircularGenerator::editorQml() {
  return QStringLiteral("CircularGeneratorEditor.qml");
}

QString CircularGenerator::mapVisualQml() {
  return QStringLiteral("CircularGeneratorMapVisual.qml");
}

QString CircularGenerator::name() {
  return QStringLiteral("Circular Generator");
}

QString CircularGenerator::abbreviation() { return QStringLiteral("C. Gen."); }

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

      auto ref = this->_reference;
      ref.setAltitude(0);
      if (!ref.isValid()) {
        qCDebug(CircularGeneratorLog) << "get(): reference invalid." << ref;
        return false;
      }
      snake::FPoint reference;
      snake::toENU(origin, ref, reference);

      auto measurementArea =
          getGeoArea<const MeasurementArea *>(*this->_d->areaList());
      if (measurementArea == nullptr) {
        qCDebug(CircularGeneratorLog) << "get(): measurement area == nullptr";
        return false;
      }
      auto geoPolygon = measurementArea->coordinateList();
      for (auto &v : geoPolygon) {
        if (v.isValid()) {
          v.setAltitude(0);
        } else {
          qCDebug(CircularGeneratorLog) << "get(): measurement area invalid.";
          for (const auto &w : geoPolygon) {
            qCDebug(CircularGeneratorLog) << 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 auto *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(CircularGeneratorLog)
                  << "get(): progress.size() != tiles->count().";
              return false;
            }
          }
        }
      } else {
        qCDebug(CircularGeneratorLog)
            << "get(): progress.size() != tiles->count().";
        return false;
      }

      auto serviceArea = getGeoArea<const SafeArea *>(*this->_d->areaList());
      if (measurementArea == nullptr) {
        qCDebug(CircularGeneratorLog) << "get(): measurement area == nullptr";
        return false;
      }
      auto geoDepot = serviceArea->depot();
      if (!geoDepot.isValid()) {
        qCDebug(CircularGeneratorLog) << "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->_deltaAlpha.rawValue().toDouble() *
                                bu::degree::degree);

      generator = [reference, depot, pPolygon, pTiles, distance, alpha,
                   minLength](snake::Transects &transects) -> bool {
        bool value = circularTransects(reference, *pPolygon, *pTiles, distance,
                                       alpha, minLength, transects);
        transects.insert(transects.begin(), snake::FLineString{depot});
        return value;
      };
      return true;
    } else {
      qCDebug(CircularGeneratorLog) << "get(): data invalid.";
      return false;
    }
  } else {
    qCDebug(CircularGeneratorLog) << "get(): data member not set.";
    return false;
  }
}

QGeoCoordinate CircularGenerator::reference() const { return _reference; }

void CircularGenerator::setReference(const QGeoCoordinate &reference) {
  if (_reference != reference) {
    _reference = reference;
    emit referenceChanged();
  }
}

void CircularGenerator::resetReferenceIfInvalid() {
  if (!this->_reference.isValid()) {
    resetReference();
  }
}

void CircularGenerator::resetReference() {
  auto measurementArea =
      getGeoArea<const MeasurementArea *>(*this->_d->areaList());

  if (measurementArea != nullptr) {
    if (measurementArea->center().isValid()) {
      setReference(measurementArea->center());
    } else {
      qCWarning(CircularGeneratorLog)
          << "measurement area center" << measurementArea->center();
    }
  } else {
    qCDebug(CircularGeneratorLog)
        << "resetReference(): measurement area == nullptr";
  }
}

void CircularGenerator::establishConnections() {
  if (this->_d && !this->_connectionsEstablished) {
    auto measurementArea =
        getGeoArea<const MeasurementArea *>(*this->_d->areaList());
    auto serviceArea = getGeoArea<const SafeArea *>(*this->_d->areaList());
    if (measurementArea != nullptr && serviceArea != nullptr) {
      GeneratorBase::establishConnections();

      connect(this->_d, &AreaData::originChanged, this,
              &GeneratorBase::generatorChanged);
      connect(measurementArea, &MeasurementArea::progressChanged, this,
              &GeneratorBase::generatorChanged);
      connect(measurementArea, &MeasurementArea::tilesChanged, this,
              &GeneratorBase::generatorChanged);
      connect(measurementArea, &MeasurementArea::centerChanged, this,
              &CircularGenerator::resetReferenceIfInvalid);
      connect(measurementArea, &MeasurementArea::pathChanged, this,
              &GeneratorBase::generatorChanged);
      connect(serviceArea, &SafeArea::depotChanged, this,
              &GeneratorBase::generatorChanged);
      connect(this->distance(), &Fact::rawValueChanged, this,
              &GeneratorBase::generatorChanged);
      connect(this->deltaAlpha(), &Fact::rawValueChanged, this,
              &GeneratorBase::generatorChanged);
      connect(this->minLength(), &Fact::rawValueChanged, this,
              &GeneratorBase::generatorChanged);
      connect(this, &CircularGenerator::referenceChanged, this,
              &GeneratorBase::generatorChanged);
      this->_connectionsEstablished = true;
    }
  }
}

void CircularGenerator::deleteConnections() {
  if (this->_d && this->_connectionsEstablished) {
    auto measurementArea =
        getGeoArea<const MeasurementArea *>(*this->_d->areaList());
    auto serviceArea = getGeoArea<const SafeArea *>(*this->_d->areaList());
    if (measurementArea != nullptr && serviceArea != nullptr) {
      GeneratorBase::deleteConnections();

      disconnect(this->_d, &AreaData::originChanged, this,
                 &GeneratorBase::generatorChanged);
      disconnect(measurementArea, &MeasurementArea::progressChanged, this,
                 &GeneratorBase::generatorChanged);
      disconnect(measurementArea, &MeasurementArea::tilesChanged, this,
                 &GeneratorBase::generatorChanged);
      disconnect(measurementArea, &MeasurementArea::centerChanged, this,
                 &CircularGenerator::resetReferenceIfInvalid);
      disconnect(measurementArea, &MeasurementArea::pathChanged, this,
                 &GeneratorBase::generatorChanged);
      disconnect(serviceArea, &SafeArea::depotChanged, this,
                 &GeneratorBase::generatorChanged);
      disconnect(this->distance(), &Fact::rawValueChanged, this,
                 &GeneratorBase::generatorChanged);
      disconnect(this->deltaAlpha(), &Fact::rawValueChanged, this,
                 &GeneratorBase::generatorChanged);
      disconnect(this->minLength(), &Fact::rawValueChanged, this,
                 &GeneratorBase::generatorChanged);
      disconnect(this, &CircularGenerator::referenceChanged, this,
                 &GeneratorBase::generatorChanged);
      this->_connectionsEstablished = true;
    }
  }
}

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

Fact *CircularGenerator::deltaAlpha() { return &_deltaAlpha; }

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

bool circularTransects(const snake::FPoint &reference,
                       const snake::FPolygon &polygon,
                       const std::vector<snake::FPolygon> &tiles,
                       snake::Length deltaR, snake::Angle deltaAlpha,
                       snake::Length minLength, snake::Transects &transects) {
  auto s1 = std::chrono::high_resolution_clock::now();

  // Check preconitions
  if (polygon.outer().size() >= 3) {
    using namespace boost::units;
    // Convert geo polygon to ENU polygon.
    std::string error;
    // Check validity.
    if (!bg::is_valid(polygon, error)) {
      qCDebug(CircularGeneratorLog) << "circularTransects(): "
                                       "invalid polygon.";
      qCDebug(CircularGeneratorLog) << error.c_str();
      std::stringstream ss;
      ss << bg::wkt(polygon);
      qCDebug(CircularGeneratorLog) << ss.str().c_str();
    } else {
      // Calculate polygon distances and angles.
      std::vector<snake::Length> distances;
      distances.reserve(polygon.outer().size());
      std::vector<snake::Angle> angles;
      angles.reserve(polygon.outer().size());
      //#ifdef DEBUG_CIRCULAR_SURVEY
      //      qCDebug(CircularGeneratorLog) << "circularTransects():";
      //#endif
      for (const auto &p : polygon.outer()) {
        snake::Length distance = bg::distance(reference, p) * si::meter;
        distances.push_back(distance);
        snake::Angle alpha = (std::atan2(p.get<1>(), p.get<0>())) * si::radian;
        alpha = alpha < 0 * si::radian ? alpha + 2 * M_PI * si::radian : alpha;
        angles.push_back(alpha);
        //#ifdef DEBUG_CIRCULAR_SURVEY
        //        qCDebug(CircularGeneratorLog) << "distances, angles,
        //        coordinates:"; qCDebug(CircularGeneratorLog) <<
        //        to_string(distance).c_str(); qCDebug(CircularGeneratorLog)
        //        << to_string(snake::Degree(alpha)).c_str();
        //        qCDebug(CircularGeneratorLog) << "x = " << p.get<0>() << "y
        //        = "
        //        << p.get<1>();
        //#endif
      }

      auto rMin = deltaR; // minimal circle radius
      snake::Angle alpha1(0 * degree::degree);
      snake::Angle alpha2(360 * degree::degree);
      // Determine r_min by successive approximation
      if (!bg::within(reference, polygon.outer())) {
        rMin = bg::distance(reference, polygon) * si::meter;
      }

      auto rMax = (*std::max_element(distances.begin(),
                                     distances.end())); // maximal circle radius

      // Scale parameters and coordinates.
      const auto rMinScaled =
          ClipperLib::cInt(std::round(rMin.value() * CLIPPER_SCALE));
      const auto deltaRScaled =
          ClipperLib::cInt(std::round(deltaR.value() * CLIPPER_SCALE));
      auto referenceScaled = ClipperLib::IntPoint{
          ClipperLib::cInt(std::round(reference.get<0>() * CLIPPER_SCALE)),
          ClipperLib::cInt(std::round(reference.get<1>() * CLIPPER_SCALE))};

      // Generate circle sectors.
      auto rScaled = rMinScaled;
      const auto nTran = long(std::ceil(((rMax - rMin) / deltaR).value()));
      vector<ClipperLib::Path> sectors(nTran, ClipperLib::Path());
      const auto nSectors =
          long(std::round(((alpha2 - alpha1) / deltaAlpha).value()));
      //#ifdef DEBUG_CIRCULAR_SURVEY
      //      qCDebug(CircularGeneratorLog) << "circularTransects(): sector
      //      parameres:"; qCDebug(CircularGeneratorLog) << "alpha1: " <<
      //      to_string(snake::Degree(alpha1)).c_str();
      //      qCDebug(CircularGeneratorLog) << "alpha2:
      //      "
      //      << to_string(snake::Degree(alpha2)).c_str();
      //      qCDebug(CircularGeneratorLog) << "n: "
      //      << to_string((alpha2 - alpha1) / deltaAlpha).c_str();
      //      qCDebug(CircularGeneratorLog)
      //      << "nSectors: " << nSectors; qCDebug(CircularGeneratorLog) <<
      //      "rMin: " << to_string(rMin).c_str();
      //      qCDebug(CircularGeneratorLog)
      //      << "rMax: " << to_string(rMax).c_str();
      //      qCDebug(CircularGeneratorLog) << "nTran: " << nTran;
      //#endif
      using ClipperCircle =
          GenericCircle<ClipperLib::cInt, ClipperLib::IntPoint>;
      for (auto &sector : sectors) {
        ClipperCircle circle(rScaled, referenceScaled);
        approximate(circle, nSectors, sector);
        rScaled += deltaRScaled;
      }
      // Clip sectors to polygonENU.
      ClipperLib::Path polygonClipper;
      snake::FPolygon shrinked;
      snake::offsetPolygon(polygon, shrinked, -0.3);
      auto &outer = shrinked.outer();
      polygonClipper.reserve(outer.size());
      for (auto it = outer.begin(); it < outer.end() - 1; ++it) {
        auto x = ClipperLib::cInt(std::round(it->get<0>() * CLIPPER_SCALE));
        auto y = ClipperLib::cInt(std::round(it->get<1>() * CLIPPER_SCALE));
        polygonClipper.push_back(ClipperLib::IntPoint{x, y});
      }
      ClipperLib::Clipper clipper;
      clipper.AddPath(polygonClipper, ClipperLib::ptClip, true);
      clipper.AddPaths(sectors, ClipperLib::ptSubject, false);
      ClipperLib::PolyTree transectsClipper;
      clipper.Execute(ClipperLib::ctIntersection, transectsClipper,
                      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 : transectsClipper.Childs) {
          clipper.AddPath(child->Contour, ClipperLib::ptSubject, false);
        }
        clipper.AddPaths(processedTiles, ClipperLib::ptClip, true);
        transectsClipper.Clear();
        clipper.Execute(ClipperLib::ctDifference, transectsClipper,
                        ClipperLib::pftNonZero, ClipperLib::pftNonZero);
      }

      // Extract transects from  PolyTree and convert them to
      // BoostLineString
      for (const auto &child : transectsClipper.Childs) {
        snake::FLineString transect;
        transect.reserve(child->Contour.size());
        for (const auto &vertex : child->Contour) {
          auto x = static_cast<double>(vertex.X) / CLIPPER_SCALE;
          auto y = static_cast<double>(vertex.Y) / CLIPPER_SCALE;
          transect.push_back(snake::FPoint(x, y));
        }
        transects.push_back(transect);
      }

      // Join sectors which where slit due to clipping.
      const double th = 0.01;
      for (auto ito = transects.begin(); ito < transects.end(); ++ito) {
        for (auto iti = ito + 1; iti < transects.end(); ++iti) {
          auto dist1 = bg::distance(ito->front(), iti->front());
          if (dist1 < th) {
            snake::FLineString temp;
            for (auto it = iti->end() - 1; it >= iti->begin(); --it) {
              temp.push_back(*it);
            }
            temp.insert(temp.end(), ito->begin(), ito->end());
            *ito = temp;
            transects.erase(iti);
            break;
          }
          auto dist2 = bg::distance(ito->front(), iti->back());
          if (dist2 < th) {
            snake::FLineString temp;
            temp.insert(temp.end(), iti->begin(), iti->end());
            temp.insert(temp.end(), ito->begin(), ito->end());
            *ito = temp;
            transects.erase(iti);
            break;
          }
          auto dist3 = bg::distance(ito->back(), iti->front());
          if (dist3 < th) {
            snake::FLineString temp;
            temp.insert(temp.end(), ito->begin(), ito->end());
            temp.insert(temp.end(), iti->begin(), iti->end());
            *ito = temp;
            transects.erase(iti);
            break;
          }
          auto dist4 = bg::distance(ito->back(), iti->back());
          if (dist4 < th) {
            snake::FLineString temp;
            temp.insert(temp.end(), ito->begin(), ito->end());
            for (auto it = iti->end() - 1; it >= iti->begin(); --it) {
              temp.push_back(*it);
            }
            *ito = temp;
            transects.erase(iti);
            break;
          }
        }
      }

      // Remove short transects
      for (auto it = transects.begin(); it < transects.end();) {
        if (bg::length(*it) < minLength.value()) {
          it = transects.erase(it);
        } else {
          ++it;
        }
      }

      qCDebug(CircularGeneratorLog)
          << "circularTransects(): transect gen. time: "
          << std::chrono::duration_cast<std::chrono::milliseconds>(
                 std::chrono::high_resolution_clock::now() - s1)
                 .count()
          << " ms";
      return true;
    }
  }
  return false;
}

} // namespace routing