// Copyright (c) 2015 INRIA Sophia-Antipolis (France).
// All rights reserved.
//
// This file is part of CGAL (www.cgal.org).
// You can redistribute it and/or modify it under the terms of the GNU
// General Public License as published by the Free Software Foundation,
// either version 3 of the License, or (at your option) any later version.
//
// Licensees holding a valid commercial license may use this file in
// accordance with the commercial license agreement provided with the software.
//
// This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
// WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
//
// $URL$
// $Id$
// SPDX-License-Identifier: GPL-3.0+
//
//
// 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/point_set_processing_assertions.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/boost/graph/named_function_params.h>
#include <CGAL/boost/graph/named_params_helper.h>

#include <boost/iterator/counting_iterator.hpp>

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

namespace CGAL {

/*!
\ingroup PkgPointSetProcessingAlgorithms

\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 wether 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 `ShapeDetectionTraits` that must provide in
addition a function `Intersect_3 intersection_3_object() const` and a
functor `Intersect_3` with:
- `boost::optional< boost::variant< Traits::Plane_3, Traits::Line_3 > > operator()(typename Traits::Plane_3, typename Traits::Plane_3)`
- `boost::optional< boost::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
  {
    CGAL::cpp11::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 optional sequence of \ref psp_namedparameters "Named Parameters" among the ones listed below.

    \cgalNamedParamsBegin
      \cgalParamBegin{point_map} a model of `ReadablePropertyMap` with value type `Kernel::Point_3`.
      If this parameter is omitted, `CGAL::Identity_property_map<Kernel::Point_3>` is used.\cgalParamEnd
      \cgalParamBegin{normal_map} a model of `ReadablePropertyMap` with value type
      `Kernel::Vector_3`.\cgalParamEnd
      \cgalParamBegin{plane_index_map} a model of `ReadablePropertyMap` with value type `int`.
      Associates the index of a point in the input range to the index of plane (-1 if point does is not assigned to
      a plane).\cgalParamEnd
      \cgalParamBegin{plane_map} a model of `ReadablePropertyMap` with value type
      `Kernel::Plane_3`. If this parameter is omitted, `CGAL::Identity_property_map<Kernel::Plane_3>`
      is used.\cgalParamEnd
      \cgalParamBegin{attraction_factor} multiple of `epsilon` used to connect simplices.\cgalParamEnd
    \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
  // deprecated
  template <typename PointRange,
            typename PointMap,
            typename NormalMap,
            typename PlaneRange,
            typename PlaneMap,
            typename IndexMap>
  CGAL_DEPRECATED_MSG("you are using the deprecated V1 API of CGAL::Point_set_with_structure(), please update your code")
  Point_set_with_structure (const PointRange& points,
                            PointMap point_map,
                            NormalMap normal_map,
                            const PlaneRange& planes,
                            PlaneMap plane_map,
                            IndexMap index_map,
                            double epsilon,
                            double attraction_factor = 3.)
  {
    init (points, planes, epsilon,
          CGAL::parameters::point_map (point_map).
          normal_map (normal_map).
          plane_map (plane_map).
          plane_index_map (index_map).
          attraction_factor (attraction_factor));
  }

  template <typename PointRange,
            typename PlaneRange,
            typename NamedParameters>
  void init (const PointRange& points,
             const PlaneRange& planes,
             double epsilon,
             const NamedParameters& np)
  {
    using boost::choose_param;

    // basic geometric types
    typedef typename Point_set_processing_3::GetPointMap<PointRange, NamedParameters>::type PointMap;
    typedef typename Point_set_processing_3::GetNormalMap<PointRange, NamedParameters>::type NormalMap;
    typedef typename Point_set_processing_3::GetPlaneMap<PlaneRange, NamedParameters>::type PlaneMap;
    typedef typename Point_set_processing_3::GetPlaneIndexMap<NamedParameters>::type PlaneIndexMap;

    CGAL_static_assertion_msg(!(boost::is_same<NormalMap,
                                typename Point_set_processing_3::GetNormalMap<PointRange, NamedParameters>::NoMap>::value),
                              "Error: no normal map");
    CGAL_static_assertion_msg(!(boost::is_same<PlaneIndexMap,
                                typename Point_set_processing_3::GetPlaneIndexMap<NamedParameters>::NoMap>::value),
                              "Error: no plane index map");

    PointMap point_map = choose_param(get_param(np, internal_np::point_map), PointMap());
    NormalMap normal_map = choose_param(get_param(np, internal_np::normal_map), NormalMap());
    PlaneMap plane_map = choose_param(get_param(np, internal_np::plane_map), PlaneMap());
    PlaneIndexMap index_map = choose_param(get_param(np, internal_np::plane_index_map), PlaneIndexMap());
    double attraction_factor = choose_param(get_param(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 = 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 CGAL::cpp11::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 desactivate 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 desactivate 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;

        typename cpp11::result_of<typename Kernel::Intersect_3(Plane, Plane)>::type
          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 = boost::get<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; // Deactive 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]);
              }

            typename cpp11::result_of<typename Kernel::Intersect_3(Plane, Plane)>::type
              result = CGAL::intersection (plane1, ortho);
            if (result)
              {
                if (const Line* l = boost::get<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 = boost::get<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]];

        typename cpp11::result_of<typename Kernel::Intersect_3(Plane, Plane)>::type
          result = CGAL::intersection(plane1, plane2);
        
        if (result)
          {
            if (const Line* l = boost::get<Line>(&*result))
              {
                typename cpp11::result_of<typename Kernel::Intersect_3(Line, Plane)>::type
                  result2 = CGAL::intersection(*l, plane3);

                if (result2)
                  {
                    if (const Point* p = boost::get<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 PkgPointSetProcessingAlgorithms

   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="http://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 optional sequence of \ref psp_namedparameters "Named Parameters" among the ones listed below.

   \cgalNamedParamsBegin
     \cgalParamBegin{point_map} a model of `ReadablePropertyMap` with value type `geom_traits::Point_3`.
     If this parameter is omitted, `CGAL::Identity_property_map<geom_traits::Point_3>` is used.\cgalParamEnd
     \cgalParamBegin{normal_map} a model of `ReadablePropertyMap` with value type
     `geom_traits::Vector_3`.\cgalParamEnd
     \cgalParamBegin{plane_index_map} a model of `ReadablePropertyMap` with value type `int`.
     Associates the index of a point in the input range to the index of plane (-1 if point does is not assigned to
     a plane).\cgalParamEnd
     \cgalParamBegin{plane_map} a model of `ReadablePropertyMap` with value type
     `geom_traits::Plane_3`. If this parameter is omitted, `CGAL::Identity_property_map<geom_traits::Plane_3>`
     is used.\cgalParamEnd
     \cgalParamBegin{attraction_factor} multiple of `epsilon` used to connect simplices.\cgalParamEnd
     \cgalParamBegin{geom_traits} an instance of a geometric traits class, model of `Kernel`\cgalParamEnd
   \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 boost::choose_param;

  typedef typename Point_set_processing_3::GetK<PointRange, NamedParameters>::Kernel 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;
}

/// \cond SKIP_IN_MANUAL
// variant with default NP
template <typename PointRange,
          typename PlaneRange,
          typename OutputIterator>
OutputIterator
structure_point_set (const PointRange& points, ///< range of points.
                     const PlaneRange& planes, ///< range of planes.
                     OutputIterator output, ///< output iterator where output points are written.
                     double epsilon) ///< size parameter.
{
  return structure_point_set
    (points, planes, output, epsilon,
     CGAL::Point_set_processing_3::parameters::all_default(points));
}
/// \endcond

#ifndef CGAL_NO_DEPRECATED_CODE
/// \cond SKIP_IN_MANUAL

namespace Shape_detection_3{
//Forward declarations
template <class Traits>
class Efficient_RANSAC;
template <typename Traits>
class Plane_map;
template <typename Traits>
class Point_to_shape_index_map;
} // end of namespace Shape_detection_3

// deprecated API
template <typename Traits,
          typename OutputIterator
>
CGAL_DEPRECATED_MSG("you are using the deprecated V1 API of CGAL::structure_point_set(), please update your code")
OutputIterator
structure_point_set (typename Traits::Input_range::iterator first,  ///< iterator over the first input point.
                     typename Traits::Input_range::iterator beyond, ///< past-the-end iterator over the input points.
                     typename Traits::Point_map point_map, ///< property map: value_type of InputIterator -> Point_3.
                     typename Traits::Normal_map normal_map, ///< property map: value_type of InputIterator -> Vector_3.
                     OutputIterator output, ///< output iterator where output points are written
                     Shape_detection_3::Efficient_RANSAC<Traits>&
                     shape_detection, ///< shape detection object
                     double epsilon, ///< size parameter
                     double attraction_factor = 3.) ///< attraction factor
{
  typename Shape_detection_3::Efficient_RANSAC<Traits>::Plane_range planes = shape_detection.planes();
  return structure_point_set (CGAL::make_range(first, beyond),
                              planes,
                              output,
                              epsilon, // epsilon for structuring points
                              CGAL::parameters::point_map (point_map).
                              normal_map (normal_map).
                              plane_map (CGAL::Shape_detection_3::Plane_map<Traits>()).
                              plane_index_map (Shape_detection_3::Point_to_shape_index_map<Traits>(CGAL::make_range(first, beyond), planes)).
                              attraction_factor(attraction_factor));
}

// deprecated API
template <typename Traits,
          typename OutputIterator
>
CGAL_DEPRECATED_MSG("you are using the deprecated V1 API of CGAL::structure_point_set(), please update your code")
OutputIterator
structure_point_set (typename Traits::Input_range::iterator first,  ///< iterator over the first input point.
                     typename Traits::Input_range::iterator beyond, ///< past-the-end iterator over the input points.
                     OutputIterator output, ///< output iterator where output points are written
                     Shape_detection_3::Efficient_RANSAC<Traits>&
                     shape_detection, ///< shape detection object
                     double epsilon, ///< size parameter
                     double attraction_factor = 3.) ///< attraction factor
{
  return structure_point_set (first, beyond,
                              typename Traits::Point_map(),
                              typename Traits::Normal_map(),
                              output,
                              shape_detection,
                              epsilon,
                              attraction_factor);
}
/// \endcond
#endif // CGAL_NO_DEPRECATED_CODE


} //namespace CGAL

#include <CGAL/enable_warnings.h>

#endif // CGAL_STRUCTURE_POINT_SET_3_H