// Copyright (c) 2015  Tel-Aviv University (Israel).
// All rights reserved.
//
// This file is part of CGAL (www.cgal.org).
//
// $URL: https://github.com/CGAL/cgal/blob/v6.1/Minkowski_sum_2/include/CGAL/Minkowski_sum_2/Minkowski_sum_by_reduced_convolution_2.h $
// $Id: include/CGAL/Minkowski_sum_2/Minkowski_sum_by_reduced_convolution_2.h b26b07a1242 $
// SPDX-License-Identifier: GPL-3.0-or-later OR LicenseRef-Commercial
//
// Author(s): Sebastian Morr    <sebastian@morr.cc>

#ifndef CGAL_MINKOWSKI_SUM_BY_REDUCED_CONVOLUTION_2_H
#define CGAL_MINKOWSKI_SUM_BY_REDUCED_CONVOLUTION_2_H

#include <CGAL/license/Minkowski_sum_2.h>


#include <CGAL/basic.h>
#include <CGAL/Arrangement_with_history_2.h>
#include <CGAL/Arr_segment_traits_2.h>

#include <CGAL/Minkowski_sum_2/AABB_collision_detector_2.h>
#include <boost/functional/hash.hpp>

#include <queue>
#include <unordered_set>

namespace CGAL {

// This algorithm was first described by Evan Behar and Jyh-Ming Lien in "Fast
// and Robust 2D Minkowski Sum Using Reduced Convolution", IROS 2011.
// This implementation is based on Alon Baram's 2013 master's thesis "Polygonal
// Minkowski Sums via Convolution: Theory and Practice" at Tel-Aviv University.
template <typename Kernel_, typename Container_>
class Minkowski_sum_by_reduced_convolution_2 {
private:
  typedef Kernel_ Kernel;
  typedef Container_ Container;

  // Basic types:
  typedef CGAL::Polygon_2<Kernel, Container> Polygon_2;
  typedef CGAL::Polygon_with_holes_2<Kernel, Container> Polygon_with_holes_2;
  typedef typename Kernel::Point_2                      Point_2;
  typedef typename Kernel::Vector_2                     Vector_2;
  typedef typename Kernel::Direction_2                  Direction_2;
  typedef typename Kernel::Triangle_2                   Triangle_2;
  typedef typename Kernel::FT                           FT;

  // Segment-related types:
  typedef Arr_segment_traits_2<Kernel>                  Traits_2;
  typedef typename Traits_2::X_monotone_curve_2         Segment_2;
  typedef std::list<Segment_2>                          Segment_list;
  typedef Arr_default_dcel<Traits_2>                    Dcel;
  typedef std::pair<int, int>                           State;

  // Arrangement-related types:
  typedef Arrangement_with_history_2<Traits_2, Dcel>    Arrangement_history_2;
  typedef typename Arrangement_history_2::Halfedge_const_handle
    Halfedge_const_handle;
  typedef typename Arrangement_history_2::Face_const_handle
    Face_const_handle;
  typedef typename Arrangement_history_2::Ccb_halfedge_const_circulator
    Ccb_halfedge_const_circulator;
  typedef typename Arrangement_history_2::Inner_ccb_const_iterator
    Inner_ccb_const_iterator;

  // Function object types:
  typename Kernel::Construct_translated_point_2 f_add;
  typename Kernel::Construct_vector_2 f_vector;
  typename Kernel::Construct_direction_2 f_direction;
  typename Kernel::Orientation_2 f_orientation;
  typename Kernel::Compare_xy_2 f_compare_xy;
  typename Kernel::Counterclockwise_in_between_2 f_ccw_in_between;

public:
  //! \brief constructs.
  Minkowski_sum_by_reduced_convolution_2() {
    // Obtain kernel functors
    Kernel ker;
    f_add = ker.construct_translated_point_2_object();
    f_vector = ker.construct_vector_2_object();
    f_direction = ker.construct_direction_2_object();
    f_orientation = ker.orientation_2_object();
    f_compare_xy = ker.compare_xy_2_object();
    f_ccw_in_between = ker.counterclockwise_in_between_2_object();
  }

  //! \brief applies the Minkowski sum reduced-convolution operator.
  template <typename OutputIterator>
  void operator()(const Polygon_2& pgn1, const Polygon_2& pgn2,
                  Polygon_2& outer_boundary, OutputIterator holes) const {
    CGAL_precondition(pgn1.is_simple());
    CGAL_precondition(pgn2.is_simple());
    CGAL_precondition(pgn1.orientation() == COUNTERCLOCKWISE);
    CGAL_precondition(pgn2.orientation() == COUNTERCLOCKWISE);

    const Polygon_with_holes_2 pwh1(pgn1);
    const Polygon_with_holes_2 pwh2(pgn2);

    common_operator(pwh1, pwh2, outer_boundary, holes);
  }

  //! \brief applies the Minkowski sum reduced-convolution operator.
  template <typename OutputIterator>
  void operator()(const Polygon_with_holes_2& pgn1,
                  const Polygon_with_holes_2& pgn2,
                  Polygon_2& outer_boundary, OutputIterator holes) const
  { common_operator(pgn1, pgn2, outer_boundary, holes); }

  //! \brief applies the Minkowski sum reduced-convolution operator.
  template <typename OutputIterator>
  void operator()(const Polygon_2& pgn1, const Polygon_with_holes_2& pgn2,
                  Polygon_2& outer_boundary, OutputIterator holes) const {
    CGAL_precondition(pgn1.is_simple());
    CGAL_precondition(pgn1.orientation() == COUNTERCLOCKWISE);
    const Polygon_with_holes_2 pwh1(pgn1);
    common_operator(pwh1, pgn2, outer_boundary, holes);
  }

private:
  //! \brief applies the Minkowski sum reduced-convolution operator.
  template <typename OutputIterator>
  void common_operator(const Polygon_with_holes_2& pgn1,
                       const Polygon_with_holes_2& pgn2,
                       Polygon_2& outer_boundary, OutputIterator holes) const {
    // If the outer boundaries of both summands are empty the Minkowski sum is
    // the entire plane.
    if (pgn1.outer_boundary().is_empty() && pgn2.outer_boundary().is_empty())
      return;

    // Initialize collision detector. It operates on pgn2 and on the inversed
    // pgn1:
    const Polygon_with_holes_2 inversed_pgn1 =
      transform(Aff_transformation_2<Kernel>(SCALING, -1), pgn1);
    AABB_collision_detector_2<Kernel, Container>
      collision_detector(pgn2, inversed_pgn1);

    // Compute the reduced convolution (see section 4.1 of Alon's master's
    // thesis)
    Segment_list reduced_convolution;
    build_reduced_convolution(pgn1, pgn2, reduced_convolution);

    // Insert the segments into an arrangement
    Arrangement_history_2 arr;
    insert(arr, reduced_convolution.begin(), reduced_convolution.end());

    // Trace the outer loop and put it in 'outer_boundary'
    // If one of the summand does not have an outer boundary, then the Minkowski
    // sum does not have an outer boundary either.
    bool is_outer_boundary_empty = pgn1.outer_boundary().is_empty() ||
      pgn2.outer_boundary().is_empty();

    if (! is_outer_boundary_empty) get_outer_loop(arr, outer_boundary);

    // Check for each face whether it is a hole in the M-sum. If it is, add it
    // to 'holes'. See chapter 3 of of Alon's master's thesis.
    for (auto fit = arr.faces_begin(); fit != arr.faces_end(); ++fit) {
      // Check whether the face is on the M-sum's border.

      // The unbounded face cannot contribute to the Minkowski sum
      if (fit->is_unbounded()) continue;

      // If the face contains holes, it can't be on the Minkowski sum's border
      if (0 < fit->number_of_holes()) continue;

      // The face needs to be orientable
      if (! test_face_orientation(arr, fit)) continue;

      // When the reversed polygon 1, translated by a point inside of this
      // face, collides with polygon 2, this cannot be a hole
      Point_2 inner_point = get_point_in_face(fit);
      if (collision_detector.check_collision(inner_point)) continue;

      add_face(fit, holes);
    }
  }

  /*! \brief builds the reduced convolution for each pair of loops in the two
   * polygons-with-holes.
   */
  void build_reduced_convolution(const Polygon_with_holes_2& pgnwh1,
                                 const Polygon_with_holes_2& pgnwh2,
                                 Segment_list& reduced_convolution) const {
    for (std::size_t x = 0; x < 1+pgnwh1.number_of_holes(); ++x) {
      for (std::size_t y = 0; y < 1+pgnwh2.number_of_holes(); ++y) {
        if ((x != 0) && (y != 0)) continue;
        if (x == 0) {
          const auto& pgn1 = pgnwh1.outer_boundary();
          if (y == 0) {
            const auto& pgn2 = pgnwh2.outer_boundary();
            build_reduced_convolution(pgn1, pgn2, reduced_convolution);
          }
          else {
            auto it2 = pgnwh2.holes_begin();
            for (std::size_t count = 0; count < y-1; ++count) ++it2;
            build_reduced_convolution(pgn1, *it2, reduced_convolution);
          }
        }
        else {
          auto it1 = pgnwh1.holes_begin();
          for (std::size_t count = 0; count < x-1; ++count) ++it1;
          if (y == 0) {
            const auto& pgn2 = pgnwh2.outer_boundary();
            build_reduced_convolution(*it1, pgn2, reduced_convolution);
          }
          else {
            auto it2 = pgnwh2.holes_begin();
            for (std::size_t count = 0; count < y-1; ++count) ++it2;
            build_reduced_convolution(*it1, *it2, reduced_convolution);
          }
        }
      }
    }
  }

  /*! \brief builds the reduced convolution using a fiber grid approach. For
   * each starting vertex, try to add two outgoing next states. If a visited
   * vertex is reached, then do not explore further. This is a BFS-like
   * iteration beginning from each vertex in the first column of the fiber
   * grid.
   */
  void build_reduced_convolution(const Polygon_2& pgn1, const Polygon_2& pgn2,
                                 Segment_list& reduced_convolution) const {
    int n1 = static_cast<int>(pgn1.size());
    int n2 = static_cast<int>(pgn2.size());
    if ((n1 == 0) || (n2 == 0)) return;

    std::vector<Point_2> p1_vertices = vertices_of_polygon(pgn1);
    std::vector<Point_2> p2_vertices = vertices_of_polygon(pgn2);

    // Init the directions of both polygons
    std::vector<Direction_2> p1_dirs = directions_of_polygon(p1_vertices);
    std::vector<Direction_2> p2_dirs = directions_of_polygon(p2_vertices);

    // Contains states that were already visited
    std::unordered_set<State, boost::hash<State>> visited_states;

    // Init the queue with vertices from the first column
    std::queue<State> state_queue;
    for (int i = n1-1; i >= 0; --i) state_queue.push(State(i, 0));

    while (state_queue.size() > 0) {
      State curr_state = state_queue.front();
      state_queue.pop();

      int i1 = curr_state.first;
      int i2 = curr_state.second;

      // If this state was already visited, skip it
      if (visited_states.count(curr_state) > 0) continue;
      visited_states.insert(curr_state);

      int next_i1 = (i1+1) % n1;
      int next_i2 = (i2+1) % n2;
      int prev_i1 = (n1+i1-1) % n1;
      int prev_i2 = (n2+i2-1) % n2;

      // Try two transitions: From (i,j) to (i+1,j) and to (i,j+1). Add
      // the respective segments, if they are in the reduced convolution.
      for (int step_in_pgn1 = 0; step_in_pgn1 <= 1; ++step_in_pgn1) {
        int new_i1, new_i2;
        if (step_in_pgn1) {
          new_i1 = next_i1;
          new_i2 = i2;
        }
        else {
          new_i1 = i1;
          new_i2 = next_i2;
        }

        // If the segment's direction lies counterclockwise in between
        // the other polygon's vertex' ingoing and outgoing directions,
        // the segment belongs to the full convolution.
        bool belongs_to_convolution;
        if (step_in_pgn1) {
          belongs_to_convolution =
            f_ccw_in_between(p1_dirs[i1], p2_dirs[prev_i2], p2_dirs[i2]) ||
            p1_dirs[i1] == p2_dirs[i2];
        }
        else {
          belongs_to_convolution =
            f_ccw_in_between(p2_dirs[i2], p1_dirs[prev_i1], p1_dirs[i1]) ||
            p2_dirs[i2] == p1_dirs[prev_i1];
        }

        if (belongs_to_convolution) {
          state_queue.push(State(new_i1, new_i2));

          // Only edges added to convex vertices can be on the M-sum's boundary.
          // This filter only leaves the *reduced* convolution.
          bool convex;
          if (step_in_pgn1) {
            convex = is_convex(p2_vertices[prev_i2], p2_vertices[i2],
              p2_vertices[next_i2]);
          }
          else {
            convex = is_convex(p1_vertices[prev_i1], p1_vertices[i1],
              p1_vertices[next_i1]);
          }

          if (convex) {
            Point_2 start_point = get_point(i1, i2, p1_vertices, p2_vertices);
            Point_2 end_point = get_point(new_i1, new_i2, p1_vertices,
                                          p2_vertices);
            reduced_convolution.push_back(Segment_2(start_point, end_point));
          }
        }
      }
    }
  }

  // Returns a vector of the polygon's vertices, in case that Container
  // is std::list and we cannot use vertex(i).
  std::vector<Point_2> vertices_of_polygon(const Polygon_2& p) const {
    std::vector<Point_2> vertices;

    for (auto it = p.vertices_begin(); it != p.vertices_end(); it++)
      vertices.push_back(*it);
    return vertices;
  }

  // Returns a sorted list of the polygon's edges
  std::vector<Direction_2>
  directions_of_polygon(const std::vector<Point_2>& points) const {
    std::vector<Direction_2> directions;
    std::size_t n = points.size();

    for (std::size_t i = 0; i < n-1; ++i) {
      directions.push_back(f_direction(f_vector(points[i], points[i+1])));
    }
    directions.push_back(f_direction(f_vector(points[n-1], points[0])));

    return directions;
  }

  /*! \brief determines whether three vertices on the outer CCB of a face are
   * locally convex.
   */
  bool is_convex(const Point_2& prev, const Point_2& curr,
                 const Point_2& next) const
  { return f_orientation(prev, curr, next) == LEFT_TURN; }

  //! \brief obtains the point corresponding to a state (i,j).
  Point_2 get_point(int i1, int i2, const std::vector<Point_2>& pgn1,
                    const std::vector<Point_2>& pgn2) const
  { return f_add(pgn1[i1], Vector_2(Point_2(ORIGIN), pgn2[i2])); }

  //! \brief puts the outer loop of the arrangement in 'outer_boundary'
  void get_outer_loop(const Arrangement_history_2& arr,
                      Polygon_2& outer_boundary) const {
    Inner_ccb_const_iterator icit = arr.unbounded_face()->inner_ccbs_begin();
    Ccb_halfedge_const_circulator circ_start = *icit;
    Ccb_halfedge_const_circulator circ = circ_start;

    do outer_boundary.push_back(circ->source()->point());
    while (--circ != circ_start);
  }

  //! \brief determines whether the face orientation is consistent.
  bool test_face_orientation(const Arrangement_history_2& arr,
                             const Face_const_handle face) const {
    // The face needs to be orientable
    Ccb_halfedge_const_circulator start = face->outer_ccb();
    Ccb_halfedge_const_circulator circ = start;
    do if (!do_original_edges_have_same_direction(arr, circ)) return false;
    while (++circ != start);

    return true;
  }

  //! \brief adds a face to 'holes'.
  template <typename OutputIterator>
  void add_face(Face_const_handle face, OutputIterator holes) const {
    Polygon_2 pgn_hole;
    Ccb_halfedge_const_circulator start = face->outer_ccb();
    Ccb_halfedge_const_circulator circ = start;
    do pgn_hole.push_back(circ->source()->point());
    while (--circ != start);
    *holes = pgn_hole;
    ++holes;
  }

  /*! \brief checks whether the convolution's original edge(s) had the same
   * direction as the arrangement's half edge.
   */
  bool do_original_edges_have_same_direction(const Arrangement_history_2& arr,
                                             Halfedge_const_handle he) const {
    for (auto segment_itr = arr.originating_curves_begin(he);
         segment_itr != arr.originating_curves_end(he); ++segment_itr)
    {
      if (f_compare_xy(segment_itr->source(), segment_itr->target()) ==
          (Comparison_result)he->direction())
      {
        return false;
      }
    }

    return true;
  }

  //! \brief obtains a point in the face's interior by finding a diagonal
  Point_2 get_point_in_face(Face_const_handle face) const {
    Ccb_halfedge_const_circulator next = face->outer_ccb();
    Ccb_halfedge_const_circulator curr = next++;

    // Move over the face's vertices until a convex corner is encountered.
    // Observe that the outer ccb of a hole is clockwise oriented.
    while (! is_convex(curr->source()->point(),
                       curr->target()->point(),
                       next->target()->point())) {
      curr = next;
      ++next;
    }

    const auto& a = curr->source()->point();
    const auto& v = curr->target()->point();
    const auto& b = next->target()->point();
    Triangle_2 ear(a, v, b);
    FT min_distance = -1;
    const Point_2* min_q = nullptr;

    // Of the remaining vertices, find the one inside of the "ear" with minimal
    // distance to v:
    while (++next != curr) {
      const Point_2& q = next->target()->point();
      if (ear.has_on_bounded_side(q)) {
        FT distance = squared_distance(q, v);
        if ((min_q == 0) || (distance < min_distance)) {
          min_distance = distance;
          min_q = &q;
        }
      }
    }

    // If there was no vertex inside of the ear, return it's centroid.
    // Otherwise, return a point between v and min_q.
    return (min_q == 0) ? centroid(ear) : midpoint(v, *min_q);
  }

  //! \brief transforms a polygon with holes.
  template <typename Transformation>
  Polygon_with_holes_2 transform(const Transformation& t,
                                 const Polygon_with_holes_2& p) const {
    Polygon_with_holes_2 result(CGAL::transform(t, p.outer_boundary()));

    auto it = p.holes_begin();
    while (it != p.holes_end()) {
      Polygon_2 p2(it->vertices_begin(), it->vertices_end());
      result.add_hole(CGAL::transform(t, p2));
      ++it;
    }
    return result;
  }
};

} // namespace CGAL

#endif

