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

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


#ifndef CGAL_PLANE_REGULARIZATION_H
#define CGAL_PLANE_REGULARIZATION_H

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

#include <boost/foreach.hpp>


namespace CGAL {

  template <typename Traits>
  class Plane_regularization
  {
  public:

    typedef Plane_regularization<Traits> Self;

    typedef typename Traits::FT FT;
    typedef typename Traits::Point_3 Point;
    typedef typename Traits::Vector_3 Vector;
    typedef typename Traits::Plane_3 Plane;

    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::Shape_base<Traits> Shape;
    typedef Shape_detection_3::Plane<Traits> Plane_shape;
  

  private:

    Traits m_traits;

    Input_iterator m_input_begin;
    Input_iterator m_input_end;
    Point_map m_point_pmap;
    Normal_map m_normal_pmap;

    std::vector<boost::shared_ptr<Plane_shape> > m_planes;

  public:

    Plane_regularization (Traits t = Traits ())
      : m_traits (t)
    {

    }

    Plane_regularization (Input_range& input_range,
                          const Shape_detection_3::Efficient_RANSAC<Traits>& shape_detection)
      : m_traits (shape_detection.traits())
    {
      m_input_begin = input_range.begin ();
      m_input_end = input_range.end ();

      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);

        }

    }

    virtual ~Plane_regularization ()
    {
      clear ();
    }

    void clear ()
    {
      std::vector<boost::shared_ptr<Plane_shape> > ().swap (m_planes);
    }


  
    void run (FT epsilon, FT tolerance_coplanarity)
    {
      // WRAPPER BEGIN
      std::vector<Plane> extracted_planes;
      std::vector<std::vector<int> > plane_point_index;
      std::vector<int> primitive_index (m_input_end - m_input_begin, -1);
      std::vector<int> label_plane (m_input_end - m_input_begin, -1);
      std::vector<Point> list_centroid;
      std::vector<FT> list_areas;
      
      for (std::size_t i = 0; i < m_planes.size (); ++ i)
        {
          extracted_planes.push_back (static_cast<Plane> (*(m_planes[i])));
          plane_point_index.push_back (std::vector<int>());
          std::copy (m_planes[i]->indices_of_assigned_points().begin (),
                     m_planes[i]->indices_of_assigned_points().begin (),
                     std::back_inserter (plane_point_index.back ()));

          std::vector < Point > listp;
          for (std::size_t j = 0; j < plane_point_index.back ().size (); ++ j)
            {
              primitive_index[j] = i;
              
              int yy = plane_point_index.back()[j];
              label_plane[yy] = i;

              Point pt = get (m_point_pmap, *(m_input_begin + yy));
              listp.push_back(pt);
            }
          list_centroid.push_back (CGAL::centroid (listp.begin (), listp.end ()));
          list_areas.push_back ((double)(plane_point_index.back().size()) / 100.);
        }
      
      // WRAPPER END



      
      // find pairs of epsilon-parallel primitives and store them in table_parallel 
      std::vector < std::vector < bool > > table_parallel; 
      for( std::size_t i=0;i<extracted_planes.size(); i++)
        {  
          std::vector < bool > table_parallel_tmp; 
          for( std::size_t j=0;j<extracted_planes.size(); j++)
            { 
			
              Vector v1=extracted_planes[i].orthogonal_vector();
              Vector v2=extracted_planes[j].orthogonal_vector();
			
              if (std::fabs(v1*v2)>1.-epsilon && i!=j)
                table_parallel_tmp.push_back(true); 
              else
                table_parallel_tmp.push_back(false); 
            }
          table_parallel.push_back(table_parallel_tmp);
        }
	


      // clustering the parallel primitives and store them in list_parallel_planes
      // & compute the normal, size and cos angle to the vertical of each cluster, and store that in list_cluster_normales, list_cluster_angle and list_cluster_area
      std::vector < std::vector < int > > list_parallel_planes;
      std::vector < Vector > list_cluster_normales;
      std::vector < double > list_cluster_cosangle_vertical; 
      std::vector < double > list_cluster_area;
      
      std::vector < bool > is_available;
      for( std::size_t i=0;i<extracted_planes.size();i++)
        is_available.push_back(true);
      
      for( std::size_t i=0;i<extracted_planes.size();i++)
        {

          if(is_available[i])
            {

              is_available[i]=false;

              //initialization containers
              std::vector < int > index_container_parallel;
              index_container_parallel.push_back(i);
              
              std::vector < int > index_container_former_ring_parallel;
              index_container_former_ring_parallel.push_back(i);
              
              std::list < int > index_container_current_ring_parallel;

              //propagation over the pairs of epsilon-parallel primitives
              bool propagation=true;
              Vector cluster_normal=extracted_planes[i].orthogonal_vector();
              double cumulated_area=list_areas[i];
			
              do
                {
                  propagation=false;

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

                      int plane_index=index_container_former_ring_parallel[k];

                      for (std::size_t it=0;it<table_parallel[plane_index].size();it++)
                        {
						
                          Vector normal_it=extracted_planes[it].orthogonal_vector();

                          if( table_parallel[plane_index][it] && is_available[it]
                              && std::fabs(normal_it*cluster_normal)>1.-epsilon )
                            {	
							
                              propagation=true;
                              index_container_current_ring_parallel.push_back(it);
                              is_available[it]=false;
							
                              if(cluster_normal*normal_it <0)
                                normal_it=-normal_it;
                            
                              cluster_normal=(FT)cumulated_area*cluster_normal+(FT)list_areas[it]*normal_it;
                              FT norm=1./sqrt(cluster_normal.squared_length()); 
                              cluster_normal=norm*cluster_normal;
                              cumulated_area+=list_areas[it];
                            }	
                        }
                    }

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

                }
              while(propagation);
			
              list_parallel_planes.push_back(index_container_parallel);
              list_cluster_normales.push_back(cluster_normal);
              list_cluster_area.push_back(cumulated_area);
              Vector v_vertical(0.,0.,1.);
              list_cluster_cosangle_vertical.push_back(std::fabs(v_vertical*cluster_normal)); 
            }
        }
      is_available.clear();

      //discovery orthogonal relationship between clusters 
      std::vector < std::vector < bool > > group_planes_orthogonal;
      for( std::size_t i=0;i<list_parallel_planes.size(); i++)
        {
          std::vector < bool > gp_tmp;
          for( std::size_t j=0;j<list_parallel_planes.size(); j++)
            gp_tmp.push_back(false);
          group_planes_orthogonal.push_back(gp_tmp);
        }

      for (std::size_t i=0; i<group_planes_orthogonal.size();i++)
        {
          for (std::size_t j=0; j<group_planes_orthogonal.size();j++)
            {

              if( i!=j && std::fabs(list_cluster_normales[i]*list_cluster_normales[j])<epsilon)
                {
                  group_planes_orthogonal[i][j]=true; 
                  group_planes_orthogonal[j][i]=true;
                }
            }
        }





      //clustering the vertical cosangle and store their centroids in cosangle_centroids and the centroid index of each cluster in list_cluster_index 
      std::vector < double > cosangle_centroids;
      std::vector < int > list_cluster_index;
      for( std::size_t i=0;i<list_cluster_cosangle_vertical.size(); i++)
        list_cluster_index.push_back(-1);
      
      int mean_index=0;
      for( std::size_t i=0;i<list_cluster_cosangle_vertical.size(); i++)
        {
          if(list_cluster_index[i]<0)
            {
              list_cluster_index[i]=mean_index;
              double mean=list_cluster_area[i]*list_cluster_cosangle_vertical[i];
              double mean_area=list_cluster_area[i];
              for (std::size_t j=i+1; j<list_cluster_cosangle_vertical.size(); j++)
                {
                  if( list_cluster_index[j]<0 && std::fabs(list_cluster_cosangle_vertical[j]-mean/mean_area)<epsilon )
                    {
                      list_cluster_index[j]=mean_index;
                      mean_area+=list_cluster_area[j];
                      mean+=list_cluster_area[j]*list_cluster_cosangle_vertical[j];
                    }
                }
              mean_index++;
              mean/=mean_area;
              cosangle_centroids.push_back(mean);
            }
        }
      //desactive Z-verticalité
      for( std::size_t i=0;i<cosangle_centroids.size(); i++)
        {
          if(cosangle_centroids[i]<epsilon)
            cosangle_centroids[i]=0;
          else if(cosangle_centroids[i]>1.-epsilon)
            cosangle_centroids[i]=1;
        }
      for (std::size_t i=0; i<group_planes_orthogonal.size();i++)
        list_cluster_cosangle_vertical[i]=cosangle_centroids[list_cluster_index[i]];
	





      //display console
      //      /*
      std::cout<<std::endl<<std::endl<<"clusters of parallel primitives:";
      for (std::size_t i=0; i<list_parallel_planes.size();i++)
        {
          std::cout<<std::endl<<i<<" -> ";
          for (std::size_t j=0; j<list_parallel_planes[i].size();j++)
            std::cout<<list_parallel_planes[i][j]<<"  ";
        }

      std::cout<<std::endl<<std::endl<<"pairs of orthogonal clusters:";
      for (std::size_t i=0; i<group_planes_orthogonal.size();i++)
        {
          std::cout<<std::endl<<i<<" -> ";
          for (std::size_t j=0;j<group_planes_orthogonal.size();j++)
            {
              if(group_planes_orthogonal[i][j])
                std::cout<<j<<"  ";
            }
          std::cout<<"     -> "<<list_cluster_cosangle_vertical[i]<<"  -> "<<cosangle_centroids[list_cluster_index[i]];
        }
      //      */


      //find subgraphs of mutually orthogonal clusters (store index of clusters in subgraph_clusters), and select the cluster of largest area
      std::vector < std::vector < int > > subgraph_clusters;
      std::vector < int > subgraph_clusters_max_area_index;
      std::vector < bool > is_free;
      for (std::size_t i=0; i<list_parallel_planes.size();i++)
        is_free.push_back(true);
      for (std::size_t i=0; i<list_parallel_planes.size();i++)
        {
          if(is_free[i])
            {
              is_free[i]=false;
              double max_area=list_cluster_area[i];
              int index_max_area=i;

              //initialization containers
              std::vector < int > index_container;
              index_container.push_back(i);
              std::vector < int > index_container_former_ring;
              index_container_former_ring.push_back(i);
              std::list < int > 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++)
                    {

                      int cluster_index=index_container_former_ring[k];

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

                              if(max_area<list_cluster_area[j])
                                {
                                  max_area=list_cluster_area[j];
                                  index_max_area=j;
                                }
                            }	
                        }
                    }

                  //update containers
                  index_container_former_ring.clear();
                  for(std::list < int >::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);
            }
        }
      is_free.clear();


      //create subgraphs of mutually orthogonal clusters in which the largest cluster is excluded and store in subgraph_clusters_prop
      std::vector < std::vector < int > > subgraph_clusters_prop;
      for (std::size_t i=0;i<subgraph_clusters.size(); i++)
        {
          int index=subgraph_clusters_max_area_index[i];
          std::vector < int > 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);
        }



      //display console
      /*
        for (std::size_t i=0;i<subgraph_clusters_prop.size(); i++)
        {
        std::cout<<std::endl<<std::endl<<"subgraph "<<i<<" ("<<subgraph_clusters_max_area_index[i]<<" max area): ";
        for (std::size_t j=0;j<subgraph_clusters_prop[i].size(); j++) std::cout<<subgraph_clusters_prop[i][j]<<"  ";
        }
      */


      //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 vertical
      std::vector< bool > cluster_is_available; 
      for( std::size_t i=0;i<list_cluster_cosangle_vertical.size();i++)
        cluster_is_available.push_back(true);

      for (std::size_t i=0; i<subgraph_clusters_prop.size();i++)
        {
	
          int index_current=subgraph_clusters_max_area_index[i];
          Vector vec_current=regularize_normal(list_cluster_normales[index_current],
                                               list_cluster_cosangle_vertical[index_current]);
          list_cluster_normales[index_current]=vec_current;
          cluster_is_available[index_current]=false;

          //initialization containers
          std::vector < int > index_container;
          index_container.push_back(index_current);
          std::vector < int > index_container_former_ring;
          index_container_former_ring.push_back(index_current);
          std::list < int > 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++)
                {

                  int cluster_index=index_container_former_ring[k];

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

                          Vector new_vect=regularize_normals_from_prior(list_cluster_normales[cluster_index],
                                                                        list_cluster_normales[j],
                                                                        list_cluster_cosangle_vertical[j]);
                          list_cluster_normales[j]=new_vect;
                        }
                    }	
                }
			
              //update containers
              index_container_former_ring.clear();
              for(std::list < int >::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);
        }



      //recompute optimal plane for each primitive after normal regularization
      for (std::size_t i=0; i<list_cluster_normales.size();i++)
        {

          Vector vec_reg=list_cluster_normales[i];

          for (std::size_t j=0; j<list_parallel_planes[i].size();j++)
            {
              int index_prim=list_parallel_planes[i][j];
              Point pt_reg=extracted_planes[index_prim].projection(list_centroid[index_prim]);
              if( extracted_planes[index_prim].orthogonal_vector() * vec_reg < 0)
                vec_reg=-vec_reg;
              Plane plane_reg(pt_reg,vec_reg);
		
              if( std::fabs(extracted_planes[index_prim].orthogonal_vector()*plane_reg.orthogonal_vector()) > 1. - epsilon)
                extracted_planes[index_prim]=plane_reg;
            }
        }





      //detecting co-planarity and store in list_coplanar_prim
      std::vector< std::vector< std::vector < int > > > list_coplanar_prim;
      for (std::size_t i=0; i<list_parallel_planes.size();i++)
        {

          std::vector< std::vector < int > > list_coplanar_prim_tmp;
          Vector vec_reg=list_cluster_normales[i];
          std::vector < int > list_cop_index;
          for( std::size_t ip=0;ip<list_parallel_planes[i].size(); ip++)
            list_cop_index.push_back(-1);

          int cop_index=0;

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

              if(list_cop_index[j]<0)
                {
			
                  std::vector < int > list_coplanar_prim_tmp_tmp;
                  list_cop_index[j]=cop_index;
                  list_coplanar_prim_tmp_tmp.push_back(index_prim);
			
                  Point pt_reg=extracted_planes[index_prim].projection(list_centroid[index_prim]);
                  Plane plan_reg(pt_reg,vec_reg);

                  for (std::size_t k=j+1; k<list_parallel_planes[i].size(); k++)
                    {
                      if( list_cop_index[k]<0)
                        {
					
                          int index_prim_next=list_parallel_planes[i][k];
                          Point pt_reg_next=extracted_planes[index_prim_next].projection(list_centroid[index_prim_next]);
                          Point pt_proj=plan_reg.projection(pt_reg_next);
                          double distance=distance_Point(pt_reg_next,pt_proj);
					
                          if(distance<tolerance_coplanarity )
                            {
                              list_cop_index[k]=cop_index;
                              list_coplanar_prim_tmp_tmp.push_back(index_prim_next);
                            }
                        }
                    }
                  list_coplanar_prim_tmp.push_back(list_coplanar_prim_tmp_tmp);
                  cop_index++; 
                }
            }
          list_coplanar_prim.push_back(list_coplanar_prim_tmp);
        }



      //regularize primitive position by computing barycenter of coplanar planes
      std::vector < std::vector < int > > list_primitive_reg_index_extracted_planes;
      std::vector < Plane > list_primitive_reg;

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

              Point pt_bary(0.,0.,0.);
              double area=0;

              for (std::size_t k=0; k<list_coplanar_prim[i][j].size();k++)
                {
                  int index_prim=list_coplanar_prim[i][j][k];
                  Point pt_reg=extracted_planes[index_prim].projection(list_centroid[index_prim]);

                  pt_bary=barycenter(pt_bary, area,pt_reg,list_areas[index_prim]); 
                  area+=list_areas[index_prim];
                }
              Vector vec_reg=extracted_planes[list_coplanar_prim[i][j][0]].orthogonal_vector();

              Plane plane_reg(pt_bary,vec_reg);

              bool is_reg_used=false;
              std::vector< int > list_primitive_reg_index_extracted_planes_tmp1;

              for (std::size_t k=0; k<list_coplanar_prim[i][j].size();k++)
                {
                  int index_prim=list_coplanar_prim[i][j][k];
                  if( std::fabs(extracted_planes[index_prim].orthogonal_vector()*plane_reg.orthogonal_vector()) > 1. - epsilon)
                    {
                      if(extracted_planes[index_prim].orthogonal_vector()*plane_reg.orthogonal_vector()<0)
                        extracted_planes[index_prim]=plane_reg.opposite();
                      else
                        extracted_planes[index_prim]=plane_reg;
                      is_reg_used=true;
                      list_primitive_reg_index_extracted_planes_tmp1.push_back(index_prim);
                    }
                  else{
                    list_primitive_reg.push_back(extracted_planes[index_prim]);
                    std::vector< int > list_primitive_reg_index_extracted_planes_tmp;
                    list_primitive_reg_index_extracted_planes_tmp.push_back(index_prim);
                    list_primitive_reg_index_extracted_planes.push_back(list_primitive_reg_index_extracted_planes_tmp);
                  }
                }
              if(is_reg_used) {
                list_primitive_reg.push_back(plane_reg);
                list_primitive_reg_index_extracted_planes.push_back(list_primitive_reg_index_extracted_planes_tmp1);
              }
            }
        }

      std::cout<<std::endl<<std::endl<<"NB planes final = "<<list_primitive_reg.size()<<std::endl<<std::endl;



      //merge similar planes in plane_point_index and extracted planes and HPS[i].primitive_index
      std::vector < std::vector < int > > plane_point_index_temp;
      std::vector < Plane > extracted_planes_temp;
      std::vector < bool > has_been_merged;
      for (std::size_t i=0; i<plane_point_index.size();i++)
        has_been_merged.push_back(false);

      for (std::size_t i=0; i<plane_point_index.size();i++)
        {
	
          if (!has_been_merged[i])
            {
              extracted_planes_temp.push_back(extracted_planes[i]);
              int label_index=extracted_planes_temp.size()-1;
              plane_point_index_temp.push_back(plane_point_index[i]);
              for (std::size_t k=0; k<plane_point_index[i].size();k++)
                {
                  int index_pt=plane_point_index[i][k];
                  primitive_index[index_pt]=label_index;
                  label_plane[index_pt]=label_index;
                }
	
              for (std::size_t j=i+1;j<plane_point_index.size();j++)
                {

                  if(extracted_planes[i]==extracted_planes[j])
                    { //if identical (do opposite plane too ?) then store the second in the first
			
                      has_been_merged[j]=true;

                      std::vector< int > plane_point_index_new
                        = plane_point_index_temp[plane_point_index_temp.size()-1];
                      for (std::size_t k=0; k<plane_point_index[j].size();k++)
                        {
                          int ind=plane_point_index[j][k];
                          plane_point_index_new.push_back(ind);
                          primitive_index[ind]=label_index;
                          label_plane[ind]=label_index;
                        }
                      plane_point_index_temp[plane_point_index_temp.size()-1]=plane_point_index_new;

                    }
                }
            }
        }

      extracted_planes=extracted_planes_temp;
      plane_point_index=plane_point_index_temp;
    
    }

    FT distance_Point (const Point& a, const Point& b)
    {
      return std::sqrt (CGAL::squared_distance (a, b));
    }
  
    Vector regularize_normal (const Vector& n, FT cos_vertical)
    {
      FT A = 1 - cos_vertical * cos_vertical;
      FT B = 1 + (n.y() * n.y()) / (n.x() * n.x());
    
      FT vx = std::sqrt (A/B);
    
      if (n.x() < 0)
        vx = -vx;
    
      FT vy = vx * (n.y() / n.x()); 

      Vector res (vx, vy, cos_vertical);

      return res / std::sqrt (res * res);
    }

  
    Vector regularize_normals_from_prior (const Vector& np,
                                          const Vector& n,
                                          FT cos_vertical)
    {
      FT vx, vy;

      if (np.x() != 0)
        { 
          FT a = (np.y() * np.y()) / (np.x() * np.x()) + 1;
          FT b = 2 * np.y() * np.z() * cos_vertical / np.x();
          FT c= cos_vertical * cos_vertical-1;

          if (4 * a * c > b * b)
            return regularize_normal (n, cos_vertical); 
          else
            {
              FT delta = std::sqrt (b * b-4 * a * c);
              FT vy1= (-b-delta) / (2 * a);
              FT vy2= (-b+delta) / (2 * a);

              vy = (std::fabs(n.y()-vy1) < std::fabs(n.y()-vy2))
                ? vy1 : vy2;

              vx = (-np.y() * vy-np.z() * cos_vertical) / np.x();
            }
        }
      else if (np.y() != 0)
        {
          vy = -np.z() * cos_vertical / np.y();
          vx = std::sqrt (1 - cos_vertical * cos_vertical - vy * vy);
        
          if (n.x() < 0)
            vx = -vx;
        }
      else
        return regularize_normal (n, cos_vertical); 

      Vector res (vx, vy, cos_vertical);
      FT norm = std::max(1e-5, 1. / sqrt(res.squared_length ()));

      return norm * res;
    }

  

  };


}; // namespace CGAL

#endif // CGAL_PLANE_REGULARIZATION_H