#include "FMPCAlgo.h"

namespace FM
{

/* PROTECTED FUNCTIONS*/

double FMPCAlgo::distanceCurvatureCorrection(int idx, npoint3 vec, double dist){
    double newDist = dist;
    double KLocal = abs(PC->interpolateCurv(idx,vec));

    /*
    double varC = KLocal*dist;

    if(varC*varC > EPS_NUM){
        newDist = acos(1-.5*varC*varC)/KLocal;
    }

    if(printComputations){
        std::cout << "Correction computations : " << std::endl;
        std::cout << " K interp = " << KLocal;
        if(KLocal > EPS_NUM) std::cout << " / varC =" << varC;
        std::cout << std::endl;
    }
    */


    double x = dist*KLocal/2.;
    double facCorr = 1 + x*x/6. + x*x*x*x*3./40.;// + x*x*x*x*x*x*5./112.; // from asin(x) McLaurin develop

    newDist = dist*facCorr;

    #ifdef DBUG
    std::cout << "  CORRECTION : old dist = " << dist << std::endl;
    std::cout << "  CORRECTION : K interp = " << KLocal << std::endl;
    std::cout << "  CORRECTION : new dist = " << newDist << std::endl;
    #endif

    return newDist;
}
/*
double FMPCAlgo::distanceCorrection(int idx1, int idx2, double xi, double dist)
{
    double newDist = dist;
    npoint3 pt1, pt2;
    PC->getNode(idx1,pt1);
    PC->getNode(idx2,pt2);

    npoint3 vec(pt2-pt1);
    double l12 = vec.normalize();

    double KLocal = abs(PC->interpolateCurv(idx1,vec));

    if(KLocal > EPS_NUM){

        double cosPhi = (1+l12*KLocal/sqrt(2))*(1-l12*KLocal/sqrt(2));
        double epsilon = 2*xi*(1-cosPhi)*(1-xi)/(1+sqrt(2*xi*(1-cosPhi)*(1-xi)+1))/KLocal;

        newDist = sqrt((dist-epsilon)*(dist+epsilon));
    }

    return newDist;
}
*/

/*
idx = pt3. Compute pt of interest at pt3 - dist*vec using plane (pt3, vec) and plane (pt1, pt interest)
*/
double FMPCAlgo::distanceCurvatureCorrection2(int idx3, int idx1, npoint3 vec, double dist)
{
    // 1. find axis of line
    npoint3 norm1, norm3;
    PC->getNormal(idx1,norm1);
    PC->getNormal(idx3,norm3);

    npoint3 pt1, pt3;
    PC->getNode(idx1,pt1);
    PC->getNode(idx3,pt3);

    // npoint3 vec2(pt2-pt1);
    npoint3 vec2(pt3 - pt1 - dist*vec);

    npoint3 nP2(crossprod(-1*vec,norm3));
    npoint3 nP1(crossprod(vec2,norm1));

    #ifdef DBUG
    std::cout << " CORRECTION : nP1 = " << PC->printNode(nP1) << std::endl;
    std::cout << " CORRECTION : nP2 = " << PC->printNode(nP2) << std::endl;
    std::cout << " CORRECTION : vec = " << PC->printNode(vec) << std::endl;
    std::cout << " CORRECTION : vec2 = " << PC->printNode(vec2) << std::endl;
    std::cout << " CORRECTION : norm3 = " << PC->printNode(norm3) << std::endl;
    // std::cout << " CORRECTION : norm3 = " << PC->printNode(norm3) << std::endl;
    // std::cout << " CORRECTION : norm3 = " << PC->printNode(norm3) << std::endl;
    #endif

    npoint3 vecD(crossprod(nP1,nP2));
    vecD.normalize();

    npoint3 eX(crossprod(norm3,nP2));
    eX.normalize();


    // 2. find point on surface
    npoint3 inter(pt3 - dist*vec);

    double curvK = abs(PC->interpolateCurv(idx3,vec));
    double distLin;

    npoint3 newLine;

    double interX = (inter-pt3)*eX;
    double interY = (inter-pt3)*norm3;
    double vecDX = vecD*eX;
    double vecDY = vecD*norm3;
    
    if(curvK > EPS_NUM){
        double B = 2*(curvK*interX*vecDX + curvK*interY*vecDY + vecDY);
        double C = curvK*(interX*interX + interY*interY) + 2*interY;

        #ifdef DBUG
        std::cout << " CORRECTION : B = " << B << "  C = " << C << std::endl;
        std::cout << " CORRECTION : vecDX = " << vecDX << "  vecDY = " << vecDY << std::endl;
        std::cout << " CORRECTION : vecD = " << PC->printNode(vecD) << std::endl;
        #endif

        // double rho = B*B - 4*curvK*C;

        double d3s1 = (signbit(B)) ? (-1*B + sqrt(B*B - 4*curvK*C))/2/curvK : (-1*B - sqrt(B*B - 4*curvK*C))/2/curvK;
        double d3s2 = C/curvK/d3s1;

        double dInter = (abs(d3s1) > abs(d3s2)) ? d3s2 : d3s1;

        #ifdef DBUG
        std::cout << " CORRECTION : distance intersect = " << dInter << " (chosen btwn " << d3s1 << " & " << d3s2 << ")" << std::endl;
        #endif

        //npoint3 ptSurface = (vecD*norm3 > 0) ? (inter + dInter*vecD) : (inter - dInter*vecD);
        npoint3 ptSurface = inter - dInter*vecD;

        newLine = npoint3(ptSurface-pt3);
        distLin = newLine.normalize();

        //std::cout << " CORRECTION : new cut line distance = " << distLin << std::endl;

    } else {
        // double B = 2*vecDY;
        // double C = -2*interY;
        newLine = npoint3(inter-pt3);
        distLin = newLine.norm();
    }

    return distanceCurvatureCorrection(idx3, newLine, distLin);
}

/* INITIALISATION FUNCTIONS */

void FMPCAlgo::initiateFM(npoint3 pt0)
{
    if(printComputations) std::cout << "== Initialisation (initiateFM) ==" << std::endl;

    std::vector<std::pair<int,double>> closest;
    int nbrNeig = PC->getNbrNeighbour();
    PC->findKClosest(pt0,closest,nbrNeig*2,nbrNeig,0.);

    // skip process if it is a node
    if(closest[0].second < EPS_NUM) return initiateFMNode(closest[0].first);

    std::vector<npoint3> grads(closest.size());
    std::vector<int> idxs(closest.size());

    //for(const std::pair<int,double>& nd : closest){
    for(int i = 0; i < closest.size(); i++){
        double distance = closest[i].second;

        npoint3 ptI;
        PC->getNode(closest[i].first,ptI);
        npoint3 vec(ptI-pt0);
        vec.normalize();

        if(curvCorrection) distance = distanceCurvatureCorrection(closest[i].first,vec,distance);

        grads[i] = vec;
        idxs[i] = closest[i].first;

        data->setDistance(closest[i].first,distance);

        if(printComputations) std::cout << " Node " << closest[i].first << " " << PC->printNode(closest[i].first) << " -> distance = " << distance << std::endl;
    }

    // 4inter : set gradients (FM1 & 2)
    setGradients(idxs,grads);

    // 4inter : set normals (FM 2)
    setNormals(idxs,pt0);

    for(int idx : idxs){
        node_status[idx] = status::fix;
        setNodeInfo(idx,methodComp::initiated,0.,std::vector<int>{});
    } 

    if(printComputations) std::cout << "== Initialisation : end ==" << std::endl;

    if(printComputations) std::cout << "== Setting narrow band ==" << std::endl;
    
    // 4. set narrow band
    initNB(idxs);

    if(printComputations) std::cout << "== Narrow band set ==" << std::endl;
}

void FMPCAlgo::initiateFMsmall(npoint3 pt0)
{
    if(printComputations) std::cout << "== Initialisation (initiateFM) ==" << std::endl;
    //1. find neighbours
    std::vector<std::pair<int,double>> closest; // closest nodes to pt0
    int nbrNeig = PC->getNbrNeighbour();
    PC->findKClosest(pt0,closest,nbrNeig*2,nbrNeig,0.);
    int nbrPts = closest.size(); // size of closest

    #ifdef DBUG
    std::cout << "InitiateFM(pt) -> Closest points (" << nbrPts <<  "):" << std::endl;
    for(std::pair<int,double> clPt : closest) std::cout << " " << PC->printNode(clPt.first) << std::endl;
    #endif

    //2. find smallest surface including the projected point

    int lastIdx = 2;
    npoint3 npt0; // new pt0
    std::vector<int> lstIdx = {closest[0].first,closest[1].first,closest[2].first};
    bool triNotFound = !PC->testProjectionPatch(lstIdx, pt0, npt0);
    while(triNotFound && lastIdx < nbrPts){
        lastIdx++;
        lstIdx.push_back(closest[lastIdx].first);
        if(lastIdx < nbrPts) triNotFound = !PC->testProjectionPatch(lstIdx, pt0, npt0);
    }

    #ifdef DBUG
    std::cout << "InitiateFM(pt) -> end try find patch : ";
    if(triNotFound) std::cout << "impossible to project on surface.";
    else std::cout << "projection find on patch of the " << lastIdx+1 << " first nodes.";
    std::cout << std::endl;
    #endif

    if(triNotFound){
        // if it arrives here that means the point is outside the surface
        // project pt0 on the line betwwen the two closest points
        npoint3 pt01, pt02;
        PC->getNode(closest[0].first, pt01);
        PC->getNode(closest[1].first, pt02);

        npoint3 vecEdge(pt02-pt01), pt0p(pt0-pt01);
        double lengthEdge = vecEdge.normalize();
        double refProj = pt0p*vecEdge;

        if(printComputations){
            std::cout << "initiateFM : cannot set point source on surface." << std::endl;
        }

        // directly set at one the node of the closest edge
        if(refProj <= 0) return initiateFMNode(closest[0].first);
        else if(refProj >= lengthEdge) return initiateFMNode(closest[1].first);

        // by default project the point on the edge and start over
        npoint3 npt0((pt0p*vecEdge)*vecEdge + pt01);
        return initiateFM(npt0);
    }
    else {
        // 3. set nodes of the patch found
        if(printComputations) std::cout << "initiateFM : setting patch found :" << std::endl;

        std::vector<npoint3> grads;

        for(int idx : lstIdx){
            npoint3 pt;
            PC->getNode(idx,pt);
            npoint3 vec(pt-npt0);

            double distance = 0.;
            if(vec.norm() > 0){
                distance = vec.normalize();

                if(curvCorrection) distance = distanceCurvatureCorrection(idx,vec,distance);
                
                grads.push_back(vec);
            }
            else {
                grads.push_back(npoint3());
            }

            data->setDistance(idx,distance);

            node_status[idx] = status::fix;


            if(printComputations) std::cout << " Node " << idx << " " << PC->printNode(idx) << " -> distance = " << distance << std::endl;
        }

        // 4inter : set gradients (FM1 & 2)
        setGradients(lstIdx,grads);

        // 4inter : set normals (FM 2)
        setNormals(lstIdx,npt0);

        // 4. set narrow band
        initNB(lstIdx);
    }
    if(printComputations) std::cout << "== Initialisation : end ==" << std::endl;
}

/* NEW VERSION BELOW */

void FMPCAlgo::initiateFMplane1(npoint3 pt0, npoint3 norm)
{
    //this function walks on the plane.
    // Careful : gives errors when the intersection curve is not a straight line !
    // this function is still underdevelopment

    if(printComputations) std::cout << "== Initialisation (initiateFMplane1) ==" << std::endl;

    std::map<int,double> ptsConcerned; // pts concerned by the initialization (pts to fix)
    std::vector<int> pts2recheck; // pts may be concerned by monotonicity criterium
    std::vector<int> order; // order of the idx set in ptsConcerned

    // 1. find closest node to pt0
    int idxClosest;
    double dist0 = PC->findClosest(pt0,idxClosest);
    npoint3 pt1;
    PC->getNode(idxClosest,pt1);

    #ifdef DBUG
    std::cout << "initiateFMplane1 : closest point : node " << idxClosest << " " << PC->printNode(idxClosest) << std::endl;
    #endif

    // 2. get all points to set by going through the neighbourhood

    ptsConcerned.insert({idxClosest,dist0});
    order.push_back(idxClosest);

    for(int i=0; i<order.size(); i++){
        npoint3 ptI;
        PC->getNode(order[i],ptI);
        double distI = (ptI-pt0)*norm;

        // a. get neighbourhood
        std::vector<int> adj;
        PC->getAdj(order[i],adj);

        if(abs(distI) < EPS_NUM){
            // pt I is on the plane : all neighbours are concerned
            for(int idx : adj){
                npoint3 ptJ;
                PC->getNode(idx,ptJ);
                ptsConcerned.insert({idx,(ptJ-pt0)*norm});
                if(std::find(order.begin(),order.end(),idx)==order.end()) order.push_back(idx);
            }
        }
        else {
            // pt I is not on the plane : neighbours on or on the other side of the plane are concerned

            for(int idx : adj){
                npoint3 ptJ;
                PC->getNode(idx,ptJ);

                double distJ = (ptJ-pt0)*norm;

                if(distI*distJ <= 0){
                    ptsConcerned.insert({idx,distJ});
                    if(std::find(order.begin(),order.end(),idx)==order.end()) order.push_back(idx);
                } else {
                    pts2recheck.push_back(idx);
                }
            }

        }
        
    }

    std::map<int,double> pts2set;
    std::map<int,std::vector<int>> ptsTooFar;
    std::map<int,npoint3> grad2set;

    // loop to set node that have at least 2 cut on the plane
    for (const std::pair<const int, double>& pt : ptsConcerned) {
        std::vector<int> adj;
        std::vector<int> otherSideNodes;
        PC->getAdj(pt.first,adj);

        if(printComputations) std::cout << "pt concerned : " << pt.first << " (" << pt.second << ")" << std::endl;

        if(abs(pt.second) > EPS_NUM){
            // pt is not on the plane

            for(int idx : adj){
                auto it = std::find_if(ptsConcerned.begin(), ptsConcerned.end(),
                    [&](const std::pair<int, double>& p) { return p.first == idx; });
                
                if (it != ptsConcerned.end() && it->second * pt.second < EPS_NUM) otherSideNodes.push_back(idx);
            }

            if(printComputations) std::cout << "    " << otherSideNodes.size() << " nodes to use found." << std::endl;

            // add loop to find the good combinaison of nodes
            // if none -> Dijsktra ? or same method as with 1 node ?

            if(otherSideNodes.size() > 1) {
                npoint3 pt2, pt1, pt3;
                PC->getNode(pt.first,pt3);
                PC->getNode(otherSideNodes[0],pt1);
                PC->getNode(otherSideNodes[1],pt2);

                // find intersection with plane
                npoint3 v1(pt3-pt1), v2(pt3-pt2);
                double l1 = v1.normalize();
                double l2 = v2.normalize();

                npoint3 ptA(pt1 + ((norm*(pt0-pt1))/(norm*v1))*v1);
                npoint3 ptB(pt2 + ((norm*(pt0-pt2))/(norm*v2))*v2);

                npoint3 eT(ptB-ptA);
                double leT = eT.normalize();

                // compute height of the triangle
                double xi = (pt3-ptA)*eT /leT;
                npoint3 ht((pt3-ptA)-((pt3-ptA)*eT)*eT);

                pts2set[pt.first] = ht.normalize();

                if(curvCorrection) pts2set[pt.first] = distanceCurvatureCorrection(pt.first,ht,pts2set[pt.first]);

                grad2set[pt.first] = ht;
                
            } else {
                ptsTooFar.insert({pt.first,otherSideNodes});
            }
        } else {
            // pt is on the plane
            if(printComputations) std::cout << "  -> pt on the plane, push to be set." << std::endl;

            pts2set[pt.first] = 0;
            grad2set[pt.first] = npoint3();
        }
    }

    if(printComputations) std::cout << "There are " << ptsTooFar.size() << " nodes that do not have enough neighbour" << std::endl;

    for(auto pt : ptsTooFar){
        // select the neighbour set with multiple points previously

        if(printComputations) std::cout << "  special set " << pt.first << " " << PC->printNode(pt.first) << std::endl;

        int idx1 = pt.second[0];
        
        std::vector<int> adj0;
        PC->getAdj(pt.first,adj0);

        int idxSub;
        double distPlaneMax = __DBL_MAX__;
        for(int& idx : adj0){
            if(idx != idx1 && pts2set.find(idx) != pts2set.end() && pts2set[idx] < distPlaneMax){
                idxSub = idx;
                distPlaneMax = pts2set[idx];
            }
        }

        #ifdef DBUG
        std::cout << "  will use neighbour " << idxSub << " " << PC->printNode(idxSub) << std::endl;
        #endif

        std::vector<int> adj1;
        PC->getAdj(idxSub,adj1);
        int idx2;
        for(int& idx : adj1){
            // std::cout << " > " << idx << " dot prod = " << pts2set[idxSub]*pts2set[idx] << std::endl;
            if(idx != idx1 && pts2set.find(idx) != pts2set.end() && pts2set[idxSub]*pts2set[idx] <= 0){
                idx2 = idx;
                break;
            }
        }

        #ifdef DBUG
        std::cout << "  nodes found for computation : " << idx1 << " " << PC->printNode(idx1) << " and " << idx2 << " " << PC->printNode(idx2) << std::endl;
        #endif

        // same as previous for size > 1
        npoint3 pt2, pt1, pt3;
        PC->getNode(pt.first,pt3);
        PC->getNode(idx1,pt1);
        PC->getNode(idx2,pt2);

        // find intersection with plane
        npoint3 v1(pt3-pt1), v2(pt3-pt2);
        double l1 = v1.normalize();
        double l2 = v2.normalize();

        npoint3 ptA(pt1 + ((norm*(pt0-pt1))/(norm*v1))*v1);
        npoint3 ptB(pt2 + ((norm*(pt0-pt2))/(norm*v2))*v2);


        npoint3 eT(ptB-ptA);
        eT.normalize();

        // compute height of the triangle
        npoint3 ht((pt3-ptA)-((pt3-ptA)*eT)*eT);

        pts2set[pt.first] = ht.normalize();

        if(curvCorrection) pts2set[pt.first] = distanceCurvatureCorrection(pt.first,ht,pts2set[pt.first]);

        grad2set[pt.first] = ht;
    }

    // check in recheck

    // affect values
    setValues(pts2set);

    setGradients(grad2set);

    // setNormals


    for(const std::pair<int,double>& pt : pts2set) node_status[pt.first] = status::fix;

    if(printComputations){
        std::cout << "initiateFMplane1 : " << pts2set.size() << " nodes set :" << std::endl;
        for(const std::pair<const int,const double>& pt : pts2set){
            std::cout << " Node " << pt.first << " " << PC->printNode(pt.first) << " -> distance = " << pt.second << std::endl;
        }
    }
    

    // 4. set narrowband
    if(printComputations) std::cout << "== Setting narrow band ==" << std::endl;

    // 4. set NB
    initNB(pts2set);

    if(printComputations) std::cout << "== Narrow band set ==" << std::endl;





}

/* void FMPCAlgo::initiateFMplane1(npoint3 pt0, npoint3 norm)
{
    if(printComputations) std::cout << "== Initialisation (initiateFMplane1) ==" << std::endl;

    std::set<std::pair<int,double>> ptsConcerned; // pts concerned by the initialization (pts to fix)
    std::vector<int> pts2recheck; // pts may be concerned by monotonicity criterium
    std::vector<int> order; // order of the idx set in ptsConcerned

    // 1. find closest node to pt0
    int idxClosest;
    double dist0 = PC->findClosest(pt0,idxClosest);
    npoint3 pt1;
    PC->getNode(idxClosest,pt1);

    #ifdef DBUG
    std::cout << "initiateFMplane1 : closest point : node " << idxClosest << " " << PC->printNode(idxClosest) << std::endl;
    #endif

    // 2. get all points to set by going through the neighbourhood

    ptsConcerned.insert(std::pair<int,double>(idxClosest,dist0));
    order.push_back(idxClosest);

    for(int i=0; i<order.size(); i++){
        npoint3 ptI;
        PC->getNode(order[i],ptI);
        double distI = (ptI-pt0)*norm;

        // a. get neighbourhood
        std::vector<int> adj;
        PC->getAdj(order[i],adj);

        if(abs(distI) < EPS_NUM){
            // pt I is on the plane : all neighbours are concerned
            for(int idx : adj){
                npoint3 ptJ;
                PC->getNode(idx,ptJ);
                ptsConcerned.insert(std::pair<int,double>(idx,(ptJ-pt0)*norm));
                if(std::find(order.begin(),order.end(),idx)==order.end()) order.push_back(idx);
            }
        }
        else {
            // pt I is not on the plane : neighbours on or on the other side of the plane are concerned

            for(int idx : adj){
                npoint3 ptJ;
                PC->getNode(idx,ptJ);

                double distJ = (ptJ-pt0)*norm;

                if(distI*distJ <= 0){
                    ptsConcerned.insert(std::pair<int,double>(idx,distJ));
                    if(std::find(order.begin(),order.end(),idx)==order.end()) order.push_back(idx);
                } else {
                    pts2recheck.push_back(idx);
                }
            }

        }
        
    }

    std::map<int,double> pts2set;
    std::map<int,npoint3> grad2set;

    // set points in ptsConcerned
    for(const std::pair<int,double>& pt : ptsConcerned){
        std::vector<int> adj;
        std::vector<int> otherSideNodes;
        PC->getAdj(pt.first,adj);

        std::cout << "pt concerned : " << pt.first << " (" << pt.second << ")" << std::endl;


        if(abs(pt.second) > EPS_NUM ){

            std::cout << "  pt not on plane" << std::endl;

            for(int idx : adj){
                auto it = std::find_if(ptsConcerned.begin(), ptsConcerned.end(),
                    [&](const std::pair<int, double>& p) { return p.first == idx; });
                
                if (it != ptsConcerned.end() && it->second * pt.second < EPS_NUM) otherSideNodes.push_back(idx);
            }

            std::cout << "    " << otherSideNodes.size() << " nodes to use found." << std::endl;

            npoint3 pt3;
            PC->getNode(pt.first,pt3);
    
            if(otherSideNodes.size() > 1){
                npoint3 pt2, pt1;
                PC->getNode(otherSideNodes[0],pt1);
                PC->getNode(otherSideNodes[1],pt2);

                // find intersection with plane
                npoint3 v1(pt3-pt1), v2(pt3-pt2);
                double l1 = v1.normalize();
                double l2 = v2.normalize();

                npoint3 ptA(pt1 + ((norm*(pt0-pt1))/(norm*v1))*v1);
                npoint3 ptB(pt2 + ((norm*(pt0-pt2))/(norm*v2))*v2);

                npoint3 eT(ptB-ptA);
                eT.normalize();

                // compute height of the triangle
                npoint3 ht((pt3-ptA)-((pt3-ptA)*eT)*eT);

                // pts2set[pt.first] = ht.normalize();
                grad2set[pt.first] = ht;

            } else {
                npoint3 pt1, pt2;
                PC->getNode(otherSideNodes[0],pt1);

                // find another node that is also neighbour with the node on the other side
                // and the closest to the node to compute

                int idxRef;
                std::vector<int> adj2;
                PC->getAdj(otherSideNodes[0],adj2);

                for(int i=0; i < adj.size(); i++){
                    if(adj[i] == otherSideNodes[0]){
                        idxRef = i;
                        break;
                    }
                }

                std::cout << " idxRef = " << idxRef << std::endl;

                // compute average normal at node otherSideNodes for reference
                // évite les triangles dans la matière
                npoint3 normalLocal = PC->computeApproximatedNormal(otherSideNodes[0]);

                std::cout << "  aprox norm = " << PC->printNode(normalLocal) << std::endl;

                int disMin = adj.size();
                int idxPt2;

                for(int i=0; i<adj.size(); i++){
                    if(std::find(adj2.begin(),adj2.end(),adj[i]) != adj2.end()){
                        int dis = abs(i - idxRef);

                        if(dis < disMin){
                            npoint3 ptInter;
                            PC->getNode(adj[i],ptInter);

                            npoint3 wTriTemp(crossprod(pt1-pt3,ptInter-pt3));
                            wTriTemp.normalize();

                            std::cout << "   wTriTemp : " << PC->printNode(wTriTemp) << std::endl;

                            std::cout << "    wTriTemp*normalLocal = " << wTriTemp*normalLocal << std::endl;

                            if(abs(wTriTemp*normalLocal) > 0.5){
                                disMin = dis;
                                idxPt2 = i;
                            }
                        }
                    }
                }

                std::cout << " adj =";
                for(int i : adj) std::cout << " " << i;
                std::cout << std::endl;

                std::cout << " adj2 =";
                for(int i : adj2) std::cout << " " << i;
                std::cout << std::endl;

                std::cout << " idx closest : " << idxPt2 << std::endl;

                PC->getNode(adj[idxPt2],pt2);

                // compute normal to trig

                std::cout << "  pt1 = " << PC->printNode(pt1) << std::endl;
                std::cout << "  pt2 = " << PC->printNode(pt2) << std::endl;
                std::cout << "  pt3 = " << PC->printNode(pt3) << std::endl;

                npoint3 v1(pt1-pt3);
                v1.normalize();

                npoint3 ptA(pt1 + ((norm*(pt0-pt1))/(norm*v1))*v1);

                std::cout << "  ptA = " << PC->printNode(ptA) << std::endl;

                npoint3 wTri(crossprod(ptA-pt3,pt2-pt3));
                wTri.normalize();

                std::cout << "  wTri = " << PC->printNode(wTri) << std::endl;

                npoint3 normTaken = (pt.second > 0) ? norm : -1*norm;

                npoint3 grad(normTaken-(normTaken*wTri)*wTri);
                grad.normalize();

                std::cout << "  grad = " << PC->printNode(grad) << std::endl;

                pts2set[pt.first] = (pt3-ptA)*grad;
                grad2set[pt.first] = grad;

            }
        }
        else {

            std::cout << "  pt is on plane" << std::endl;

            pts2set[pt.first] = 0;
            grad2set[pt.first] = npoint3();
        }
    }

    // loop on the node to recheck

    // affect values
    setValues(pts2set);

    setGradients(grad2set);

    // setNormals


    for(const std::pair<int,double>& pt : pts2set) node_status[pt.first] = status::fix;

    if(printComputations){
        std::cout << "initiateFMplane1 : " << pts2set.size() << " nodes set :" << std::endl;
        for(const std::pair<const int,const double>& pt : pts2set){
            std::cout << " Node " << pt.first << " " << PC->printNode(pt.first) << " -> distance = " << pt.second << std::endl;
        }
    }
    

    // 4. set narrowband
    if(printComputations) std::cout << "== Setting narrow band ==" << std::endl;

    // 4. set NB
    initNB(pts2set);

    if(printComputations) std::cout << "== Narrow band set ==" << std::endl;


} */

/*
void FMPCAlgo::initiateFMplane1(npoint3 pt0, npoint3 norm)
{*/
    /*
    Explanation of the algorithm :
        starting from the closest node to pt0, we go from one node to another following the
        graph. If the graph link crosses the plane, the node must be considered for the initialisation
        (distance must be fixed). To keep track of the node to set, the map pts2set is the list of the
        node with their respective values. The vector order is the queue of the nodes that must be
        set in map. Order (and pts2set) increases if the node that is currently being set has at least a neighbour
        on the other side of the plane.

        This algorithm set all nodes near the plane that has at least a neighbour on the other side.
        The distance set is not signed. Since it goes from node to node, it naturally stops at borders.

        in this code :
        - vector order helps to keep track of the propagation. in the loops only the adjacent of the order_i are
        concerned (adj_j) : it checks if adj_k of adj_j is on the other side. order contains the indexes of the nodes
        that was added to pts2set and its neighbours will be checked in another iteration.
    */
/*
    if(printComputations) std::cout << "== Initialisation (initiateFMplane1) ==" << std::endl;
    std::map<int,double> pts2set; // points to set map<index,distance measured>
    std::vector<int> order; // order of the key added to map
    std::set<std::pair<double,int>> pts2recheckPlus;
    std::set<std::pair<double,int>> pts2recheckMinus;

    // 1. find closest node to pt0
    int idxClosest;
    PC->findClosest(pt0,idxClosest);
    npoint3 pt1;
    PC->getNode(idxClosest,pt1);

    #ifdef DBUG
    std::cout << "initiateFMplane1 : closest point : node " << idxClosest << " " << PC->printNode(idxClosest) << std::endl;
    #endif

    double distMaxPlus;
    double distMaxMinus;

    // 2. get all points to set by going through the neighbourhood
    pts2set[idxClosest] = (pt1-pt0)*norm;
    order.push_back(idxClosest);
    bool planeDetected;
    for(int i=0; i<order.size(); i++){
        // a. get neighbourhood
        std::vector<int> adj;
        PC->getAdj(order[i],adj);
        for(int j=0; j<adj.size(); j++){
            // b. get neighbourhood of neighbour
            std::vector<int> adj2;
            PC->getAdj(adj[j], adj2);
            npoint3 ndJ;
            PC->getNode(adj[j],ndJ);
            double distPtJ = (ndJ-pt0)*norm; // distance node adj[j] from plane

            // c. check with the neighbourhood if plane crossed. If its the case, add node to queue or update.
            if(std::find(order.begin(),order.begin()+i,adj[j]) == (order.begin()+i)){
                // goes here if node adj[j] has not been checked

                if(abs(distPtJ) < EPS_NUM){
                    if(pts2set.find(adj[j])==pts2set.end()) order.push_back(adj[j]); //add if node is not already in the queue
                    pts2set[adj[j]] = 0;
                }
                else {
                    planeDetected = false;
                    for(int k=0; k < adj2.size() && !planeDetected; k++){ // loop on neighbour to see if need to check node j later
                        npoint3 ndK;
                        PC->getNode(adj2[k],ndK);
                        if(distPtJ*((ndK-pt0)*norm) < EPS_NUM){
                            // if normal components are opposite sign (= different side of plane) -> add node j to check
                            if(distPtJ >= 0 && distPtJ > distMaxPlus) distMaxPlus = distPtJ;
                            if(distPtJ < 0 && distPtJ < distMaxMinus) distMaxMinus = distPtJ;
                            planeDetected = true;
                        }
                    }

                    // calcul ici de la vraie valeur de dist

                    if(planeDetected){
                        if(pts2set.find(adj[j])==pts2set.end()) order.push_back(adj[j]); //add if node is not already in the queue
                        pts2set[adj[j]] = distPtJ;
                    }
                    else{
                        if(distPtJ >= 0) pts2recheckPlus.insert(std::pair<double,int>(distPtJ,adj[j]));
                        else pts2recheckMinus.insert(std::pair<double,int>(distPtJ,adj[j]));
                    }
                }
               
            }

        }
    }

    std::cout << "dist max ini = " << distMaxMinus << " - " << distMaxPlus << std::endl;

    std::cout << "pt2recheck + : " << std::endl;
    for(const std::pair<double,int>& pt : pts2recheckPlus) std::cout << " node " << pt.second << " " << pt.first << std::endl;

    std::cout << "pt2recheck - : " << std::endl;
    for(const std::pair<double,int>& pt : pts2recheckMinus) std::cout << " node " << pt.second << " " << pt.first << std::endl;

    for(const std::pair<double,int>& pt : pts2recheckPlus){
        if(pt.first <= distMaxPlus) pts2set[pt.second] = pt.first;
        else break;
    }

    for(std::set<std::pair<double,int>>::reverse_iterator it = pts2recheckMinus.rbegin(); it != pts2recheckMinus.rend(); it++){
        if(it->first >= distMaxMinus) pts2set[it->second] = it->first;
        else break;
    }

    // 3. set them all
    setValues(pts2set);

    // 4inter : set gradients (FM 1 & 2)
    setGradients(pts2set,norm);

    // 4inter : set normals (FM 2)
    setNormals(pts2set,norm);

    for(const std::pair<int,double>& pt : pts2set) node_status[pt.first] = status::fix;

    if(printComputations){
        std::cout << "initiateFMplane1 : " << pts2set.size() << " nodes set :" << std::endl;
        for(const std::pair<const int,const double>& pt : pts2set){
            std::cout << " Node " << pt.first << " " << PC->printNode(pt.first) << " -> distance = " << pt.second << std::endl;
        }
    }

    if(printComputations) std::cout << "== Initialisation : end ==" << std::endl;

    // 4. set narrowband
    if(printComputations) std::cout << "== Setting narrow band ==" << std::endl;
    
    initNB(pts2set);

    if(printComputations) std::cout << "== Narrow band set ==" << std::endl;

}
*/

void FMPCAlgo::initiateFMplane2(npoint3 pt0, npoint3 norm)
{
    if(printComputations) std::cout << "== Initialisation (initiateFMplane2) ==" << std::endl;
    int nbrNode = PC->getNbrNodes();
    std::map<int,double> pts2set;
    std::set<std::pair<double,int>> pts2recheckPlus;
    std::set<std::pair<double,int>> pts2recheckMinus;

    npoint3 ptI, ptJ;
    std::vector<int> adjI;
    bool planeDetected;

    double distMaxPlus;
    double distMaxMinus;

    // 1. find all nodes that have neighbours on the other side
    for(int i=0; i<nbrNode; i++){
        PC->getNode(i,ptI);
        PC->getAdj(i,adjI);
        double distPtI = (ptI-pt0)*norm;

        planeDetected = false;

        for(int j=0; j<adjI.size() && !planeDetected; j++){
            PC->getNode(adjI[j],ptJ);
            if(distPtI*((ptJ-pt0)*norm) < EPS_NUM){
                pts2set[i] = distPtI;
                planeDetected = true;
                if(distPtI >= 0 && distPtI > distMaxPlus) distMaxPlus = distPtI;
                if(distPtI < 0 && distPtI < distMaxMinus) distMaxMinus = distPtI;
            }
        }

        if(!planeDetected){
            if(distPtI >= 0) pts2recheckPlus.insert(std::pair<double,int>(distPtI,i));
            else pts2recheckMinus.insert(std::pair<double,int>(distPtI,i));
        }
    }

    // 1bis recheck all points
    for(const std::pair<double,int>& pt : pts2recheckPlus){
        if(pt.first <= distMaxPlus) pts2set[pt.second] = pt.first;
        else break;
    }

    for(std::set<std::pair<double,int>>::reverse_iterator it = pts2recheckMinus.rbegin(); it != pts2recheckMinus.rend(); it++){
        if(it->first >= distMaxMinus) pts2set[it->second] = it->first;
        else break;
    }

    // 2. set them all
    setValues(pts2set);

    // 3inter. set gradients (FM 1 & 2)
    setGradients(pts2set,norm);

    // 3inter : set normals (FM 2)
    setNormals(pts2set,norm);

    for(const std::pair<int,double>& pt : pts2set) node_status[pt.first] = status::fix;

    if(printComputations){
        std::cout << "initiateFMplane2 : " << pts2set.size() << " nodes set :" << std::endl;
        for(const std::pair<const int,const double>& pt : pts2set){
            std::cout << " Node " << pt.first << " " << PC->printNode(pt.first) << " -> distance = " << pt.second << std::endl;
        }
    }

    if(printComputations) std::cout << "== Initialisation : end ==" << std::endl;

    // 4. set narrowband
    if(printComputations) std::cout << "== Setting narrow band ==" << std::endl;

    // 4. set NB
    initNB(pts2set);

    if(printComputations) std::cout << "== Narrow band set ==" << std::endl;
}

void FMPCAlgo::initiateCurve(geoFunction linFunc, npoint3 norm)
{
    if(printComputations) std::cout << "== Initialisation (initiateCurve) ==" << std::endl;
    int nbrNode = PC->getNbrNodes();
    std::map<int,double> pts2set;
    for(int i=0; i<nbrNode; i++){
        npoint3 pt;
        PC->getNode(i,pt);
        if(linFunc(pt[0],pt[1],pt[2])){
            std::cout << "pt to add : " << i << std::endl;
            pts2set.insert(std::pair<int,double>(i,0));
        } 
    }

    setValues(pts2set);
    setGradients(pts2set,norm);

    for(const std::pair<int,double>& pt : pts2set) node_status[pt.first] = status::fix;

    if(printComputations){
        std::cout << "initiateFM curve : " << pts2set.size() << " nodes set :" << std::endl;
        for(const std::pair<const int,const double>& pt : pts2set){
            std::cout << " Node " << pt.first << " " << PC->printNode(pt.first) << " -> distance = " << pt.second << std::endl;
        }
    }

    if(printComputations) std::cout << "== Setting narrow band ==" << std::endl;
    initNB(pts2set);
    if(printComputations) std::cout << "== Narrow band set ==" << std::endl;
}

void FMPCAlgo::initiateCurve(geoFunction linFunc, npoint3 norm(double, double, double))
{
    int nbrNode = PC->getNbrNodes();
    std::map<int,double> pts2set;
    std::map<int,npoint3> grad2set;
    for(int i=0; i<nbrNode; i++){
        npoint3 pt;
        PC->getNode(i,pt);
        if(linFunc(pt[0],pt[1],pt[2])){
            pts2set.insert(std::pair<int,double>(i,0));
            grad2set.insert(std::pair<int,npoint3>(i,norm(pt[0],pt[1],pt[2])));
        } 
    }

    setValues(pts2set);
    setGradients(grad2set);

    for(const std::pair<int,double>& pt : pts2set) node_status[pt.first] = status::fix;

    initNB(pts2set);
}

void FMPCAlgo::setValues(std::map<int, double> &pts)
{
    for(const std::pair<int,double>& pt : pts){
        data->setDistance(pt.first,abs(pt.second)); //  ! not signed
    }
}


void FMPCAlgo::initiateFMNode(int idx)
{
    if(printComputations) std::cout << "== Initialisation (initiateFMNode) ==" << std::endl;

    std::vector<int> adj;

    data->setDistance(idx,0.); // starting node set 0

    PC->getAdj(idx,adj);
    npoint3 nd0, ndI;
    PC->getNode(idx,nd0);

    std::vector<npoint3> grads(adj.size()+1);
    std::vector<int> idxs(adj.size()+1);

    idxs[0] = idx;
    grads[0] = npoint3();

    for(int i=0; i<adj.size(); i++){
        PC->getNode(adj[i],ndI);
        npoint3 vec(ndI-nd0);
        
        double distance = vec.normalize();

        if(curvCorrection) distance = distanceCurvatureCorrection(adj[i],vec,distance);

        data->setDistance(adj[i],distance);

        idxs[i+1] = adj[i];
        grads[i+1] = vec; 
    }

    setGradients(idxs,grads);

    setNormals(idxs);

    for(int idx : idxs){
        node_status[idx] = status::fix;
        setNodeInfo(idx,methodComp::initiated,0.,std::vector<int>{});
    }

    if(printComputations){
        for(int idx: idxs){
            std::cout << " Node " << idx<< " " << PC->printNode(idx) << " -> distance = " << data->getDistance(idx) << std::endl;
        }
    }

    if(printComputations) std::cout << "== Initialisation : end ==" << std::endl;

    
    if(printComputations) std::cout << "== Setting narrow band ==" << std::endl;

    initNB(idxs);

    if(printComputations) std::cout << "== Narrow band set ==" << std::endl;
    
}

void FMPCAlgo::initiateFMEdge(npoint3 pt1, npoint3 pt2)
{
    // to do
    std::vector<int> idxEdge;
    PC->findNodesEdge(pt1,pt2,idxEdge);
}

void FMPCAlgo::initNB(std::vector<int> &idxs)
{
    for(int idx : idxs){
        std::vector<int> adj;
        PC->getAdj(idx,adj);
        for(int adjI : adj) addPtInNB(adjI,idx);
    }
}

void FMPCAlgo::initNB(std::map<int, double>& pts)
{
    for(const std::pair<const int,const double>& pt : pts){
        std::vector<int> adj;
        PC->getAdj(pt.first,adj);
        for(int ndAdj : adj) addPtInNB(ndAdj,pt.first);
    }
}


/* PROPAGATION FUNCTION */

void FMPCAlgo::propagateFM()
{
    if(printComputations) std::cout << "== Propagation ==" << std::endl;
    while(!ptsInNB.empty()){

        // 1. take 1st element of ptsInNB (set of narrowband)
        std::set<std::pair<double,int>>::iterator firstInNB = ptsInNB.begin();

        // 2. fix node (value has already been assigned)
        node_status[firstInNB->second] = status::fix;

        if(printComputations){
            std::cout << ">> Node " << firstInNB->second << " " << PC->printNode(firstInNB->second)
                << " fixed with " << firstInNB->first << std::endl;
        }

        // 3. add its neighbours to narrowband or update value
        std::vector<int> adj;
        PC->getAdj(firstInNB->second,adj);

        // std::cout << "   Neighbourhood :";
        // for(int iAdj : adj) std::cout << " " << iAdj;
        // std::cout << std::endl;

        for(int i=0; i<adj.size(); i++){
            if(updatable(firstInNB->second,adj[i])) addPtInNB(adj[i],firstInNB->second);
        }

        // 4. remove present node from narrowband
        ptsInNB.erase(firstInNB);
    }
    if(printComputations) std::cout << "== Propagation ended ==" << std::endl;
}

void FMPCAlgo::addPtInNB(int idx, int idx1)
{
if(node_status[idx] != status::fix){
    resultComputation results = computeFM(idx,idx1);

    if(results.infos.methodUsed != methodComp::undefined){
    if(node_status[idx] == status::inNB){
        // need to find corresponding pair in set
        std::set<std::pair<double,int>>::iterator it = ptsInNB.begin();
        while(it->second != idx) it++;

        // if((node_info[idx].methodUsed == methodComp::edge && results.infos.methodUsed == methodComp::marching) || validSelection(it->second,results)){
        bool pathFinallyFound = node_info[idx].methodUsed == methodComp::marchingEdge && results.infos.methodUsed == methodComp::marching;
        bool pathLost = node_info[idx].methodUsed == methodComp::marching && results.infos.methodUsed == methodComp::marchingEdge;
        if(!pathLost && (pathFinallyFound || validSelection(it->second,results))){
            ptsInNB.erase(it);
            ptsInNB.insert(std::pair<double,int>(results.distance,idx));
            setInfoPtNB(idx,results);

            setNodeInfo(idx,results.infos);

            if(printComputations){
                std::cout << "-- update Node " << idx << " " 
                    << PC->printNode(idx) << " : distance = " << results.distance << std::endl;
            }
        }
    } else {
        setInfoPtNB(idx,results);
        node_status[idx] = status::inNB;
        ptsInNB.insert(std::pair<double,int>(results.distance,idx));

        setNodeInfo(idx,results.infos);

        if(printComputations){
            std::cout << "-- add Node " << idx << " " 
                << PC->printNode(idx) << " : distance = " << results.distance << std::endl;
        }
    }
    } else {
        if(printComputations) std::cout << "-- undefined skip" << std::endl;
    }

}
}

void FMPCAlgo::drawNodeMethod(data_container &dataC) const
{
    properties prop;
    prop.pointsize = 5;
    for(int i = 0; i < PC->getNbrNodes(); i++){
        npoint3 pt;
        PC->getNode(i,pt);
        switch (node_info[i].methodUsed)
        {
        case methodComp::marching :
            prop.c = color(255,0,127,255);
            dataC.setproppoints(prop);
            dataC.add_point(pt);
            break;
        
        case methodComp::edge :
            prop.c = color(0,255,255,255);
            dataC.setproppoints(prop);
            dataC.add_point(pt);
            break;
        
        case methodComp::initiated :
            prop.c = color(51,255,51,255);
            dataC.setproppoints(prop);
            dataC.add_point(pt);
            break;
        
        default:
            prop.c = color(255,0,0,255);
            dataC.setproppoints(prop);
            dataC.add_point(pt);
            break;
        }
    }
}

void FMPCAlgo::drawPathes(data_container &dataC) const
{
    PC->displayNodes(dataC);

    color colors[3] = {color(128,128,0,255),color(200,0,128,255),color(15,150,10,255)};

    for(int i = 0; i < node_info.size(); i++){
        if(node_info[i].methodUsed == methodComp::edge){
            dataC.setcolorlines(color(0,128,128,255));
            npoint3 pt0, pt1;
            PC->getNode(i,pt0);
            PC->getNode(node_info[i].nodeLinked[0], pt1);
            dataC.add_line(line(pt0,pt1));
        }

        if(node_info[i].methodUsed == methodComp::marching){
            dataC.setcolorlines(colors[i%3]);
            npoint3 pt0, pt1, pt2;
            PC->getNode(i,pt0);
            PC->getNode(node_info[i].nodeLinked[0], pt1);
            PC->getNode(node_info[i].nodeLinked[1], pt2);
            dataC.add_line(line(pt0,pt1));
            dataC.add_line(line(pt0,pt2));
        }

        if(node_info[i].methodUsed == methodComp::marchingEdge){
            dataC.setcolorlines(color(242, 232, 94, 255));
            npoint3 pt0, pt1, pt2;
            PC->getNode(i,pt0);
            PC->getNode(node_info[i].nodeLinked[0], pt1);
            PC->getNode(node_info[i].nodeLinked[1], pt2);
            dataC.add_line(line(pt0,pt1));
            dataC.add_line(line(pt0,pt2));
        }
    }
}

void FMPCAlgo::drawElementsUsed(int idx, data_container &dataC) const
{
    std::vector<int> idxGeo;

    getNodeGeodesic(idx,idxGeo);

    for(int i=0; i < idxGeo.size()-1; i++){
        if(node_info[idxGeo[i]].methodUsed == methodComp::edge){
            dataC.setcolorlines(color(0,128,128,255));
            npoint3 pt0, pt1;
            PC->getNode(idxGeo[i],pt0);
            PC->getNode(node_info[idxGeo[i]].nodeLinked[0], pt1);
            dataC.add_line(line(pt0,pt1));
        }

        if(node_info[idxGeo[i]].methodUsed == methodComp::marching){
            dataC.setcolorlines(color(128,128,0,255));
            npoint3 pt0, pt1, pt2;
            PC->getNode(idxGeo[i],pt0);
            PC->getNode(node_info[idxGeo[i]].nodeLinked[0], pt1);
            PC->getNode(node_info[idxGeo[i]].nodeLinked[1], pt2);
            dataC.add_line(line(pt0,pt1));
            dataC.add_line(line(pt0,pt2));
            dataC.add_line(line(pt1,pt2));
        }
    }
    data->displayGradients(dataC,&idxGeo);
    drawNodeMethod(dataC);
    drawGeodesic(idx, dataC);
}

void FMPCAlgo::drawElementsUsed2(int idx, data_container &dataC) const
{
    std::vector<int> ptIdx;

    getNodeInvolved(idx,ptIdx);

    std::vector<color> colors{color(255,255,0), color(0, 255, 128),
                        color(255,192,203), color(191,255,0), color(255, 250, 200),
                        color(255,160,140), color(200,180,255)};

    int colorI = 0;

    for(int idxI : ptIdx){
        if(node_info[idxI].methodUsed == methodComp::edge){
            dataC.setcolorlines(color(0,128,128,255));
            npoint3 pt0, pt1;
            PC->getNode(idxI,pt0);
            PC->getNode(node_info[idxI].nodeLinked[0], pt1);
            dataC.add_line(line(pt0,pt1));
        }

        if(node_info[idxI].methodUsed == methodComp::marching){
            dataC.setcolorlines(colors[colorI]);
            npoint3 pt0, pt1, pt2;
            PC->getNode(idxI,pt0);
            PC->getNode(node_info[idxI].nodeLinked[0], pt1);
            PC->getNode(node_info[idxI].nodeLinked[1], pt2);
            dataC.add_line(line(pt0,pt1));
            dataC.add_line(line(pt0,pt2));
            dataC.add_line(line(pt1,pt2));
            colorI = (colorI + 1) % colors.size();
        }
    }
    drawNodeMethod(dataC);
    drawGeodesic(idx, dataC);
}

void FMPCAlgo::drawGeodesic(int idx, data_container &dataC) const
{
    std::vector<int> idxGeo;

    getNodeGeodesic(idx,idxGeo);

    properties prop;
    prop.c = color(255,165,0,255);
    prop.thickness = 2;

    dataC.setproplines(prop);

    npoint3 pt0, pt1;
    for(int i=0; i < idxGeo.size()-1; i++){
        PC->getNode(idxGeo[i],pt0);
        PC->getNode(idxGeo[i+1],pt1);
        dataC.add_line(line(pt0,pt1));
    }

}

void FMPCAlgo::getNodeGeodesic(int idx, std::vector<int> &ptIdx) const
{
    int presentIdx = idx;
    int nextIdx;

    ptIdx.clear();
    ptIdx.push_back(presentIdx);

    while(node_info[presentIdx].methodUsed != methodComp::initiated && node_info[presentIdx].methodUsed != methodComp::undefined){
        if(node_info[presentIdx].methodUsed == methodComp::marching){
            double dist1 = data->getDistance(node_info[presentIdx].nodeLinked[0]);
            double dist2 = data->getDistance(node_info[presentIdx].nodeLinked[1]);
            nextIdx = (dist1 < dist2) ? node_info[presentIdx].nodeLinked[0] : node_info[presentIdx].nodeLinked[1];
        }
        else if(node_info[presentIdx].methodUsed == methodComp::edge){
            nextIdx = node_info[presentIdx].nodeLinked[0];
        }

        ptIdx.push_back(nextIdx);
        presentIdx = nextIdx;
    }

    ptIdx.push_back(presentIdx);
}

void FMPCAlgo::getNodeInvolved(int idx, std::vector<int> &ptIdx) const
{
    std::queue<int> node2check;
    std::unordered_set<int> nodeChecked;

    ptIdx.clear();

    node2check.push(idx);

    while(!node2check.empty()){
        int idxC = node2check.front();
        node2check.pop();

        if(nodeChecked.find(idxC) != nodeChecked.end()) continue;

        if(node_info[idxC].methodUsed == methodComp::marching || node_info[idxC].methodUsed == methodComp::edge){
            for(int i=0; i < node_info[idxC].nodeLinked.size(); i++) node2check.push(node_info[idxC].nodeLinked[i]);
        }

        ptIdx.push_back(idxC);
        nodeChecked.insert(idxC);
    }

}

/***************************************************************************
 * 
 * FMPCAlgo12 
 * 
 ***************************************************************************
 */



void FMPCAlgo12::filterPointsForFM(int &idx3, int &idx1, std::vector<int> &sndPoints)
{
    sndPoints.clear();

    PC->getAdj(idx3,sndPoints);

    if(printComputations){
        std::cout << "filterPointsForFM (1: " << idx1 << " - 3: " << idx3 << ") : nodes ";
        for(int idx : sndPoints) std::cout << idx << " ";
        std::cout << std::endl;
    }
    
    // delete those in which FMM cannot be applied
    sndPoints.erase(
        std::remove_if(sndPoints.begin(), sndPoints.end(),
        [&](int& idx2){return node_status[idx2]!=status::fix || idx2==idx1 || !isFMpossible(idx1,idx2,idx3); }), sndPoints.end()
    );

    if(printComputations){
        std::cout << " -> nodes selected : ";
        for(int idx : sndPoints) std::cout << idx << " ";
        std::cout << std::endl;
    }

    // reorder the vector based on geodesic distance (new march 26)
    std::sort(sndPoints.begin(),sndPoints.end(), [this](int a, int b) {
        return data->getDistance(a) > data->getDistance(b);
    });

    if(printComputations){
        std::cout << " -> now ordered :";
        for(int idx : sndPoints) std::cout << idx << " ";
        std::cout << std::endl;
    }
}

void FMPCAlgo12::setGradients(std::map<int, double> &pts, npoint3 &grad)
{
    npoint3 norm;
    for(const std::pair<int,double>& pt : pts){
        if(pt.second < -1*EPS_NUM) norm = npoint3(-1*grad);
        else if (abs(pt.second) < EPS_NUM) norm = npoint3();
        else norm = npoint3(grad);

        data->setGradient(pt.first,norm);
    }
}

void FMPCAlgo12::setGradients(std::map<int,npoint3> &pts)
{
    for(std::map<int,npoint3>::iterator it = pts.begin(); it != pts.end(); it++)
    {
        data->setGradient(it->first,it->second);
    }
}

// version < 31/01/26
/*
resultComputation FMPCAlgo12::computeFM(int idx, int idx1)
{
    if(printComputations) std::cout << " computeFM for node " << idx << " " << PC->printNode(idx) << std::endl;

    resultComputation results;

    std::vector<int> adj;
    std::vector<int> scndPoint;
    PC->getAdj(idx,adj);
    for(int ix : adj) if(node_status[ix]==status::fix && ix!=idx1) scndPoint.push_back(ix);

    if(scndPoint.size() == 0){
        npoint3 pt1, pt3;
        PC->getNode(idx1,pt1);
        PC->getNode(idx,pt3);
        npoint3 vec(pt3-pt1);

        double dist0 = vec.normalize();

        if(curvCorrection) dist0 = distanceCurvatureCorrection(idx,vec,dist0);

        results.distance = data->getDistance(idx1) + dist0;
        results.grad = vec;
        results.normal = getNormal2pts(idx,idx1);

        results.infos = infoComp{methodComp::edge,0.,std::vector<int>{idx1}};

        if(printComputations) std::cout << "  Dijsktra called. From node " << idx1 
            << " -> distance = " << results.distance << " / grad = " << PC->printNode(vec) << std::endl;
    }
    else {
        double distance, distMin = __DBL_MAX__;
        npoint3 grad, gradT, nTri;
        npoint3* norm = nullptr;

        // scndPoint -> scndPointFM1 , scndPointDij
        // if scndPointFM1.size() > 0 -> for loop only on scndPointFM1 otherwise scndPointDij
        std::vector<int> idxInside;
        std::vector<int> idxOutside;

        infoComp infos;

        categorizeTriangle(idx,idx1,scndPoint,idxInside,idxOutside);

        if(idxInside.size() > 0) scndPoint = idxInside;

        for(int idx2 : scndPoint){
            distance = computeDataMain(idx1,idx2,idx,gradT,norm);

            if(printComputations){
                std::cout << "  test triangle " << idx1 << " " << idx2 
                    << " -> distance = " << distance << " / grad = " << PC->printNode(gradT) << std::endl;
            }

            if(distance < distMin){
                distMin = distance;
                grad = gradT;
                if(norm){ 
                    nTri = *norm;
                    // std::cout << "TRIANG NORM = " << PC->printNode(*norm) << std::endl;
                    delete norm; 
                    norm = nullptr;}
                else { nTri = npoint3(); }

                infos = infoComp{methodComp::marching,0.,std::vector<int>{idx1,idx2}};

                if(printComputations) std::cout << "   triangle conserved." << std::endl;

            }
        }

        results.distance = distMin;
        results.grad = grad;
        results.normal = nTri;
        results.infos = infos;
    }

    if(printComputations) std::cout << " computeFM for node " << idx << " ended" << std::endl;

    return results;
}
*/
/*
resultComputation FMPCAlgo12::computeFM(int idx, int idx1){

    if(printComputations) std::cout << " computeFM for node " << idx << " " << PC->printNode(idx) << std::endl;

    resultComputation results;
    std::vector<int> scndPoint;

    // 1. get node available for computation
    std::vector<int> adj;

    PC->getAdj(idx,adj);
    for(int ix : adj) if(node_status[ix]==status::fix && ix!=idx1) scndPoint.push_back(ix);

    // 2. get possible triangles
    std::vector<std::pair<double,int>> candidates;
    if(!scndPoint.empty()) categorizeTriangle(idx,idx1,scndPoint,candidates);

    // 3.1. No points nor triangles possible -> Dijkstra
    if(candidates.empty()){
        npoint3 pt1, pt3;
        PC->getNode(idx1,pt1);
        PC->getNode(idx,pt3);
        npoint3 vec(pt3-pt1);

        double dist0 = vec.normalize();

        if(curvCorrection) dist0 = distanceCurvatureCorrection(idx,vec,dist0);

        results.distance = data->getDistance(idx1) + dist0;
        results.grad = vec;
        results.normal = getNormal2pts(idx,idx1);

        results.infos = infoComp{methodComp::edge,0.,std::vector<int>{idx1}};

        if(printComputations) std::cout << "  Dijsktra called. From node " << idx1 
            << " -> distance = " << results.distance << " / grad = " << PC->printNode(vec) << std::endl;
    }
    // 3.2. Available triangle(s) -> FM
    else {
        npoint3 gradT;
        npoint3* norm = nullptr;
        double distMin = __DBL_MAX__, distance;

        for(std::pair<double,int>& cand : candidates){
            distance = computeDataMain(idx1,cand.second,idx,gradT,norm);

            if(printComputations){
                std::cout << "  test triangle " << idx1 << " " << cand.second << " (coef Error : " << cand.first << ")"
                    << " -> distance = " << distance << " / grad = " << PC->printNode(gradT) << std::endl;
            }

            if(distance < distMin){
                distMin = distance;
                
                results.distance = distance;
                results.grad = gradT;
                if(norm){ // FM1 do not give normal but FM2 does
                    results.normal = *norm;
                    delete norm;
                    norm = nullptr;
                }

                results.infos = infoComp{methodComp::marching,0.,std::vector<int>{idx1,cand.second}};

                if(printComputations) std::cout << "   triangle conserved." << std::endl;

                // break;
            }
        }
    }

    if(printComputations) std::cout << " computeFM for node " << idx << " ended" << std::endl;

    return results;
}
*/

resultComputation FMPCAlgo12::computeFM(int idx, int idx1){

    // if(printComputations) std::cout << " computeFM for node " << idx << " " << PC->printNode(idx) << std::endl;

    resultComputation results;

    // 1. get node good for computation
    std::vector<int> sndPoints;
    filterPointsForFM(idx,idx1,sndPoints);

    // 2.1. No points nor triangles possible -> Dijkstra
    if(sndPoints.empty()){

        // DIJKSTRA DISABLE

        // npoint3 pt1, pt3;
        // PC->getNode(idx1,pt1);
        // PC->getNode(idx,pt3);
        // npoint3 vec(pt3-pt1);

        // double dist0 = vec.normalize();

        // if(curvCorrection) dist0 = distanceCurvatureCorrection(idx,vec,dist0);

        // results.distance = data->getDistance(idx1) + dist0;
        // results.grad = vec;
        // results.normal = getNormal2pts(idx,idx1);

        // results.infos = infoComp{methodComp::edge,0.,std::vector<int>{idx1}};

        // if(printComputations) std::cout << "  Dijsktra called. From node ";
    }
    // 2.2. Available triangle(s) -> FM
    else {
        results = computeTriangles(idx1, idx, sndPoints);

        // if(printComputations) std::cout << "  FM called. From node ";
    }

    // if(printComputations) std::cout << idx1 << " -> distance = " << results.distance << " / grad = " << PC->printNode(results.grad) << std::endl;

    return results;
}

double FMPCAlgo12::interpolate(npoint3 &pt, npoint3 &grad)
{
    double distance;
    std::vector<std::pair<int,double>> closest;
    int nbrNeig = PC->getNbrNeighbour();
    PC->findKClosest(pt,closest,nbrNeig*2,nbrNeig,0.);

    if(closest[0].second > EPS_NUM){
        std::vector<int> ndIdx; // nodes that can be used for interpolation

        for(const std::pair<int,double>& ptClose : closest){
            npoint3 gradPt, ptIns;
            data->getGradient(ptClose.first,gradPt);
            PC->getNode(ptClose.first,ptIns);
            if((pt-ptIns)*gradPt >= 0) ndIdx.push_back(ptClose.first);
        }

        #ifdef DBUG
        std::cout << "interpolate : nIdx size = " << ndIdx.size() << std::endl;
        for(int id : ndIdx)
            std::cout << " Node " << id << " : " << PC->printNode(id) << " with dist = " << data->getDistance(id) << std::endl;
        #endif

        if(ndIdx.size() >= 2){
            double distTemp, distMin = __DBL_MAX__;
            npoint3 gradTemp;
            for(int i=0; i<ndIdx.size()-1; i++){
                for(int j=i+1; j<ndIdx.size(); j++){
                    distTemp = computeInterpolation(ndIdx[i],ndIdx[j],pt,gradTemp);
                    if(distTemp < distMin){
                        distance = distTemp;
                        distMin = distTemp;
                        grad = gradTemp;
                    }
                }
            }

        } else {
            npoint3 ndPt;
            PC->getNode(ndIdx[0],ndPt);
            npoint3 vec(pt-ndPt);
            distance = data->getDistance(ndIdx[0]) + vec.normalize();
            grad = vec;
        }
    } else {
        distance = data->getDistance(closest[0].first) + closest[0].second;
        data->getGradient(closest[0].first,grad);
    }

    return distance;
}
}