/****************************************************************************
* MeshLab                                                           o o     *
* A versatile mesh processing toolbox                             o     o   *
*                                                                _   O  _   *
* Copyright(C) 2005                                                \/)\/    *
* Visual Computing Lab                                            /\/|      *
* ISTI - Italian National Research Council                           |      *
*                                                                    \      *
* All rights reserved.                                                      *
*                                                                           *
* This program is free software; 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 2 of the License, or         *
* (at your option) any later version.                                       *
*                                                                           *
* This program is distributed in the hope that it will be useful,           *
* but WITHOUT ANY WARRANTY; without even the implied warranty of            *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the             *
* GNU General Public License (http://www.gnu.org/licenses/gpl.txt)          *
* for more details.                                                         *
*                                                                           *
****************************************************************************/


/****************************************************************************
  History


****************************************************************************/

#include "qhull_tools.h"
#include <fstream>
#include <iostream>


using namespace std;
using namespace vcg;

#define TestMode 0

//Internal prototype
static coordT *qh_readpointsFromMesh(int *numpoints, int *dimension, MeshModel &m);
static double calculate_circumradius(pointT* p0,pointT* p1,pointT* p2, int dim);


/***************************************************************************/
/*                                                                         */
/* qhull conventions:													   */
/* coplanar point - a nearly incident point to a hyperplane				   */
/* point - d coordinates												   */
/* vertex - extreme point of the input set								   */
/* ridge - d-1 vertices between two neighboring facets					   */
/* facet - a facet with vertices, ridges, coplanar points, neighboring     */
/*			facets, and hyperplane										   */
/* simplicial facet - a facet with d vertices, d ridges, and d neighbors   */
/* non-simplicial facet - a facet with more than d vertices                */
/*                                                                         */
/*                                                                         */
/***************************************************************************/

/*	dim  --> dimension of points
    numpoints --> number of points
    m --> original mesh

    compute_convex_hull(int dim, int numpoints, MeshModel &m)
        build convex hull from a set of vertices of a mesh with Qhull library (http://www.qhull.org/html/qconvex.htm).

    returns
        the convex hull as a list of simplicial (triangulated) facets, if there are no errors;
        NULL otherwise.
*/
facetT *compute_convex_hull(qhT* qh, int dim, int numpoints, MeshModel &m)
{
    coordT *points;					/* array of coordinates for each point*/
    boolT ismalloc= True;			/* True if qhull should free points in qh_freeqhull() or reallocation */
    char flags[]= "qhull Tcv";		/* option flags for qhull, see qh_opt.htm */
    FILE *outfile= NULL;			/* output from qh_produce_output()
                                       use NULL to skip qh_produce_output() */
    FILE *errfile= stderr;			/* error messages from qhull code */
    int exitcode;					/* 0 if no error from qhull */

    /* initialize points[] here.
       points is an array of coordinates. Each triplet of coordinates represents a 3d vertex */
    points= qh_readpointsFromMesh(&numpoints, &dim, m);

    exitcode= qh_new_qhull (qh, dim, numpoints, points, ismalloc,
                            flags, outfile, errfile);

    //By default, Qhull merges coplanar facets. So, it's necessary to triangulate the convex hull.
    //If you call qh_triangulate() method , all facets will be simplicial (e.g., triangles in 2-d)
    //In theory calling qh_triangulate() or using option 'Qt' should give the same result, but,
    //in this case, option Qt does not triangulate the output because coplanar faces are still merged.
    qh_triangulate(qh);
    if (!exitcode) { /* if no error */
        /* 'qh->facet_list' contains the convex hull */
        return qh->facet_list;
    }

    return NULL;
};

/*	dim  --> dimension of points
    numpoints --> number of points
    m --> original mesh

    compute_delaunay(qhT* qh, int dim, int numpoints, MeshModel &m)
        build Delauanay triangulation from a set of vertices of a mesh with Qhull library (http://www.qhull.org/html/qdelaun.htm).

        The Delaunay triangulation DT(P) of a set of points P in d-dimensional spaces is a triangulation of the convex hull
        such that no point in P is inside the circum-sphere of any simplex in DT(P).

        By default, qdelaunay merges regions with cocircular or cospherical input sites.
        If you want a simplicial triangulation use triangulated output ('Qt') or joggled input ('QJ').

        Option 'QJ' joggle the input to avoid cospherical and coincident sites. The result is triangulated.

        Same result could be achieved with options Qz Qt.
        Option 'Qz' adds a point above the paraboloid of lifted sites for a Delaunay triangulation.
        It allows the Delaunay triangulation of cospherical sites.
        Option 'Qt' triangulates all non-simplicial facets before generating results

    returns
        true if no errors occurred;
        false otherwise.
*/
bool compute_delaunay(qhT* qh, int dim, int numpoints, MeshModel &m)
{
    coordT *points;						 /* array of coordinates for each point*/
    boolT ismalloc= True;				 /* True if qhull should free points in qh_freeqhull() or reallocation */
    char flags[]= "qhull d QJ Tcv";	     /* option flags for qhull, see qh_opt.htm */
    FILE *outfile= NULL;				 /* output from qh_produce_output()
                                            use NULL to skip qh_produce_output() */
    FILE *errfile= stderr;				 /* error messages from qhull code */
    int exitcode;						 /* 0 if no error from qhull */

    /* initialize points[] here.
       points is an array of coordinates. Each triplet of coordinates represents a 3d vertex */
    points = qh_readpointsFromMesh(&numpoints, &dim, m);

    exitcode = qh_new_qhull (qh, dim, numpoints, points, ismalloc,
                            flags, outfile, errfile);

    //qh_triangulate();
    if (!exitcode) { /* if no error */
        /* 'qh->facet_list' contains the convex hull */
        return true;
    }

    return false;
};

/*	dim  --> dimension of points
    numpoints --> number of points
    m --> original mesh
    pm --> new mesh
    threshold --> factor that, multiplied to the bbox diagonal, set a threshold used to discard the voronoi vertices too far from the origin.
                  They can cause problems to the qhull library.
                  Growing values of 'threshold' will add more voronoi vertices for a better surface reconstruction.

    compute_voronoi(int dim, int numpoints, MeshModel &m, MeshModel &pm)
        Implements a Voronoi filtering (Amenta and Bern 1998) with Qhull library (http://www.qhull.org/).
        The algorithm computes a piecewise-linear approximation of a smooth surface from a finite set of sample points
        It uses a subset of the Voronoi vertices to remove triangles from the Delaunay triangulation.

        After computing the Voronoi diagram, foreach sample point it chooses the two farthest opposite Voronoi vertices.
        Then computes a Delaunay triangulation of the sample points and the selected Voronoi vertices, and keep
        only those triangles in witch all three vertices are sample points.

        Options 'Qt' (triangulated output) and 'QJ' (joggled input) may produce unexpected results.

        Option 'Qt' triangulated output. If a Voronoi vertex is defined by cospherical data, Qhull duplicates
        the vertex. For example, if the data contains a square, the output will contain two copies of the Voronoi vertex.

        Option 'QJ' joggle the input to avoid Voronoi vertices defined by more than dim+1 points.
        It is less accurate than triangulated output ('Qt').

        Qhull returns the Delauanay triangulation as a list of tetrahedral facets.

    returns
        true if no errors occurred;
        false otherwise.

*/
bool compute_voronoi(qhT* qh, int dim, int numpoints, MeshModel &m, MeshModel &pm, Scalarm threshold)
{
    coordT *points;						/* array of coordinates for each point*/
    boolT ismalloc= True;				/* True if qhull should free points in qh_freeqhull() or reallocation */
    char flags[]= "qhull d QJ Tcv";	    /* option flags for qhull, see qh_opt.htm */
    FILE *outfile= NULL;				/* output from qh_produce_output()
                                           use NULL to skip qh_produce_output() */
    FILE *errfile= stderr;				/* error messages from qhull code */
    int exitcode;						/* 0 if no error from qhull */

    /* initialize points[] here.
       points is an array of coordinates. Each triplet of coordinates represents a 3d vertex */
    points= qh_readpointsFromMesh(&numpoints, &dim, m);
    coordT* copypoints= (coordT*)malloc(numpoints*dim*sizeof(coordT));
    for(int i=0;i<numpoints*dim;i++)
        copypoints[i] = points[i];

    //First Delaunay Triangulation
    exitcode= qh_new_qhull (qh, dim, numpoints, points, ismalloc,
                            flags, outfile, errfile);

    #if(TestMode)
        ofstream firstTest;
        firstTest.open("firstTest.txt",  ios::out);
        firstTest << "Poli infiniti da non prendere:\n ";

        ofstream secondTest;
        secondTest.open("secondTest.txt",  ios::out);
        secondTest << "Punti di input:\n ";

        ofstream thirdTest;
        thirdTest.open("testAfter.txt",  ios::out);
        thirdTest << "Punti di output:\n ";

    #endif

    if (!exitcode) { /* if no error */
        /* 'qh->facet_list' contains the convex hull */

        //Sets Voronoi vertex as facet center
        qh_setvoronoi_all(qh);

        double *voronoi_vertex;

        //poles_set contains the selected Voronoi vertices.
        //Every facet has a Voronoi vertex; so poles_set size is at most qh->num_facets
        setT* poles_set= qh_settemp(qh, qh->num_facets);

        vector<double*> voronoi_vertices; //Voronoi vertices of the region being considered
        vector<double*> normals;          //vector of the outer normals of the convex hull facets that are
                                          //neighbor to the point being considered

        vertexT *vertex;
        FORALLvertices {
            double *first_pole;
            double *second_pole;
            voronoi_vertices.clear();
            normals.clear();
            double max_dist=0; //distance from first_pole to vertex

            if (qh->hull_dim == 3)
               qh_order_vertexneighbors(qh, vertex);

            bool is_on_convexhull =false;

            //Considering the neighboring facets of the vertex in order to
            //compute the Voronoi region for that vertex
            facetT *neighbor, **neighborp;

            //Finding first_pole.
            FOREACHneighbor_(vertex) {
                if (neighbor->upperdelaunay)
                    is_on_convexhull =true;
                else {
                    voronoi_vertex = neighbor->center;
                    voronoi_vertices.push_back(neighbor->center);

                    if (neighbor->toporient)
                        normals.push_back(neighbor->normal);
                    double* vertex1= vertex->point;
                    double* vertex2= voronoi_vertex;
                    double dist = qh_pointdist(vertex1, vertex2,dim);
                    if(dist>max_dist){
                        max_dist=dist;
                        first_pole=voronoi_vertex;
                    }
                }
            }

            if(is_on_convexhull){ //This is a Voronoy vertex at infinity
                //Compute normals average
                double* avg_normal= new double[3];
                for(int i=0;i<3;i++){
                    avg_normal[i]=0;
                    for(size_t j=0; j< normals.size();j++)
                        avg_normal[i]+= (normals[j])[i];
                }
                first_pole=avg_normal;

                #if(TestMode)
                    firstTest << first_pole[0]<< " " << first_pole[1]<<" " << first_pole[2]<<"\n ";
                #endif

            }
            else{
                assert(first_pole!=NULL);
                if(first_pole!=NULL){

                    //Test if the Voronoi vertex is too far from the origin.
                    //It can cause problems to the qhull library.
                    bool discard=false;
                    for(int i =0;i<3;i++)
                    {
                        double* bbCenter = new double[dim];
                        double* pole = first_pole;
                        for(int i=0;i<dim;i++)
                            bbCenter[i] = pm.cm.bbox.Center()[i];
                        if(qh_pointdist(bbCenter,pole,dim)>(threshold*pm.cm.bbox.Diag()))
                            discard=true;
                    }
                    if(!discard)
                        qh_setunique(qh, &poles_set, first_pole);
                }
            }

            //vector vertex-first_pole
            double* sp1= new double[3];
            //vector vertex-second_pole
            double* sp2= new double[3];
            for(int i=0; i<3;i++)
                sp1[i]= first_pole[i] - vertex->point[i];

            //Finding second_pole
            double max_dist2=0;
            for(size_t i=0; i<voronoi_vertices.size();i++){
                if(voronoi_vertices[i]!= first_pole){
                    double* vertex1= vertex->point;
                    double* vertex2= voronoi_vertices[i];
                    double dist = qh_pointdist(vertex1, vertex2,dim);

                    //vector vertex-second_pole
                    for(int j=0; j<dim;j++)
                       sp2[j]= (voronoi_vertices[i])[j] - vertex->point[j];

                    //Calculate dot-product between vector vertex-first_pole and vector vertex-second_pole
                    double dotProd=0;
                    for(int k= 0; k<3;k++ )
                        dotProd += sp1[k] * sp2[k];

                    //Test if first_pole and second_pole are opposite and if second pole is the farthest from vertex
                    if(dotProd<=0 && dist>max_dist2){
                        max_dist2=dist;
                        second_pole = voronoi_vertices[i];
                    }
                }
            }
            assert(second_pole!=NULL);

            if(second_pole!=NULL){
                //Test if the Voronoi vertex is too far from the origin.
                //It can cause problems to the qhull library.
                bool discard=false;
                for(int i =0;i<3;i++)
                {
                    double* bbCenter = new double[dim];
                    double* pole = second_pole;
                    for(int i=0;i<dim;i++)
                        bbCenter[i] = pm.cm.bbox.Center()[i];
                    if(qh_pointdist(bbCenter,pole,dim)>(threshold*pm.cm.bbox.Diag()))
                        discard=true;
                }
                if(!discard)
                    qh_setunique(qh, &poles_set, second_pole);
            }

        }

        int numpoles = qh_setsize(qh, poles_set);
        int tot_newpoints = numpoints + numpoles;

        //Union of the sample points and the selected Voronoi vertices
        coordT * newpoints = (coordT*)malloc(tot_newpoints*dim*sizeof(coordT));

        int i=0;
        for(i=0;i<numpoints*3;i++){
            newpoints[i]= copypoints[i];
        }

        double *pole, **polep;
        FOREACHsetelement_(double,poles_set,pole){
            for(int j=0;j<3;j++)
                newpoints[i++]= pole[j];
        }

        #if(TestMode)
            for(int u=0;u<tot_newpoints*3;u+=3)
                secondTest << newpoints[u]<< " " << newpoints[u+1]<<" "<< newpoints[u+2]<<"\n";
        #endif

        //clean old triangulation
        int curlong, totlong;	  /* memory remaining after qh_memfreeshort */
        qh_freeqhull(qh, !qh_ALL);
        qh_memfreeshort (qh, &curlong, &totlong);
        if (curlong || totlong)
            fprintf (errfile, "qhull internal warning (user_eg, #1): did not free %d bytes of long memory (%d pieces)\n", totlong, curlong);


        //Second Delaunay Triangulation
        char flags2[]= "qhull d QJ Tcv";	/* option flags for qhull, see qh_opt.htm */
        exitcode= qh_new_qhull (qh, dim, tot_newpoints, newpoints, ismalloc,flags2, outfile, errfile);


        if (!exitcode) { /* if no error */
            /* 'qh->facet_list' contains the convex hull */

            //qh->num_vertices-numpoles=numpoints
            tri::Allocator<CMeshO>::AddVertices(pm.cm,qh->num_vertices-numpoles);

            /*ivp length is 'qh->num_vertices' because each vertex is accessed through its ID whose range is
              0<=qh_pointid(vertex->point)<qh->num_vertices*/
            vector<tri::Allocator<CMeshO>::VertexPointer> ivp(qh->num_vertices);

            vertexT* vertex;
            int i=0;
            FORALLvertices{
                if ((*vertex).point && qh_pointid(qh, vertex->point)<numpoints){
                    pm.cm.vert[i].P()[0] = (*vertex).point[0];
                    pm.cm.vert[i].P()[1] = (*vertex).point[1];
                    pm.cm.vert[i].P()[2] = (*vertex).point[2];
                    ivp[qh_pointid(qh, vertex->point)] = &pm.cm.vert[i];
                    i++;

                    #if(TestMode)
                        thirdTest << (*vertex).point[0]<< " " << (*vertex).point[1]<<" " << (*vertex).point[2]<<"\n ";
                    #endif

                }
            }

            //Take only the triangles in which all three vertices are sample points
            facetT *facet, *neighbor;
            qh->visit_id++;
            FORALLfacets {
                if (!facet->upperdelaunay) {
                    facet->visitid= qh->visit_id;
                    qh_makeridges(qh, facet);
                    ridgeT *ridge, **ridgep;
                    FOREACHridge_(facet->ridges) {
                        neighbor= otherfacet_(ridge, facet);
                        if (neighbor->visitid != qh->visit_id) {
                            tri::Allocator<CMeshO>::FaceIterator fi=tri::Allocator<CMeshO>::AddFaces(pm.cm,1);
                            vertexT *vertex;
                            int vertex_n, vertex_i;
                            FOREACHvertex_i_(qh, ridge->vertices){
                                //Test if the facet has only sample points as vertices
                                if(qh_pointid(qh, vertex->point)<numpoints)
                                    (*fi).V(vertex_i)= ivp[qh_pointid(qh, vertex->point)];
                                else{
                                    tri::Allocator<CMeshO>::DeleteFace(pm.cm,*fi);
                                    break;
                                }
                            }
                        }
                    }
                }
            }
        }
    }

    int curlong, totlong;	  /* memory remaining after qh_memfreeshort */
    qh_freeqhull(qh, !qh_ALL);
    qh_memfreeshort (qh, &curlong, &totlong);
    if (curlong || totlong)
        fprintf (stderr, "qhull internal warning (main): did not free %d bytes of long memory (%d pieces)\n",
                 totlong, curlong);
    if(!exitcode)
        return true;
    else
        return false;
}

/*
    dim  --> dimension of points
    numpoints --> number of points
    m --> original mesh
    pm --> new mesh
    alpha --> upper bound for the radius of the empty circumsphere of each simplex
    alphashape --> true to calculate alpha shape, false alpha complex

    compute_alpha_shapes(int dim, int numpoints, MeshModel &m, MeshModel &pm, double alpha, bool alphashape)
        build alpha complex or alpha shapes (Edelsbrunner and P.Mucke 1994)from a set of vertices of a mesh with Qhull library (http://www.qhull.org/
        Insert the minimum value of alpha (the circumradius of the triangle) in attribute Quality foreach face.

        The Alpha Shape is the boundary of the alpha complex, that is a subcomplex of the Delaunay triangulation.
        For a given value of 'alpha', the alpha complex includes all the simplices in the Delaunay
        triangulation which have an empty circumsphere with radius equal or smaller than 'alpha'.
        Note that for 'alpha' = 0, the alpha complex consists just of the set P, and for sufficiently large 'alpha',
        the alpha complex is the Delaunay triangulation DT(P) of P.

        Qhull returns the Delauanay triangulation as a list of tetrahedral facets.

    returns
        true if no errors occurred;
        false otherwise.
*/

bool compute_alpha_shapes(qhT* qh, int dim, int numpoints, MeshModel &m, MeshModel &pm, double alpha, bool alphashape){

    coordT *points;						/* array of coordinates for each point*/
    boolT ismalloc= True;				/* True if qhull should free points in qh_freeqhull() or reallocation */
    char flags[]= "qhull d QJ Tcv";	    /* option flags for qhull, see qh_opt.htm */
    FILE *outfile= NULL;				/* output from qh_produce_output()
                                           use NULL to skip qh_produce_output() */
    FILE *errfile= stderr;				/* error messages from qhull code */
    int exitcode;						/* 0 if no error from qhull */

    /* initialize points[] here.
       points is an array of coordinates. Each triplet of coordinates represents a 3d vertex */
    points= qh_readpointsFromMesh(&numpoints, &dim, m);

    int ridgesCount=0;

    exitcode= qh_new_qhull (qh, dim, numpoints, points, ismalloc,
                            flags, outfile, errfile);

    if (!exitcode) { /* if no error */
        /* 'qh->facet_list' contains the delaunay triangulation */

        //Set facet->center as the Voronoi center
        qh_setvoronoi_all(qh);

        setT *vertices = qh_facetvertices (qh, qh->facet_list, NULL, false);
        int convexNumVert = qh_setsize(qh, vertices);

        //Insert all the sample points, because, even with alpha=0, the alpha shape/alpha complex will
        //contain them.
        tri::Allocator<CMeshO>::AddVertices(pm.cm,convexNumVert);

        /*ivp length is 'qh->num_vertices' because each vertex is accessed through its ID whose range is
          0<=qh_pointid(vertex->point)<qh->num_vertices*/
        vector<tri::Allocator<CMeshO>::VertexPointer> ivp(qh->num_vertices);
        vertexT *vertex;
        int i=0;
        FORALLvertices{
            if ((*vertex).point){
                pm.cm.vert[i].P()[0] = (*vertex).point[0];
                pm.cm.vert[i].P()[1] = (*vertex).point[1];
                pm.cm.vert[i].P()[2] = (*vertex).point[2];
                ivp[qh_pointid(qh, vertex->point)] = &pm.cm.vert[i];
                i++;
            }
        }

        //Set of alpha complex triangles for alphashape filtering
        setT* set= qh_settemp(qh, 4* qh->num_facets);

        facetT *facet, *neighbor;
        qh->visit_id++;
        int numFacets=0;
        FORALLfacets {
            numFacets++;
            if (!facet->upperdelaunay) {
                //For all facets (that are tetrahedrons)calculate the radius of the empty circumsphere considering
                //the distance between the circumcenter and a vertex of the facet
                vertexT* vertex = (vertexT *)(facet->vertices->e[0].p);
                double* center = facet->center;
                double radius =  qh_pointdist(vertex->point,center,dim);

                if (radius>alpha) // if the facet is not good consider the ridges
                {
                    //if calculating the alphashape, unmark the facet ('good' is used as 'marked').
                    if(alphashape)
                        facet->good=false;

                    //Compute each ridge (triangle) once and test the cironference radius with alpha
                    facet->visitid= qh->visit_id;
                    qh_makeridges(qh, facet);
                    ridgeT *ridge, **ridgep;
                    int goodTriangles=0;
                    FOREACHridge_(facet->ridges) {
                        neighbor= otherfacet_(ridge, facet);
                        if (( neighbor->visitid != qh->visit_id)){
                            //Calculate the radius of the circumference
                            pointT* p0 = ((vertexT*) (ridge->vertices->e[0].p))->point;
                            pointT* p1 = ((vertexT*) (ridge->vertices->e[1].p))->point;
                            pointT* p2 = ((vertexT*) (ridge->vertices->e[2].p))->point;

                            radius = calculate_circumradius(p0,p1,p2, dim);

                            if(radius <=alpha){
                                goodTriangles++;
                                //if calculating alpha complex, build the mesh. It needs no filtering.
                                if(!alphashape){
                                    tri::Allocator<CMeshO>::FaceIterator fi=tri::Allocator<CMeshO>::AddFaces(pm.cm,1);
                                    ridgesCount++;

                                    //Store the circumradius of the face in face quality
                                    (*fi).Q() = radius;

                                    int vertex_n, vertex_i;
                                    FOREACHvertex_i_(qh, ridge->vertices)
                                        (*fi).V(vertex_i)= ivp[qh_pointid(qh, vertex->point)];
                                }
                                else
                                    //if calculating alpha shape, save the triangle (ridge) for subsequent filtering
                                    qh_setappend(qh, &set, ridge);
                            }
                        }
                    }

                    //If calculating the alphashape, mark the facet('good' is used as 'marked').
                    //This facet will have some triangles hidden by the facet's neighbor.
                    if(alphashape && goodTriangles==4)
                        facet->good=true;

                }
                else //the facet is good. Put all the triangles of the tetrahedron in the mesh
                {
                    //Compute each ridge (triangle) once
                    facet->visitid= qh->visit_id;
                    //If calculating the alphashape, mark the facet('good' is used as 'marked').
                    //This facet will have some triangles hidden by the facet's neighbor.
                    if(alphashape)
                        facet->good=true;
                    qh_makeridges(qh, facet);
                    ridgeT *ridge, **ridgep;
                    FOREACHridge_(facet->ridges) {
                        neighbor= otherfacet_(ridge, facet);
                        if ((neighbor->visitid != qh->visit_id)){
                            //if calculating alphacomplex build mesh. It needs no filtering
                            if(!alphashape){
                                tri::Allocator<CMeshO>::FaceIterator fi=tri::Allocator<CMeshO>::AddFaces(pm.cm,1);

                                pointT* p0 = ((vertexT*) (ridge->vertices->e[0].p))->point;
                                pointT* p1 = ((vertexT*) (ridge->vertices->e[1].p))->point;
                                pointT* p2 = ((vertexT*) (ridge->vertices->e[2].p))->point;
                                radius = calculate_circumradius(p0,p1,p2, dim);

                                //Store the circumradius of the face in face quality
                                (*fi).Q() = radius;

                                ridgesCount++;
                                int vertex_n, vertex_i;
                                FOREACHvertex_i_(qh, ridge->vertices)
                                    (*fi).V(vertex_i)= ivp[qh_pointid(qh, vertex->point)];
                            }
                            else
                                //if calculating alpha shapes, save the triangle for subsequent filtering
                                qh_setappend(qh, &set, ridge);
                        }
                    }
                }
            }
        }
        assert(numFacets== qh->num_facets);

        if(alphashape){
            //Filter the triangles (only the ones on the boundary of the alpha complex) and build the mesh

            ridgeT *ridge, **ridgep;
            FOREACHridge_(set) {
                if ((!ridge->top->good || !ridge->bottom->good || ridge->top->upperdelaunay || ridge->bottom->upperdelaunay)){
                    tri::Allocator<CMeshO>::FaceIterator fi=tri::Allocator<CMeshO>::AddFaces(pm.cm,1);
                    ridgesCount++;
                    int vertex_n, vertex_i;
                    FOREACHvertex_i_(qh, ridge->vertices)
                        (*fi).V(vertex_i)= ivp[qh_pointid(qh, vertex->point)];
                }
            }
        }
    }
    assert(pm.cm.fn == ridgesCount);

    int curlong, totlong;	  /* memory remaining after qh_memfreeshort */
    qh_freeqhull(qh, !qh_ALL);
    qh_memfreeshort (qh, &curlong, &totlong);
    if (curlong || totlong)
        fprintf (stderr, "qhull internal warning (main): did not free %d bytes of long memory (%d pieces)\n",
                     totlong, curlong);

    if(!exitcode)
        return true;
    else
        return false;
}

/*
    dim  --> dimension of points
    numpoints --> number of points
    m --> original mesh
    pm --> new mesh that is the convex hull of the flipped points, if convex_hullFP is true
    pm2 --> new mesh that is the convex hull of the non flipped points, if triangVP is true
    viewpoint -> the viewpoint
    threshold -> bounds the radius of the sphere used to select visible points
    convex_hullFP --> true if you want to show the partial Convex Hull of the transformed points cloud
                    false otherwise
    triangVP --> true if you want to show a triangulation of the visible points
                 false otherwise

    bool visible_points(int dim, int numpoints, MeshModel &m, MeshModel &pm,MeshModel &pm2, vcg::Point3f viewpointP,float threshold,bool convex_hullFP,bool triangVP)
        Select the visible points in a point cloud, as viewed from a given viewpoint.
        It uses the Qhull library (http://www.qhull.org/.
        The algorithm used (Katz, Tal and Basri 2007) determines visibility without reconstructing a surface or estimating normals.
        A point is considered visible if its transformed point lies on the convex hull of a transformed points cloud from the original mesh points.

    returns
        the number of visible points if no errors occurred;
        -1 otherwise.
*/

int visible_points(qhT* qh, int dim, int numpoints, MeshModel &m, MeshModel &pm,MeshModel &pm2, Point3m viewpointP,float threshold,bool convex_hullFP,bool triangVP){
    boolT ismalloc= True;			/* True if qhull should free points in qh_freeqhull() or reallocation */
    char flags[]= "qhull Tcv";		/* option flags for qhull, see qh_opt.htm */
    FILE *outfile= NULL;			/* output from qh_produce_output()
                                       use NULL to skip qh_produce_output() */
    FILE *errfile= stderr;			/* error messages from qhull code */
    int exitcode;					/* 0 if no error from qhull */

    double* viewpoint = new double[dim];
    for(int i=0;i<3;i++)
        viewpoint[i] = viewpointP[i];

    //Every point in flipped_points and points share the same index
    //Flipped points are numpoints+1 because they include the viewpoint too.
    coordT *flipped_points = (coordT*)malloc((numpoints+1)*(dim)*sizeof(coordT));
    coordT *points = (coordT*)malloc((numpoints)*(dim)*sizeof(coordT));
    //Positions in dist are the indexes of the points
    double *dist = (double*)malloc((numpoints)*sizeof(double));

    double *point = new double[dim];
    double radius = 0;
    vector<CMeshO::VertexPointer> ivp(numpoints); //Vector of pointers to the input points
    int cnt=0;
    //Compute the radius as the distance between the viewpoint ant its farthest point
    for(CMeshO::VertexIterator vi=m.cm.vert.begin(); vi!=m.cm.vert.end(); ++vi){
        if(!(*vi).IsD()){
            ivp[cnt]=&(*vi);
            //Move the point so that viewpoint is the origin
            for(int ii=0;ii<dim;++ii)
                points[3*cnt + ii] = point[ii]=(*vi).P()[ii]- viewpoint[ii];
            double distance = 0;
            for(int k= 0; k<dim;k++ )
                distance += point[k] * point[k];
            dist[cnt] = sqrt(distance);
            if(dist[cnt]>radius)
                radius=dist[cnt];
            cnt++;
        }
    }
    assert(cnt==numpoints);

    //Adjust radius according to user param.
    radius = radius * pow(10,threshold);

    //Calculate the flipped points
    for (int j=0; j<numpoints;j++){
        double k= 2*(radius-dist[j])/dist[j];
        for(int ii=0;ii<dim;++ii)
            flipped_points[3*j + ii]=points[3*j + ii]+ k*points[3*j + ii];
    }


    //Attach viewpoint
    for(int ii=0;ii<dim;++ii)
        flipped_points[3*numpoints + ii]= viewpoint[ii];


    //Make a convex hull from the flipped points and the viewpoint
    exitcode= qh_new_qhull (qh, dim, numpoints+1, flipped_points, ismalloc,
                            flags, outfile, errfile);

    //By default, Qhull merges coplanar facets. So, it's necessary to triangulate the convex hull.
    //If you call qh_triangulate() method , all facets will be simplicial (e.g., triangles in 2-d)
    //In theory calling qh_triangulate() or using option 'Qt' should give the same result, but,
    //in this case, option Qt does not triangulate the output because coplanar faces are still merged.
    qh_triangulate(qh);

    int selected=0;
    if (!exitcode) { /* if no error */
        /* 'qh->facet_list' contains the convex hull */


        /*ivp length is 'numpoints' because each vertex is accessed through its ID whose range is
          0<=qh_pointid(vertex->point)<numpoints. qh->num_vertices is < numpoints*/
        vector<CMeshO::VertexPointer> ivp_flipped(numpoints); //Vector of pointers to the flipped points
        vector<CMeshO::VertexPointer> ivp_non_flipped(numpoints); //Vector of pointers to the non flipped points

        vertexT *vertex;
        selected=qh->num_vertices;
        //Eliminate the viewpoint
        FORALLvertices{
            if ((*vertex).point && qh_pointid(qh, vertex->point)>=numpoints)
                selected--;
        }

        if(convex_hullFP)
            tri::Allocator<CMeshO>::AddVertices(pm.cm,selected);

        if(triangVP)
            tri::Allocator<CMeshO>::AddVertices(pm2.cm,selected);

        int i=0;
        int j=0;
        FORALLvertices{
            //Do not add the viewpoint
            if ((*vertex).point && qh_pointid(qh, vertex->point)<numpoints){
                //mark the vertex in the mesh that is correspondent to the flipped one (share the same index)
                ivp[qh_pointid(qh, vertex->point)]->SetS();

                if(convex_hullFP){ //Add flipped points to the new mesh
                    pm.cm.vert[i].P()[0] = (*vertex).point[0];
                    pm.cm.vert[i].P()[1] = (*vertex).point[1];
                    pm.cm.vert[i].P()[2] = (*vertex).point[2];
                    ivp_flipped[qh_pointid(qh, vertex->point)] = &pm.cm.vert[i];
                    i++;
                }
                if(triangVP){ //Add visible points to the new mesh
                    pm2.cm.vert[j].P()[0] = (*ivp[qh_pointid(qh, vertex->point)]).P()[0];
                    pm2.cm.vert[j].P()[1] = (*ivp[qh_pointid(qh, vertex->point)]).P()[1];
                    pm2.cm.vert[j].P()[2] = (*ivp[qh_pointid(qh, vertex->point)]).P()[2];
                    ivp_non_flipped[qh_pointid(qh, vertex->point)] = &pm2.cm.vert[j];
                    j++;
                }
            }
        }
//		vcg::tri::UpdateColor<CMeshO>::VertexSelected(m.cm);

        if(convex_hullFP){
            //Add to the new mesh only the faces of the convex hull whose vertices aren't the viewpoint
            facetT *facet;
            FORALLfacets {
                vertexT *vertex;
                int vertex_n, vertex_i;
                tri::Allocator<CMeshO>::FaceIterator fi=tri::Allocator<CMeshO>::AddFaces(pm.cm,1);
                FOREACHvertex_i_(qh, (*facet).vertices){
                    if(qh_pointid(qh, vertex->point)<numpoints)
                        (*fi).V(vertex_i)= ivp_flipped[qh_pointid(qh, vertex->point)];
                    else{
                        tri::Allocator<CMeshO>::DeleteFace(pm.cm,*fi);
                        break;
                    }
                }
            }
        }
        if(triangVP){
            //Add to the new mesh only the faces whose vertices are visible points, discard the viewpoint
            facetT *facet;
            FORALLfacets {
                vertexT *vertex;
                int vertex_n, vertex_i;
                tri::Allocator<CMeshO>::FaceIterator fi=tri::Allocator<CMeshO>::AddFaces(pm2.cm,1);
                FOREACHvertex_i_(qh, (*facet).vertices){
                    if(qh_pointid(qh, vertex->point)<numpoints)
                        (*fi).V(vertex_i)= ivp_non_flipped[qh_pointid(qh, vertex->point)];
                    else{
                        tri::Allocator<CMeshO>::DeleteFace(pm2.cm,*fi);
                        break;
                    }
                }
            }
        }
    }

    int curlong, totlong;	  /* memory remaining after qh_memfreeshort */
    qh_freeqhull(qh, !qh_ALL);
    qh_memfreeshort (qh, &curlong, &totlong);
    if (curlong || totlong)
        fprintf (stderr, "qhull internal warning (main): did not free %d bytes of long memory (%d pieces)\n",
                     totlong, curlong);

    if (!exitcode)
        return selected;
    else
        return -1;
}

/* dim  --> dimension of points
    numpoints --> number of points
    m --> original mesh

    coordT *qh_readpointsFromMesh(int *numpoints, int *dimension, MeshModel &m)
        build an array of coordinates from the vertices of the mesh m.
        Each triplet of coordinates represents a 3d vertex.

    returns
        the array of coordinates.
*/

coordT *qh_readpointsFromMesh(int *numpoints, int *dimension, MeshModel &m) {
    coordT *points, *coords;

    coords= points=
    (coordT*)malloc((*numpoints)*(*dimension)*sizeof(coordT));

    int cnt=0;
    CMeshO::VertexIterator vi;
    for(vi=m.cm.vert.begin(); vi!=m.cm.vert.end(); ++vi)
    if(!(*vi).IsD()){
        for(int ii=0;ii<*dimension;++ii)
            *(coords++)=(*vi).P()[ii];
        ++cnt;
    }
    assert(cnt==m.cm.vn);

    return(points);
}

/*
    double calculate_circumradius(pointT* p0,pointT* p1,pointT* p2, int dim)
        calculate the radius of the circumference passing through p0, p1, p2.
*/


double calculate_circumradius(pointT* p0,pointT* p1,pointT* p2, int dim){
    coordT a = qh_pointdist(p0,p1,dim);
    coordT b = qh_pointdist(p1,p2,dim);
    coordT c = qh_pointdist(p2,p0,dim);

    coordT sum =(a + b + c)*0.5;
    coordT area = sum*(a+b-sum)*(a+c-sum)*(b+c-sum);
    return (double) (a*b*c)/(4*sqrt(area));
}
