// 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$
//
//
// 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/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/Fuzzy_iso_box.h>
#include <CGAL/Search_traits_d.h>

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

#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 Traits 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 Traits>
class Point_set_with_structure
{
  typedef Point_set_with_structure<Traits> Self;

  typedef typename Traits::FT FT;
  typedef typename Traits::Segment_3 Segment;
  typedef typename Traits::Line_3 Line;
  typedef typename Traits::Plane_3 Plane;

  typedef typename Traits::Point_2 Point_2;


  typedef Shape_detection_3::Shape_base<Traits> Shape;

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

public:


  typedef typename Traits::Point_3 Point;
  typedef typename Traits::Vector_3 Vector;
  typedef typename Traits::Point_map Point_map;
  typedef typename Traits::Normal_map Normal_map;
  typedef typename Traits::Input_range Input_range;
  typedef typename Input_range::iterator Input_iterator;
  typedef Shape_detection_3::Plane<Traits> Plane_shape;

  /// 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)
    { planes[0] = a; planes[1] = b; active = true; }
  };
  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;
    }
  };
      

  Traits m_traits;

  std::vector<Point> m_points;
  std::vector<Vector> m_normals;
  std::vector<std::size_t> m_indices;
  std::vector<Point_status> m_status;
  Point_map m_point_map;
  Normal_map m_normal_map;
    
  std::vector<boost::shared_ptr<Plane_shape> > m_planes;
  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.

    \note Both property maps can be omitted if the default constructors of these property maps can be safely used.

  */
  Point_set_with_structure (Input_iterator begin, ///< iterator over the first input point.
                            Input_iterator end, ///< past-the-end iterator over the input points.
                            Point_map point_map, ///< property map: value_type of InputIterator -> Point_3. 
                            Normal_map normal_map, ///< property map: value_type of InputIterator -> Vector_3. 
                            const Shape_detection_3::Efficient_RANSAC<Traits>&
                            shape_detection, ///< shape detection object
                            double epsilon, ///< size parameter
                            double attraction_factor = 3.) ///< attraction factor
    : m_traits (shape_detection.traits()),
      m_point_map(point_map), m_normal_map (normal_map)
  {
    constructor (begin, end, shape_detection, epsilon, attraction_factor);
  }

  /// \cond SKIP_IN_MANUAL
  Point_set_with_structure (Input_iterator begin, ///< iterator over the first input point.
                            Input_iterator end, ///< past-the-end iterator over the input points.
                            const Shape_detection_3::Efficient_RANSAC<Traits>&
                            shape_detection, ///< shape detection object
                            double epsilon, ///< size parameter
                            double attraction_factor = 3.) ///< attraction factor
    : m_traits (shape_detection.traits())
  {
    constructor (begin, end, shape_detection, epsilon, attraction_factor);
  }

  void constructor(Input_iterator begin, ///< iterator over the first input point.
                   Input_iterator end, ///< past-the-end iterator over the input points.
                   const Shape_detection_3::Efficient_RANSAC<Traits>&
                   shape_detection, ///< shape detection object
                   double epsilon, ///< size parameter
                   double attraction_factor = 3.) ///< attraction factor
  {
    m_points.reserve(end - begin);
    m_normals.reserve(end - begin);
    for (Input_iterator it = begin; it != end; ++ it)
      {
        m_points.push_back (get(m_point_map, *it));
        m_normals.push_back (get(m_normal_map, *it));
      }
      
    m_indices.resize (m_points.size (), (std::numeric_limits<std::size_t>::max)());
    m_status.resize (m_points.size (), POINT);

    BOOST_FOREACH (boost::shared_ptr<Shape> shape, shape_detection.shapes())
      {
        boost::shared_ptr<Plane_shape> pshape
          = boost::dynamic_pointer_cast<Plane_shape>(shape);
        
        // Ignore all shapes other than plane
        if (pshape == boost::shared_ptr<Plane_shape>())
          continue;
        m_planes.push_back (pshape);

        for (std::size_t i = 0; i < pshape->indices_of_assigned_points().size (); ++ i)
          {
            m_indices[pshape->indices_of_assigned_points()[i]] = m_planes.size () - 1;
            m_status[pshape->indices_of_assigned_points()[i]] = PLANE;
          }
      }

    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.

   */
  std::vector<boost::shared_ptr<Plane_shape> > adjacency (std::size_t i) const
  {
    std::vector<boost::shared_ptr<Plane_shape> > out;

    if (m_status[i] == PLANE || m_status[i] == RESIDUS)
      out.push_back (m_planes[m_indices[i]]);
    else if (m_status[i] == EDGE)
      {
        out.push_back (m_planes[m_edges[m_indices[i]].planes[0]]);
        out.push_back (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)
          out.push_back (m_planes[m_corners[m_indices[i]].planes[j]]);
      }
    return out;
  }

  /*!

    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_planes.size (); ++ i)
      for (std::size_t j = 0; j < m_planes[i]->indices_of_assigned_points ().size(); ++ j)
        {
          std::size_t ind = m_planes[i]->indices_of_assigned_points ()[j];
          m_points[ind] = static_cast<Plane> (*(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
        Plane plane = static_cast<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_planes[c]->indices_of_assigned_points ().size(); ++ j)
          {
            std::size_t ind = m_planes[c]->indices_of_assigned_points ()[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_planes[c]->indices_of_assigned_points ()[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]->plane_normal();
                    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]->plane_normal());
                m_indices.push_back (c);
                m_status.push_back (RESIDUS);
              }
      }

  }

  void find_pairs_of_adjacent_primitives (double radius)
  {
    typedef typename Traits::Search_traits 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)
      {
        boost::shared_ptr<Plane_shape> plane1 = m_planes[m_edges[i].planes[0]];
        boost::shared_ptr<Plane_shape> plane2 = m_planes[m_edges[i].planes[1]];       

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

        typename cpp11::result_of<typename Traits::Intersect_3(Plane, Plane)>::type
          result = CGAL::intersection(static_cast<Plane>(*plane1),
                                      static_cast<Plane>(*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 < plane1->indices_of_assigned_points ().size(); ++ k)
          {
            std::size_t index_point = plane1->indices_of_assigned_points ()[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 < plane2->indices_of_assigned_points ().size(); ++ k)
          {
            std::size_t index_point = plane2->indices_of_assigned_points ()[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)
      {
        boost::shared_ptr<Plane_shape> plane1 = m_planes[m_edges[i].planes[0]];
        boost::shared_ptr<Plane_shape> 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->plane_normal () + 0.5 * plane2->plane_normal();
							
        //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 < plane1->indices_of_assigned_points().size(); ++ k)
          {
            std::size_t index_point = plane1->indices_of_assigned_points()[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 < plane2->indices_of_assigned_points().size(); ++ k)
          {
            std::size_t index_point = plane2->indices_of_assigned_points()[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 Traits::Intersect_3(Plane, Plane)>::type
              result = CGAL::intersection (static_cast<Plane> (*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]]->plane_normal());
                        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 (static_cast<Plane> (*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]]->plane_normal());
                        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
        Plane plane1 = static_cast<Plane> (*(m_planes[m_corners[i].planes[0]]));
        Plane plane2 = static_cast<Plane> (*(m_planes[m_corners[i].planes[1]]));
        Plane plane3 = static_cast<Plane> (*(m_planes[m_corners[i].planes[2]]));

        typename cpp11::result_of<typename Traits::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 Traits::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]]->plane_normal();
          
        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]]->plane_normal()
                                     + 0.5 * m_planes[m_edges[m_corners[k].edges[ed]].planes[1]]->plane_normal());
                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]]->plane_normal()
                                 + 0.5 * m_planes[m_edges[m_corners[k].edges[ed]].planes[1]]->plane_normal());
            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 Traits 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)`
///
/// @tparam OutputIterator Type of the output iterator. The type of the objects
/// put in it is `std::pair<Traits::Point_3, Traits::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.
///
/// @note If no plane is found in the shape detection object, the
/// algorithm does nothing and the output points are the unaltered
/// input points.
///
/// @note Both property maps can be omitted if the default constructors of these property maps can be safely used.
template <typename Traits,
          typename OutputIterator
>
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
{
  Point_set_with_structure<Traits> pss (first, beyond, point_map, normal_map,
                                        shape_detection, epsilon, attraction_factor);

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

  return output;
}


/// \cond SKIP_IN_MANUAL
template <typename Traits,
          typename OutputIterator
>
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




} //namespace CGAL

#endif // CGAL_STRUCTURE_POINT_SET_3_H