#include "CircularGenerator.h"

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

#define CLIPPER_SCALE 1000000
#include "Wima/Geometry/GenericCircle.h"
#include "clipper/clipper.hpp"
// Clipper and GenericCircle
using namespace ClipperLib;
template <> auto get<0>(const IntPoint &p) { return p.X; }
template <> auto get<1>(const IntPoint &p) { return p.Y; }

namespace routing {

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

CircularGenerator::CircularGenerator(QObject *parent) : GeneratorBase(parent) {}

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(const WimaPlanData &data,
                            GeneratorBase &generator) {
  auto generator = [depot, pPolygon, pTiles, distance, alpha,
                    minLength](snake::Transects &transects) -> bool {
    bool value = circularTransects(*pPolygon, *pTiles, distance, alpha,
                                   minLength, transects);
    transects.insert(transects.begin(), snake::FLineString{depot});
    return value;
  };
}

bool circularTransects(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.
    snake::FPoint origin{0, 0};
    std::string error;
    // Check validity.
    if (!bg::is_valid(polygon, error)) {
      qCWarning(CircularGeneratorLog) << "circularTransects(): "
                                         "invalid polygon.";
      qCWarning(CircularGeneratorLog) << error.c_str();
      std::stringstream ss;
      ss << bg::wkt(polygon);
      qCWarning(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
      //      qCWarning(CircularGeneratorLog) << "circularTransects():";
      //#endif
      for (const auto &p : polygon.outer()) {
        snake::Length distance = bg::distance(origin, 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
        //        qCWarning(CircularGeneratorLog) << "distances, angles,
        //        coordinates:"; qCWarning(CircularGeneratorLog) <<
        //        to_string(distance).c_str(); qCWarning(CircularGeneratorLog)
        //        << to_string(snake::Degree(alpha)).c_str();
        //        qCWarning(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(origin, polygon.outer())) {
        rMin = bg::distance(origin, 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 originScaled =
          ClipperLib::IntPoint{ClipperLib::cInt(std::round(origin.get<0>())),
                               ClipperLib::cInt(std::round(origin.get<1>()))};

      // 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
      //      qCWarning(CircularGeneratorLog) << "circularTransects(): sector
      //      parameres:"; qCWarning(CircularGeneratorLog) << "alpha1: " <<
      //      to_string(snake::Degree(alpha1)).c_str();
      //      qCWarning(CircularGeneratorLog) << "alpha2:
      //      "
      //      << to_string(snake::Degree(alpha2)).c_str();
      //      qCWarning(CircularGeneratorLog) << "n: "
      //      << to_string((alpha2 - alpha1) / deltaAlpha).c_str();
      //      qCWarning(CircularGeneratorLog)
      //      << "nSectors: " << nSectors; qCWarning(CircularGeneratorLog) <<
      //      "rMin: " << to_string(rMin).c_str();
      //      qCWarning(CircularGeneratorLog)
      //      << "rMax: " << to_string(rMax).c_str();
      //      qCWarning(CircularGeneratorLog) << "nTran: " << nTran;
      //#endif
      using ClipperCircle =
          GenericCircle<ClipperLib::cInt, ClipperLib::IntPoint>;
      for (auto &sector : sectors) {
        ClipperCircle circle(rScaled, originScaled);
        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;
        }
      }

      qCWarning(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