// 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
//

/**
 * \ingroup PkgPointSetShapeDetection3
 * \file CGAL/regularize_planes.h
 *
 */


#ifndef CGAL_REGULARIZE_PLANES_H
#define CGAL_REGULARIZE_PLANES_H

#include <CGAL/Shape_detection_3.h>
#include <CGAL/centroid.h>

#include <boost/foreach.hpp>


namespace CGAL {
  
namespace internal {
namespace PlaneRegularization {

template <typename Traits>
struct Plane_cluster
{
  bool is_free;
  std::vector<std::size_t> planes;
  std::vector<std::size_t> coplanar_group;
  std::vector<std::size_t> orthogonal_clusters;
  typename Traits::Vector_3 normal;
  typename Traits::FT cosangle_symmetry;
  typename Traits::FT area;
  typename Traits::FT cosangle_centroid;
};

  
template <typename Traits>
typename Traits::Vector_3 regularize_normal
  (const typename Traits::Vector_3& n,
   const typename Traits::Vector_3& symmetry_direction,
   typename Traits::FT cos_symmetry)
{
  typedef typename Traits::FT FT;
  typedef typename Traits::Point_3 Point;
  typedef typename Traits::Vector_3 Vector;
  typedef typename Traits::Line_3 Line;
  typedef typename Traits::Plane_3 Plane;

  if (symmetry_direction == CGAL::NULL_VECTOR)
    return n;
      
  Point pt_symmetry = CGAL::ORIGIN + cos_symmetry* symmetry_direction;

  Plane plane_symmetry (pt_symmetry, symmetry_direction);
  Point pt_normal = CGAL::ORIGIN + n;

  if (n != symmetry_direction || n != -symmetry_direction)
    {
      Plane plane_cut (CGAL::ORIGIN, pt_normal, CGAL::ORIGIN + symmetry_direction);
      Line line;
      CGAL::Object ob_1 = CGAL::intersection(plane_cut, plane_symmetry);
      if (!assign(line, ob_1))
        return n;

      FT delta = std::sqrt ((FT)1. - cos_symmetry * cos_symmetry);

      Point projected_origin = line.projection (CGAL::ORIGIN);
      Vector line_vector (line);
      line_vector = line_vector / std::sqrt (line_vector * line_vector);
      Point pt1 = projected_origin + delta * line_vector;
      Point pt2 = projected_origin - delta * line_vector;

      if (CGAL::squared_distance (pt_normal, pt1) <= CGAL::squared_distance (pt_normal, pt2))
        return Vector (CGAL::ORIGIN, pt1);
      else
        return Vector (CGAL::ORIGIN, pt2);

    }
  else
    return n;
}

template <typename Traits>  
typename Traits::Vector_3 regularize_normals_from_prior
  (const typename Traits::Vector_3& np,
   const typename Traits::Vector_3& n,
   const typename Traits::Vector_3& symmetry_direction,
   typename Traits::FT cos_symmetry)
{
  typedef typename Traits::FT FT;
  typedef typename Traits::Point_3 Point;
  typedef typename Traits::Vector_3 Vector;
  typedef typename Traits::Line_3 Line;
  typedef typename Traits::Plane_3 Plane;

  if (symmetry_direction == CGAL::NULL_VECTOR)
    return n;

  Plane plane_orthogonality (CGAL::ORIGIN, np);
  Point pt_symmetry = CGAL::ORIGIN + cos_symmetry* symmetry_direction;

  Plane plane_symmetry (pt_symmetry, symmetry_direction);
		
  Line line;
  CGAL::Object ob_1 = CGAL::intersection (plane_orthogonality, plane_symmetry);
  if (!assign(line, ob_1))
    return regularize_normal<Traits> (n, symmetry_direction, cos_symmetry);

  Point projected_origin = line.projection (CGAL::ORIGIN);
  FT R = CGAL::squared_distance (Point (CGAL::ORIGIN), projected_origin);

  if (R <= 1)  // 2 (or 1) possible points intersecting the unit sphere and line
    {
      FT delta = std::sqrt ((FT)1. - R);
      Vector line_vector(line); 
      line_vector = line_vector / std::sqrt (line_vector * line_vector);
      Point pt1 = projected_origin + delta * line_vector;
      Point pt2 = projected_origin - delta * line_vector;
			
      Point pt_n = CGAL::ORIGIN + n;
      if (CGAL::squared_distance (pt_n, pt1) <= CGAL::squared_distance (pt_n, pt2))
        return Vector (CGAL::ORIGIN, pt1);
      else
        return Vector (CGAL::ORIGIN, pt2);
    }
  else //no point intersecting the unit sphere and line
    return regularize_normal<Traits> (n,symmetry_direction, cos_symmetry);

}

template <typename Traits,
          typename RandomAccessIterator,
          typename PlaneContainer,
          typename PointPMap,
          typename CentroidContainer,
          typename AreaContainer>
void compute_centroids_and_areas (RandomAccessIterator input_begin,
                                  PlaneContainer& planes,
                                  PointPMap point_pmap,
                                  CentroidContainer& centroids,
                                  AreaContainer& areas)
{
  typedef typename Traits::FT FT;
  typedef typename Traits::Point_3 Point;
  
  for (std::size_t i = 0; i < planes.size (); ++ i)
    {
      std::vector < Point > listp;
      for (std::size_t j = 0; j < planes[i]->indices_of_assigned_points ().size (); ++ j)
        {
          std::size_t yy = planes[i]->indices_of_assigned_points()[j];
          Point pt = get (point_pmap, *(input_begin + yy));
          listp.push_back(pt);
        }
      centroids.push_back (CGAL::centroid (listp.begin (), listp.end ()));
      areas.push_back ((FT)(planes[i]->indices_of_assigned_points().size()) / (FT)100.);
    }
}


template <typename Traits,
          typename PlaneContainer,
          typename PlaneClusterContainer,
          typename AreaContainer>
void compute_parallel_clusters (PlaneContainer& planes,
                                PlaneClusterContainer& clusters,
                                AreaContainer& areas,
                                typename Traits::FT tolerance_cosangle,
                                const typename Traits::Vector_3& symmetry_direction)
{

  typedef typename Traits::FT FT;
  typedef typename Traits::Vector_3 Vector;
  
  typedef typename PlaneClusterContainer::value_type Plane_cluster;
  
  // find pairs of epsilon-parallel primitives and store them in parallel_planes
  std::vector<std::vector<std::size_t> > parallel_planes (planes.size ());
  for (std::size_t i = 0; i < planes.size (); ++ i)
    {
      Vector v1 = planes[i]->plane_normal ();
          
      for (std::size_t j = 0; j < planes.size(); ++ j)
        {
          if (i == j)
            continue;
              
          Vector v2 = planes[i]->plane_normal ();
              
          if (std::fabs (v1 * v2) > 1. - tolerance_cosangle)
            parallel_planes[i].push_back (j);
        }
    }


  std::vector<bool> is_available (planes.size (), true);
      
  for (std::size_t i = 0; i < planes.size(); ++ i)
    {

      if(is_available[i])
        {
          is_available[i] = false;

          clusters.push_back (Plane_cluster());
          Plane_cluster& clu = clusters.back ();

          //initialization containers
          clu.planes.push_back (i);
              
          std::vector<std::size_t> index_container_former_ring_parallel;
          index_container_former_ring_parallel.push_back(i);
              
          std::list<std::size_t> index_container_current_ring_parallel;

          //propagation over the pairs of epsilon-parallel primitives
          bool propagation=true;
          clu.normal = planes[i]->plane_normal ();
          clu.area = areas[i];
			
          do
            {
              propagation = false;

              for (std::size_t k = 0; k < index_container_former_ring_parallel.size(); ++ k)
                {

                  std::size_t plane_index = index_container_former_ring_parallel[k];

                  for (std::size_t l = 0; l < parallel_planes[plane_index].size(); ++ l)
                    {
                      std::size_t it = parallel_planes[plane_index][l];
                          
                      Vector normal_it =  planes[it]->plane_normal ();

                      if(is_available[it]
                         && std::fabs (normal_it*clu.normal) > 1. - tolerance_cosangle )
                        {	
                          propagation = true;
                          index_container_current_ring_parallel.push_back(it);
                          is_available[it]=false;
                              
                          if(clu.normal * normal_it <0)
                            normal_it = -normal_it;

                          clu.normal = (FT)clu.area * clu.normal
                            + (FT)areas[it] * normal_it;
                          FT norm = (FT)1. / std::sqrt (clu.normal.squared_length()); 
                          clu.normal = norm * clu.normal;
                          clu.area += areas[it];
                        }	
                    }
                }

              //update containers
              index_container_former_ring_parallel.clear();
              for (std::list<std::size_t>::iterator it = index_container_current_ring_parallel.begin();
                   it != index_container_current_ring_parallel.end(); ++it)
                {
                  index_container_former_ring_parallel.push_back(*it);
                  clu.planes.push_back(*it);
                }
              index_container_current_ring_parallel.clear();

            }
          while(propagation);

          if (symmetry_direction != CGAL::NULL_VECTOR)
            clu.cosangle_symmetry = std::fabs(symmetry_direction * clu.normal);
        }
    }
  is_available.clear();
}

template <typename Traits,
          typename PlaneClusterContainer>
void cluster_symmetric_cosangles (PlaneClusterContainer& clusters,
                                  typename Traits::FT tolerance_cosangle)
{
  typedef typename Traits::FT FT;
  
  std::vector < FT > cosangle_centroids;
  std::vector < std::size_t> list_cluster_index;
  for( std::size_t i = 0; i < clusters.size(); ++ i)
    list_cluster_index.push_back(static_cast<std::size_t>(-1));
      
  std::size_t mean_index = 0;
  for (std::size_t i = 0; i < clusters.size(); ++ i)
    {
      if(list_cluster_index[i] == static_cast<std::size_t>(-1))
        {
          list_cluster_index[i] = mean_index;
          FT mean = clusters[i].area * clusters[i].cosangle_symmetry;
          FT mean_area = clusters[i].area;
              
          for (std::size_t j = i+1; j < clusters.size(); ++ j)
            {
              if (list_cluster_index[j] == static_cast<std::size_t>(-1)
                  && std::fabs (clusters[j].cosangle_symmetry -
                                mean / mean_area) < tolerance_cosangle)
                {
                  list_cluster_index[j] = mean_index;
                  mean_area += clusters[j].area;
                  mean += clusters[j].area * clusters[j].cosangle_symmetry;
                }
            }
          ++ mean_index;
          mean /= mean_area;
          cosangle_centroids.push_back (mean);
        }
    }

  for (std::size_t i = 0; i < cosangle_centroids.size(); ++ i)
    {
      if (cosangle_centroids[i] < tolerance_cosangle)
        cosangle_centroids[i] = 0;
      else if (cosangle_centroids[i] > 1. - tolerance_cosangle)
        cosangle_centroids[i] = 1;
    }
  for (std::size_t i = 0; i < clusters.size(); ++ i)
    clusters[i].cosangle_symmetry = cosangle_centroids[list_cluster_index[i]];
}


template <typename Traits,
          typename PlaneClusterContainer>
void subgraph_mutually_orthogonal_clusters (PlaneClusterContainer& clusters,
                                            const typename Traits::Vector_3& symmetry_direction)
{
  typedef typename Traits::FT FT;
  typedef typename Traits::Vector_3 Vector;
  
  std::vector < std::vector < std::size_t> > subgraph_clusters;
  std::vector < std::size_t> subgraph_clusters_max_area_index;

  for (std::size_t i = 0; i < clusters.size(); ++ i)
    clusters[i].is_free = true;

  for (std::size_t i = 0; i < clusters.size(); ++ i)
    {
      if(clusters[i].is_free)
        {
          clusters[i].is_free = false;
          FT max_area = clusters[i].area;
          std::size_t index_max_area = i;

          //initialization containers
          std::vector < std::size_t > index_container;
          index_container.push_back(i);
          std::vector < std::size_t > index_container_former_ring;
          index_container_former_ring.push_back(i);
          std::list < std::size_t > index_container_current_ring;

          //propagation
          bool propagation=true;
          do
            {
              propagation=false;

              //neighbors
              for (std::size_t k=0;k<index_container_former_ring.size();k++)
                {

                  std::size_t cluster_index=index_container_former_ring[k];

                  for (std::size_t j = 0; j < clusters[cluster_index].orthogonal_clusters.size(); ++ j)
                    {
                      if(clusters[j].is_free)
                        { 	
                          propagation = true;
                          index_container_current_ring.push_back(j);
                          clusters[j].is_free = false;

                          if(max_area < clusters[j].area)
                            {
                              max_area = clusters[j].area;
                              index_max_area = j;
                            }
                        }	
                    }
                }

              //update containers
              index_container_former_ring.clear();
              for(std::list < std::size_t>::iterator it = index_container_current_ring.begin();
                  it != index_container_current_ring.end(); ++it)
                {
                  index_container_former_ring.push_back(*it);
                  index_container.push_back(*it);
                }
              index_container_current_ring.clear();

            }
          while(propagation);
          subgraph_clusters.push_back(index_container);
          subgraph_clusters_max_area_index.push_back(index_max_area);
        }
    }


  //create subgraphs of mutually orthogonal clusters in which the
  //largest cluster is excluded and store in
  //subgraph_clusters_prop
  std::vector < std::vector < std::size_t> > subgraph_clusters_prop;
  for (std::size_t i=0;i<subgraph_clusters.size(); i++)
    {
      std::size_t index=subgraph_clusters_max_area_index[i];
      std::vector < std::size_t> subgraph_clusters_prop_temp;
      for (std::size_t j=0;j<subgraph_clusters[i].size(); j++)
        if(subgraph_clusters[i][j]!=index)
          subgraph_clusters_prop_temp.push_back(subgraph_clusters[i][j]);

      subgraph_clusters_prop.push_back(subgraph_clusters_prop_temp);
    }



  //regularization of cluster normals : in eachsubgraph, we start
  //from the largest area cluster and we propage over the subgraph
  //by regularizing the normals of the clusters accorting to
  //orthogonality and cosangle to symmetry direction

  for (std::size_t i = 0; i < clusters.size(); ++ i)
    clusters[i].is_free = true;

  for (std::size_t i = 0; i < subgraph_clusters_prop.size(); ++ i)
    {
	
      std::size_t index_current=subgraph_clusters_max_area_index[i];
      Vector vec_current=regularize_normal<Traits>
        (clusters[index_current].normal,
         symmetry_direction,
         clusters[index_current].cosangle_symmetry);
      clusters[index_current].normal = vec_current;
      clusters[index_current].is_free = false;

      //initialization containers
      std::vector < std::size_t> index_container;
      index_container.push_back(index_current);
      std::vector < std::size_t> index_container_former_ring;
      index_container_former_ring.push_back(index_current);
      std::list < std::size_t> index_container_current_ring;

      //propagation
      bool propagation=true;
      do
        {
          propagation=false;

          //neighbors
          for (std::size_t k=0;k<index_container_former_ring.size();k++)
            {

              std::size_t cluster_index=index_container_former_ring[k];

              for (std::size_t j = 0; j < clusters[cluster_index].orthogonal_clusters.size(); ++ j)
                {
						
                  if(clusters[j].is_free)
                    { 	
							
                      propagation = true;
                      index_container_current_ring.push_back(j);
                      clusters[j].is_free = false;

                      Vector new_vect=regularize_normals_from_prior<Traits>
                        (clusters[cluster_index].normal,
                         clusters[j].normal,
                         symmetry_direction,
                         clusters[j].cosangle_symmetry);
                      clusters[j].normal = new_vect;
                    }
                }	
            }
			
          //update containers
          index_container_former_ring.clear();
          for(std::list < std::size_t>::iterator it = index_container_current_ring.begin();
              it != index_container_current_ring.end(); ++it)
            {
              index_container_former_ring.push_back(*it);
              index_container.push_back(*it);
            }
          index_container_current_ring.clear();
        }while(propagation);
    }
}
                                    


} // namespace PlaneRegularization
} // namespace internal

  
  /*! 

    Given a set of detected planes with their respective inlier sets,
    this function enables to regularize the planes: planes almost
    parallel are made exactly parallel. In addition, some additional
    regularization can be performed:

    - Plane clusters that are almost orthogonal can be made exactly
    orthogonal.

    - Planes that are parallel and almost coplanar can be made exactly
    coplanar.

    - Planes that are almost symmetrical with a user-defined axis can be
    made exactly symmetrical.

    Planes are directly modified. Points are left unaltered, as well as
    their relationships to planes (no transfer of point from a primitive
    plane to another).

    The implementation follows \cgalCite{cgal:vla-lod-15}.

    \tparam Traits a model of `EfficientRANSACTraits`

    \param shape_detection Shape detection engine used to detect
    shapes from the input data. This engine may handle any types of
    primitive shapes but only planes will be regularized.

    \warning The `shape_detection` parameter must have already
    detected shapes and must have been using `input_range` as input.

    \param tolerance_angle Tolerance of deviation between normal
    vectors of planes so that they are considered parallel (in
    degrees).

    \param tolerance_coplanarity Maximal distance between two
    parallel planes such that they are considered coplanar. The
    default value is 0, meaning that coplanarity is not taken into
    account for regularization.

    \param regularize_orthogonality Make almost orthogonal clusters
    of plane exactly orthogonal.

    \param symmetry_direction Make clusters that are almost
    symmetrical in the symmetry direction exactly symmetrical. This
    parameter is ignored if it is equal to `CGAL::NULL_VECTOR`
  (default value).

  \return The number of clusters of parallel planes found.
*/ 

template <typename RandomAccessIterator,
          typename EfficientRANSACTraits>
void regularize_planes (RandomAccessIterator input_begin,
                        RandomAccessIterator /*input_end*/,
                        const Shape_detection_3::Efficient_RANSAC<EfficientRANSACTraits>& shape_detection,
                        typename EfficientRANSACTraits::FT tolerance_angle
                        = (typename EfficientRANSACTraits::FT)25.0,
                        typename EfficientRANSACTraits::FT tolerance_coplanarity
                        = (typename EfficientRANSACTraits::FT)0.0,
                        bool regularize_orthogonality = true,
                        typename EfficientRANSACTraits::Vector_3 symmetry_direction
                        = CGAL::NULL_VECTOR)
{
  typedef typename EfficientRANSACTraits::FT FT;
  typedef typename EfficientRANSACTraits::Point_3 Point;
  typedef typename EfficientRANSACTraits::Vector_3 Vector;
  typedef typename EfficientRANSACTraits::Plane_3 Plane;
  typedef typename EfficientRANSACTraits::Point_map Point_map;

  typedef Shape_detection_3::Shape_base<EfficientRANSACTraits> Shape;
  typedef Shape_detection_3::Plane<EfficientRANSACTraits> Plane_shape;

  typedef typename internal::PlaneRegularization::Plane_cluster<EfficientRANSACTraits>
    Plane_cluster;

  std::vector<boost::shared_ptr<Plane_shape> > planes;
    
  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;
      planes.push_back (pshape);
    }


  /*
   * Compute centroids and areas
   */
  std::vector<Point> centroids;
  std::vector<FT> areas;
  internal::PlaneRegularization::compute_centroids_and_areas<EfficientRANSACTraits>
    (input_begin, planes, Point_map(), centroids, areas);

  FT tolerance_cosangle = (FT)1. - std::cos (tolerance_angle);
      
  // clustering the parallel primitives and store them in clusters
  // & compute the normal, size and cos angle to the symmetry
  // direction of each cluster
  std::vector<Plane_cluster> clusters;
  internal::PlaneRegularization::compute_parallel_clusters<EfficientRANSACTraits>
    (planes, clusters, areas, tolerance_cosangle, symmetry_direction);

  if (regularize_orthogonality)
    {
      //discovery orthogonal relationship between clusters 
      for (std::size_t i = 0; i < clusters.size(); ++ i)
        {
          for (std::size_t j = i + 1; j < clusters.size(); ++ j)
            {
              
              if (std::fabs (clusters[i].normal * clusters[j].normal) < tolerance_cosangle)
                {
                  clusters[i].orthogonal_clusters.push_back (j);
                  clusters[j].orthogonal_clusters.push_back (i);
                }
            }
        }
    }
      
  //clustering the symmetry cosangle and store their centroids in
  //cosangle_centroids and the centroid index of each cluster in
  //list_cluster_index
  if (symmetry_direction != CGAL::NULL_VECTOR)
    internal::PlaneRegularization::cluster_symmetric_cosangles<EfficientRANSACTraits>
      (clusters, tolerance_cosangle);

  //find subgraphs of mutually orthogonal clusters (store index of
  //clusters in subgraph_clusters), and select the cluster of
  //largest area
  if (regularize_orthogonality)
    internal::PlaneRegularization::subgraph_mutually_orthogonal_clusters<EfficientRANSACTraits>
      (clusters, symmetry_direction);
      
  //recompute optimal plane for each primitive after normal regularization
  for (std::size_t i=0; i < clusters.size(); ++ i)
    {

      Vector vec_reg = clusters[i].normal;

      for (std::size_t j = 0; j < clusters[i].planes.size(); ++ j)
        {
          std::size_t index_prim = clusters[i].planes[j];
          Point pt_reg = planes[index_prim]->projection (centroids[index_prim]);
          if( planes[index_prim]->plane_normal () * vec_reg < 0)
            vec_reg=-vec_reg;
          Plane plane_reg(pt_reg,vec_reg);
              
          if( std::fabs(planes[index_prim]->plane_normal () * plane_reg.orthogonal_vector ()) > 1. - tolerance_cosangle)
            planes[index_prim]->update (plane_reg);
        }
    }


  //detecting co-planarity and store in list_coplanar_prim
  for (std::size_t i = 0; i < clusters.size(); ++ i)
    {
      Vector vec_reg = clusters[i].normal;

      for (std::size_t ip = 0; ip < clusters[i].planes.size(); ++ ip)
        clusters[i].coplanar_group.push_back (static_cast<std::size_t>(-1));

      std::size_t cop_index=0;

      for (std::size_t j = 0; j < clusters[i].planes.size(); ++ j)
        {
          std::size_t index_prim = clusters[i].planes[j];

          if (clusters[i].coplanar_group[j] == static_cast<std::size_t>(-1))
            {
              clusters[i].coplanar_group[j] = cop_index;
			
              Point pt_reg = planes[index_prim]->projection(centroids[index_prim]);
              Plane plan_reg(pt_reg,vec_reg);

              for (std::size_t k = j + 1; k < clusters[i].planes.size(); ++ k)
                {
                  if (clusters[i].coplanar_group[k] == static_cast<std::size_t>(-1))
                    {
                      std::size_t index_prim_next = clusters[i].planes[k];
                      Point pt_reg_next = planes[index_prim_next]->projection(centroids[index_prim_next]);
                      Point pt_proj=plan_reg.projection(pt_reg_next);
                      FT distance = std::sqrt (CGAL::squared_distance(pt_reg_next,pt_proj));
					
                      if (distance < tolerance_coplanarity)
                        clusters[i].coplanar_group[k] = cop_index;
                    }
                }
              cop_index++; 
            }
        }

      //regularize primitive position by computing barycenter of cplanar planes
      std::vector<Point> pt_bary (cop_index, Point ((FT)0., (FT)0., (FT)0.));
      std::vector<FT> area (cop_index, 0.);
      
      for (std::size_t j = 0; j < clusters[i].planes.size (); ++ j)
        {
          std::size_t index_prim = clusters[i].planes[j];
          std::size_t group = clusters[i].coplanar_group[j];
              
          Point pt_reg = planes[index_prim]->projection(centroids[index_prim]);

          pt_bary[group] = CGAL::barycenter (pt_bary[group], area[group], pt_reg, areas[index_prim]); 
          area[group] += areas[index_prim];
        }


      for (std::size_t j = 0; j < clusters[i].planes.size (); ++ j)
        {
          std::size_t index_prim = clusters[i].planes[j];
          std::size_t group = clusters[i].coplanar_group[j];
              
          Plane plane_reg (pt_bary[group], vec_reg);
              
          if (planes[index_prim]->plane_normal ()
              * plane_reg.orthogonal_vector() < 0)
            planes[index_prim]->update (plane_reg.opposite());
          else
            planes[index_prim]->update (plane_reg);
        }
    }
      
}
  /// @}


} // namespace CGAL

#endif // CGAL_REGULARIZE_PLANES_H