// Copyright (c) 2015 INRIA Sophia-Antipolis (France).
// All rights reserved.
//
// This file is part of CGAL (www.cgal.org).
//
// $URL$
// $Id$
// SPDX-License-Identifier: GPL-3.0-or-later OR LicenseRef-Commercial
//
//
// Author(s)     : Florent Lafarge, Simon Giraudot
//

#ifndef CGAL_STRUCTURE_POINT_SET_3_H
#define CGAL_STRUCTURE_POINT_SET_3_H

#include <CGAL/license/Point_set_processing_3.h>

#include <CGAL/disable_warnings.h>

#include <CGAL/property_map.h>
#include <CGAL/assertions.h>
#include <CGAL/intersections.h>

#include <CGAL/centroid.h>

#include <CGAL/Kd_tree.h>
#include <CGAL/Fuzzy_sphere.h>
#include <CGAL/Search_traits_d.h>
#include <CGAL/Search_traits_3.h>

#include <CGAL/Delaunay_triangulation_3.h>
#include <CGAL/Triangulation_vertex_base_with_info_3.h>

#include <CGAL/Named_function_parameters.h>
#include <CGAL/boost/graph/named_params_helper.h>

#include <boost/iterator/counting_iterator.hpp>

#include <iterator>
#include <list>
#include <limits>

namespace CGAL {

/*!
\ingroup PkgPointSetProcessing3Algorithms

\brief A 3D point set with structure information based on a set of
detected planes.

Given a point set in 3D space along with a set of fitted planes, this
class stores a simplified and structured version of the point
set. Each output point is assigned to one, two or more primitives
(depending whether it belongs to a planar section, an edge or a if it
is a vertex). The implementation follow \cgalCite{cgal:la-srpss-13}.

\tparam Kernel a model of `EfficientRANSACTraits` that must provide in
addition a function `Intersect_3 intersection_3_object() const` and a
functor `Intersect_3` with:
- `std::optional< std::variant< Traits::Plane_3, Traits::Line_3 > > operator()(typename Traits::Plane_3, typename Traits::Plane_3)`
- `std::optional< std::variant< Traits::Line_3, Traits::Point_3 > > operator()(typename Traits::Line_3, typename Traits::Plane_3)`

*/
template <typename Kernel>
class Point_set_with_structure
{
  typedef Point_set_with_structure<Kernel> Self;

  typedef typename Kernel::FT FT;
  typedef typename Kernel::Segment_3 Segment;
  typedef typename Kernel::Line_3 Line;
  typedef typename Kernel::Point_2 Point_2;

  enum Point_status { POINT, RESIDUS, PLANE, EDGE, CORNER, SKIPPED };

public:


  typedef typename Kernel::Point_3 Point;
  typedef typename Kernel::Vector_3 Vector;
  typedef typename Kernel::Plane_3 Plane;

  /// Tag classifying the coherence of a triplet of points with
  /// respect to an inferred surface
  enum Coherence_type
    {
      INCOHERENT = -1, ///< Incoherent (facet violates the underlying structure)
      FREEFORM = 0,    ///< Free-form coherent (facet is between 3 free-form points)
      VERTEX = 1,      ///< Structure coherent, facet adjacent to a vertex
      CREASE = 2,      ///< Structure coherent, facet adjacent to an edge
      PLANAR = 3       ///< Structure coherent, facet inside a planar section
    };

private:

  class My_point_property_map{
    const std::vector<Point>& points;
  public:
    typedef Point value_type;
    typedef const value_type& reference;
    typedef std::size_t key_type;
    typedef boost::lvalue_property_map_tag category;

    My_point_property_map (const std::vector<Point>& pts) : points (pts) {}

    reference operator[] (key_type k) const { return points[k]; }
    friend inline reference get (const My_point_property_map& ppmap, key_type i) { return ppmap[i]; }
  };

  struct Edge
  {
    std::array<std::size_t, 2> planes;
    std::vector<std::size_t> indices; // Points belonging to intersection
    Line support;
    bool active;

    Edge (std::size_t a, std::size_t b)
      : support (Point (FT(0.), FT(0.), FT(0.)),
                 Vector (FT(0.), FT(0.), FT(0.)))
      , active(true)
    { planes[0] = a; planes[1] = b; }
  };
  struct Corner
  {
    std::vector<std::size_t> planes;
    std::vector<std::size_t> edges;
    std::vector<Vector> directions;
    Point support;
    bool active;

    Corner (std::size_t p1, std::size_t p2, std::size_t p3,
            std::size_t e1, std::size_t e2, std::size_t e3)
    {
      planes.resize (3); planes[0] = p1; planes[1] = p2; planes[2] = p3;
      edges.resize (3); edges[0] = e1; edges[1] = e2; edges[2] = e3;
      active = true;
    }
  };


  std::vector<Point> m_points;
  std::vector<Vector> m_normals;
  std::vector<std::size_t> m_indices;
  std::vector<Point_status> m_status;

  std::vector<Plane> m_planes;
  std::vector<std::vector<std::size_t> > m_indices_of_assigned_points;
  std::vector<Edge> m_edges;
  std::vector<Corner> m_corners;

public:


  /*!
    Constructs a structured point set based on the input points and the
    associated shape detection object.

    \tparam PointRange is a model of `ConstRange`. The value type of
    its iterator is the key type of the named parameter `point_map`.
    \tparam PlaneRange is a model of `ConstRange`. The value type of
    its iterator is the key type of the named parameter `plane_map`.

    \param points input point range
    \param planes input plane range.
    \param epsilon size parameter.
    \param np a sequence of \ref bgl_namedparameters "Named Parameters" among the ones listed below:

    \cgalNamedParamsBegin
      \cgalParamNBegin{point_map}
        \cgalParamDescription{a property map associating points to the elements of the point set `points`}
        \cgalParamType{a model of `ReadablePropertyMap` whose key type is the value type
                       of the iterator of `PointRange` and whose value type is `geom_traits::Point_3`}
        \cgalParamDefault{`CGAL::Identity_property_map<geom_traits::Point_3>`}
      \cgalParamNEnd

      \cgalParamNBegin{normal_map}
        \cgalParamDescription{a property map associating normals to the elements of the point set `points`}
        \cgalParamType{a model of `ReadablePropertyMap` whose key type is the value type
                       of the iterator of `PointRange` and whose value type is `geom_traits::Vector_3`}
      \cgalParamNEnd

      \cgalParamNBegin{plane_index_map}
        \cgalParamDescription{a property map associating the index of a point in the input range
                              to the index of plane (`-1` if the point is not assigned to a plane)}
        \cgalParamType{a class model of `ReadablePropertyMap` with `std::size_t` as key type and `int` as value type}
        \cgalParamDefault{There is no default, this parameters is mandatory.}
      \cgalParamNEnd

      \cgalParamNBegin{plane_map}
        \cgalParamDescription{a property map containing the planes associated to the elements of the plane range `planes`}
         \cgalParamType{a class model of `ReadablePropertyMap` with `PlaneRange::iterator::value_type`
                        as key type and `geom_traits::Plane_3` as value type}
        \cgalParamDefault{`CGAL::Identity_property_map<Kernel::Plane_3>`}
      \cgalParamNEnd

      \cgalParamNBegin{attraction_factor}
        \cgalParamDescription{multiple of a tolerance `epsilon` used to connect simplices}
        \cgalParamType{floating scalar value}
        \cgalParamDefault{`3`}
      \cgalParamNEnd
    \cgalNamedParamsEnd
  */
  template <typename PointRange,
            typename PlaneRange,
            typename NamedParameters>
  Point_set_with_structure (const PointRange& points,
                            const PlaneRange& planes,
                            double epsilon,
                            const NamedParameters& np)
  {
    init (points, planes, epsilon, np);
  }

  /// \cond SKIP_IN_MANUAL

  template <typename PointRange,
            typename PlaneRange,
            typename NamedParameters>
  void init (const PointRange& points,
             const PlaneRange& planes,
             double epsilon,
             const NamedParameters& np)
  {
    using parameters::choose_parameter;
    using parameters::get_parameter;
    using parameters::is_default_parameter;

    // basic geometric types
    typedef Point_set_processing_3_np_helper<PointRange, NamedParameters> NP_helper;
    typedef typename NP_helper::Const_point_map PointMap;
    typedef typename NP_helper::Normal_map NormalMap;
    typedef typename Point_set_processing_3::GetPlaneMap<PlaneRange, NamedParameters>::type PlaneMap;
    typedef typename Point_set_processing_3::GetPlaneIndexMap<NamedParameters>::type PlaneIndexMap;

    CGAL_assertion_msg(NP_helper::has_normal_map(points, np), "Error: no normal map");
    static_assert(!is_default_parameter<NamedParameters, internal_np::plane_index_t>::value,
                              "Error: no plane index map");

    PointMap point_map = NP_helper::get_const_point_map(points, np);
    NormalMap normal_map = NP_helper::get_normal_map(points, np);
    PlaneMap plane_map = choose_parameter<PlaneMap>(get_parameter(np, internal_np::plane_map));
    PlaneIndexMap index_map = choose_parameter<PlaneIndexMap>(get_parameter(np, internal_np::plane_index_map));
    double attraction_factor = choose_parameter(get_parameter(np, internal_np::attraction_factor), 3.);

    m_points.reserve(points.size());
    m_normals.reserve(points.size());
    m_indices_of_assigned_points.resize (planes.size());

    m_indices.resize (points.size (), (std::numeric_limits<std::size_t>::max)());
    m_status.resize (points.size (), POINT);

    std::size_t idx = 0;
    for (typename PointRange::const_iterator it = points.begin();
         it != points.end(); ++ it)
    {
      m_points.push_back (get(point_map, *it));
      m_normals.push_back (get(normal_map, *it));
      int plane_index = static_cast<int>(get (index_map, idx));
      if (plane_index != -1)
      {
        m_indices_of_assigned_points[std::size_t(plane_index)].push_back(idx);
        m_indices[idx] = std::size_t(plane_index);
        m_status[idx] = PLANE;
      }
      ++ idx;
    }


    m_planes.reserve (planes.size());
    for (typename PlaneRange::const_iterator it = planes.begin();
         it != planes.end(); ++ it)
      m_planes.push_back (get (plane_map, *it));

    run (epsilon, attraction_factor);
    clean ();
  }
  /// \endcond

  std::size_t size () const { return m_points.size (); }
  std::pair<Point, Vector> operator[] (std::size_t i) const
  { return std::make_pair (m_points[i], m_normals[i]); }
  const Point& point (std::size_t i) const { return m_points[i]; }
  const Vector& normal (std::size_t i) const { return m_normals[i]; }

  /*!

    Returns all `Plane_shape` objects that are adjacent to the point
    with index `i`.

    \note Points not adjacent to any plane are free-form points,
    points adjacent to 1 plane are planar points, points adjacent to 2
    planes are edge points and points adjacent to 3 or more planes are
    vertices.

   */
  template <typename OutputIterator>
  void adjacency (std::size_t i, OutputIterator output) const
  {
    if (m_status[i] == PLANE || m_status[i] == RESIDUS)
      *(output ++) = m_planes[m_indices[i]];
    else if (m_status[i] == EDGE)
      {
        *(output ++) = m_planes[m_edges[m_indices[i]].planes[0]];
        *(output ++) = m_planes[m_edges[m_indices[i]].planes[1]];
      }
    else if (m_status[i] == CORNER)
      {
        for (std::size_t j = 0; j < m_corners[m_indices[i]].planes.size(); ++ j)
          *(output ++) = m_planes[m_corners[m_indices[i]].planes[j]];
      }
  }

  /*!

    Computes the coherence of a facet between the 3 points indexed by
    `f` with respect to the underlying structure.

   */
  Coherence_type facet_coherence (const std::array<std::size_t, 3>& f) const
  {
    // O- FREEFORM CASE
    if (m_status[f[0]] == POINT &&
        m_status[f[1]] == POINT &&
        m_status[f[2]] == POINT)
      return FREEFORM;

    // 1- PLANAR CASE
    if (m_status[f[0]] == PLANE &&
        m_status[f[1]] == PLANE &&
        m_status[f[2]] == PLANE)
      {
        if (m_indices[f[0]] == m_indices[f[1]] &&
            m_indices[f[0]] == m_indices[f[2]])
          return PLANAR;
        else
          return INCOHERENT;
      }

    for (std::size_t i = 0; i < 3; ++ i)
      {
        Point_status sa = m_status[f[(i+1)%3]];
        Point_status sb = m_status[f[(i+2)%3]];
        Point_status sc = m_status[f[(i+3)%3]];
        std::size_t a = m_indices[f[(i+1)%3]];
        std::size_t b = m_indices[f[(i+2)%3]];
        std::size_t c = m_indices[f[(i+3)%3]];

        // O- FREEFORM CASE
        if (sa == POINT && sb == POINT && sc == PLANE)
          return FREEFORM;
        if (sa == POINT && sb == PLANE && sc == PLANE)
          {
            if (b == c)
              return FREEFORM;
            else
              return INCOHERENT;
          }

        // 2- CREASE CASES
        if (sa == EDGE && sb == EDGE && sc == PLANE)
          {
            if ((c == m_edges[a].planes[0] ||
                 c == m_edges[a].planes[1]) &&
                (c == m_edges[b].planes[0] ||
                 c == m_edges[b].planes[1]))
              return CREASE;
            else
              return INCOHERENT;
          }

        if (sa == EDGE && sb == PLANE && sc == PLANE)
          {
            if (b == c &&
                (b == m_edges[a].planes[0] ||
                 b == m_edges[a].planes[1]))
              return CREASE;
            else
              return INCOHERENT;
          }


        // 3- CORNER CASES
        if (sc == CORNER)
          {
            if (sa == EDGE && sb == EDGE)
              {
                bool a0 = false, a1 = false, b0 = false, b1 = false;

                if ((m_edges[a].planes[0] != m_edges[b].planes[0] &&
                     m_edges[a].planes[0] != m_edges[b].planes[1] &&
                     m_edges[a].planes[1] != m_edges[b].planes[0] &&
                     m_edges[a].planes[1] != m_edges[b].planes[1]))
                  return INCOHERENT;

                for (std::size_t j = 0; j < m_corners[c].planes.size (); ++ j)
                  {
                    if (m_corners[c].planes[j] == m_edges[a].planes[0])
                      a0 = true;
                    else if (m_corners[c].planes[j] == m_edges[a].planes[1])
                      a1 = true;
                    if (m_corners[c].planes[j] == m_edges[b].planes[0])
                      b0 = true;
                    else if (m_corners[c].planes[j] == m_edges[b].planes[1])
                      b1 = true;
                  }
                if (a0 && a1 && b0 && b1)
                  return VERTEX;
                else
                  return INCOHERENT;
              }
            else if (sa == PLANE && sb == PLANE)
              {
                if (a != b)
                  return INCOHERENT;

                for (std::size_t j = 0; j < m_corners[c].planes.size (); ++ j)
                  if (m_corners[c].planes[j] == a)
                    return VERTEX;

                return INCOHERENT;
              }
            else if (sa == PLANE && sb == EDGE)
              {
                bool pa = false, b0 = false, b1 = false;
                if (a != m_edges[b].planes[0] && a != m_edges[b].planes[1])
                  return INCOHERENT;

                for (std::size_t j = 0; j < m_corners[c].planes.size (); ++ j)
                  {
                    if (m_corners[c].planes[j] == a)
                      pa = true;
                    if (m_corners[c].planes[j] == m_edges[b].planes[0])
                      b0 = true;
                    else if (m_corners[c].planes[j] == m_edges[b].planes[1])
                      b1 = true;
                  }
                if (pa && b0 && b1)
                  return VERTEX;
                else
                  return INCOHERENT;
              }
            else if (sa == EDGE && sb == PLANE)
              {
                bool a0 = false, a1 = false, pb = false;
                if (b != m_edges[a].planes[0] && b != m_edges[a].planes[1])
                  return INCOHERENT;

                for (std::size_t j = 0; j < m_corners[c].planes.size (); ++ j)
                  {
                    if (m_corners[c].planes[j] == b)
                      pb = true;
                    if (m_corners[c].planes[j] == m_edges[a].planes[0])
                      a0 = true;
                    else if (m_corners[c].planes[j] == m_edges[a].planes[1])
                      a1 = true;
                  }
                if (a0 && a1 && pb)
                  return VERTEX;
                else
                  return INCOHERENT;
              }
            else
              return INCOHERENT;
          }
      }


    return INCOHERENT;
  }


  /// \cond SKIP_IN_MANUAL
private:


  void clean ()
  {
    std::vector<Point> points;
    std::vector<Vector> normals;
    std::vector<std::size_t> indices;
    std::vector<Point_status> status;

    for (std::size_t i = 0; i < m_points.size (); ++ i)
      if (m_status[i] != SKIPPED)
        {
          points.push_back (m_points[i]);
          normals.push_back (m_normals[i]);
          status.push_back (m_status[i]);
          if (m_status[i] == RESIDUS)
            status.back () = PLANE;
          indices.push_back (m_indices[i]);
        }

    m_points.swap (points);
    m_normals.swap (normals);
    m_indices.swap (indices);
    m_status.swap (status);
  }


  void run (double epsilon, double attraction_factor = 3.)
  {
    if (m_planes.empty ())
      return;

    double radius = epsilon * attraction_factor;

#ifdef CGAL_PSP3_VERBOSE
    std::cerr << "Computing planar points... " << std::endl;
#endif

    project_inliers ();
    resample_planes (epsilon);

#ifdef CGAL_PSP3_VERBOSE
    std::cerr << " -> Done" << std::endl;
    std::cerr << "Finding adjacent primitives... " << std::endl;
#endif

    find_pairs_of_adjacent_primitives (radius);

#ifdef CGAL_PSP3_VERBOSE
    std::cerr << " -> Found " << m_edges.size () << " pair(s) of adjacent primitives." << std::endl;
    std::cerr << "Computing edges... " << std::endl;
#endif

    compute_edges (epsilon);

#ifdef CGAL_PSP3_VERBOSE
    std::cerr << " -> Done" << std::endl;
    std::cerr << "Creating edge-anchor points... " << std::endl;
    {
      std::size_t size_before = m_points.size ();
#endif

      create_edge_anchor_points (radius, epsilon);

#ifdef CGAL_PSP3_VERBOSE
      std::cerr << " -> " << m_points.size () - size_before << " anchor point(s) created." << std::endl;
    }

    std::cerr << "Computating first set of corners... " << std::endl;
#endif

    compute_corners (radius);

#ifdef CGAL_PSP3_VERBOSE
    std::cerr << " -> Found " << m_corners.size () << " triple(s) of adjacent primitives/edges." << std::endl;
    std::cerr << "Merging corners... " << std::endl;
    {
      std::size_t size_before = m_points.size ();
#endif

      merge_corners (radius);

#ifdef CGAL_PSP3_VERBOSE
      std::cerr << " -> " << m_points.size () - size_before << " corner point(s) created." << std::endl;
    }

    std::cerr << "Computing corner directions... " << std::endl;
#endif

    compute_corner_directions (epsilon);

#ifdef CGAL_PSP3_VERBOSE
    std::cerr << " -> Done" << std::endl;
    std::cerr << "Refining sampling... " << std::endl;
#endif

    refine_sampling (epsilon);

#ifdef CGAL_PSP3_VERBOSE
    std::cerr << " -> Done" << std::endl;

    std::cerr << "Cleaning data set... " << std::endl;
#endif

    clean ();

#ifdef CGAL_PSP3_VERBOSE
    std::cerr << " -> Done" << std::endl;
#endif
  }

  void project_inliers ()
  {
    for(std::size_t i = 0; i < m_indices_of_assigned_points.size (); ++ i)
      for (std::size_t j = 0; j < m_indices_of_assigned_points[i].size(); ++ j)
        {
          std::size_t ind = m_indices_of_assigned_points[i][j];
          m_points[ind] = m_planes[i].projection (m_points[ind]);
        }
  }

  void resample_planes (double epsilon)
  {
    double grid_length = epsilon * (std::sqrt(2.) - 1e-3);

    for (std::size_t c = 0; c < m_planes.size (); ++ c)
      {
        //plane attributes and 2D projection vectors
        const Plane& plane = m_planes[c];
        Vector vortho = plane.orthogonal_vector();
        Vector b1 = plane.base1();
        Vector b2 = plane.base2();

        b1 = b1 / std::sqrt (b1 * b1);
        b2 = b2 / std::sqrt (b2 * b2);

        std::vector<Point_2> points_2d;

        //storage of the 2D points in "pt_2d"
        for (std::size_t j = 0; j < m_indices_of_assigned_points[c].size(); ++ j)
          {
            std::size_t ind = m_indices_of_assigned_points[c][j];
            const Point& pt = m_points[ind];
            points_2d.push_back (Point_2 (b1.x() * pt.x() + b1.y() * pt.y() + b1.z() * pt.z(),
                                          b2.x() * pt.x() + b2.y() * pt.y() + b2.z() * pt.z()));
          }


        //creation of a 2D-grid with cell width = grid_length, and image structures
        CGAL::Bbox_2 box_2d = CGAL::bbox_2 (points_2d.begin(), points_2d.end());
        std::size_t Nx = static_cast<std::size_t>((box_2d.xmax() - box_2d.xmin()) / grid_length) + 1;
        std::size_t Ny = static_cast<std::size_t>((box_2d.ymax() - box_2d.ymin()) / grid_length) + 1;

        std::vector<std::vector<bool> > Mask (Nx, std::vector<bool> (Ny, false));
        std::vector<std::vector<bool> > Mask_border (Nx, std::vector<bool> (Ny, false));
        std::vector<std::vector<std::vector<std::size_t> > >
          point_map (Nx, std::vector<std::vector<std::size_t> > (Ny, std::vector<std::size_t>()));

        //storage of the points in the 2D-grid "point_map"
        for (std::size_t i = 0; i < points_2d.size(); ++ i)
          {
            std::size_t ind_x = static_cast<std::size_t>((points_2d[i].x() - box_2d.xmin()) / grid_length);
            std::size_t ind_y = static_cast<std::size_t>((points_2d[i].y() - box_2d.ymin()) / grid_length);
            Mask[ind_x][ind_y] = true;
            point_map[ind_x][ind_y].push_back (m_indices_of_assigned_points[c][i]);
          }

        //hole filing in Mask in 4-connexity
        for (std::size_t j = 1; j < Ny - 1; ++ j)
          for (std::size_t i = 1; i < Nx - 1; ++ i)
            if( !Mask[i][j]
                && Mask[i-1][j] && Mask[i][j-1]
                && Mask[i][j+1] && Mask[i+1][j] )
              Mask[i][j]=true;

        //finding mask border in 8-connexity
        for (std::size_t j = 1; j < Ny - 1; ++ j)
          for (std::size_t i = 1; i < Nx - 1; ++ i)
            if( Mask[i][j] &&
                ( !Mask[i-1][j-1] || !Mask[i-1][j] ||
                  !Mask[i-1][j+1] || !Mask[i][j-1] ||
                  !Mask[i][j+1] || !Mask[i+1][j-1] ||
                  !Mask[i+1][j]|| !Mask[i+1][j+1] ) )
              Mask_border[i][j]=true;

        for (std::size_t j = 0; j < Ny; ++ j)
          {
            if (Mask[0][j])
              Mask_border[0][j]=true;
            if (Mask[Nx-1][j])
              Mask_border[Nx-1][j]=true;
          }

        for (std::size_t i = 0; i < Nx; ++ i)
          {
            if(Mask[i][0])
              Mask_border[i][0]=true;
            if(Mask[i][Ny-1])
              Mask_border[i][Ny-1]=true;
          }

        //saving of points to keep
        for (std::size_t j = 0; j < Ny; ++ j)
          for (std::size_t i = 0; i < Nx; ++ i)
            if( point_map[i][j].size()>0)
              {
                //inside: recenter (cell center) the first point of the cell and deactivate the others points
                if (!Mask_border[i][j] && Mask[i][j])
                  {
                    double x2pt = (i+0.5) * grid_length + box_2d.xmin();
                    double y2pt = (j+0.4) * grid_length + box_2d.ymin();

                    if (i%2 == 1)
                      {
                        x2pt = (i+0.5) * grid_length + box_2d.xmin();
                        y2pt = (j+0.6) * grid_length + box_2d.ymin();
                      }

                    FT X1 = x2pt * b1.x() + y2pt * b2.x() - plane.d() * vortho.x();
                    FT X2 = x2pt * b1.y() + y2pt * b2.y() - plane.d() * vortho.y();
                    FT X3 = x2pt * b1.z() + y2pt * b2.z() - plane.d() * vortho.z();

                    std::size_t index_pt = point_map[i][j][0];
                    m_points[index_pt] = Point (X1, X2, X3);
                    m_normals[index_pt] = m_planes[c].orthogonal_vector();
                    m_status[index_pt] = PLANE;

                    for (std::size_t np = 1; np < point_map[i][j].size(); ++ np)
                      m_status[point_map[i][j][np]] = SKIPPED;
                  }

                //border: recenter (barycenter) the first point of the cell and deactivate the others points
                else if (Mask_border[i][j] && Mask[i][j])
                  {
                    std::vector<Point> pts;
                    for (std::size_t np = 0; np < point_map[i][j].size(); ++ np)
                      pts.push_back (m_points[point_map[i][j][np]]);

                    m_points[point_map[i][j][0]] = CGAL::centroid (pts.begin (), pts.end ());
                    m_status[point_map[i][j][0]] = PLANE;
                    for (std::size_t np = 1; np < point_map[i][j].size(); ++ np)
                      m_status[point_map[i][j][np]] = SKIPPED;
                  }
              }
        // point use to filling 4-connexity holes are store in HPS_residus
            else if (point_map[i][j].size()==0 && !Mask_border[i][j] && Mask[i][j])
              {
                double x2pt = (i+0.5) * grid_length + box_2d.xmin();
                double y2pt = (j+0.49) * grid_length + box_2d.ymin();
                if(i%2==1)
                  {
                    x2pt = (i+0.5) * grid_length + box_2d.xmin();
                    y2pt = (j+0.51) * grid_length + box_2d.ymin();
                  }
                FT X1 = x2pt * b1.x() + y2pt * b2.x() - plane.d() * vortho.x();
                FT X2 = x2pt * b1.y() + y2pt * b2.y() - plane.d() * vortho.y();
                FT X3 = x2pt * b1.z() + y2pt * b2.z() - plane.d() * vortho.z();

                m_points.push_back (Point (X1, X2, X3));
                m_normals.push_back (m_planes[c].orthogonal_vector());
                m_indices.push_back (c);
                m_status.push_back (RESIDUS);
              }
      }

  }

  void find_pairs_of_adjacent_primitives (double radius)
  {
    typedef typename CGAL::Search_traits_3<Kernel> Search_traits_base;
    typedef Search_traits_adapter <std::size_t, typename Pointer_property_map<Point>::type, Search_traits_base> Search_traits;
    typedef CGAL::Kd_tree<Search_traits> Tree;
    typedef CGAL::Fuzzy_sphere<Search_traits> Fuzzy_sphere;

    typename Pointer_property_map<Point>::type pmap = make_property_map(m_points);

    Tree tree (boost::counting_iterator<std::size_t, boost::use_default, std::ptrdiff_t> (0),
               boost::counting_iterator<std::size_t, boost::use_default, std::ptrdiff_t> (m_points.size()),
               typename Tree::Splitter(),
               Search_traits (pmap));

    std::vector<std::vector<bool> > adjacency_table (m_planes.size (),
                                                     std::vector<bool> (m_planes.size (), false));

    //compute a basic adjacency relation (two primitives are neighbors
    //if at least one point of the primitive 1 is a k-nearest neighbor
    //of a point of the primitive 2 and vice versa)
    for (std::size_t i = 0; i < m_points.size (); ++ i)
      {
        std::size_t ind_i = m_indices[i];

        if (ind_i == (std::numeric_limits<std::size_t>::max)())
          continue;

        Fuzzy_sphere query (i, radius, 0., tree.traits());

        std::vector<std::size_t> neighbors;
        tree.search (std::back_inserter (neighbors), query);


        for (std::size_t k = 0; k < neighbors.size(); ++ k)
          {
            std::size_t ind_k = m_indices[neighbors[k]];
            if (ind_k != (std::numeric_limits<std::size_t>::max)() && ind_k != ind_i)
              adjacency_table[ind_i][ind_k] = true;
          }
      }

    //verify the symmetry and store the pairs of primitives in
    //m_edges
    for (std::size_t i = 0; i < adjacency_table.size() - 1; ++ i)
      for (std::size_t j = i + 1; j < adjacency_table[i].size(); ++ j)
        if ((adjacency_table[i][j]) && (adjacency_table[j][i]))
          m_edges.push_back (Edge (i, j));

  }

  void compute_edges (double epsilon)
  {
    for (std::size_t i = 0; i < m_edges.size(); ++ i)
      {
        const Plane& plane1 = m_planes[m_edges[i].planes[0]];
        const Plane& plane2 = m_planes[m_edges[i].planes[1]];

        double angle_A = std::acos (CGAL::abs (plane1.orthogonal_vector() * plane2.orthogonal_vector()));
        double angle_B = CGAL_PI - angle_A;

        const auto result = CGAL::intersection(plane1, plane2);
        if (!result)
          {
#ifdef CGAL_PSP3_VERBOSE
            std::cerr << "Warning: bad plane/plane intersection" << std::endl;
#endif
            continue;
          }

        if (const Line* l = std::get_if<Line>(&*result))
          m_edges[i].support = *l;
        else
          {
#ifdef CGAL_PSP3_VERBOSE
            std::cerr << "Warning: bad plane/plane intersection" << std::endl;
#endif
            continue;
          }

        Vector direction_p1 (0., 0., 0.);
        for (std::size_t k = 0; k < m_indices_of_assigned_points[m_edges[i].planes[0]].size(); ++ k)
          {
            std::size_t index_point = m_indices_of_assigned_points[m_edges[i].planes[0]][k];

            const Point& point = m_points[index_point];
            Point projected = m_edges[i].support.projection (point);
            if (std::sqrt (CGAL::squared_distance (point, projected))
                < 2 * (std::min) (4., 1 / std::sin (angle_A)) * epsilon
                && m_status[index_point] != SKIPPED)
              direction_p1 = direction_p1 + Vector (projected, point);
          }
        if (direction_p1.squared_length() > 0)
          direction_p1 = direction_p1 / std::sqrt (direction_p1 * direction_p1);

        Vector direction_p2 (0., 0., 0.);
        for (std::size_t k = 0; k < m_indices_of_assigned_points[m_edges[i].planes[1]].size(); ++ k)
          {
            std::size_t index_point = m_indices_of_assigned_points[m_edges[i].planes[1]][k];

            const Point& point = m_points[index_point];
            Point projected = m_edges[i].support.projection (point);
            if (std::sqrt (CGAL::squared_distance (point, projected))
                < 2 * (std::min) (4., 1 / std::sin (angle_A)) * epsilon
                && m_status[index_point] != SKIPPED)
              direction_p2 = direction_p2 + Vector (projected, point);
          }
        if (direction_p2.squared_length() > 0)
          direction_p2 = direction_p2 / std::sqrt (direction_p2 * direction_p2);

        double angle = std::acos (direction_p1 * direction_p2);

        if (direction_p1.squared_length() == 0
            || direction_p2.squared_length() == 0
            || (CGAL::abs (angle - angle_A) > 1e-2
                && CGAL::abs (angle - angle_B) > 1e-2 ))
          {
            m_edges[i].active = false;
          }
      }
  }

  void create_edge_anchor_points (double radius, double epsilon)
  {
    double d_DeltaEdge = std::sqrt (2.) * epsilon;
    double r_edge = d_DeltaEdge / 2.;

    for (std::size_t i = 0; i < m_edges.size(); ++ i)
      {
        const Plane& plane1 = m_planes[m_edges[i].planes[0]];
        const Plane& plane2 = m_planes[m_edges[i].planes[1]];

        const Line& line = m_edges[i].support;

        if (!(m_edges[i].active))
          {
            continue;
          }

        Vector normal = 0.5 * plane1.orthogonal_vector () + 0.5 * plane2.orthogonal_vector();

        //find set of points close (<attraction_radius) to the edge and store in intersection_points
        std::vector<std::size_t> intersection_points;
        for (std::size_t k = 0; k < m_indices_of_assigned_points[m_edges[i].planes[0]].size(); ++ k)
          {
            std::size_t index_point = m_indices_of_assigned_points[m_edges[i].planes[0]][k];
            const Point& point = m_points[index_point];
            Point projected = line.projection (point);
            if (CGAL::squared_distance (point, projected) < radius * radius)
              intersection_points.push_back (index_point);
          }
        for (std::size_t k = 0; k < m_indices_of_assigned_points[m_edges[i].planes[1]].size(); ++ k)
          {
            std::size_t index_point = m_indices_of_assigned_points[m_edges[i].planes[1]][k];
            const Point& point = m_points[index_point];
            Point projected = line.projection (point);
            if (CGAL::squared_distance (point, projected) < radius * radius)
              intersection_points.push_back (index_point);
          }

        if (intersection_points.empty ())
          {
            continue;
          }

        const Point& t0 = m_points[intersection_points[0]];
        Point t0p = line.projection (t0);
        double dmin = 0.;
        double dmax = 0.;
        Point Pmin = t0p;
        Point Pmax = t0p;
        Vector dir = line.to_vector ();

        //compute the segment of the edge
        for (std::size_t k = 0; k < intersection_points.size(); ++ k)
          {
            std::size_t ind = intersection_points[k];
            const Point& point = m_points[ind];
            Point projected = line.projection (point);
            double d = Vector (t0p, projected) * dir;

            if (d < dmin)
              {
                dmin = d;
                Pmin = projected;
              }
            else if (d > dmax)
              {
                dmax = d;
                Pmax = projected;
              }
          }

        // make a partition in a 1D image by voting if at the same
        // time at least one point of plane1 and one of point2 fall in
        // the same cell (same step as for planes)
        Segment seg (Pmin,Pmax);
        std::size_t number_of_division = static_cast<std::size_t>(std::sqrt (seg.squared_length ()) / d_DeltaEdge) + 1;
        std::vector<std::vector<std::size_t> > division_tab (number_of_division);

        for (std::size_t k = 0; k < intersection_points.size(); ++ k)
          {
            std::size_t ind = intersection_points[k];
            const Point& point = m_points[ind];
            Point projected = line.projection (point);

            std::size_t tab_index = static_cast<std::size_t>(std::sqrt (CGAL::squared_distance (seg[0], projected))
                                                             / d_DeltaEdge);

            division_tab[tab_index].push_back (ind);
          }

        //C1-CREATE the EDGE
        std::vector<int> index_of_edge_points;
        for (std::size_t j = 0; j < division_tab.size(); ++ j)
          {
            bool p1found = false, p2found = false;
            for (std::size_t k = 0; k < division_tab[j].size () && !(p1found && p2found); ++ k)
              {
                if (m_indices[division_tab[j][k]] == m_edges[i].planes[0])
                  p1found = true;
                if (m_indices[division_tab[j][k]] == m_edges[i].planes[1])
                  p2found = true;
              }

            if (!(p1found && p2found))
              {
                division_tab[j].clear();
                continue;
              }

            Point perfect (seg[0].x() + (seg[1].x() - seg[0].x()) * (j + 0.5) / double(number_of_division),
                           seg[0].y() + (seg[1].y() - seg[0].y()) * (j + 0.5) / double(number_of_division),
                           seg[0].z() + (seg[1].z() - seg[0].z()) * (j + 0.5) / double(number_of_division));

            // keep closest point, replace it by perfect one and skip the others
            double dist_min = (std::numeric_limits<double>::max)();
            std::size_t index_best = 0;

            for (std::size_t k = 0; k < division_tab[j].size(); ++ k)
              {
                std::size_t inde = division_tab[j][k];

                if (CGAL::squared_distance (line, m_points[inde]) < d_DeltaEdge * d_DeltaEdge)
                  m_status[inde] = SKIPPED; // Deactivate points too close (except best, see below)

                double distance = CGAL::squared_distance (perfect, m_points[inde]);
                if (distance < dist_min)
                  {
                    dist_min = distance;
                    index_best = inde;
                  }
              }

            m_points[index_best] = perfect;
            m_normals[index_best] = normal;
            m_status[index_best] = EDGE;
            m_indices[index_best] = i;
            m_edges[i].indices.push_back (index_best);

          }

        //C2-CREATE the ANCHOR
        Vector direction_p1(0,0,0);
        Vector direction_p2(0,0,0);

        for (std::size_t j = 0; j < division_tab.size() - 1; ++ j)
          {
            if (division_tab[j].empty () || division_tab[j+1].empty ())
              continue;
            Point anchor (seg[0].x() + (seg[1].x() - seg[0].x()) * (j + 1) / double(number_of_division),
                          seg[0].y() + (seg[1].y() - seg[0].y()) * (j + 1) / double(number_of_division),
                          seg[0].z() + (seg[1].z() - seg[0].z()) * (j + 1) / double(number_of_division));

            Plane ortho = seg.supporting_line().perpendicular_plane(anchor);

            std::vector<Point> pts1, pts2;
            //Computation of the permanent angle and directions
            for (std::size_t k = 0; k < division_tab[j].size(); ++ k)
              {
                std::size_t inde = division_tab[j][k];
                std::size_t plane = m_indices[inde];
                if (plane == m_edges[i].planes[0])
                  pts1.push_back (m_points[inde]);
                else if (plane == m_edges[i].planes[1])
                  pts2.push_back (m_points[inde]);
              }

            auto result = CGAL::intersection (plane1, ortho);
            if (result)
              {
                if (const Line* l = std::get_if<Line>(&*result))
                  {
                    if (!(pts1.empty()))
                      {
                        Vector vecp1 = l->to_vector();
                        vecp1 = vecp1/ std::sqrt (vecp1 * vecp1);
                        Vector vtest1 (anchor, CGAL::centroid (pts1.begin (), pts1.end ()));
                        if (vtest1 * vecp1<0)
                          vecp1 = -vecp1;

                        direction_p1 = direction_p1+vecp1;

                        Point anchor1 = anchor + vecp1 * r_edge;
                        m_points.push_back (anchor1);
                        m_normals.push_back (m_planes[m_edges[i].planes[0]].orthogonal_vector());
                        m_indices.push_back (m_edges[i].planes[0]);
                        m_status.push_back (PLANE);
                      }
                  }
                else
                  {
#ifdef CGAL_PSP3_VERBOSE
                    std::cerr<<"Warning: bad plane/plane intersection"<<std::endl;
#endif
                  }
              }
            else
              {
#ifdef CGAL_PSP3_VERBOSE
                std::cerr<<"Warning: bad plane/plane intersection"<<std::endl;
#endif
              }


            result = CGAL::intersection (plane2,ortho);
            if (result)
              {
                if (const Line* l = std::get_if<Line>(&*result))
                  {
                    if (!(pts2.empty()))
                      {
                        Vector vecp2 = l->to_vector();
                        vecp2 = vecp2 / std::sqrt (vecp2 * vecp2);
                        Vector vtest2 (anchor, CGAL::centroid (pts2.begin (), pts2.end ()));
                        if (vtest2 * vecp2 < 0)
                          vecp2 =- vecp2;

                        direction_p2 = direction_p2+vecp2;

                        Point anchor2 = anchor + vecp2 * r_edge;
                        m_points.push_back (anchor2);
                        m_normals.push_back (m_planes[m_edges[i].planes[1]].orthogonal_vector());
                        m_indices.push_back (m_edges[i].planes[1]);
                        m_status.push_back (PLANE);
                      }
                  }
                else
                  {
#ifdef CGAL_PSP3_VERBOSE
                    std::cerr<<"Warning: bad plane/plane intersection"<<std::endl;
#endif
                  }
              }
            else
              {
#ifdef CGAL_PSP3_VERBOSE
                std::cerr<<"Warning: bad plane/plane intersection"<<std::endl;
#endif
              }
          }

        //if not information enough (not enough edges to create
        //anchor) we unactivate the edge, else we update the angle
        //and directions
        if ( !(direction_p1.squared_length()>0 || direction_p2.squared_length()>0) )
          {
            m_edges[i].active = false;
            for (std::size_t j = 0; j < m_edges[i].indices.size (); ++ j)
              m_status[m_edges[i].indices[j]] = SKIPPED;
          }
      }
  }

  void compute_corners (double radius)
  {
    if (m_edges.size () < 3)
      return;

    std::vector<std::vector<std::size_t> > plane_edge_adj (m_planes.size());
    for (std::size_t i = 0; i < m_edges.size (); ++ i)
      if (m_edges[i].active)
        {
          plane_edge_adj[m_edges[i].planes[0]].push_back (i);
          plane_edge_adj[m_edges[i].planes[1]].push_back (i);
        }

    std::vector<std::set<std::size_t> > edge_adj (m_edges.size ());

    for (std::size_t i = 0; i < plane_edge_adj.size (); ++ i)
      {
        if (plane_edge_adj[i].size () < 2)
          continue;

        for (std::size_t j = 0; j < plane_edge_adj[i].size ()- 1; ++ j)
          for (std::size_t k = j + 1; k < plane_edge_adj[i].size (); ++ k)
            {
              edge_adj[plane_edge_adj[i][j]].insert (plane_edge_adj[i][k]);
              edge_adj[plane_edge_adj[i][k]].insert (plane_edge_adj[i][j]);
            }
      }

    for (std::size_t i = 0; i < edge_adj.size (); ++ i)
      {
        if (edge_adj[i].size () < 2)
          continue;

        std::set<std::size_t>::iterator end = edge_adj[i].end();
        end --;

        for (std::set<std::size_t>::iterator jit = edge_adj[i].begin ();
             jit != end; ++ jit)
          {
            std::size_t j = *jit;
            if (j < i)
              continue;

            std::set<std::size_t>::iterator begin = jit;
            begin ++;
            for (std::set<std::size_t>::iterator kit = begin;
                 kit != edge_adj[i].end (); ++ kit)
              {
                std::size_t k = *kit;
                if (k < j)
                  continue;

                std::set<std::size_t> planes;
                planes.insert (m_edges[i].planes[0]);
                planes.insert (m_edges[i].planes[1]);
                planes.insert (m_edges[j].planes[0]);
                planes.insert (m_edges[j].planes[1]);
                planes.insert (m_edges[k].planes[0]);
                planes.insert (m_edges[k].planes[1]);

                if (planes.size () == 3) // Triple found
                  {
                    std::vector<std::size_t> vecplanes (planes.begin (), planes.end ());
                    m_corners.push_back (Corner (vecplanes[0], vecplanes[1], vecplanes[2],
                                                 i, j, k));
                  }
              }
          }
      }


    for (std::size_t i = 0; i < m_corners.size (); ++ i)
      {
        //calcul pt d'intersection des 3 plans
        const Plane& plane1 = m_planes[m_corners[i].planes[0]];
        const Plane& plane2 = m_planes[m_corners[i].planes[1]];
        const Plane& plane3 = m_planes[m_corners[i].planes[2]];

        const auto result = CGAL::intersection(plane1, plane2);
        if (result)
          {
            if (const Line* l = std::get_if<Line>(&*result))
              {
                const auto result2 = CGAL::intersection(*l, plane3);
                if (result2)
                  {
                    if (const Point* p = std::get_if<Point>(&*result2))
                      m_corners[i].support = *p;
                    else
                      {
#ifdef CGAL_PSP3_VERBOSE
                        std::cerr << "Warning: bad plane/line intersection" << std::endl;
#endif
                        m_corners[i].active = false;
                        continue;
                      }
                  }
                else
                  {
#ifdef CGAL_PSP3_VERBOSE
                    std::cerr << "Warning: bad plane/line intersection" << std::endl;
#endif
                    m_corners[i].active = false;
                    continue;

                  }
              }
            else
              {
#ifdef CGAL_PSP3_VERBOSE
                std::cerr << "Warning: bad plane/plane intersection" << std::endl;
#endif
                m_corners[i].active = false;
                continue;
              }
          }
        else
          {
#ifdef CGAL_PSP3_VERBOSE
            std::cerr << "Warning: bad plane/plane intersection" << std::endl;
#endif
            m_corners[i].active = false;
            continue;
          }

        // test if point is in bbox + delta
        CGAL::Bbox_3 bbox = CGAL::bbox_3 (m_points.begin (), m_points.end ());

        double margin_x = 0.1 * (bbox.xmax() - bbox.xmin());
        double X_min = bbox.xmin() - margin_x;
        double X_max = bbox.xmax() + margin_x;
        double margin_y = 0.1 * (bbox.ymax() - bbox.ymin());
        double Y_min = bbox.ymin() - margin_y;
        double Y_max = bbox.ymax() + margin_y;
        double margin_z = 0.1* (bbox.zmax() - bbox.zmin());
        double Z_min = bbox.zmin() - margin_z;
        double Z_max = bbox.zmax() + margin_z;

        if ((m_corners[i].support.x() < X_min) || (m_corners[i].support.x() > X_max)
            || (m_corners[i].support.y() < Y_min) || (m_corners[i].support.y() > Y_max)
            || (m_corners[i].support.z() < Z_min) || (m_corners[i].support.z() > Z_max))
          {
            m_corners[i].active = false;
            continue;
          }

        // test if corner is in neighborhood of at least one point each of the 3 planes
        std::vector<bool> neighborhood (3, false);

        for (std::size_t k = 0; k < 3; ++ k)
          {
            for (std::size_t j = 0; j < m_edges[m_corners[i].edges[k]].indices.size(); ++ j)
              {
                const Point& p = m_points[m_edges[m_corners[i].edges[k]].indices[j]];

                if (CGAL::squared_distance (m_corners[i].support, p) < radius * radius)
                  {
                    neighborhood[k] = true;
                    break;
                  }
              }
          }

        if ( !(neighborhood[0] && neighborhood[1] && neighborhood[2]) )
          m_corners[i].active = false;
      }
  }

  void merge_corners (double radius)
  {
    for (std::size_t k = 0; k < m_corners.size(); ++ k)
      {
        if (!(m_corners[k].active))
          continue;

        int count_plane_number=3;

        for (std::size_t kb = k + 1; kb < m_corners.size(); ++ kb)
          {
            if (!(m_corners[kb].active))
              continue;

            int count_new_plane = 0;

            if (CGAL::squared_distance (m_corners[kb].support, m_corners[k].support) >= radius * radius)
              continue;

            for (std::size_t i = 0; i < m_corners[kb].planes.size (); ++ i)
              {
                bool testtt = true;
                for (std::size_t l = 0; l < m_corners[k].planes.size(); ++ l)
                  if (m_corners[kb].planes[i] == m_corners[k].planes[l])
                    {
                      testtt = false;
                      break;
                    }
                if (!testtt)
                  continue;

                m_corners[k].planes.push_back (m_corners[kb].planes[i]);
                ++ count_new_plane;
                m_corners[kb].active = false;

                std::vector<bool> is_edge_in (3, false);
                for (std::size_t l = 0; l < m_corners[k].edges.size(); ++ l)
                  {
                    for (std::size_t j = 0; j < 3; ++ j)
                      if (m_corners[k].edges[l] == m_corners[kb].edges[j])
                        is_edge_in[j] = true;
                  }
                for (std::size_t j = 0; j < 3; ++ j)
                  if (!(is_edge_in[j]))
                    m_corners[k].edges.push_back (m_corners[kb].edges[j]);

              }

            //update barycenter
            m_corners[k].support = CGAL::barycenter (m_corners[k].support, count_plane_number,
                                                     m_corners[kb].support, count_new_plane);
            count_plane_number += count_new_plane;
          }

        // Compute normal vector
        Vector normal (0., 0., 0.);
        for (std::size_t i = 0; i < m_corners[k].planes.size(); ++ i)
          normal = normal + (1. / (double)(m_corners[k].planes.size()))
            * m_planes[m_corners[k].planes[i]].orthogonal_vector();

        m_points.push_back (m_corners[k].support);
        m_normals.push_back (normal);
        m_indices.push_back (k);
        m_status.push_back (CORNER);
      }
  }

  void compute_corner_directions (double epsilon)
  {
    for (std::size_t k = 0; k < m_corners.size(); ++ k)
      {
        for (std::size_t ed = 0; ed < m_corners[k].edges.size(); ++ ed)
          {
            if (m_corners[k].edges[ed] < m_edges.size())
              {
                const Edge& edge = m_edges[m_corners[k].edges[ed]];

                Vector direction (0., 0., 0.);
                for (std::size_t i = 0; i < edge.indices.size(); ++ i)
                  {
                    std::size_t index_pt = edge.indices[i];
                    if (std::sqrt (CGAL::squared_distance (m_corners[k].support,
                                                           m_points[index_pt])) < 5 * epsilon)
                      direction = direction + Vector (m_corners[k].support, m_points[index_pt]);
                  }

                if (direction.squared_length() > 1e-5)
                  m_corners[k].directions.push_back (direction / std::sqrt (direction * direction));
                else
                  m_corners[k].directions.push_back (Vector (0., 0., 0.));
              }
            else
              m_corners[k].directions.push_back (Vector (0., 0., 0.));
          }
      }
  }

  void refine_sampling (double epsilon)
  {
    double d_DeltaEdge = std::sqrt (2.) * epsilon;

    for (std::size_t k = 0; k < m_corners.size(); ++ k)
      {
        if (!(m_corners[k].active))
          continue;

        for (std::size_t ed = 0; ed < m_corners[k].edges.size(); ++ ed)
          {
            const Edge& edge = m_edges[m_corners[k].edges[ed]];

            for (std::size_t i = 0; i < edge.indices.size(); ++ i)
              {
                //if too close from a corner, ->remove
                if (CGAL::squared_distance (m_corners[k].support, m_points[edge.indices[i]])
                    < d_DeltaEdge * d_DeltaEdge)
                  m_status[edge.indices[i]] = SKIPPED;

                //if too close from a corner (non dominant side), ->remove
                if (m_corners[k].directions[ed].squared_length() > 0
                    && (m_corners[k].directions[ed]
                        * Vector (m_corners[k].support, m_points[edge.indices[i]]) < 0)
                    && (CGAL::squared_distance (m_corners[k].support, m_points[edge.indices[i]])
                        < 4 * d_DeltaEdge * d_DeltaEdge))
                  m_status[edge.indices[i]] = SKIPPED;
              }

          }
      }

    for (std::size_t k = 0; k < m_corners.size(); ++ k)
      {
        if (!(m_corners[k].active))
          continue;

        for (std::size_t ed = 0; ed < m_corners[k].edges.size(); ++ ed)
          {
            if (m_corners[k].directions[ed].squared_length() <= 0.)
              continue;

            Edge& edge = m_edges[m_corners[k].edges[ed]];

            //rajouter un edge a epsilon du cote dominant si pas de point entre SS_edge/2 et 3/2*SS_edge
            bool is_in_interval = false;
            for (std::size_t i = 0; i < edge.indices.size(); ++ i)
              {
                std::size_t index_pt = edge.indices[i];
                double dist = CGAL::squared_distance (m_corners[k].support,
                                                      m_points[index_pt]);
                if (m_status[index_pt] != SKIPPED
                    && dist < 1.5 * d_DeltaEdge && dist > d_DeltaEdge / 2)
                  {
                    Vector move (m_corners[k].support,
                                 m_points[index_pt]);
                    if (move * m_corners[k].directions[ed] > 0.)
                      {
                        is_in_interval = true;
                        break;
                      }
                  }
              }

            //rajouter un edge a 1 epsilon du cote dominant si pas de point entre SS_edge/2 et 3/2*SS_edge
            if (!is_in_interval)
              {
                Point new_edge = m_corners[k].support + m_corners[k].directions[ed] * d_DeltaEdge;
                m_points.push_back (new_edge);
                m_normals.push_back (0.5 * m_planes[m_edges[m_corners[k].edges[ed]].planes[0]].orthogonal_vector()
                                     + 0.5 * m_planes[m_edges[m_corners[k].edges[ed]].planes[1]].orthogonal_vector());
                m_status.push_back (EDGE);
                m_indices.push_back (m_corners[k].edges[ed]);
                edge.indices.push_back (m_points.size() - 1);
              }

            //rajouter un edge a 1/3 epsilon du cote dominant
            Point new_edge = m_corners[k].support + m_corners[k].directions[ed] * d_DeltaEdge / 3;
            m_points.push_back (new_edge);
            m_normals.push_back (0.5 * m_planes[m_edges[m_corners[k].edges[ed]].planes[0]].orthogonal_vector()
                                 + 0.5 * m_planes[m_edges[m_corners[k].edges[ed]].planes[1]].orthogonal_vector());
            m_status.push_back (EDGE);
            m_indices.push_back (m_corners[k].edges[ed]);
            edge.indices.push_back (m_points.size() - 1);
          }
      }

  }
  /// \endcond
};





// ----------------------------------------------------------------------------
// Public section
// ----------------------------------------------------------------------------

/**
   \ingroup PkgPointSetProcessing3Algorithms

   This is an implementation of the Point Set Structuring algorithm. This
   algorithm takes advantage of a set of detected planes: it detects adjacency
   relationships between planes and resamples the detected planes, edges and
   corners to produce a structured point set.

   The size parameter `epsilon` is used both for detecting adjacencies and for
   setting the sampling density of the structured point set.

   For more details, please refer to \cgalCite{cgal:la-srpss-13}.

   \tparam PointRange is a model of `ConstRange`. The value type of
   its iterator is the key type of the named parameter `point_map`.
   \tparam PlaneRange is a model of `ConstRange`. The value type of
   its iterator is the key type of the named parameter `plane_map`.
   \tparam OutputIterator Type of the output iterator. The type of the
   objects put in it is `std::pair<Kernel::Point_3, Kernel::Vector_3>`.
   Note that the user may use a
   <A HREF="https://www.boost.org/libs/iterator/doc/function_output_iterator.html">function_output_iterator</A>
   to match specific needs.

   \param points input point range
   \param planes input plane range.
   \param output output iterator where output points are written
   \param epsilon size parameter.
   \param np a sequence of \ref bgl_namedparameters "Named Parameters" among the ones listed below

   \cgalNamedParamsBegin
      \cgalParamNBegin{point_map}
        \cgalParamDescription{a property map associating points to the elements of the point set `points`}
        \cgalParamType{a model of `ReadablePropertyMap` whose key type is the value type
                       of the iterator of `PointRange` and whose value type is `geom_traits::Point_3`}
        \cgalParamDefault{`CGAL::Identity_property_map<geom_traits::Point_3>`}
      \cgalParamNEnd

      \cgalParamNBegin{normal_map}
        \cgalParamDescription{a property map associating normals to the elements of the point set `points`}
        \cgalParamType{a model of `ReadablePropertyMap` whose key type is the value type
                       of the iterator of `PointRange` and whose value type is `geom_traits::Vector_3`}
      \cgalParamNEnd

      \cgalParamNBegin{plane_index_map}
        \cgalParamDescription{a property map associating the index of a point in the input range
                              to the index of plane (`-1` if the point is not assigned to a plane)}
        \cgalParamType{a class model of `ReadablePropertyMap` with `std::size_t` as key type and `int` as value type}
        \cgalParamDefault{There is no default, this parameters is mandatory.}
      \cgalParamNEnd

      \cgalParamNBegin{plane_map}
        \cgalParamDescription{a property map containing the planes associated to the elements of the plane range `planes`}
         \cgalParamType{a class model of `ReadablePropertyMap` with `PlaneRange::iterator::value_type`
                        as key type and `geom_traits::Plane_3` as value type}
        \cgalParamDefault{`CGAL::Identity_property_map<Kernel::Plane_3>`}
      \cgalParamNEnd

      \cgalParamNBegin{attraction_factor}
        \cgalParamDescription{multiple of a tolerance `epsilon` used to connect simplices}
        \cgalParamType{floating scalar value}
        \cgalParamDefault{`3`}
      \cgalParamNEnd

     \cgalParamNBegin{geom_traits}
       \cgalParamDescription{an instance of a geometric traits class}
       \cgalParamType{a model of `Kernel`}
       \cgalParamDefault{a \cgal Kernel deduced from the point type, using `CGAL::Kernel_traits`}
     \cgalParamNEnd
   \cgalNamedParamsEnd

*/
template <typename PointRange,
          typename PlaneRange,
          typename OutputIterator,
          typename NamedParameters
          >
OutputIterator
structure_point_set (const PointRange& points,
                     const PlaneRange& planes,
                     OutputIterator output,
                     double epsilon,
                     const NamedParameters& np)
{
  using parameters::choose_parameter;
  using parameters::get_parameter;

  typedef Point_set_processing_3_np_helper<PointRange, NamedParameters> NP_helper;
  typedef typename NP_helper::Geom_traits Kernel;

  Point_set_with_structure<Kernel> pss (points, planes, epsilon, np);

  for (std::size_t i = 0; i < pss.size(); ++ i)
    *(output ++) = pss[i];

  return output;
}

} //namespace CGAL

#include <CGAL/enable_warnings.h>

#endif // CGAL_STRUCTURE_POINT_SET_3_H