#pragma once

#define _USE_MATH_DEFINES
#include <algorithm>
#include <cmath>
#include <list>
#include <map>
#include <mapbox/geometry/wagyu/config.hpp>
#include <mapbox/geometry/wagyu/ring.hpp>
#include <mapbox/geometry/wagyu/ring_util.hpp>
#include <set>
#include <unordered_map>
#include <utility>

#ifdef DEBUG
#include <iostream>
#include <sstream>
#endif

namespace mapbox {
namespace geometry {
namespace wagyu {

template <typename T>
struct point_ptr_pair {
  point_ptr<T> op1;
  point_ptr<T> op2;

  constexpr point_ptr_pair(point_ptr<T> o1, point_ptr<T> o2)
      : op1(o1), op2(o2) {}

  point_ptr_pair(point_ptr_pair<T> const& p) = default;

  point_ptr_pair(point_ptr_pair<T>&& p)
      : op1(std::move(p.op1)), op2(std::move(p.op2)) {}

  point_ptr_pair& operator=(point_ptr_pair<T>&& p) {
    op1 = std::move(p.op1);
    op2 = std::move(p.op2);
    return *this;
  }
};

#ifdef DEBUG

template <class charT, class traits, typename T>
inline std::basic_ostream<charT, traits>& operator<<(
    std::basic_ostream<charT, traits>& out,
    const std::unordered_multimap<ring_ptr<T>, point_ptr_pair<T>>& dupe_ring) {
  out << " BEGIN CONNECTIONS: " << std::endl;
  for (auto& r : dupe_ring) {
    out << "  Ring: ";
    if (r.second.op1->ring) {
      out << r.second.op1->ring->ring_index;
    } else {
      out << "---";
    }
    out << " to ";
    if (r.second.op2->ring) {
      out << r.second.op2->ring->ring_index;
    } else {
      out << "---";
    }
    out << "  ( at " << r.second.op1->x << ", " << r.second.op1->y << " )";
    out << "  Ring1 ( ";
    if (r.second.op1->ring) {
      out << "area: " << r.second.op1->ring->area << " parent: ";
      if (r.second.op1->ring->parent) {
        out << r.second.op1->ring->parent->ring_index;
      } else {
        out << "---";
      }
    } else {
      out << "---";
    }
    out << " )";
    out << "  Ring2 ( ";
    if (r.second.op2->ring) {
      out << "area: " << r.second.op2->ring->area << " parent: ";
      if (r.second.op2->ring->parent) {
        out << r.second.op2->ring->parent->ring_index;
      } else {
        out << "---";
      }
    } else {
      out << "---";
    }
    out << " )";
    out << std::endl;
  }
  out << " END CONNECTIONS: " << std::endl;
  return out;
}

#endif

template <typename T>
bool find_intersect_loop(
    std::unordered_multimap<ring_ptr<T>, point_ptr_pair<T>>& dupe_ring,
    std::list<std::pair<ring_ptr<T>, point_ptr_pair<T>>>& iList,
    ring_ptr<T> ring_parent, ring_ptr<T> ring_origin, ring_ptr<T> ring_search,
    std::set<ring_ptr<T>>& visited, point_ptr<T> orig_pt, point_ptr<T> prev_pt,
    ring_manager<T>& rings) {
  {
    auto range = dupe_ring.equal_range(ring_search);
    // Check for direct connection
    for (auto& it = range.first; it != range.second;) {
      ring_ptr<T> it_ring1 = it->second.op1->ring;
      ring_ptr<T> it_ring2 = it->second.op2->ring;
      if (!it_ring1 || !it_ring2 || it_ring1 != ring_search ||
          (!it_ring1->is_hole() && !it_ring2->is_hole())) {
        it = dupe_ring.erase(it);
        continue;
      }
      if (it_ring2 == ring_origin &&
          (ring_parent == it_ring2 || ring_parent == it_ring2->parent) &&
          *prev_pt != *it->second.op2 && *orig_pt != *it->second.op2) {
        iList.emplace_front(ring_search, it->second);
        return true;
      }
      ++it;
    }
  }
  auto range = dupe_ring.equal_range(ring_search);
  visited.insert(ring_search);
  // Check for connection through chain of other intersections
  for (auto& it = range.first;
       it != range.second && it != dupe_ring.end() && it->first == ring_search;
       ++it) {
    ring_ptr<T> it_ring = it->second.op2->ring;
    if (visited.count(it_ring) > 0 || it_ring == nullptr ||
        (ring_parent != it_ring && ring_parent != it_ring->parent) ||
        value_is_zero(it_ring->area()) || *prev_pt == *it->second.op2) {
      continue;
    }
    if (find_intersect_loop(dupe_ring, iList, ring_parent, ring_origin, it_ring,
                            visited, orig_pt, it->second.op2, rings)) {
      iList.emplace_front(ring_search, it->second);
      return true;
    }
  }
  return false;
}

template <typename T>
struct point_ptr_cmp {
  inline bool operator()(point_ptr<T> op1, point_ptr<T> op2) {
    if (op1->y != op2->y) {
      return (op1->y > op2->y);
    } else if (op1->x != op2->x) {
      return (op1->x < op2->x);
    } else {
      std::size_t depth_1 = ring_depth(op1->ring);
      std::size_t depth_2 = ring_depth(op2->ring);
      return depth_1 > depth_2;
    }
  }
};

template <typename T>
void correct_orientations(ring_manager<T>& manager) {
  for (auto& r : manager.rings) {
    if (!r.points) {
      continue;
    }
    r.recalculate_stats();
    if (r.size() < 3) {
      remove_ring_and_points(&r, manager, false);
      continue;
    }
    if (ring_is_hole(&r) != r.is_hole()) {
      reverse_ring(r.points);
      r.recalculate_stats();
    }
  }
}

template <typename T>
point_vector<T> sort_ring_points(ring_ptr<T> r) {
  point_vector<T> sorted_points;
  point_ptr<T> point_itr = r->points;
  point_ptr<T> last_point = point_itr->prev;
  while (point_itr != last_point) {
    sorted_points.push_back(point_itr);
    point_itr = point_itr->next;
  }
  sorted_points.push_back(last_point);
  std::stable_sort(sorted_points.begin(), sorted_points.end(),
                   [](point_ptr<T> const& pt1, point_ptr<T> const& pt2) {
                     if (pt1->y != pt2->y) {
                       return (pt1->y > pt2->y);
                     }
                     return (pt1->x < pt2->x);
                   });
  return sorted_points;
}

template <typename T>
ring_ptr<T> correct_self_intersection(point_ptr<T> pt1, point_ptr<T> pt2,
                                      ring_manager<T>& manager) {
  if (pt1->ring != pt2->ring) {
    return static_cast<ring_ptr<T>>(nullptr);
  }

  ring_ptr<T> ring = pt1->ring;

  // split the polygon into two ...
  point_ptr<T> pt3 = pt1->prev;
  point_ptr<T> pt4 = pt2->prev;
  pt1->prev = pt4;
  pt4->next = pt1;
  pt2->prev = pt3;
  pt3->next = pt2;

  ring_ptr<T> new_ring = create_new_ring(manager);
  std::size_t size_1 = 0;
  std::size_t size_2 = 0;
  mapbox::geometry::box<T> box1({0, 0}, {0, 0});
  mapbox::geometry::box<T> box2({0, 0}, {0, 0});
  double area_1 = area_from_point(pt1, size_1, box1);
  double area_2 = area_from_point(pt2, size_2, box2);

  if (std::fabs(area_1) > std::fabs(area_2)) {
    ring->points = pt1;
    ring->set_stats(area_1, size_1, box1);
    new_ring->points = pt2;
    new_ring->set_stats(area_2, size_2, box2);
  } else {
    ring->points = pt2;
    ring->set_stats(area_2, size_2, box2);
    new_ring->points = pt1;
    new_ring->set_stats(area_1, size_1, box1);
  }
  update_points_ring(new_ring);
  return new_ring;
}

template <typename T>
void correct_repeated_points(ring_manager<T>& manager,
                             ring_vector<T>& new_rings,
                             point_vector_itr<T> const& begin,
                             point_vector_itr<T> const& end) {
  for (auto itr1 = begin; itr1 != end; ++itr1) {
    if ((*itr1)->ring == nullptr) {
      continue;
    }
    for (auto itr2 = std::next(itr1); itr2 != end; ++itr2) {
      if ((*itr2)->ring == nullptr) {
        continue;
      }
      ring_ptr<T> new_ring = correct_self_intersection(*itr1, *itr2, manager);
      if (new_ring != nullptr) {
        new_rings.push_back(new_ring);
      }
    }
  }
}

template <typename T>
void find_and_correct_repeated_points(ring_ptr<T> r, ring_manager<T>& manager,
                                      ring_vector<T>& new_rings) {
  auto sorted_points = sort_ring_points(r);
  // Find sets of repeated points
  std::size_t count = 0;
  auto prev_itr = sorted_points.begin();
  auto itr = std::next(prev_itr);
  while (itr != sorted_points.end()) {
    if (*(*prev_itr) == *(*(itr))) {
      ++count;
      ++prev_itr;
      ++itr;
      if (itr != sorted_points.end()) {
        continue;
      } else {
        ++prev_itr;
      }
    } else {
      ++prev_itr;
      ++itr;
    }
    if (count == 0) {
      continue;
    }
    auto first = prev_itr;
    std::advance(first, -(static_cast<int>(count) + 1));
    correct_repeated_points(manager, new_rings, first, prev_itr);
    count = 0;
  }
}

template <typename T>
void reassign_children_if_necessary(ring_ptr<T> new_ring,
                                    ring_ptr<T> sibling_ring,
                                    ring_manager<T>& manager,
                                    ring_vector<T>& new_rings) {
  auto& children =
      sibling_ring == nullptr ? manager.children : sibling_ring->children;
  for (auto c : children) {
    if (c == nullptr) {
      continue;
    }
    if (std::find(new_rings.begin(), new_rings.end(), c) != new_rings.end()) {
      continue;
    }
    if (poly2_contains_poly1(c, new_ring)) {
      reassign_as_child(c, new_ring, manager);
    }
  }
}

template <typename T>
bool find_parent_in_tree(ring_ptr<T> r, ring_ptr<T> possible_parent,
                         ring_manager<T>& manager) {
  // Before starting this we are assuming that possible_parent
  // and r have opposite signs of their areas

  // First we must search all grandchildren
  for (auto c : possible_parent->children) {
    if (c == nullptr) {
      continue;
    }
    for (auto gc : c->children) {
      if (gc == nullptr) {
        continue;
      }
      if (find_parent_in_tree(r, gc, manager)) {
        return true;
      }
    }
  }

  if (poly2_contains_poly1(r, possible_parent)) {
    reassign_as_child(r, possible_parent, manager);
    return true;
  }
  return false;
}

template <typename T>
void assign_new_ring_parents(ring_manager<T>& manager,
                             ring_ptr<T> original_ring,
                             ring_vector<T>& new_rings) {
  // First lets remove any rings that have zero area
  // or have no points
  new_rings.erase(std::remove_if(new_rings.begin(), new_rings.end(),
                                 [](ring_ptr<T> const& r) {
                                   if (r->points == nullptr) {
                                     return true;
                                   }
                                   return value_is_zero(r->area());
                                 }),
                  new_rings.end());

  if (new_rings.empty()) {
    // No new rings created simply return;
    return;
  }

  // We should not have to re-assign the parent of the original ring
  // because we always maintained the largest ring during splitting
  // on repeated points.

  double original_ring_area = original_ring->area();
  bool original_positive = original_ring_area > 0.0;

  // If there is only one new ring the logic is very simple and we
  // do not have to check which ring contains, we only need to compare
  // the areas of the original ring and that of the new ring.
  if (new_rings.size() == 1) {
    double new_ring_area = new_rings.front()->area();
    bool new_positive = new_ring_area > 0.0;
    if (original_positive == new_positive) {
      // The rings should be siblings
      assign_as_child(new_rings.front(), original_ring->parent, manager);
      reassign_children_if_necessary(new_rings.front(), original_ring, manager,
                                     new_rings);
    } else {
      // The new ring is a child of original ring
      // Check the
      assign_as_child(new_rings.front(), original_ring, manager);
      reassign_children_if_necessary(new_rings.front(), original_ring->parent,
                                     manager, new_rings);
    }
    return;
  }

  // Now we want to sort rings from the largest in absolute area to the smallest
  // as we will assign the rings with the largest areas first
  std::stable_sort(new_rings.begin(), new_rings.end(),
                   [](ring_ptr<T> const& r1, ring_ptr<T> const& r2) {
                     return std::fabs(r1->area()) > std::fabs(r2->area());
                   });

  for (auto r_itr = new_rings.begin(); r_itr != new_rings.end(); ++r_itr) {
    double new_ring_area = (*r_itr)->area();
    bool new_positive = new_ring_area > 0.0;
    bool same_orientation = new_positive == original_positive;
    bool found = false;
    // First lets check the trees of any new_rings that might have
    // been assigned as siblings to the original ring.
    for (auto s_itr = new_rings.begin(); s_itr != r_itr; ++s_itr) {
      if ((*s_itr)->parent != original_ring->parent) {
        continue;
      }
      if (same_orientation) {
        for (auto s_child : (*s_itr)->children) {
          if (s_child == nullptr) {
            continue;
          }
          if (find_parent_in_tree(*r_itr, s_child, manager)) {
            reassign_children_if_necessary(*r_itr, original_ring, manager,
                                           new_rings);
            found = true;
            break;
          }
        }
      } else {
        if (find_parent_in_tree(*r_itr, *s_itr, manager)) {
          reassign_children_if_necessary(*r_itr, original_ring->parent, manager,
                                         new_rings);
          found = true;
        }
      }
      if (found) {
        break;
      }
    }

    if (found) {
      continue;
    }

    // Next lets check the tree of the original_ring
    if (same_orientation) {
      for (auto o_child : original_ring->children) {
        if (o_child == nullptr) {
          continue;
        }
        if (find_parent_in_tree(*r_itr, o_child, manager)) {
          reassign_children_if_necessary(*r_itr, original_ring, manager,
                                         new_rings);
          found = true;
          break;
        }
      }
      if (!found) {
        // If we didn't find any parent and the same orientation
        // then it must be a sibling of the original ring
        assign_as_child(*r_itr, original_ring->parent, manager);
        reassign_children_if_necessary(*r_itr, original_ring, manager,
                                       new_rings);
      }
    } else {
      if (find_parent_in_tree(*r_itr, original_ring, manager)) {
        reassign_children_if_necessary(*r_itr, original_ring->parent, manager,
                                       new_rings);
      } else {
        throw std::runtime_error("Unable to find a proper parent ring");
      }
    }
  }
}

template <typename T>
bool correct_ring_self_intersections(ring_manager<T>& manager, ring_ptr<T> r,
                                     bool correct_tree) {
  if (r->corrected || !r->points) {
    return false;
  }

  ring_vector<T> new_rings;

  find_and_correct_repeated_points(r, manager, new_rings);

  if (correct_tree) {
    assign_new_ring_parents(manager, r, new_rings);
  }

  r->corrected = true;
  return true;
}

template <typename T>
void process_single_intersection(
    std::unordered_multimap<ring_ptr<T>, point_ptr_pair<T>>& connection_map,
    point_ptr<T> op_j, point_ptr<T> op_k, ring_manager<T>& manager) {
  ring_ptr<T> ring_j = op_j->ring;
  ring_ptr<T> ring_k = op_k->ring;
  if (ring_j == ring_k) {
    return;
  }

  if (!ring_j->is_hole() && !ring_k->is_hole()) {
    // Both are not holes, return nothing to do.
    return;
  }

  ring_ptr<T> ring_origin;
  ring_ptr<T> ring_search;
  ring_ptr<T> ring_parent;
  point_ptr<T> op_origin_1;
  point_ptr<T> op_origin_2;
  if (!ring_j->is_hole()) {
    ring_origin = ring_j;
    ring_parent = ring_origin;
    ring_search = ring_k;
    op_origin_1 = op_j;
    op_origin_2 = op_k;
  } else if (!ring_k->is_hole()) {
    ring_origin = ring_k;
    ring_parent = ring_origin;
    ring_search = ring_j;
    op_origin_1 = op_k;
    op_origin_2 = op_j;

  } else {
    // both are holes
    // Order doesn't matter
    ring_origin = ring_j;
    ring_parent = ring_origin->parent;
    ring_search = ring_k;
    op_origin_1 = op_j;
    op_origin_2 = op_k;
  }
  if (ring_parent != ring_search->parent) {
    // The two holes do not have the same parent, do not add them
    // simply return!
    return;
  }
  bool found = false;
  std::list<std::pair<ring_ptr<T>, point_ptr_pair<T>>> iList;
  {
    auto range = connection_map.equal_range(ring_search);
    // Check for direct connection
    for (auto& it = range.first; it != range.second;) {
      if (!it->second.op1->ring) {
        it = connection_map.erase(it);
        continue;
      }
      if (!it->second.op2->ring) {
        it = connection_map.erase(it);
        continue;
      }
      ring_ptr<T> it_ring2 = it->second.op2->ring;
      if (it_ring2 == ring_origin) {
        found = true;
        if (*op_origin_1 != *(it->second.op2)) {
          iList.emplace_back(ring_search, it->second);
          break;
        }
      }
      ++it;
    }
  }
  if (iList.empty()) {
    auto range = connection_map.equal_range(ring_search);
    std::set<ring_ptr<T>> visited;
    visited.insert(ring_search);
    // Check for connection through chain of other intersections
    for (auto& it = range.first;
         it != range.second && it != connection_map.end() &&
         it->first == ring_search;
         ++it) {
      ring_ptr<T> it_ring = it->second.op2->ring;
      if (it_ring != ring_search && *op_origin_2 != *it->second.op2 &&
          it_ring != nullptr &&
          (ring_parent == it_ring || ring_parent == it_ring->parent) &&
          !value_is_zero(it_ring->area()) &&
          find_intersect_loop(connection_map, iList, ring_parent, ring_origin,
                              it_ring, visited, op_origin_2, it->second.op2,
                              manager)) {
        found = true;
        iList.emplace_front(ring_search, it->second);
        break;
      }
    }
  }
  if (!found) {
    point_ptr_pair<T> intPt_origin = {op_origin_1, op_origin_2};
    point_ptr_pair<T> intPt_search = {op_origin_2, op_origin_1};
    connection_map.emplace(ring_origin, std::move(intPt_origin));
    connection_map.emplace(ring_search, std::move(intPt_search));
    return;
  }

  if (iList.empty()) {
    // The situation where both origin and search are holes might have a missing
    // search condition, we must check if a new pair must be added.
    bool missing = true;
    auto rng = connection_map.equal_range(ring_origin);
    // Check for direct connection
    for (auto& it = rng.first; it != rng.second; ++it) {
      ring_ptr<T> it_ring2 = it->second.op2->ring;
      if (it_ring2 == ring_search) {
        missing = false;
      }
    }
    if (missing) {
      point_ptr_pair<T> intPt_origin = {op_origin_1, op_origin_2};
      connection_map.emplace(ring_origin, std::move(intPt_origin));
    }
    return;
  }
  if (ring_origin->is_hole()) {
    for (auto& iRing : iList) {
      ring_ptr<T> ring_itr = iRing.first;
      if (!ring_itr->is_hole()) {
        // Make the hole the origin!
        point_ptr<T> op1 = op_origin_1;
        op_origin_1 = iRing.second.op1;
        iRing.second.op1 = op1;
        point_ptr<T> op2 = op_origin_2;
        op_origin_2 = iRing.second.op2;
        iRing.second.op2 = op2;
        iRing.first = ring_origin;
        ring_origin = ring_itr;
        ring_parent = ring_origin;
        break;
      }
    }
  }
  bool origin_is_hole = ring_origin->is_hole();

  // Switch
  point_ptr<T> op_origin_1_next = op_origin_1->next;
  point_ptr<T> op_origin_2_next = op_origin_2->next;
  op_origin_1->next = op_origin_2_next;
  op_origin_2->next = op_origin_1_next;
  op_origin_1_next->prev = op_origin_2;
  op_origin_2_next->prev = op_origin_1;

  for (auto& iRing : iList) {
    point_ptr<T> op_search_1 = iRing.second.op1;
    point_ptr<T> op_search_2 = iRing.second.op2;
    point_ptr<T> op_search_1_next = op_search_1->next;
    point_ptr<T> op_search_2_next = op_search_2->next;
    op_search_1->next = op_search_2_next;
    op_search_2->next = op_search_1_next;
    op_search_1_next->prev = op_search_2;
    op_search_2_next->prev = op_search_1;
  }

  ring_ptr<T> ring_new = create_new_ring(manager);
  ring_origin->corrected = false;
  std::size_t size_1 = 0;
  std::size_t size_2 = 0;
  mapbox::geometry::box<T> box1({0, 0}, {0, 0});
  mapbox::geometry::box<T> box2({0, 0}, {0, 0});
  double area_1 = area_from_point(op_origin_1, size_1, box1);
  double area_2 = area_from_point(op_origin_2, size_2, box2);
  if (origin_is_hole && ((area_1 < 0.0))) {
    ring_origin->points = op_origin_1;
    ring_origin->set_stats(area_1, size_1, box1);
    ring_new->points = op_origin_2;
    ring_new->set_stats(area_2, size_2, box2);
  } else {
    ring_origin->points = op_origin_2;
    ring_origin->set_stats(area_2, size_2, box2);
    ring_new->points = op_origin_1;
    ring_new->set_stats(area_1, size_1, box1);
  }

  update_points_ring(ring_origin);
  update_points_ring(ring_new);

  ring_origin->bottom_point = nullptr;

  for (auto& iRing : iList) {
    ring_ptr<T> ring_itr = iRing.first;
    ring_itr->bottom_point = nullptr;
    if (origin_is_hole) {
      ring1_replaces_ring2(ring_origin, ring_itr, manager);
    } else {
      ring1_replaces_ring2(ring_origin->parent, ring_itr, manager);
    }
  }
  if (origin_is_hole) {
    assign_as_child(ring_new, ring_origin, manager);
    // The parent ring in this situation might need to give up children
    // to the new ring.
    for (auto c : ring_parent->children) {
      if (c == nullptr) {
        continue;
      }
      if (poly2_contains_poly1(c, ring_new)) {
        reassign_as_child(c, ring_new, manager);
      }
    }
  } else {
    // The new ring and the origin ring need to be siblings
    // however some children ring from the ring origin might
    // need to be re-assigned to the new ring
    assign_as_sibling(ring_new, ring_origin, manager);
    for (auto c : ring_origin->children) {
      if (c == nullptr) {
        continue;
      }
      if (poly2_contains_poly1(c, ring_new)) {
        reassign_as_child(c, ring_new, manager);
      }
    }
  }

  std::list<std::pair<ring_ptr<T>, point_ptr_pair<T>>> move_list;

  for (auto& iRing : iList) {
    auto range_itr = connection_map.equal_range(iRing.first);
    if (range_itr.first != range_itr.second) {
      for (auto& it = range_itr.first; it != range_itr.second; ++it) {
        ring_ptr<T> it_ring = it->second.op1->ring;
        ring_ptr<T> it_ring2 = it->second.op2->ring;
        if (it_ring == nullptr || it_ring2 == nullptr || it_ring == it_ring2) {
          continue;
        }
        if (it_ring->is_hole() || it_ring2->is_hole()) {
          move_list.emplace_back(it_ring, it->second);
        }
      }
      connection_map.erase(iRing.first);
    }
  }

  auto range_itr = connection_map.equal_range(ring_origin);
  for (auto& it = range_itr.first; it != range_itr.second;) {
    ring_ptr<T> it_ring = it->second.op1->ring;
    ring_ptr<T> it_ring2 = it->second.op2->ring;
    if (it_ring == nullptr || it_ring2 == nullptr || it_ring == it_ring2) {
      it = connection_map.erase(it);
      continue;
    }
    if (it_ring != ring_origin) {
      if (it_ring->is_hole() || it_ring2->is_hole()) {
        move_list.emplace_back(it_ring, it->second);
      }
      it = connection_map.erase(it);
    } else {
      if (it_ring->is_hole() || it_ring2->is_hole()) {
        ++it;
      } else {
        it = connection_map.erase(it);
      }
    }
  }

  if (!move_list.empty()) {
    connection_map.insert(move_list.begin(), move_list.end());
  }

  return;
}

template <typename T>
void correct_chained_repeats(
    ring_manager<T>& manager,
    std::unordered_multimap<ring_ptr<T>, point_ptr_pair<T>>& connection_map,
    point_vector_itr<T> const& begin, point_vector_itr<T> const& end) {
  for (auto itr1 = begin; itr1 != end; ++itr1) {
    if ((*itr1)->ring == nullptr) {
      continue;
    }
    for (auto itr2 = std::next(itr1); itr2 != end; ++itr2) {
      if ((*itr2)->ring == nullptr) {
        continue;
      }
      process_single_intersection(connection_map, *itr1, *itr2, manager);
    }
  }
}

template <typename T>
void correct_chained_rings(ring_manager<T>& manager) {
  if (manager.all_points.size() < 2) {
    return;
  }
  // Setup connection map which is a map of rings and their
  // connection point pairs with other rings.
  std::unordered_multimap<ring_ptr<T>, point_ptr_pair<T>> connection_map;
  connection_map.reserve(manager.rings.size());

  // Now lets find and process any points
  // that overlap -- we should have solved
  // all situations where these points
  // would be self intersections of a ring with
  // earlier processing so this should just be
  // points where different rings are touching.
  std::size_t count = 0;
  auto prev_itr = manager.all_points.begin();
  auto itr = std::next(prev_itr);
  while (itr != manager.all_points.end()) {
    if (*(*prev_itr) == *(*(itr))) {
      ++count;
      ++prev_itr;
      ++itr;
      if (itr != manager.all_points.end()) {
        continue;
      } else {
        ++prev_itr;
      }
    } else {
      ++prev_itr;
      ++itr;
    }
    if (count == 0) {
      continue;
    }
    auto first = prev_itr;
    std::advance(first, -(static_cast<int>(count) + 1));
    correct_chained_repeats(manager, connection_map, first, prev_itr);
    count = 0;
  }
}

template <typename T>
ring_vector<T> sort_rings_largest_to_smallest(ring_manager<T>& manager) {
  ring_vector<T> sorted_rings;
  sorted_rings.reserve(manager.rings.size());
  for (auto& r : manager.rings) {
    sorted_rings.push_back(&r);
  }
  std::stable_sort(sorted_rings.begin(), sorted_rings.end(),
                   [](ring_ptr<T> const& r1, ring_ptr<T> const& r2) {
                     if (!r1->points || !r2->points) {
                       return r1->points != nullptr;
                     }
                     return std::fabs(r1->area()) > std::fabs(r2->area());
                   });
  return sorted_rings;
}

template <typename T>
ring_vector<T> sort_rings_smallest_to_largest(ring_manager<T>& manager) {
  ring_vector<T> sorted_rings;
  sorted_rings.reserve(manager.rings.size());
  for (auto& r : manager.rings) {
    sorted_rings.push_back(&r);
  }
  std::stable_sort(sorted_rings.begin(), sorted_rings.end(),
                   [](ring_ptr<T> const& r1, ring_ptr<T> const& r2) {
                     if (!r1->points || !r2->points) {
                       return r1->points != nullptr;
                     }
                     return std::fabs(r1->area()) < std::fabs(r2->area());
                   });
  return sorted_rings;
}

template <typename T>
struct collinear_path {
  // Collinear edges are in opposite directions
  // such that start_1 is at the same position
  // of end_2 and vise versa. Start to end is
  // always forward (next) on path.
  point_ptr<T> start_1;
  point_ptr<T> end_1;
  point_ptr<T> start_2;
  point_ptr<T> end_2;
};

template <typename T>
struct collinear_result {
  point_ptr<T> pt1;
  point_ptr<T> pt2;
};

template <typename T>
collinear_result<T> fix_collinear_path(collinear_path<T>& path) {
  // Left and right are just the opposite ends of the
  // collinear path, they may not be actually left
  // and right of each other.
  // The left end is start_1 and end_2
  // The right end is start_2 and end_1

  // NOTE spike detection is checking that the
  // pointers are the same values, not position!
  // additionally duplicate points we will treat
  // if they are a spike left.
  bool spike_left = (path.start_1 == path.end_2);
  bool spike_right = (path.start_2 == path.end_1);

  if (spike_left && spike_right) {
    // If both ends are spikes we should simply
    // delete all the points. (they should be in a loop)
    point_ptr<T> itr = path.start_1;
    while (itr != nullptr) {
      itr->prev->next = nullptr;
      itr->prev = nullptr;
      itr->ring = nullptr;
      itr = itr->next;
    }
    return {nullptr, nullptr};
  } else if (spike_left) {
    point_ptr<T> prev = path.start_2->prev;
    point_ptr<T> itr = path.start_2;
    while (itr != path.end_1) {
      itr->prev->next = nullptr;
      itr->prev = nullptr;
      itr->ring = nullptr;
      itr = itr->next;
    }
    prev->next = path.end_1;
    path.end_1->prev = prev;
    return {path.end_1, nullptr};
  } else if (spike_right) {
    point_ptr<T> prev = path.start_1->prev;
    point_ptr<T> itr = path.start_1;
    while (itr != path.end_2) {
      itr->prev->next = nullptr;
      itr->prev = nullptr;
      itr->ring = nullptr;
      itr = itr->next;
    }
    prev->next = path.end_2;
    path.end_2->prev = prev;
    return {path.end_2, nullptr};
  } else {
    point_ptr<T> prev_1 = path.start_1->prev;
    point_ptr<T> prev_2 = path.start_2->prev;
    point_ptr<T> itr = path.start_1;
    do {
      itr->prev->next = nullptr;
      itr->prev = nullptr;
      itr->ring = nullptr;
      itr = itr->next;
    } while (itr != path.end_1 && itr != nullptr);
    itr = path.start_2;
    do {
      itr->prev->next = nullptr;
      itr->prev = nullptr;
      itr->ring = nullptr;
      itr = itr->next;
    } while (itr != path.end_2 && itr != nullptr);
    if (path.start_1 == path.end_1 && path.start_2 == path.end_2) {
      return {nullptr, nullptr};
    } else if (path.start_1 == path.end_1) {
      prev_2->next = path.end_2;
      path.end_2->prev = prev_2;
      return {path.end_2, nullptr};
    } else if (path.start_2 == path.end_2) {
      prev_1->next = path.end_1;
      path.end_1->prev = prev_1;
      return {path.end_1, nullptr};
    } else {
      prev_1->next = path.end_2;
      path.end_2->prev = prev_1;
      prev_2->next = path.end_1;
      path.end_1->prev = prev_2;
      return {path.end_1, path.end_2};
    }
  }
}

template <typename T>
collinear_path<T> find_start_and_end_of_collinear_edges(point_ptr<T> pt_a,
                                                        point_ptr<T> pt_b) {
  // Search backward on A, forwards on B first
  bool same_ring = pt_a->ring == pt_b->ring;
  point_ptr<T> back = pt_a;
  point_ptr<T> forward = pt_b;
  bool first = true;
  do {
    while (*(back->prev) == *back && back != forward) {
      back = back->prev;
      if (back == pt_a) {
        break;
      }
    }
    if (back == forward) {
      back = back->prev;
      forward = forward->next;
      break;
    }
    while (*(forward->next) == *forward && back != forward) {
      forward = forward->next;
      if (forward == pt_b) {
        break;
      }
    }
    if (!first && (back == pt_a || forward == pt_b)) {
      break;
    }
    if (back == forward) {
      back = back->prev;
      forward = forward->next;
      break;
    }
    back = back->prev;
    forward = forward->next;
    first = false;
  } while (*back == *forward);
  point_ptr<T> start_a = back->next;
  // If there are repeated points at the diverge we want to select
  // only the first of those repeated points.
  while (!same_ring && *start_a == *start_a->next && start_a != pt_a) {
    start_a = start_a->next;
  }
  point_ptr<T> end_b = forward->prev;
  while (!same_ring && *end_b == *end_b->prev && end_b != pt_b) {
    end_b = end_b->prev;
  }
  // Search backward on B, forwards on A next
  back = pt_b;
  forward = pt_a;
  first = true;
  do {
    while (*(back->prev) == *back && back != forward) {
      back = back->prev;
      if (back == pt_b) {
        break;
      }
    }
    if (back == forward) {
      back = back->prev;
      forward = forward->next;
      break;
    }
    while (*(forward->next) == *forward && back != forward) {
      forward = forward->next;
      if (forward == pt_a) {
        break;
      }
    }
    if (!first && (back == pt_b || forward == pt_a)) {
      break;
    }
    if (back == forward || (!first && (back == end_b || forward == start_a))) {
      back = back->prev;
      forward = forward->next;
      break;
    }
    back = back->prev;
    forward = forward->next;
    first = false;
  } while (*back == *forward);
  point_ptr<T> start_b = back->next;
  while (!same_ring && *start_b == *start_b->next && start_b != pt_b) {
    start_b = start_b->next;
  }
  point_ptr<T> end_a = forward->prev;
  while (!same_ring && *end_a == *end_a->prev && end_a != pt_a) {
    end_a = end_a->prev;
  }
  return {start_a, end_a, start_b, end_b};
}

template <typename T>
bool has_collinear_edge(point_ptr<T> pt_a, point_ptr<T> pt_b) {
  // It is assumed pt_a and pt_b are at the same location.
  return (*pt_a->next == *pt_b->prev || *pt_b->next == *pt_a->prev);
}

template <typename T>
void process_collinear_edges_same_ring(point_ptr<T> pt_a, point_ptr<T> pt_b,
                                       ring_manager<T>& manager) {
  ring_ptr<T> original_ring = pt_a->ring;
  // As they are the same ring that are forming a collinear edge
  // we should expect the creation of two different rings.
  auto path = find_start_and_end_of_collinear_edges(pt_a, pt_b);
  auto results = fix_collinear_path(path);
  if (results.pt1 == nullptr) {
    // If pt1 is a nullptr, both values
    // are nullptrs. This mean the ring was
    // removed during this processing.
    remove_ring(original_ring, manager, false);
  } else if (results.pt2 == nullptr) {
    // If a single point is only returned, we simply removed a spike.
    // In this case, we don't need to worry about parent or children
    // and we simply need to set the points and clear the area
    original_ring->points = results.pt1;
    original_ring->recalculate_stats();
  } else {
    // If we have two seperate points, the ring has split into
    // two different rings.
    ring_ptr<T> ring_new = create_new_ring(manager);
    ring_new->points = results.pt2;
    ring_new->recalculate_stats();
    update_points_ring(ring_new);
    original_ring->points = results.pt1;
    original_ring->recalculate_stats();
  }
}

template <typename T>
void process_collinear_edges_different_rings(point_ptr<T> pt_a,
                                             point_ptr<T> pt_b,
                                             ring_manager<T>& manager) {
  ring_ptr<T> ring_a = pt_a->ring;
  ring_ptr<T> ring_b = pt_b->ring;
  bool ring_a_larger = std::fabs(ring_a->area()) > std::fabs(ring_b->area());
  auto path = find_start_and_end_of_collinear_edges(pt_a, pt_b);
  // This should result in two rings becoming one.
  auto results = fix_collinear_path(path);
  if (results.pt1 == nullptr) {
    remove_ring(ring_a, manager, false);
    remove_ring(ring_b, manager, false);
    return;
  }
  // Rings should merge into a single ring of the same orientation.
  // Therefore, we we will need to replace one ring with the other
  ring_ptr<T> merged_ring = ring_a_larger ? ring_a : ring_b;
  ring_ptr<T> deleted_ring = ring_a_larger ? ring_b : ring_a;

  merged_ring->points = results.pt1;
  update_points_ring(merged_ring);
  merged_ring->recalculate_stats();
  if (merged_ring->size() < 3) {
    remove_ring_and_points(merged_ring, manager, false);
  }
  remove_ring(deleted_ring, manager, false);
}

template <typename T>
bool remove_duplicate_points(point_ptr<T> pt_a, point_ptr<T> pt_b,
                             ring_manager<T>& manager) {
  if (pt_a->ring == pt_b->ring) {
    if (pt_a->next == pt_b) {
      pt_a->next = pt_b->next;
      pt_a->next->prev = pt_a;
      pt_b->next = nullptr;
      pt_b->prev = nullptr;
      pt_b->ring = nullptr;
      if (pt_a->ring->points == pt_b) {
        pt_a->ring->points = pt_a;
      }
      return true;
    } else if (pt_b->next == pt_a) {
      pt_a->prev = pt_b->prev;
      pt_a->prev->next = pt_a;
      pt_b->next = nullptr;
      pt_b->prev = nullptr;
      pt_b->ring = nullptr;
      if (pt_a->ring->points == pt_b) {
        pt_a->ring->points = pt_a;
      }
      return true;
    }
  }
  while (*pt_a->next == *pt_a && pt_a->next != pt_a) {
    point_ptr<T> remove = pt_a->next;
    pt_a->next = remove->next;
    pt_a->next->prev = pt_a;
    remove->next = nullptr;
    remove->prev = nullptr;
    remove->ring = nullptr;
    if (pt_a->ring->points == remove) {
      pt_a->ring->points = pt_a;
    }
  }
  while (*pt_a->prev == *pt_a && pt_a->prev != pt_a) {
    point_ptr<T> remove = pt_a->prev;
    pt_a->prev = remove->prev;
    pt_a->prev->next = pt_a;
    remove->next = nullptr;
    remove->prev = nullptr;
    remove->ring = nullptr;
    if (pt_a->ring->points == remove) {
      pt_a->ring->points = pt_a;
    }
  }
  if (pt_a->next == pt_a) {
    remove_ring_and_points(pt_a->ring, manager, false);
    return true;
  }
  if (pt_b->ring == nullptr) {
    return true;
  }
  while (*pt_b->next == *pt_b && pt_b->next != pt_b) {
    point_ptr<T> remove = pt_b->next;
    pt_b->next = remove->next;
    pt_b->next->prev = pt_b;
    remove->next = nullptr;
    remove->prev = nullptr;
    remove->ring = nullptr;
    if (pt_b->ring->points == remove) {
      pt_b->ring->points = pt_b;
    }
  }
  while (*pt_b->prev == *pt_b && pt_b->prev != pt_b) {
    point_ptr<T> remove = pt_b->prev;
    pt_b->prev = remove->prev;
    pt_b->prev->next = pt_b;
    remove->next = nullptr;
    remove->prev = nullptr;
    remove->ring = nullptr;
    if (pt_b->ring->points == remove) {
      pt_b->ring->points = pt_b;
    }
  }
  if (pt_b->next == pt_b) {
    remove_ring_and_points(pt_b->ring, manager, false);
    return true;
  }
  if (pt_a->ring == nullptr) {
    return true;
  }
  return false;
}

template <typename T>
bool process_collinear_edges(point_ptr<T> pt_a, point_ptr<T> pt_b,
                             ring_manager<T>& manager) {
  // Neither point assigned to a ring (deleted points)
  if (!pt_a->ring || !pt_b->ring) {
    return false;
  }

  if (remove_duplicate_points(pt_a, pt_b, manager)) {
    return true;
  }

  if (!has_collinear_edge(pt_a, pt_b)) {
    if (pt_a->ring == pt_b->ring) {
      correct_self_intersection(pt_a, pt_b, manager);
      return true;
    }
    return false;
  }

  if (pt_a->ring == pt_b->ring) {
    process_collinear_edges_same_ring(pt_a, pt_b, manager);
  } else {
    process_collinear_edges_different_rings(pt_a, pt_b, manager);
  }
  return true;
}

template <typename T>
void correct_collinear_repeats(ring_manager<T>& manager,
                               point_vector_itr<T> const& begin,
                               point_vector_itr<T> const& end) {
  for (auto itr1 = begin; itr1 != end; ++itr1) {
    if ((*itr1)->ring == nullptr) {
      continue;
    }
    for (auto itr2 = begin; itr2 != end;) {
      if ((*itr1)->ring == nullptr) {
        break;
      }
      if ((*itr2)->ring == nullptr || *itr2 == *itr1) {
        ++itr2;
        continue;
      }
      if (process_collinear_edges(*itr1, *itr2, manager)) {
        itr2 = begin;
      } else {
        ++itr2;
      }
    }
  }
}

template <typename T>
void correct_collinear_edges(ring_manager<T>& manager) {
  if (manager.all_points.size() < 2) {
    return;
  }
  std::size_t count = 0;
  auto prev_itr = manager.all_points.begin();
  auto itr = std::next(prev_itr);
  while (itr != manager.all_points.end()) {
    if (*(*prev_itr) == *(*(itr))) {
      ++count;
      ++prev_itr;
      ++itr;
      if (itr != manager.all_points.end()) {
        continue;
      } else {
        ++prev_itr;
      }
    } else {
      ++prev_itr;
      ++itr;
    }
    if (count == 0) {
      continue;
    }
    auto first = prev_itr;
    std::advance(first, -(static_cast<int>(count) + 1));
    correct_collinear_repeats(manager, first, prev_itr);
    count = 0;
  }
}

template <typename T>
void correct_tree(ring_manager<T>& manager) {
  // It is possible that vatti resulted in some parent child
  // relationships that are not quite right, therefore, we
  // need to just rebuild the entire tree of rings

  // First lets process the rings in order of size from largest
  // area to smallest, we know right away that no smaller ring could ever
  // contain a larger ring so we can use this to our advantage
  // as we iterate over the rings.
  using rev_itr = typename ring_vector<T>::reverse_iterator;
  ring_vector<T> sorted_rings = sort_rings_largest_to_smallest(manager);
  for (auto itr = sorted_rings.begin(); itr != sorted_rings.end(); ++itr) {
    if ((*itr)->points == nullptr) {
      continue;
    }
    if ((*itr)->size() < 3 || value_is_zero((*itr)->area())) {
      remove_ring_and_points(*itr, manager, false);
      continue;
    }
    (*itr)->corrected = true;
    bool found = false;
    // Search in reverse from the current iterator back to the begining
    // to see if any of those rings might be its parent.
    for (auto r_itr = rev_itr(itr); r_itr != sorted_rings.rend(); ++r_itr) {
      // If orientations are not different, this can't be its parent.
      if ((*r_itr)->is_hole() == (*itr)->is_hole()) {
        continue;
      }
      if (poly2_contains_poly1(*itr, *r_itr)) {
        reassign_as_child(*itr, *r_itr, manager);
        found = true;
        break;
      }
    }
    if (!found) {
      if ((*itr)->is_hole()) {
        throw std::runtime_error("Could not properly place hole to a parent.");
      } else {
        // Assign to base of tree by passing nullptr
        reassign_as_child(*itr, static_cast<ring_ptr<T>>(nullptr), manager);
      }
    }
  }
}

template <typename T>
bool correct_self_intersections(ring_manager<T>& manager, bool correct_tree) {
  bool fixed_intersections = false;
  auto sorted_rings = sort_rings_smallest_to_largest(manager);
  for (auto const& r : sorted_rings) {
    if (correct_ring_self_intersections(manager, r, correct_tree)) {
      fixed_intersections = true;
    }
  }
  return fixed_intersections;
}

template <typename T>
void correct_topology(ring_manager<T>& manager) {
  // Sort all the points, this will be used for the locating of chained rings
  // and the collinear edges and only needs to be done once.
  std::stable_sort(manager.all_points.begin(), manager.all_points.end(),
                   point_ptr_cmp<T>());

  // Initially the orientations of the rings
  // could be incorrect, we need to adjust them
  correct_orientations(manager);

  // We should only have to fix collinear edges once.
  // During this we also correct self intersections
  correct_collinear_edges(manager);

  correct_self_intersections(manager, false);

  correct_tree(manager);

  bool fixed_intersections = true;
  while (fixed_intersections) {
    correct_chained_rings(manager);
    fixed_intersections = correct_self_intersections(manager, true);
  }
}
}  // namespace wagyu
}  // namespace geometry
}  // namespace mapbox
