#include "FMPointCloud.h"

namespace FM{

geoFunction make_circle(npoint3 center, npoint3 normal, double radius, double epsilon)
{
return [center,normal,radius,epsilon](double x, double y, double z){
    npoint3 pt(x,y,z);
    bool inSphere = (pt-center).norm() > radius-epsilon && (pt-center).norm() < radius+epsilon;
    bool inPlane = abs((pt-center)*normal) < epsilon;
    return inSphere && inPlane;
};
}

geoFunction make_sphere(npoint3 center, double radius, double epsilon)
{
return [center,radius,epsilon](double x, double y, double z){
    npoint3 pt(x,y,z);
    return (pt-center).norm() > radius-epsilon && (pt-center).norm() < radius+epsilon;
};
}

geoFunction make_line(npoint3 pt0, npoint3 pt1, double epsilon)
{
npoint3 vecDir(pt1-pt0);
vecDir.normalize();
return [pt0,vecDir,epsilon](double x, double y, double z){
    npoint3 vTest(x-pt0[0],y-pt0[1],z-pt0[2]);
    std::cout << "vtest = " << vTest[0] << " " << vTest[1] << " " << vTest[2] << std::endl;
    std::cout << " compo perp = " << (vTest - (vTest*vecDir)*vecDir).norm() << "  espilon = " << epsilon << std::endl;
    return (vTest - (vTest*vecDir)*vecDir).norm() < epsilon;
};
}

/* PROTECTED FUNCTIONS */
void FMPointCloud::addNode(double x, double y, double z)
{
    ANNpointArray nodes_new = (ANNpointArray)realloc(nodes,(nbr_nodes+1)*sizeof(ANNpoint));

    if(nodes_new == nullptr){
        std::cerr << "ANN reallocation failed." << std::endl;
    }

    nodes = nodes_new;

    nodes[nbr_nodes][0] = x;
    nodes[nbr_nodes][1] = y;
    nodes[nbr_nodes][2] = z;

    nbr_nodes += 1;

    neighbourhood.push_back(std::vector<int>());
    curvs.push_back({{0.,0.,0.},{0.,0.,0.},0.,0.});
    topol.push_back(std::pair<FM::topology,void*>(FM::topology::isNone,nullptr));
    keyRadii.push_back(0.);
}

void FMPointCloud::setNodes(std::vector<std::vector<double>> &pts)
{
    int nbrPts = pts.size();
    nodes = annAllocPts(nbrPts,3);

    if(nodes == nullptr){
        std::cerr << "ANN allocation failed." << std::endl;
    }

    for(int i=0; i < nbrPts; i++){
        for(int j=0; j < 3; j++){
            nodes[i][j] = pts[i][j];
        }
    }

    neighbourhood.reserve(nbrPts);
    curvs.reserve(nbrPts);
    topol.reserve(nbrPts);
    keyRadii.reserve(nbrPts);
    nbr_nodes = nbrPts;
}

void FMPointCloud::deleteLink(int idx1, int idx2)
{
    std::vector<int>::iterator it21 = std::find(neighbourhood[idx1].begin(),neighbourhood[idx1].end(),idx2);
    std::vector<int>::iterator it12 = std::find(neighbourhood[idx2].begin(),neighbourhood[idx2].end(),idx1);
    if(it12 != neighbourhood[idx2].end() && it21 != neighbourhood[idx1].end()){
        neighbourhood[idx1].erase(it21);
        neighbourhood[idx2].erase(it12);
    } else {
        std::cerr << "Unable to supress the link between nodes " << idx1 << " and " << idx2 <<std::endl;
    }
}

void FMPointCloud::computeKeyRadius()
{
    // test 1 : rayon moyen (distances euclidiennes)
    double mean;
    int nodeTemp;
    double distTemp;
    

    for(int i=0; i < nbr_nodes; i++){
        mean = 0;
        npoint3 ptemp(0.,0.,0.);
        npoint3 ni(nodes[i][0],nodes[i][1],nodes[i][2]);
//        std::cout << "Node " << i << " " << printNode(i) << " : ";

        for(int j=0; j < neighbourhood[i].size(); j++){
            nodeTemp = neighbourhood[i][j];
            distTemp = 0;
            for(int k=0; k<3; k++)
            {
                ptemp[k]+=nodes[nodeTemp][k];
                distTemp += (nodes[nodeTemp][k]-nodes[i][k])*(nodes[nodeTemp][k]-nodes[i][k]);
            }
            mean += sqrt(distTemp);
            // std::cout << sqrt(distTemp) << " ";
        }

        mean /= neighbourhood[i].size();
        ptemp*=1./neighbourhood[i].size();
        double dist=(ni-ptemp).norm();
//        std::cout << neighbourhood[i].size() << " " << mean << " " << dist << " " << dist/mean <<  std::endl;
        keyRadii[i] = mean;
//        keyRadii[i] = dist/mean;
        // keyRadii[i] = 0.03;

        // std::cout << "mean = " << mean  << std::endl;
        // int countInf;
        // for(countInf = 0; countInf < neighbourhood[i].size(); countInf++){
        //     nodeTemp = neighbourhood[i][countInf];
        //     distTemp = 0;
        //     for(int k=0; k<3; k++) distTemp += (nodes[nodeTemp][k]-nodes[i][k])*(nodes[nodeTemp][k]-nodes[i][k]);
        //     if(sqrt(distTemp)>mean) break;
        // }
        // std::cout << "mean = " << mean << " " << countInf << "/" << neighbourhood[i].size() <<std::endl;
    }
}

// void FMPointCloud::computeRadiusFirstRing()
// {
//     bool keepGoing;
//     int j;
//     npoint3 pt0, ptJ, nLoc, ptInt;
//     double distJ0, distJ1, deltaD;

//     for(int i=0; i < nbr_nodes; i++){
//         getNode(i,pt0);
//         int nbr_neig = neighbourhood[i].size();

//         nLoc = computeApproximatedNormal(i);

//         std::vector<double> dist(nbr_neig);
//         for(j=0; j < nbr_neig; j++){
//             getNode(neighbourhood[i][j],ptJ);
//             ptInt = ptJ - pt0;
//             dist[j] = (ptInt - (ptInt*nLoc)*nLoc).norm();
//         }

//         keepGoing = true;

//         std::cout << "Node " << i << " dists : ";
//         for(int k =0; k<nbr_neig; k++) std::cout << dist[k] << " ";

//         for(j=1; j< nbr_neig && keepGoing; j++){
//             deltaD = dist[j]-dist[j-1];
//             keepGoing = deltaD < 0.5* dist[0];
//         }

//         std::cout << "/chosen : (" << j-2 << ") " << dist[j-2] << std::endl;
//         radiiRing1[i] = dist[j-2];
//     }
// }

void FMPointCloud::getBarycentricCoord(npoint3 pt, int i, int j, int k, double *u, double *v) const
{
    npoint3 ptI, ptJ, ptK;
    getNode(i, ptI);
    getNode(j, ptJ);
    getNode(k, ptK);

    // reference is point I
    npoint3 U(ptJ-ptI), V(ptK-ptI), P(pt-ptI);
    npoint3 w(crossprod(U,V));
    w.normalize();
    P -= (P*w)*w;

    double denom = (U*U)*(V*V) - (U*V)*(V*U);

    *u = ((V*V)*(P*U) - (U*V)*(P*V))/denom;
    *v = ((U*U)*(P*V) - (V*U)*(P*U))/denom;
}

npoint3 FMPointCloud::getNormalTriangle(int i, int j, int k)
{
    npoint3 ptI, ptJ, ptK;
    getNode(i,ptI);
    getNode(j,ptJ);
    getNode(k,ptK);

    npoint3 vec1(ptJ-ptI);
    npoint3 vec2(ptK-ptI);

    // std::cout << "getNormalTri : vec 1 (" << vec1.x() << "," << vec1.y() << "," << vec1.z() << ")" << std::endl;
    // std::cout << "getNormalTri : vec 2 (" << vec2.x() << "," << vec2.y() << "," << vec2.z() << ")" << std::endl;

    npoint3 norm(crossprod(vec1,vec2));
    if(norm.norm() > 0) norm.normalize();
    else norm = npoint3();

    // verif sens avec la normal de la surface pour le sens posisitif défini par une ref ?

    // std::cout << "getNormalTri : norm (" << norm.x() << "," << norm.y() << "," << norm.z() << ")" << std::endl;

    return norm;
}

npoint3 FMPointCloud::getNormalTriangle(int i, int j)
{
    std::vector<int> adjI;
    getAdj(i, adjI);
    int k;
    for(int l=0; l<adjI.size(); l++){
        if(adjI[l] != i && adjI[l] != j){
            k = adjI[l];
            break;
        }
    }

    return getNormalTriangle(i,j,k);
}

npoint3 FMPointCloud::getNormalPatch(std::vector<npoint3> pts)
{
    int nbrPts = pts.size();
    int nbrTrig = 0;
    npoint3 norm = npoint3();
    // std::cout << "getNormalPatch, nbr pts = " << pts.size() << std::endl;
    bool first = true;
    for(int i=0; i<nbrPts-2; i++){
        for(int j=i+1; j<nbrPts-1; j++){
            for(int k=j+1; k< nbrPts; k++){
                npoint3 insNormal(crossprod(pts[j]-pts[i],pts[k]-pts[i]));
                insNormal.normalize();
                // std::cout << "Normal " << i << " " << j << " " << k << " (" << insNormal.x() << "," << insNormal.y() << "," << insNormal.z() << ")" << std::endl;
                if(first){
                    norm = insNormal;
                    first = false;
                    nbrTrig++;
                } else {
                    if (norm*insNormal < 0) insNormal *= -1;
                    norm += insNormal;
                    nbrTrig++;
                }
            }
        }
    }

    if(nbrTrig > 0 && norm.norm() > 0){
        norm /= (double) nbrTrig;
        norm.normalize();
        return norm;
    } else {
        std::cerr << "Error computing normal of patch." << std::endl;
        return npoint3();
    }
}

bool FMPointCloud::testProjectionPatch(std::vector<int> &idxNodes, npoint3 &pt0, npoint3 &npt0) const
{
    int listSize = idxNodes.size();
    npoint3 ptI, ptJ, ptK;
    for(int i=0; i<listSize-2; i++){
        getNode(idxNodes[i],ptI);
        for(int j=i+1; j<listSize-1; j++){
            getNode(idxNodes[j],ptJ);
            for(int k=j+1; k<listSize; k++){
                getNode(idxNodes[k],ptK);
                npoint3 u(ptJ-ptI), v(ptK-ptI), P(pt0-ptI);
                npoint3 w(crossprod(u,v));
                w.normalize();

                // delete the normal component if there is any (projecting P in the plane)
                P -= (P*w)*w;

                double uTemp, vTemp;
                getBarycentricCoord(pt0,idxNodes[i],idxNodes[j],idxNodes[k],&uTemp,&vTemp);

                #ifdef DBUG
                std::cout << "Nodes " << idxNodes[i] << " " << idxNodes[j] << " " << idxNodes[k]
                    << " -> u=" << uTemp << " v=" << vTemp << std::endl;
                #endif

                if (uTemp > -1*EPS_NUM && vTemp > -1*EPS_NUM && (uTemp+vTemp) < (1+EPS_NUM)){
                    npt0 = npoint3(ptI+P);
                    #ifdef DBUG
                    std::cout << " test succeed -> return true" << std::endl;
                    #endif
                    return true;
                }
            }
        }
    }
    #ifdef DBUG
    std::cout << " test failed -> return false" << std::endl;
    #endif
    return false;
}

bool FMPointCloud::isAcute(npoint3 &pt1, npoint3 &pt2, npoint3 &pt3)
{
    // return (pt3-pt1)*(pt2-pt1) > 0 && (pt1-pt3)*(pt2-pt3) > 0 && (pt1-pt2)*(pt3-pt2) > 0;
    // return (pt3-pt1)*(pt2-pt1) >= 0 && (pt1-pt3)*(pt2-pt3) >= 0 && (pt1-pt2)*(pt3-pt2) >= 0;
    return (pt1-pt3)*(pt2-pt3) >= 0;
}

/* CONSTRUCTORS */

FMPointCloud::FMPointCloud(std::string fname, int maxnb) : FMPointCloud()
{
    int ptPos = fname.find_last_of(".");
    std::string fileType = (ptPos != std::string::npos) ? fname.substr(ptPos+1) : "";

    if(fileType == "csv" || fileType == "CSV"){
        readFromCSV(fname);
    } else if (fileType == "msh" || fileType == "MSH")
    {
        readFromMSH(fname);
    } else if (fileType == "stl" || fileType == "STL")
    {
        readFromSTL(fname);
    } else if (fileType == "txt" || fileType == "TXT")
    {
        readFromFile(fname);
    } else {
        std::cerr << "File extension not recognized. Try 'csv', 'msh', 'stl' or 'txt'." << std::endl;
    }

    buildTree();
    buildGraph(maxnb);
    
}

/* GET/SET FUNCTIONS */

void FMPointCloud::getNode(int i, npoint3 &pt) const
{
    if(i < nbr_nodes && i >= 0){
        pt = npoint3(nodes[i][0],nodes[i][1],nodes[i][2]);
    } else {
        pt = npoint3();
        std::cerr << "getNode : index out of reach. resquesting index " << i << std::endl;
    }
}

void FMPointCloud::readFromCSV(std::string fname)
{
    std::cout << "Reading CSV " << fname << std::endl;

    ifstream ptFile(fname);
    vector<vector<double>> pts;
    if(ptFile.is_open()){
    while(ptFile){
        string line;
        getline(ptFile,line);
        if(!line.empty()){
            string temp;
            vector<double> tempPt(3);
            int j = 0;
            for(int i=0; i < line.length(); ++i){
                if(line[i] != ';'){
                    temp += line[i];
                } else {
                    tempPt[j] = stod(temp);
                    j++;
                    temp.clear();
                }
            }
            //addNode(tempPt[0],tempPt[1],tempPt[2]);
            pts.push_back(std::vector<double>{tempPt[0],tempPt[1],tempPt[2]});
        }
    }
    ptFile.close();
    }

    setNodes(pts);

    std::cout << "Reading CSV : " << pts.size() << " nodes saved." << std::endl;
}

/* READ FILE FUNCTIONS */

void FMPointCloud::readFromFile(std::string fname)
{
    ifstream f(fname);
    std::vector<std::vector<double>> pts;
    double x,y,z;
    if (!f.fail())
    {
        do
        {
        f >> x >> y >> z ;
        if (!f.eof())
        {
            //addNode(x,y,z);
            pts.push_back(std::vector<double>{x,y,z});
        }
        }
        while (!f.eof());
        f.close();
    }
    setNodes(pts);
}

void FMPointCloud::readFromSTL(std::string fname)
{
    stlmesh stl;
    std::vector<std::vector<double>> pts;
    stl.load_stl(fname);
    int sz=stl.vertices.size();
    for (int i=0;i<sz;++i)
    {
        //addNode(stl.vertices[i].first[0],stl.vertices[i].first[1],stl.vertices[i].first[2]);
        pts.push_back(std::vector<double>{stl.vertices[i].first[0],stl.vertices[i].first[1],stl.vertices[i].first[2]});
    }
    setNodes(pts);
}

void FMPointCloud::readFromMSH(std::string fname)
{
    std::vector<std::vector<double>> pts;
    GmshInitialize();
    model = new FM::FMModel();
    model->readMSH(fname);
    model->buildAdj(1<<(model->getDim()-1));//highest dim only
    std::map<MVertex *,std::vector<MElement*> >::iterator it=model->v_to_ele.begin(),ite=model->v_to_ele.end();
    for (;it!=ite;++it)
    {
        MVertex *v=it->first;
        //addNode(v->x(),v->y(),v->z());
        pts.push_back(std::vector<double>{v->x(),v->y(),v->z()});
    }

    setNodes(pts);
}

void FMPointCloud::importGraphCSR(std::string fname)
{
    std::cout << "Reading CSR " << fname << std::endl;

    ifstream f(fname);
    std::vector<int> idxR;
    std::vector<int> neigList;
    if (!f.fail())
    {
        // first line
        std::string line;
        getline(f,line);
        stringstream sts(line);
        int idx;
        while (sts >> idx) idxR.push_back(idx);

        std::cout << " size idxR : " << idxR.size() << std::endl;

        // second line
        neigList.reserve((idxR.size())*15);
        getline(f,line);
        sts = stringstream(line);
        while (sts >> idx) neigList.push_back(idx);

        f.close();
        // std::cout << "neighbourhood capacity : " << neighbourhood.capacity() << std::endl;

        // std::cout << "Neig list : ";
        // for(auto n : neigList) std::cout << n << " ";
        // std::cout << std::endl;

        // copy into neighbourhood
        for(int i=0; i < idxR.size()-1; i++) neighbourhood.push_back(std::vector<int>(neigList.begin()+idxR[i],neigList.begin()+idxR[i+1]));
        neighbourhood.push_back(vector<int>(neigList.begin()+idxR[idxR.size()-1],neigList.end()));

        std::cout << "neighbours copied" << std::endl;

    } else {
        std:cerr << "Failed to read CSR file " << fname << std::endl;
    }
}

/* SET TOPOLOGY FUNCTIONS */

void FMPointCloud::findNodesEdge(npoint3 &pt1, npoint3 &pt2, std::vector<int> &idxEdge)
{
    // not finished !
    // TODO: utiliser l'algorithme de Jarvis en passant de point en point (algo utilsé dans
    // chaque voisinage indépendamment l'un de l'autre)
    int idx1, idx2, idxTemp;
    findClosest(pt1,idx1);
    findClosest(pt2,idx2);

    idxEdge.clear();
    idxEdge = {idx1};

    while(idxTemp != idx2){
        
    }

    idxEdge.push_back(idx2);
}

void FMPointCloud::findNodesEdge(npoint3 &pt1, npoint3 &pt2, double radius, std::vector<int> &idxEdge)
{
    idxEdge.clear();

    npoint3 vec1(pt2-pt1);
    vec1.normalize();

    for(int i = 0; i < nbr_nodes; i++){
        npoint3 nd;
        getNode(i,nd);
        npoint3 vecI(nd-pt1);

        double dist = (vecI - (vecI*vec1)*vec1).norm();

        if(abs(dist) <= radius) idxEdge.push_back(i);
    }
}

void FMPointCloud::setTopology(std::vector<npoint3> pts, std::vector<npoint3> vects)
{
    // save vertices and edges
    std::vector<int> idxs;
    for(npoint3 pt : pts) {
        int idx;
        if(!findNode(pt.x(),pt.y(),pt.z(),&idx,nullptr)) std::cerr << "Node not found !" << std::endl;
        idxs.push_back(idx);
    }
    idxs.push_back(idxs[0]);


    for(int i=0; i< (idxs.size()-1); i++){
        FMPointCloud::setEdge(idxs[i],idxs[i+1],vects[i]);
    }
    for(int i=1; i < (idxs.size()-1); i++){
        FMPointCloud::setVertex(idxs[i],edges[i-1],edges[i]);
    }
    FMPointCloud::setVertex(idxs[0],edges[edges.size()-1],edges[0]);

    // set points
    for(int i=0; i < nbr_nodes; i++){
        if(topol[i].first==FM::topology::isNone){
            npoint3 pti;
            getNode(i,pti);

            for(FM::edge& edg : edges){
                npoint3 vecTest(pti[0]-edg.p0[0],pti[1]-edg.p0[1],pti[2]-edg.p0[2]);
                double prod0 = vecTest*edg.edgeVec;
                if(prod0 >= 0 && prod0 <= edg.length && abs(vecTest*edg.vec)<=1e-10){
                    topol[i] = std::pair<FM::topology,void*>(FM::topology::onEdge, &edg);
                    break;
                }
            }
        }
    }

    // std::cout << "Set topology : ";
    // std::cout << vertices.size() << " vertices and " << edges.size() << " edges";
    // int nbr_edge=0;
    // for(int i=0; i <nbr_nodes; i++){
    //     if(topol[i].first==FM::topology::onEdge) nbr_edge++;
    // }
    // std::cout << " (" << nbr_edge << "/" << nbr_nodes << "nodes detected on edges)." << std::endl;
}

void FMPointCloud::setEdge(int idx0, int idx1, npoint3 vec)
{
    FM::edge edg(nodes[idx0],nodes[idx1],vec);
    edges.push_back(edg);
}

void FMPointCloud::setVertex(int idx, FM::edge e0, FM::edge e1)
{
    FM::vertex vt = {e0,e1};
    vertices.push_back(vt);

    topol[idx] = std::pair<FM::topology,void*>(FM::topology::isVertex, &vertices[vertices.size()-1]);
}

/*  SEARCH NODE FUNCTIONS (KD-TREE) */

void FMPointCloud::buildTree()
{
    if(kdTree) delete kdTree;
    kdTree = new ANNkd_tree(nodes,nbr_nodes,3);
}

double FMPointCloud::findKClosest(npoint3 pt,std::vector<std::pair<int,double> > &closest,int k,int keep, double dist)
{
    ANNpoint query = annAllocPt(3);
    ANNidxArray nnIdx;
    ANNdistArray dists;

    double meand = 0.;
    for(int i=0; i < 3; i++) query[i] = pt[i];

    if(k > nbr_nodes) k = getNbrNodes();
    if(keep > k) keep = k;
    if(kdTree){
        if(dist > 0.){
            int kk = kdTree->annkFRSearch(query,dist);
            nnIdx = new ANNidx[kk];
            dists = new ANNdist[kk];
            kdTree->annkFRSearch(query,dist,k,nnIdx,dists);
        }
        else {
            nnIdx = new ANNidx[k];
            dists = new ANNdist[k];
            kdTree->annkSearch(query,k,nnIdx,dists,0.);
        }
        closest.resize(keep);

        for(int i=0; i < keep; i++){
            meand += dists[i] = sqrt(dists[i]);
            closest[i] = std::make_pair(nnIdx[i],dists[i]);
        }
        meand /= keep;
        delete[] nnIdx;
        delete[] dists;

    } else {
        std::cerr << "findKClosest() needs a search structure - call buildTree() first" << std::endl;
    }
    annDeallocPt(query);

    return meand;
}

double FMPointCloud::findClosest(npoint3 pt, int &closest)
{
    closest = -1;
    double dist = 1e15;

    for(int i=0; i<nbr_nodes; i++){
        npoint3 pt1;
        getNode(i,pt1);
        double dist_temp = (pt1-pt).norm();
        if(dist_temp<dist){
            dist = dist_temp;
            closest = i;
        }
    }

    return dist;
}

 /* BUILD GRAPH FUNCTIONS */

void FMPointCloud::buildAdj(int maxnb, bool chk)
{
    if(kdTree){
        std::vector<std::pair<int,double>> closest;

        for(int i=0; i < nbr_nodes; i++){
            npoint3 pt;
            getNode(i,pt);
            // std::cout << "pt : " << pt.x() << " " << pt.y() << " " << pt.z() << std::endl;
            double res = findKClosest(pt,closest,maxnb*2,maxnb,0.);
            neighbourhood[i].reserve(closest.size()-1);

            double sumdist = 0.;
            int n = 0;

            for(int j=0; j<closest.size(); j++){
                if(closest[j].first != i){
                    if((n>3) && chk){
                        double mean = sumdist/n;
                        if(closest[j].second < 1.7*mean){
                            neighbourhood[i].push_back(closest[j].first);
                            n++;
                            sumdist += closest[j].second;
                        }
                    }
                    else {
                        neighbourhood[i].push_back(closest[j].first);
                        n++;
                        sumdist += closest[j].second;
                    }
                }
            }
        }
    }
    else {
        std::cerr << "buildAdj() needs a search structure - call buildTree() first" << std::endl;
    }
}

void FMPointCloud::symmetrizeAdj(bool keep)
{
    if (keep)
    {
        for (int i=0;i<nbr_nodes;++i)
        {
            for (int j=0;j<neighbourhood[i].size();++j)
            {
                bool found=false;
                for (int k=0;k<neighbourhood[neighbourhood[i][j]].size();++k)
                {
                    if (neighbourhood[neighbourhood[i][j]][k]==i) found=true;
                }
                if (!found)
                {
                    neighbourhood[neighbourhood[i][j]].push_back(i);
                }
            }
        }
    }
    else
    {
        for (int i=0;i<nbr_nodes;++i)
        {
            for (int j=0;j<neighbourhood[i].size();++j)
            {
                bool found=false;
                for (int k=0;k<neighbourhood[neighbourhood[i][j]].size();++k)
                {
                    if (neighbourhood[neighbourhood[i][j]][k]==i) found=true;
                }
                if (!found)
                {
                    neighbourhood[i].erase(neighbourhood[i].begin()+j);
                    j--;
//                    neighbourhood[neighbourhood[i][j]].push_back(i);
                }
            }
        }
    }
}

void FMPointCloud::trimOutsideAdj()
{
    for(int i=0; i <nbr_nodes; i++){
        if(topol[i].first==FM::topology::onEdge){
            FM::edge* edg = (FM::edge*) topol[i].second;

            for(std::vector<int>::iterator it=neighbourhood[i].begin(); it != neighbourhood[i].end(); ){
                // double prod=0;
                // for(int k = 0; k <3; k++) prod += (nodes[*it][k]-nodes[i][k])*edg->vec[k];
                if(edg->checkNotInside(nodes[*it])){
                    std::cout << "---> topology removed for node " << i << "(" << nodes[i][0] << "," << nodes[i][1] << "," << nodes[i][2] << ") : node " << *it;
                    std::cout << " (" << nodes[*it][0] << "," << nodes[*it][1] << "," << nodes[*it][2] << ")" << std::endl;
                    // double delete : keep symmetrized
                    neighbourhood[*it].erase(std::find(neighbourhood[*it].begin(),neighbourhood[*it].end(),i));
                    it = neighbourhood[i].erase(it);
                } else {
                    it++;
                }
            }

        }
        else if(topol[i].first==FM::topology::isVertex){
            //TODO: debug
            // FM::vertex* vert = (FM::vertex*) topol[i].second;
            // int count = 0; int sizeNeig = neighbourhood[i].size()-1;

            // for(std::vector<int>::iterator it=neighbourhood[i].begin(); it != neighbourhood[i].end();){
            //     double prod1=0, prod2=0;
            //     for(int k = 0; k <3; k++){
            //         prod1 += (nodes[*it][k]-nodes[i][k])*vert->e0.vec[k];
            //         prod2 += (nodes[*it][k]-nodes[i][k])*vert->e1.vec[k];
            //     }
            //     std::cout << "iteration " << count << "/" << sizeNeig << std::endl;
            //     // if(vert->checkNotInside(nodes[*it])){
            //     // if(prod1 < -1e-10 || prod2 < -1e-10){
            //     if(vert->e0.checkNotInside(nodes[*it])||vert->e1.checkNotInside(nodes[*it])){
            //         std::cout<< " relation nodes " << *it << "|" << i << " removed" << std::endl;
            //         neighbourhood[*it].erase(std::find(neighbourhood[*it].begin(),neighbourhood[*it].end(),i));
            //         it = neighbourhood[i].erase(it);
                    
            //     } else {
            //         it++;
            //     }
            //     count++;
            // }
        }
    }
}

void FMPointCloud::quickCornerTopolCorrect(npoint3& nor1, npoint3& nor2)
{
    std::vector<int> idx2supp;
    for(int i; i < nbr_nodes; i++){
        idx2supp.clear();
        npoint3 nodeI;
        getNode(i,nodeI);
        for(int idx : neighbourhood[i]){
            npoint3 nodeN;
            getNode(idx,nodeN);
            npoint3 link = nodeN-nodeI;
            if(abs(link*nor1)>1e-12 && abs(link*nor2)>1e-12){
                //std::cout << "link 2 delete found"<< std::endl;
                idx2supp.push_back(idx);
            }
        }
        for(int idx : idx2supp){
            deleteLink(i,idx);
        }
    }
}

 void FMPointCloud::augmentAdj(int nblayers)
 {
    std::vector<std::set<int> > buf;
    buf.resize(nbr_nodes);
    int layers = 0;
    while(layers < nblayers){
      for (int i=0;i<nbr_nodes;++i)
      {
        for (int j=0;j<neighbourhood[i].size();++j)
        {
          buf[i].insert(neighbourhood[i][j]);
          for (int k=0;k<neighbourhood[neighbourhood[i][j]].size();++k)
          {
            int kk=neighbourhood[neighbourhood[i][j]][k];
            if (kk!=i)
            buf[i].insert(kk);
          }
        }
      }
      for (int i=0;i<nbr_nodes;++i)
      {
        std::vector<int> buf2;
        std::copy(buf[i].begin(),buf[i].end(), std::back_inserter(buf2));
        neighbourhood[i]=buf2;
      }
      buf.clear();
      layers++;
    }
 }

 /* READ GRAPH FUNCTIONS */

 bool FMPointCloud::findNode(double x, double y, double z, int* idx, double* dist)
 {
    npoint3 pt(x,y,z);
    int idx_;
    double dist_ = findClosest(pt,idx_);
    bool isANode = false;

    if(dist_ < 1e-10){
        dist_ = 0.;
        isANode = true;
    }
    *idx = idx_;
    if(dist) *dist = dist_;

    return isANode;
 }

 void FMPointCloud::findNodesLine(npoint3 pt1, npoint3 pt2, double fDist, std::vector<int> &pts)
 {
    int sampMax = (pt2-pt1).norm()/(fDist/2);
    double t=0.;
    std::set<std::pair<double,int>> posPoints; // set of nodes that are possibly on the line
    std::vector<int> foundPoints; // nodes on the line

    // 1. find all nodes close to the line
    // follow the line from pt1 to pt2 and look for closest points
    for(int i=0; i <= sampMax; i++){
        t = (double) i / (double) sampMax;
        npoint3 ptT( (pt2-pt1)*t + pt1);
        int idx;
        double dist = findClosest(ptT,idx);
        if(dist < fDist){
            // if close enough, insert this node in the set of possible nodes
            posPoints.insert(std::make_pair(dist,idx));
        }
    }

    // 2. from all nodes previously found (posPoints), keep only once each node
    for(std::set<std::pair<double,int>>::iterator it = posPoints.begin(); it != posPoints.end(); it++){
        // if node is not already in vector foundPoints, add it (otherwise not)
        if(std::find(foundPoints.begin(),foundPoints.end(),it->second) == foundPoints.end())
        {
            foundPoints.push_back(it->second);
        }
    }
    
    pts = std::vector<int>(foundPoints);
 }

 void FMPointCloud::findNodesGeo(geoFunction fnc, vector<int> &pts)
 {
    pts.clear();
    npoint3 nd;

    for(int i=0; i < nbr_nodes; i++){
        getNode(i,nd);
        if(fnc(nd[0],nd[1],nd[2])) pts.push_back(i);
    }
 }

 /* SET CURVATURES AND NORMALS */

#ifdef USE_PCL
 void FMPointCloud::computeNormCurv()
 {
    typedef pcl::PointXYZ PointT;
    // int nPts=nodes.size();
    int nPts = nbr_nodes;
    pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
    pcl::NormalEstimation<PointT, pcl::Normal> ne;
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
    pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
    cloud->resize(nPts);
    for(int i=0;i<nPts;++i)
    {
        (*cloud)[i].x=nodes[i][0];
        (*cloud)[i].y=nodes[i][1];
        (*cloud)[i].z=nodes[i][2];
    }
    ne.setInputCloud (cloud);
    ne.setSearchMethod (tree);
    ne.setKSearch(20);
    ne.compute (*normals);
    pcl::PrincipalCurvaturesEstimation<PointT, pcl::Normal,pcl::PrincipalCurvatures> pce;
    pcl::PointCloud<pcl::PrincipalCurvatures>::Ptr principal_curvatures(new pcl::PointCloud<pcl::PrincipalCurvatures> ());
    pce.setInputCloud(cloud);
    pce.setInputNormals(normals);
    pce.setSearchMethod(tree);
    pce.setKSearch(10);
    pce.compute (*principal_curvatures);
    for(int i=0;i<nPts;++i)
    {
        curvs[i].n[0] = (*normals)[i].normal_x;
        curvs[i].n[1] = (*normals)[i].normal_y;
        curvs[i].n[2] = (*normals)[i].normal_z;

        curvs[i].pcv[0] = (*principal_curvatures)[i].principal_curvature_x;
        curvs[i].pcv[1] = (*principal_curvatures)[i].principal_curvature_y;
        curvs[i].pcv[2] = (*principal_curvatures)[i].principal_curvature_z;

        curvs[i].pc1 = (*principal_curvatures)[i].pc1;
        curvs[i].pc2 = (*principal_curvatures)[i].pc2;

      if(i%50==0){
        std::cout << "Princ cuvature node " << i << " : " << curvs[i].pc1 << " and " << curvs[i].pc2 << std::endl;
        std::cout << "  normal : " << curvs[i].n[0] << ";" << curvs[i].n[1] << ";" << curvs[i].n[2];
        std::cout << " / pcv : " << curvs[i].pcv[0] << ";" << curvs[i].pcv[1] << ";" << curvs[i].pcv[2] << std::endl;
      }
    }
    
    
//    pcl::PrincipalCurvatures descriptor = principal_curvatures->points[0];
//    std::cout << descriptor << std::endl;
 }
#else
 void FMPointCloud::computeNormCurv()
 {
    std::cerr << "computeNormCurv() needs the PCL (point cloud library)" << std::endl;
    applyNormCurvPlane(npoint3(0,0,1),npoint3(1,0,0));
 }
#endif

/* // mthd with the average of the normals of the triangles
 void FMPointCloud::computeApproximatedNormals(std::vector<npoint3> &normals)
 { 
    if(normals.size() != nbr_nodes) normals.resize(nbr_nodes);

    std::queue<int> idx2compute;
    std::vector<bool> seen(nbr_nodes,false);

    idx2compute.push(0);
    seen[0] = true;

    std::vector<int> adj, adjI;
    std::vector<npoint3> norms;
    npoint3 pt0, pt1, pt2, normTemp, vecT, ref;

    while(!idx2compute.empty()){
        int idx = idx2compute.front();
        idx2compute.pop();

        getAdj(idx,adj);
        getNode(idx,pt0);

        norms.clear();        

        for(int i=0; i<adj.size()-1; i++){
            getNode(adj[i],pt1);
            getAdj(adj[i],adjI);

            for(int j=i+1; j<adj.size(); j++){
                if(std::find(adjI.begin(),adjI.end(),adj[j])!=adjI.end()){
                    getNode(adj[j],pt2);
                    normTemp = npoint3(crossprod(pt1-pt0,pt2-pt0));
                    if(normTemp.norm() > EPS_NUM){
                        normTemp.normalize();
                        if(!norms.empty() && norms[0]*normTemp < 0) normTemp *= -1;
                        norms.push_back(normTemp);
                    }
                }
            }
        }

        npoint3 normAtIdx;
        for(npoint3 normSing : norms) normAtIdx += normSing;
        normAtIdx /= norms.size();
        normAtIdx.normalize();

        bool setNeighbourFound = false;

        for(int idxA : adj){
            if(normals[idxA].norm() > EPS_NUM && abs(normals[idxA]*normAtIdx) > EPS_NUM){
                ref = normals[idxA];
                setNeighbourFound = true;
                break;
            }
        }

        if(setNeighbourFound && ref*normAtIdx < 0) normAtIdx *= -1;

        normals[idx] = normAtIdx;

        for(int idxA: adj) {
            if(!seen[idxA]) {
                idx2compute.push(idxA);
                seen[idxA] = true;
            }
        }

    }


 }
*/
 // mthd with PCA
 void FMPointCloud::computeApproximatedNormals(std::vector<npoint3> &normals)
 { 
    if(normals.size() != nbr_nodes) normals.resize(nbr_nodes);

    std::queue<int> idx2compute;
    std::vector<bool> seen(nbr_nodes,false);

    idx2compute.push(0);
    seen[0] = true;

    std::vector<int> adj;
    npoint3 pt0, ptC, ptI;

    Eigen::Matrix3d covMat;

    while(!idx2compute.empty()){
        int idx = idx2compute.front();
        idx2compute.pop();

        getAdj(idx,adj);
        std::vector<npoint3> pts(adj.size()+1);

        getNode(idx,pt0);
        pts[0] = pt0;

        for(int i=0; i<adj.size(); i++){
            getNode(adj[i],ptI);
            pts[1+i] = ptI;
        }

        #ifdef DBUG
        std::cout << "App normal point : " << printNode(pt0) << std::endl;
        std::cout << "   pts size = " << pts.size() << std::endl;
        #endif

        // center
        ptC = npoint3();
        for(npoint3 pt : pts) ptC += pt;
        ptC /= pts.size();

        #ifdef DBUG
        std::cout << "  centroid : " << printNode(ptC) << std::endl;
        #endif

        
        // covariance matrix
        covMat.setZero();
        for(npoint3 pt : pts){
            npoint3 dP = pt - ptC;

            covMat(0,0) += dP[0]*dP[0];
            covMat(0,1) += dP[0]*dP[1];
            covMat(0,2) += dP[0]*dP[2];
            covMat(1,1) += dP[1]*dP[1];
            covMat(1,2) += dP[1]*dP[2];
            covMat(2,2) += dP[2]*dP[2];
        }

        covMat(1,0) = covMat(0,1);
        covMat(2,0) = covMat(0,2);
        covMat(2,1) = covMat(1,2);

        #ifdef DBUG
        std::cout << "  cov matrix :" << std::endl;
        std::cout << " " << covMat(0,0) << std::endl;
        std::cout << " " << covMat(1,0) << " " << covMat(1,1) << std::endl;
        std::cout << " " << covMat(2,0) << " " << covMat(2,1) << " " << covMat(2,2) << std::endl;
        #endif
        
        // find approx normal
        Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver(covMat);

        Eigen::Vector3d normalEigen = solver.eigenvectors().col(0);


        npoint3 normal;
        for(int i=0; i<3; i++) normal[i] = normalEigen[i];

        #ifdef DBUG
        std::cout << " eigen norm result : " << printNode(normal) << std::endl;
        #endif

        // conserve direction
        bool setNeighbourFound = false;
        npoint3 ref;
        for(int idxA : adj){
            if(normals[idxA].norm() > EPS_NUM && abs(normals[idxA]*normal) > EPS_NUM){
                ref = normals[idxA];
                setNeighbourFound = true;
                break;
            }
        }

        if(setNeighbourFound && ref*normal < 0) normal *= -1;
        
        // save normal
        normals[idx] = normal;

        #ifdef DBUG
        std::cout << " normal saved : " << printNode(normals[idx]) << std::endl;
        #endif

        // npoint3 vecSphere = pts[0]/pts[0].norm();
        // std::cout << "  should be : " << printNode(vecSphere);
        // if((vecSphere*normal)*(ref*normal) <= 0) std::cout << std::endl;
        // else std::cout << " <<< !!!" << std::endl;

        // add neighbours not visited yet to the queue
        for(int idxA: adj) {
            if(!seen[idxA]) {
                idx2compute.push(idxA);
                seen[idxA] = true;
            }
        }
    }

 }

 npoint3 FMPointCloud::computeApproximatedNormal(int &ptIdx)
 {
    std::vector<int> pts = neighbourhood[ptIdx];

    double ptCenter[3] = {0,0,0};

    Eigen::Matrix3d covMat;

    // center point

    pts.push_back(ptIdx);

    for(int idx : pts){
        // std::cout << " compute approx node : " << "(" << nodes[idx][0] << "," << nodes[idx][1] << "," << nodes[idx][2] << ")" << std::endl;
        for(int i=0; i < 3; i++){
            ptCenter[i] += nodes[idx][i];
        }
        // std::cout << " compute approx normal center : " << "(" << ptCenter[0] << "," << ptCenter[1] << "," << ptCenter[2] << ")" << std::endl;
    }

    for(double& coord : ptCenter) coord /= pts.size();

    // std::cout << " compute approx normal center : " << "(" << ptCenter[0] << "," << ptCenter[1] << "," << ptCenter[2] << ")" << std::endl;


    // covariance matrix

    for(int idx : pts){
        double dP[3] = {nodes[idx][0]-ptCenter[0],nodes[idx][1]-ptCenter[1],nodes[idx][2]-ptCenter[2]}; // pt - ptC

        covMat(0,0) += dP[0]*dP[0];
        covMat(0,1) += dP[0]*dP[1];
        covMat(0,2) += dP[0]*dP[2];
        covMat(1,1) += dP[1]*dP[1];
        covMat(1,2) += dP[1]*dP[2];
        covMat(2,2) += dP[2]*dP[2];
    }

    covMat(1,0) = covMat(0,1);
    covMat(2,0) = covMat(0,2);
    covMat(2,1) = covMat(2,1);

    // find approx normal
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver(covMat);

    Eigen::Vector3d normalEigen = solver.eigenvectors().col(0);

    npoint3 normal;
    for(int i=0; i<3; i++) normal[i] = normalEigen[i];
    
    return normal;
 }

 void FMPointCloud::applyNormCurvPlane(npoint3 norm, npoint3 tang)
 {
    norm.normalize();
    tang.normalize();

    for(int i=0; i < nbr_nodes; i++){
        curvs[i].n[0] = norm.x();
        curvs[i].n[1] = norm.y();
        curvs[i].n[2] = norm.z();
        curvs[i].pcv[0] = tang.x();
        curvs[i].pcv[1] = tang.y();
        curvs[i].pcv[2] = tang.z();
        curvs[i].pc1 = 0.;
        curvs[i].pc2 = 0.;
    }
 }

 void FMPointCloud::applyNormCurvCylinder(npoint3 axis, npoint3 ptOnAxis, double radius)
 {
    for(int i=0; i < nbr_nodes; i++){
        npoint3 ptAbs;
        getNode(i,ptAbs);
        npoint pt(ptAbs-ptOnAxis);
        npoint3 ptRad(pt - (pt*axis)*axis);
        ptRad.normalize();

        axis.normalize();
        npoint3 ptTg(crossprod(ptRad,axis));
        ptTg.normalize();

        curvs[i].n[0] = ptRad.x();
        curvs[i].n[1] = ptRad.y();
        curvs[i].n[2] = ptRad.z();
        curvs[i].pcv[0] = ptTg.x();
        curvs[i].pcv[1] = ptTg.y();
        curvs[i].pcv[2] = ptTg.z();
        curvs[i].pc1 = 1/radius;
        curvs[i].pc2 = 0.;
    }
 }

 void FMPointCloud::applyNormCurvCylinder(npoint3 axis, npoint3 ptOnAxis, double radius, double noise)
 {
    std::default_random_engine generator;
    std::normal_distribution<double> distNK1(1/radius, 1/radius*noise/100);
    std::normal_distribution<double> distNK2(0, 1/radius*noise/100);

    for(int i=0; i < nbr_nodes; i++){
        npoint3 ptAbs;
        getNode(i,ptAbs);
        npoint pt(ptAbs-ptOnAxis);
        npoint3 ptRad(pt - (pt*axis)*axis);
        ptRad.normalize();

        axis.normalize();
        npoint3 ptTg(crossprod(ptRad,axis));
        ptTg.normalize();

        curvs[i].n[0] = ptRad.x();
        curvs[i].n[1] = ptRad.y();
        curvs[i].n[2] = ptRad.z();
        curvs[i].pcv[0] = ptTg.x();
        curvs[i].pcv[1] = ptTg.y();
        curvs[i].pcv[2] = ptTg.z();
        curvs[i].pc1 = distNK1(generator);
        curvs[i].pc2 = distNK2(generator);
    }
 }

 void FMPointCloud::applyNormCurvSphere(npoint3 center, npoint3 refAxis, double radius)
 {
    for(int i=0; i < nbr_nodes; i++){
        npoint3 pt;
        getNode(i,pt);

        npoint3 norml(pt-center);
        norml.normalize();

        npoint3 tangeant(crossprod(norml,refAxis));
        if(tangeant.norm() > 0) tangeant.normalize();
        else tangeant = npoint3(1,0,0);

        curvs[i].n[0] = norml.x();
        curvs[i].n[1] = norml.y();
        curvs[i].n[2] = norml.z();
        curvs[i].pcv[0] = tangeant.x();
        curvs[i].pcv[1] = tangeant.y();
        curvs[i].pcv[2] = tangeant.z();
        curvs[i].pc1 = 1/radius;
        curvs[i].pc2 = 1/radius;
    }
 }

 /* GET CURVATURES AND NORMALS */

 double FMPointCloud::interpolateCurv(int idx, npoint3 vec){
    // 1. project vec into the plane of main curvature vectors
    npoint3 nLoc(curvs[idx].n);
    npoint3 vecInP(vec - (vec*nLoc)*nLoc);

    // ? check if vecInP is significant

    // 2. compute angle with 1st main curvature
    npoint3 vecK1(curvs[idx].pcv);
    double cosTh = (vecInP*vecK1)/vecInP.norm();

    // 3. normal curvature
    double KLoc = curvs[idx].pc1 * cosTh*cosTh + curvs[idx].pc2 * (1-cosTh*cosTh); 

    #ifdef DBUG
    std::cout << " INTERP CURV : normal = " << printNode(nLoc) << std::endl;
    std::cout << " INTERP CURV : main curv vec = " << printNode(vecK1) << std::endl;
    std::cout << " INTERP CURV : compose vec tangeant = " << printNode(vecInP) << std::endl;
    std::cout << " INTERP CURV : cos theta tange = " << cosTh << std::endl;
    std::cout << " INTERP CURV : K interp = " << KLoc << std::endl;
    #endif

    return abs(KLoc);
 }

 /* DISPLAY FUNCTIONS (FLTK) */


 void FMPointCloud::displayNodes(data_container &data) const
 {
    data.setcolorpoints(color(255,255,255,255));
    for(int i=0; i<nbr_nodes; i++){
        npoint3 pt;
        getNode(i,pt);
        data.add_point(pt);
    }
 }

 void FMPointCloud::displayNodesKeyRadii(data_container &data) const
 {
    properties p;
    p.pointsize=3;
    data.setproppoints(p);
    for(int i=0; i<nbr_nodes; i++){
        npoint3 pt;
        getNode(i,pt);
        int val;
        if (keyRadii[i]< 0.5) val = 0;
        if ((keyRadii[i]>= 0.5)&&(keyRadii[i]< 0.75))  val = 128;
        if (keyRadii[i]>= 0.75)  val = 255;
//        int val=int(255*keyRadii[i]*1);
        if (val>255) val=255;
        data.setcolorpoints(color(val,val,val,255));
        data.add_point(pt);
    }
 }
 
 void FMPointCloud::displayTree(data_container &data, int rep) const
 {
    int multi = (nbr_nodes<50) ? nbr_nodes/2 : 50;
    rep = (rep<1) ? nbr_nodes/multi: rep;

    // 1. nodes
//    displayNodes(data);

    // 2. neighbourhood
    data.setcolorlines(color(128,128,128,255));
    for(int i=0; i<nbr_nodes; i+=rep){
        npoint3 pt0;
        getNode(i,pt0);
        for(int j=0; j<neighbourhood[i].size(); j++){
            npoint3 pt1;
            getNode(neighbourhood[i][j],pt1);
            data.add_line(line(pt0,pt1));
        }
    }
 }

 void FMPointCloud::displayNodeRelations(data_container &data, int idx) const
 {
    // 1. nodes
    displayNodes(data);

    // 2. neighbourhood
    data.setcolorlines(color(128,128,128,255));
    npoint3 pt0;
    getNode(idx,pt0);
    for(int j=0; j<neighbourhood[idx].size(); j++){
        npoint3 pt1;
        getNode(neighbourhood[idx][j],pt1);
        data.add_line(line(pt0,pt1));
    }
 }

 void FMPointCloud::displayEdgeNodes(data_container &data) const
 {
    // 1. add nodes
    data.setcolorlines(color(128,128,128,255));
    properties prop;
    for(int i=0; i<nbr_nodes; i++){
        if(topol[i].first==FM::topology::isVertex){
            prop.c = color(255,0,255,255);
            prop.pointsize = 15;
            data.setproppoints(prop);
        }
        else if (topol[i].first==FM::topology::onEdge){
            prop.c = color(102,255,51,255);
            prop.pointsize = 10;
            data.setproppoints(prop);
        }
        else{
            prop.c = color(255,255,255,255);
            prop.pointsize = 1;
            data.setproppoints(prop);
        }
            

        npoint3 pt;
        getNode(i,pt);
        data.add_point(pt);

        // 2. neighbourhood of vertex and edges
        if(topol[i].first==FM::topology::isVertex || topol[i].first==FM::topology::onEdge){
            for(int j=0; j<neighbourhood[i].size(); j++){
                npoint3 pt1;
                getNode(neighbourhood[i][j],pt1);
                data.add_line(line(pt,pt1));
            }
        }

    }
 }

 void FMPointCloud::displayCurvatures(data_container &data, bool addNodes, int rep) const
 {
    rep = (rep<1) ? nbr_nodes/100 : rep;

    // 1. nodes (if wanted)
    if(addNodes) displayNodes(data);

    // 2. normals
    data.setcolorlines(color(131, 171, 158,255));
    for(int i=0; i <nbr_nodes; i+=rep){
        npoint3 pt;
        getNode(i,pt);
        npoint3 norm(curvs[i].n);
        data.add_line(line(pt - 0.01*norm, pt + 0.01*norm));
    }

    // 3. main curvature
    data.setcolorlines(color(218, 255, 154,255));
    for(int i=0; i<nbr_nodes; i+=rep){
        npoint3 pt;
        getNode(i, pt);
        npoint3 curv(curvs[i].pcv);
        data.add_line(line(pt - 0.01*curv, pt + 0.01*curv));
    }

 }

 std::string FMPointCloud::printNode(int idx) const
 {
    std::stringstream output;
    npoint3 ndCoor;
    getNode(idx,ndCoor);
    output << "(";
    output << std::setprecision(std::numeric_limits<double>::max_digits10);
    for(int i=0; i<3; i++){
        output << ndCoor[i];
        if(i!=2) output << ",";
    }
    output << ")";
    return output.str();
 }

 std::string FMPointCloud::printNode(npoint3 &pt) const
 {
    std::stringstream output;
    output << "(";
    output << std::setprecision(std::numeric_limits<double>::max_digits10);
    for(int i=0; i<3; i++){
        output << pt[i];
        if(i!=2) output << ",";
    }
    output << ")";
    return output.str();
 }

/* EXPORT FUNCTIONS */

 void FMPointCloud::exportGraphCSR(std::string fileName) const
 {
    if(fileName.empty()){
        std::cerr << "exportGraphCSR : no file given. Export as graph.csr" << std::endl;
        fileName = "nodes.csr";
    } else {
        fileName += ".csr";
    }

    std::vector<int> idxs;
    std::vector<int> idxN;
    idxs.reserve(nbr_nodes+1);
    idxN.reserve(nbr_nodes*nbr_neighbour);

    int writerIdxN = 0;
    for(int i=0; i<nbr_nodes; i++){
        idxs.push_back(writerIdxN);
        for(int iN : neighbourhood[i]){
            idxN.push_back(iN);
        }
        writerIdxN += neighbourhood[i].size();
    }

    ofstream csrFile;
    csrFile.open(fileName);
    if(csrFile.is_open()){
        //for(int id : idxs){
        for(int i=0; i < idxs.size(); i++){
            csrFile << idxs[i];
            if(i < idxs.size()-1) csrFile << " ";
        }
        
        csrFile << std::endl;
        for(int i=0; i < idxN.size(); i++){
            csrFile << idxN[i];
            if(i < idxN.size()-1) csrFile << " ";
        }
        //for(int id : idxN) csrFile << id << " ";

        csrFile.close();
    } else {
        std::cerr << "exportGraphCSR : failed to write " << fileName << std::endl;
    }
 }

 void FMPointCloud::exportNodesCSV(std::string fileName) const
 {
    if(fileName.empty()){
        std::cerr << "exportNodesCSV : no file given. Export as nodes.csv" << std::endl;
        fileName = "nodes.csv";
    } else {
        fileName += ".csv";
    }

    ofstream csvFile;
    csvFile.open(fileName);
    if(csvFile.is_open()){
        //csvFile << "X;Y;Z;" << std::endl;
        csvFile << setprecision(numeric_limits<double>::max_digits10);

        for(int i=0; i<nbr_nodes; i++){
            for(int j=0; j<3; j++) csvFile << nodes[i][j] << ";";
            csvFile << std::endl;
        }
        csvFile.close();
    } else {
        std::cerr << "exportNodesCSV : failed to write " << fileName << std::endl;
    }
 }


}