#include "FM2PC.h"

namespace FM
{
    /* USEFUL FUNCTIONS */
    double radiusInscribedCircle(npoint3 u, npoint3 v){
        if(abs(u*v) != u.norm()*v.norm()){
            double s =  0.5*(u.norm()+v.norm()+(u-v).norm());
            return sqrt((s*(s-u.norm())*(s-v.norm())*(s-(u-v).norm()))/s);
        } else {
            return 0.;
        }
    }

    /* PROPAGATION FUNCTIONS */

    /*
    double FM2PC::computeDataMain(int &idx1, int &idx2, int &idx3, npoint3 &grad, npoint3 *&norm)
    {
        npoint3 grad1, grad2;
        double dist;

        npoint3 normTri = getNormal3pts(idx1,idx2,idx3);

        //std::cout << "COMPUTE DATAMAIN : norm = " << PC->printNode(normTri) << std::endl;

        double dist1 = computeDataTrig(idx1,idx2,idx3,grad1,normTri);

        double dist2 = computeDataTrig(idx2,idx1,idx3,grad2,normTri);

        std::cout << "computeDataMain -> dist1 = " << dist1 << " dist2 = " << dist2 << " |delta = " << abs(dist1-dist2) << std::endl;

        if(dist1 < dist2){
            dist = dist1;
            grad = grad1;
        } else {
            dist = dist2;
            grad = grad2;
        }
        norm = new npoint3(normTri);

        return dist;
    }
    */
    /*
    double FM2PC::computeDataTrig(int &idx1, int &idx2, npoint3 &pt3, npoint3 &grad, npoint3 &norm)
    {
        // 1. triangle data
        npoint3 pt1, pt2, grad1;
        PC->getNode(idx1,pt1);
        PC->getNode(idx2,pt2);
        data->getGradient(idx1,grad1);

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

        double tp1 = data->getDistance(idx1);
        double tp2 = data->getDistance(idx2);

        npoint3 e12(pt2-pt1), e13(pt3-pt1), e23(pt3-pt2);
        double d13 = e13.normalize() + tp1-tp2; // because wavefront at pt 2
        double d23 = e23.normalize();
        e12.normalize();

        if(grad1.norm()<EPS_NUM) grad1 = e13;

        // 2. rotation gradient & normal computation
        // norm = rotateGradient(idx1,e12,e13,grad1);
        rotateGradient(idx1,grad1,norm);

        // 3. finding wavefront
        double d3w = 0.;
        double K2, tanA, u, v, Bcoef, Ccoef, d3s1, d3s2;
        npoint3 grad3;

        bool KSafe = abs(grad1*e12) < 1;//-EPS_NUM;
        bool PnotAlligned = abs(e13*e23) < 1;//-EPS_NUM;
        npoint3 T1, grad3local;
        double b1, a12;

        if(KSafe && PnotAlligned){
            npoint3 P2p = pt1 - (tp1-tp2)*grad1;
            npoint3 deltaP2 = P2p - pt2;

            K2 = 2*(deltaP2*grad1)/(deltaP2*deltaP2);

            T1 = npoint3(crossprod(grad1,norm));
            T1.normalize();
            b1 = (pt3-P2p)*grad1;
            tanA = K2*((pt3-P2p)*T1)/(K2*b1+1);

            grad3local = npoint3(tanA,1,0); // si erreur numérique voir code ancienne version
            grad3local.normalize();
            if(K2*b1 < -1) grad3local *= -1;

            grad3 = grad3local[0]*T1 + grad3local[1]*grad1;

            // 4. checks
            double den = e23*e13;
            u = (grad3*e13 - den*(grad3*e23))/(1-den*den);
            v = (grad3*e23 - den*(grad3*e13))/(1-den*den);

            if(u >= 0 && v >= 0){
                if(K2!=0){
                    a12 = (pt3-P2p)*T1;
                    Bcoef = 2*(K2*a12*grad3local[0]+K2*b1*grad3local[1]+grad3local[1]);
                    Ccoef = K2*a12*a12 + K2*b1*b1 + 2*b1;

                    d3s1 = (signbit(Bcoef)) ? (-1*Bcoef + sqrt(Bcoef*Bcoef - 4*K2*Ccoef))/2/K2 : (-1*Bcoef - sqrt(Bcoef*Bcoef - 4*K2*Ccoef))/2/K2;
                    d3s2 = Ccoef/K2/d3s1;

                    d3w = (abs(d3s1)>abs(d3s2)) ? -1*d3s2 : -1*d3s1;
                    grad = grad3;

                    if(curvCorrection) { }
                } else {
                    Bcoef = 2*grad3local[1];
                    Ccoef = 2*b1;
                    d3w = Ccoef/Bcoef;
                    grad = grad1;

                    if(curvCorrection) {}
                }
            }
            else { // grad outside -> follow edges
                if(v < 0 && u >= 0){
                    d3w = d13;
                    grad = e13;

                    if(curvCorrection) {}
                } else if(u<0 && v >= 0){
                    d3w = d23;
                    grad = e23;

                    if(curvCorrection) {}
                } else {
                    d3w = (d13 < d23) ? d13 : d23;
                    grad = (d13 < d23) ? e13 : e23;

                    if(curvCorrection) {}
                }
            }

        }
        else { // special case : pts 1 and 2 are aligned with the grad
            d3w = (d13 < d23) ? d13 : d23;
            grad = (d13 < d23) ? e13 : e23;
        }

        

        if(printComputations){
            std::cout << " grad 1 : " << PC->printNode(grad1);
            std::cout << " t1 = " << tp1 << " t2 = " << tp2 << std::endl;
            if(KSafe && PnotAlligned){
                std::cout << "  K2 = " << K2 << std::endl;
                std::cout << "  normal = " << PC->printNode(norm) << std::endl;
                #ifdef DBUG
                std::cout << "  b1 = " << b1 << " / a2 = " << a12 << std::endl;
                std::cout << "  T1 = " << PC->printNode(T1) << std::endl;
                #endif
                std::cout << "  tan A = " << tanA << " -> grad3 = " << PC->printNode(grad3) << std::endl;
                std::cout << " u = " << u << " / v = " << v <<std::endl;
                if(u >= 0 && v >= 0) {
                    std::cout << " Bcoef = " << Bcoef << " / Ccoef = " << Ccoef << std::endl;
                    std::cout << " d3s1 = " << d3s1 << " / d3s2 = " << d3s2 << " / d3w = " << d3w << std::endl;
                } else {
                    std::cout << "  grad outside triangle -> following side ";
                    if(v < 0 && u >= 0) std::cout << " 1-3 -> d3w = " << d3w << std::endl;
                    else if(u<0 && v >= 0) std::cout << " 2-3 -> d3w = " << d3w << std::endl;
                    else std::cout << " shortest one -> d3w = " << d3w << std::endl;
                }
            } else {
                std::cout << " points are aligned wrto grad -> Dijsktra / d3w = " << d3w << std::endl;
            }
            std::cout << "  grad3 = " << PC->printNode(grad) << std::endl;
        }

        return tp2 + d3w;
    }
    */
    
    double FM2PC::computeDataTrig(int idx1, int idx2, int idx3, resultComputation &result, double *dTp, int *state)
    {
        // 1. triangle data
        npoint3 pt1, pt2, pt3, grad1, grad;
        PC->getNode(idx1,pt1);
        PC->getNode(idx2,pt2);
        PC->getNode(idx3,pt3);
        data->getGradient(idx1,grad1);

        double tp1 = data->getDistance(idx1);
        double tp2 = data->getDistance(idx2);

        if(printComputations){
        std::cout << "FM2 called on (1,2,3) : " << PC->printNode(pt1) << " "
            << PC->printNode(pt2) << " " << PC->printNode(pt3) << std::endl;
            std::cout << "  normal : " << PC->printNode(result.normal) << std::endl;
        }

        npoint3 e12(pt2-pt1), e13(pt3-pt1), e23(pt3-pt2);
        double d13 = e13.normalize() + tp1-tp2; // because wavefront at pt 2
        double d23 = e23.normalize();
        e12.normalize();

        // 2. rotation gradient & normal computation
        rotateGradient(idx1,grad1,result.normal);

        // 3. finding wavefront
        double d3w = 0;
        double K2, tanA, u, v, Bcoef, Ccoef, d3s1, d3s2;
        npoint3 grad3;

        //TODO: les deux ci-dessous à transférer dans le filtre
        // bool KSafe = abs(grad1*e12) < .999;
        // bool PnotAlligned = abs(e13*e23) < .999;
        npoint3 T1, grad3local;
        double b1, a12;

        // std::cout << " Ksafe = " << grad1*e12 << std::endl;

        // if(KSafe && PnotAlligned){
        // if(KSafe){
            npoint3 P2p = pt1 - (tp1-tp2)*grad1;
            npoint3 deltaP2 = P2p - pt2;
            K2 = 2*(deltaP2*grad1)/(deltaP2*deltaP2);

            // bool seekDetected = false;

            // if (K2 < 0){
            //     double num = crossprod((pt1-pt3),(pt2-pt3))*result.normal;
            //     double deno = crossprod(grad1,(pt2-pt3))*result.normal;

            //     seekDetected = (-1 > K2*num/deno) && ( (pt1+(num/deno)*grad1-pt3)*(pt2-pt3) >= 0 && (pt1+(num/deno)*grad1-pt2)*(pt3-pt2) >= 0);

            //     // std::cout << "  dp = " << num/deno << " /K2*dp = " << K2*num/deno <<std::endl;
            // }

            T1 = npoint3(crossprod(grad1,result.normal));
            T1.normalize();
            b1 = (pt3-P2p)*grad1;
            a12 = (pt3-P2p)*T1;

            // tanA = K2*((pt3-P2p)*T1)/(K2*b1+1);
            double alpha = atan2(K2*a12,K2*b1+1);

            // std::cout << "    P2p = " << PC->printNode(P2p)<< std::endl;
            // std::cout << "    T1 = " << PC->printNode(T1) << std::endl;
            // std::cout << "    b1 = " << b1 << std::endl;
            // std::cout << "    a12 = " << (pt3-P2p)*T1 << std::endl;
            // std::cout << "    tanA = " << tanA << std::endl;
            

            // grad3local = npoint3(tanA,1,0); // si erreur numérique voir code ancienne version
            // grad3local.normalize();
            // if(K2*b1 < -1){
            //     grad3local *= -1;
            //     // std::cout << "TRIGGER" << std::endl;
            // }

            // grad3local = npoint3(sin(alpha),cos(alpha),0);

            // grad3 = grad3local[0]*T1 + grad3local[1]*grad1;
            grad3 = sin(alpha)*T1 + cos(alpha)*grad1;

            // 4. checks
            double den = e23*e13;
            u = (grad3*e13 - den*(grad3*e23))/(1-den*den);
            v = (grad3*e23 - den*(grad3*e13))/(1-den*den);

            if(printComputations){
                std::cout << "  t1 = " << tp1 << "  t2 = " << tp2 << "\n";
                std::cout << "grad1 = " << PC->printNode(grad1) << "\n";
                std::cout << "  K2 = " << K2 << " grad3 = " << PC->printNode(grad3) << "\n";
                std::cout << "   u = " << u << " v = " << v << std::endl;
            }

            // bool ok2compute = u >= 0 && v >= 0 && !seekDetected;
            bool ok2compute = u >= 0 && v >= 0;
            bool failMonotonicity = false;

            if(ok2compute){
                if(K2!=0){
                    
                    // Bcoef = 2*(K2*a12*grad3local[0]+K2*b1*grad3local[1]+grad3local[1]);
                    Bcoef = 2*(K2*a12*sin(alpha) + K2*b1*cos(alpha) + cos(alpha));
                    Ccoef = K2*a12*a12 + K2*b1*b1 + 2*b1;

                    d3s1 = (signbit(Bcoef)) ? (-1*Bcoef + sqrt(Bcoef*Bcoef - 4*K2*Ccoef))/2/K2 : (-1*Bcoef - sqrt(Bcoef*Bcoef - 4*K2*Ccoef))/2/K2;
                    d3s2 = Ccoef/K2/d3s1;

                    d3w = (abs(d3s1)>abs(d3s2)) ? -1*d3s2 : -1*d3s1;
                    grad = grad3;

                } else {
                    // Bcoef = 2*grad3local[1];
                    // Ccoef = 2*b1;
                    // d3w = Ccoef/Bcoef;
                    d3w = b1;
                    grad = grad1;
                }

                if(curvCorrection) { d3w = distanceCurvatureCorrection2(idx3,idx2,grad,d3w); }
                if(state) *state = 0;

                failMonotonicity = d3w < 0;

            }
            
            if(!ok2compute || failMonotonicity){ // grad outside -> follow edges
                double coordClosest = __DBL_MAX__;

                // if(u > v && !seekDetected){
                if(u > v){
                    d3w = d13;
                    grad = e13;
                    if(dTp) *dTp = tp1-tp2;
                    coordClosest = v;
                // } else if (v < u && !seekDetected){
                } else if (v < u){
                    d3w = d23;
                    grad = e23;
                    coordClosest = u;
                } else {
                    d3w = (d13 < d23) ? d13 : d23;
                    grad = (d13 < d23) ? e13 : e23;

                    if(dTp && d13 < d23) *dTp = tp1-tp2;

                    coordClosest = (d13 < d23) ? v : u;
                }

                // if(v < 0 && u >= 0 && !seekDetected){
                //     d3w = d13;
                //     grad = e13;
                //     if(dTp) *dTp = tp1-tp2;

                //     coordClosest = v;

                // } else if(u<0 && v >= 0 && !seekDetected){
                //     d3w = d23;
                //     grad = e23;

                //     coordClosest = u;

                // } else {
                //     d3w = (d13 < d23) ? d13 : d23;
                //     grad = (d13 < d23) ? e13 : e23;

                //     if(dTp && d13 < d23) *dTp = tp1-tp2;

                //     coordClosest = (d13 < d23) ? v : u;
                    
                //     // std::cout << "WAIT ! : u = " << u << " v = " << v << std::endl;

                // }

                if(curvCorrection) { d3w = distanceCurvatureCorrection(idx3,grad,d3w); }
                // if(state) *state = (abs(coordClosest) > 1e-5) ? 1 : 0;
                if(state) *state = 1;
            }
        // }
        // else { // special case : pts 1 and 2 are aligned with the grad
        //     d3w = (d13 < d23) ? d13 : d23;
        //     grad = (d13 < d23) ? e13 : e23;

        //     if(dTp && d13 < d23) *dTp = tp1-tp2;

        //     // std::cout << " special case" << std::endl;

        //     if(curvCorrection) { d3w = distanceCurvatureCorrection(idx3,grad,d3w); }
        //     if(state) *state = 1;
        // }

        // std::cout << "  K = " << K2 
        // std::cout << " / distance = " << tp2+d3w << std::endl;

        result.grad = grad;
        result.distance = tp2;

        double K3 = K2 / (1 + K2*d3w);
        // std::cout << "  K2 = " << K2 << " K3 = " << K3 << std::endl; 
        result.infos = infoComp({methodComp::marching,K3,std::vector<int>{idx1,idx2}});

        return d3w;
    }

    /*
    double FM2PC::computeDataTrig(int &idx1, int &idx2, int &idx3, npoint3 &grad, npoint3 &norm)
    {
        npoint3 pt3;
        PC->getNode(idx3,pt3);
        if(printComputations) std::cout << "[[ FM2 called on " << idx1 << " " << idx2 << " " << idx3 << std::endl;

        // the code below is a copy of the code in the other fct computeDataTrig
        // the reason is that the curvCorrection part requires idx3

        // 1. triangle data
        npoint3 pt1, pt2, grad1;
        PC->getNode(idx1,pt1);
        PC->getNode(idx2,pt2);
        data->getGradient(idx1,grad1);

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

        double tp1 = data->getDistance(idx1);
        double tp2 = data->getDistance(idx2);

        npoint3 e12(pt2-pt1), e13(pt3-pt1), e23(pt3-pt2);
        double d13 = e13.normalize() + tp1-tp2; // because wavefront at pt 2
        double d23 = e23.normalize();
        e12.normalize();

        if(grad1.norm()<EPS_NUM) grad1 = e13;

        // 2. rotation gradient & normal computation
        // norm = rotateGradient(idx1,e12,e13,grad1);
        rotateGradient(idx1,grad1,norm);

        // 3. finding wavefront
        double d3w = 0.;
        double K2, tanA, u, v, Bcoef, Ccoef, d3s1, d3s2;
        npoint3 grad3;

        bool KSafe = abs(grad1*e12) < 1;//-EPS_NUM;
        bool PnotAlligned = abs(e13*e23) < 1;//-EPS_NUM;
        npoint3 T1, grad3local;
        double b1, a12;

        if(KSafe && PnotAlligned){
            npoint3 P2p = pt1 - (tp1-tp2)*grad1;
            npoint3 deltaP2 = P2p - pt2;

            K2 = 2*(deltaP2*grad1)/(deltaP2*deltaP2);

            T1 = npoint3(crossprod(grad1,norm));
            T1.normalize();
            b1 = (pt3-P2p)*grad1;
            tanA = K2*((pt3-P2p)*T1)/(K2*b1+1);

            grad3local = npoint3(tanA,1,0); // si erreur numérique voir code ancienne version
            grad3local.normalize();
            if(K2*b1 < -1) grad3local *= -1;

            grad3 = grad3local[0]*T1 + grad3local[1]*grad1;

            // 4. checks
            double den = e23*e13;
            u = (grad3*e13 - den*(grad3*e23))/(1-den*den);
            v = (grad3*e23 - den*(grad3*e13))/(1-den*den);

            if(u >= 0 && v >= 0){
                if(K2!=0){
                    a12 = (pt3-P2p)*T1;
                    Bcoef = 2*(K2*a12*grad3local[0]+K2*b1*grad3local[1]+grad3local[1]);
                    Ccoef = K2*a12*a12 + K2*b1*b1 + 2*b1;

                    d3s1 = (signbit(Bcoef)) ? (-1*Bcoef + sqrt(Bcoef*Bcoef - 4*K2*Ccoef))/2/K2 : (-1*Bcoef - sqrt(Bcoef*Bcoef - 4*K2*Ccoef))/2/K2;
                    d3s2 = Ccoef/K2/d3s1;

                    d3w = (abs(d3s1)>abs(d3s2)) ? -1*d3s2 : -1*d3s1;
                    grad = grad3;

                } else {
                    Bcoef = 2*grad3local[1];
                    Ccoef = 2*b1;
                    d3w = Ccoef/Bcoef;
                    grad = grad1;
                }

                if(curvCorrection) { d3w = distanceCurvatureCorrection2(idx3,idx2,grad,d3w); }
            }
            else { // grad outside -> follow edges
                if(v < 0 && u >= 0){
                    d3w = d13;
                    grad = e13;

                } else if(u<0 && v >= 0){
                    d3w = d23;
                    grad = e23;

                } else {
                    d3w = (d13 < d23) ? d13 : d23;
                    grad = (d13 < d23) ? e13 : e23;

                }

                if(curvCorrection) { d3w = distanceCurvatureCorrection(idx3,grad,d3w); }
            }

        }
        else { // special case : pts 1 and 2 are aligned with the grad
            d3w = (d13 < d23) ? d13 : d23;
            grad = (d13 < d23) ? e13 : e23;

            if(curvCorrection) { d3w = distanceCurvatureCorrection(idx3,grad,d3w); }
        }

        

        if(printComputations){
            std::cout << " grad 1 : " << PC->printNode(grad1);
            std::cout << " t1 = " << tp1 << " t2 = " << tp2 << std::endl;
            if(KSafe && PnotAlligned){
                std::cout << "  K2 = " << K2 << std::endl;
                std::cout << "  normal = " << PC->printNode(norm) << std::endl;
                #ifdef DBUG
                std::cout << "  b1 = " << b1 << " / a2 = " << a12 << std::endl;
                std::cout << "  T1 = " << PC->printNode(T1) << std::endl;
                #endif
                std::cout << "  tan A = " << tanA << " -> grad3 = " << PC->printNode(grad3) << std::endl;
                std::cout << " u = " << u << " / v = " << v <<std::endl;
                if(u >= 0 && v >= 0) {
                    std::cout << " Bcoef = " << Bcoef << " / Ccoef = " << Ccoef << std::endl;
                    std::cout << " d3s1 = " << d3s1 << " / d3s2 = " << d3s2 << " / d3w = " << d3w << std::endl;
                } else {
                    std::cout << "  grad outside triangle -> following side ";
                    if(v < 0 && u >= 0) std::cout << " 1-3 -> d3w = " << d3w << std::endl;
                    else if(u<0 && v >= 0) std::cout << " 2-3 -> d3w = " << d3w << std::endl;
                    else std::cout << " shortest one -> d3w = " << d3w << std::endl;
                }
            } else {
                std::cout << " points are aligned wrto grad -> Dijsktra / d3w = " << d3w << std::endl;
            }
            std::cout << "  grad3 = " << PC->printNode(grad) << std::endl;
        }

        return tp2 + d3w;
    }
    */
/*    npoint3 FM2PC::computeDataNormal(int &idx1, int &idx2, int &idx3)
    {
        npoint3 grad1, pt1, pt2, pt3;
        data->getGradient(idx1,grad1);

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

        npoint3 v1(pt3-pt1), v2(pt2-pt1);

        return rotateGradient(idx1,v1,v2,grad1);
    }
        */

    npoint3 FM2PC::getNormal2pts(int &idx1, int &idx2)
    { // idx1 pt to set normal (idx2 pt to help)
        npoint3 norm;

        // npoint3 pt1, pt2, pt3;
        npoint3 pt3, pt2;
        PC->getNode(idx1,pt3);
        // PC->getNode(idx2,pt1);

        std::vector<int> adj;
        PC->getAdj(idx2,adj);

        double rMin = __DBL_MAX__;

        // bool changed = false;

        for(int idx : adj){
            if(idx != idx1){
                PC->getNode(idx,pt2);
                // npoint3 v1(pt1-pt2), v2(pt3-pt2);
                // npoint3 nTemp(crossprod(v1,v2));
                double dd = (pt3-pt2).norm();
                npoint3 nTemp = getNormal3pts(idx2,idx,pt3);

                // if(nTemp.norm() > EPS_NUM && radiusInscribedCircle(v1,v2) < rMin){
                if(nTemp.norm() > EPS_NUM && dd < rMin){
                    // npoint3 grad1;
                    norm = nTemp;
                    rMin = dd;
                    // data->getGradient(idx2,grad1);
                    // norm = rotateGradient(idx2,v1,v2,grad1);
                    // changed = true;
                }
            }
        }

        // if(!changed) norm = rotate2D(pt1,pt3,normals[idx2]);

        if(norm*approxNormals[idx1] < 0) norm *= -1;

        if(printComputations) std::cout << " Normal computed for node " << idx1 << " : " << PC->printNode(norm) << std::endl;

        return norm;
    }

    npoint3 FM2PC::getNormal3pts(int &idx1, int &idx2, npoint3 &pt3)
    {
        npoint3 pt1, pt2;
        PC->getNode(idx1,pt1);
        PC->getNode(idx2,pt2);

        npoint3 normT(crossprod(pt3-pt1,pt2-pt1));
        if(approxNormals[idx1]*normT < 0) normT *= -1;

        if(normT.norm() > 0) normT.normalize();
        else normT = npoint3();

        return normT;
    }
/*
    npoint3 FM2PC::rotate2D(npoint3 &pt1, npoint3 &pt2, npoint3 &norm){
        npoint3 vec(pt2-pt1);
        double t = vec.normalize();
        double tN = vec*norm;
        
        npoint3 tP = vec - tN*norm;
        double tPn = tP.normalize();
        bool redo = false;
        npoint3 norm2;

        do {
            redo = false;
            norm2 = tPn*norm - tN*tP;
            norm2.normalize();
            if(abs(norm2*vec) > EPS_NUM){
                tP *= -1;
                redo = true;
            }
        } while(redo);

        return norm2;
    }
*/
/*
    npoi nt3 FM2PC::rotateGradient(int &idx1, npoint3 &v1, npoint3 &v2, npoint3& grad)
    {
        if(printComputations) std::cout << "   rotation called :" << std::endl;
        npoint3 norm1(normals[idx1]);
        npoint3 e1(v1), e2(v2);

        e1.normalize(); e2.normalize();
        npoint3 norm2(crossprod(v1,v2));
        norm2.normalize(); // and the case v1 v2 alligned ?

        npoint3 rotAxis(crossprod(norm1,norm2));
        npoint3 grad1(grad);

        if(rotAxis.norm() <= EPS_NUM){
            if(norm2*norm1 < 0) norm2 *= -1;
        } else {
            bool redo = false;
            do {
                redo = false;
                rotAxis.normalize();
                double cosTh = norm1*norm2;
                grad1 = grad*cosTh - norm1*(norm2*grad) + (rotAxis*grad)*(1-cosTh)*rotAxis;
                npoint3 grad1Naxis = grad1 - (grad1*rotAxis)*rotAxis;
                #ifdef DBUG
                std::cout << "rotAxis = " << PC->printNode(rotAxis) << std::endl;
                #endif
                if(abs(grad1*norm2) > 0 || (grad1Naxis*e1 <0 && grad1Naxis*e2 < 0)){
                    norm2 *= -1;
                    rotAxis *= -1;
                    redo = true;
                    #ifdef DBUG
                    std::cout << "norm2 is inverted -> rotAxis = " << PC->printNode(rotAxis) << std::endl;
                    #endif
                }
            } while(redo);
        }

        if(printComputations){
            std::cout << "   node start (1) : " << idx1 << std::endl;
            std::cout << "    grad Or : " << PC->printNode(grad) << std::endl;
            std::cout << "    norm 1 : " << PC->printNode(norm1)
                << " norm 2 : " << PC->printNode(norm2) << std::endl;
            std::cout << "     -> grad : " << PC->printNode(grad1) << std::endl;
        }

        grad = grad1;

        return norm2;
    }
 */

    void FM2PC::rotateGradient(int &idx1, npoint3 &grad1, npoint3 &normTri)
    {
        // normTri norm of the triangle to rotate into
        double cosTh = normals[idx1]*normTri;
        npoint3 rotateAxis(crossprod(normals[idx1],normTri));
        double normAxis = rotateAxis.norm();


        if(normAxis > EPS_NUM){ // rotation required
            rotateAxis /= normAxis;
            grad1 = grad1*cosTh - normals[idx1]*(normTri*grad1) + (rotateAxis*grad1)*(1-cosTh)*rotateAxis;
        }

    }

    // from version < 10/02/26
    void FM2PC::estimateError(std::vector<resultComputation> &dataTrigs)
    {
        // compare K computed to K from the grads
        npoint3 pt1, pt2, grad1, grad2, deltaP, deltaGrad;
        double Kcomp, t1, t2;
        double Kval;
        
        for(resultComputation& triData : dataTrigs){
            Kcomp = triData.infos.frontCurvature;

            data->getGradient(triData.infos.nodeLinked[0], grad1);
            data->getGradient(triData.infos.nodeLinked[1], grad2);

            rotateGradient(triData.infos.nodeLinked[0],grad1,triData.normal);
            rotateGradient(triData.infos.nodeLinked[1],grad2,triData.normal);

            t1 = data->getDistance(triData.infos.nodeLinked[0]);
            t2 = data->getDistance(triData.infos.nodeLinked[1]);

            PC->getNode(triData.infos.nodeLinked[0], pt1);
            PC->getNode(triData.infos.nodeLinked[1], pt2);

            pt1 -= (t1-t2)*grad1;
            deltaP = pt2 - pt1;
            deltaGrad = grad2 - grad1;

            // std::cout << "  deltaGrad*deltaP = " << deltaGrad*deltaP;
            // std::cout << " / deltaP*deltaP = " << deltaP*deltaP << std::endl;

            Kval = (deltaGrad*deltaP) / (deltaP*deltaP);

            triData.error = abs((Kcomp-Kval)/Kval);

            // std::cout << "estimate error Kcomp/val = " << Kcomp << " / " << Kval << std::endl;
        }

        // code to skip estimator
        // for(resultComputation& triData : dataTrigs){
        //     triData.error = triData.distance;
        // }
    }

    bool FM2PC::validSelection(int iRef, resultComputation &pCand)
    {
        double distRef = data->getDistance(iRef);
        double errorRef = node_info[iRef].error;
        
        bool rangeCovered = std::abs(distRef- pCand.distance) <= (errorRef + pCand.error);

        if(rangeCovered){
            return pCand.error < errorRef;
        } else {
            return pCand.distance < distRef;
        }
    }

    bool FM2PC::updatable(int idx1, int idx3)
    {
        npoint3 pt1, pt3, grad1;
        PC->getNode(idx1,pt1);
        PC->getNode(idx3,pt3);
        data->getGradient(idx1,grad1);

        return (pt3-pt1)*grad1 < PC->getKeyRadius(idx1)*1.1; //1.1
        // return (pt3-pt1).norm() < PC->getKeyRadius(idx1)*3; // 1.2
        // return true;
    }

    // double FM2PC::distanceCorrection2(int idx1, int idx2, int idx3, npoint3 grad, double dist)
    // {
    //     npoint3 pt1, pt2, pt3;
    //     PC->getNode(idx1,pt1);
    //     PC->getNode(idx2,pt2);
    //     PC->getNode(idx3,pt3);

    //     double xi = ((pt3-pt1)*(pt2-pt1)-((pt3-pt1)*grad)*(grad*(pt2-pt1)))/((pt2-pt1)*(pt2-pt1)-(((pt2-pt1)*grad))*((pt2-pt1)*grad));
    //     double dist2 = (pt3-pt1)*grad - xi*(pt2-pt1)*grad;

    //     // double dist3 = distanceCorrection(idx1,idx2,xi,dist2);
    //     double dist3 = distanceCurvatureCorrection2(idx3,idx1,idx2,grad,xi,dist2);

    //     return dist3*dist/dist2;
    // }

    /* INITIALISATION FUNCTIONS */
/*
    void FM2PC::setNormalsDirection(std::vector<npoint3> &norms)
    {
        int idRef = 0;
        while(norms[idRef].norm() < EPS_NUM) idRef++;
        npoint3 ref(norms[idRef]);

        for(int i=0; i < norms.size(); i++) if(ref*norms[i] < 0) norms[i] *= -1;
    }
*/

    void FM2PC::setNormals(std::vector<int> &idxs, npoint3 pt0)
    {
        std::vector<npoint3> norms(idxs.size());
        npoint3 ptI, ptJ;

        // get normals for every nodes set with smallest triangle possible
        for(int i=0; i < idxs.size(); i++){
            PC->getNode(idxs[i],ptI);
            double rMin = __DBL_MAX__;
            for(int j=0; j < idxs.size(); j++){
                if(j!=i){
                    PC->getNode(idxs[j],ptJ);
                    npoint3 v1(ptI-pt0), v2(ptJ-pt0);

                    npoint3 nTemp(crossprod(v1, v2));

                    if(nTemp.norm() > EPS_NUM && radiusInscribedCircle(v1,v2) < rMin){
                        nTemp.normalize();
                        norms[i] = nTemp;
                    }
                }
            }
        }

        // save normals
        for(int i=0; i < idxs.size(); i++){
            if(approxNormals[idxs[i]]*norms[i] < 0) norms[i] *= -1;
            normals[idxs[i]] = norms[i];
        } 

        if(printComputations){
            std::cout << " Normal set :" << std::endl;
            for(int idx : idxs){
                std::cout << "  Node " << idx << " " << PC->printNode(idx) << " : "
                    << PC->printNode(normals[idx]) << std::endl;
            }
        }
    }

    void FM2PC::setNormals(std::vector<int> &idxs)
    {
        // reminder : the first element of vector is the starting node
        npoint3 pt0;
        PC->getNode(idxs[0],pt0);
        std::vector<int> idxs2(idxs.size()-1);

        std::copy(idxs.begin()+1,idxs.end(),idxs2.begin());

        return setNormals(idxs2,pt0);
    }

    void FM2PC::setNormals(std::map<int, double> &pts, npoint3 &normVec)
    {
        std::vector<npoint3> norms(pts.size());
        npoint3 ptI;

        int nIdx = 0;
        for(const std::pair<int,double>& pt : pts){
            PC->getNode(pt.first,ptI);
            npoint3 vec1(pt.second*normVec);
            npoint3 ptPlane(ptI-vec1);
            double rMin = __DBL_MAX__;

            for(const std::pair<int,double>& pt2 : pts){
                if(pt2.first != pt.first){
                    npoint3 ptJ;
                    PC->getNode(pt2.first,ptJ);
                    npoint3 vec2(ptJ-ptPlane);
                    npoint3 normLoc(crossprod(vec1,vec2));

                    if(normLoc.norm() > EPS_NUM && radiusInscribedCircle(vec1,vec2) < rMin){
                        normLoc.normalize();
                        norms[nIdx] = normLoc;
                    }
                }
            }
            nIdx++;
        }

        nIdx = 0;
        for(const std::pair<int,double>& pt : pts){
            if(approxNormals[pt.first]*norms[nIdx] < 0) norms[nIdx] *= -1;
            normals[pt.first] = norms[nIdx];
            nIdx++;
        }

        if(printComputations){
            std::cout << " Normal set :" << std::endl;
            for(const std::pair<int,double>& pt : pts){
                std::cout << "  Node " << pt.first << " " << PC->printNode(pt.first) << " : "
                    << PC->printNode(normals[pt.first]) << std::endl;
            }
        }

    }

    /*
    void FM2PC::categorizeTriangle(int &idx3, int &idx1, std::vector<int> &idxs, std::vector<int> &inside, std::vector<int> &outside)
    {
        inside.clear();
        outside.clear();
  
        npoint3 pt1, pt2, pt3, e23, e12, g1, g2;

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

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

        data->getGradient(idx1,g1);

        if(printComputations){
            std::cout << "categorizeTriangle called : idxs =";
            for(int idx : idxs) std::cout << " " << idx;
            std::cout << std::endl;
        }

        for(int idx : idxs){
            PC->getNode(idx,pt2);
            e23 = npoint3(pt3-pt2);
            e23.normalize();
            e12 = npoint3(pt2-pt1);
            e12.normalize();

            data->getGradient(idx,g2);

            npoint3 normTri = getNormal3pts(idx1,idx,idx3);

            npoint3 grad1 = g1;
            npoint3 grad2 = g2;

            rotateGradient(idx1,grad1,normTri);
            rotateGradient(idx,grad2,normTri);

            if(crossprod(e13,e12)*crossprod(e13,grad1) <= 0 && crossprod(e23,-1*e12)*crossprod(e23,grad2)) inside.push_back(idx);
            else outside.push_back(idx);

        }

        if(printComputations){
            std::cout << "  inside = ";
            for(int idx : inside) std::cout << " " << idx;
            std::cout << std::endl;
            std::cout << "  outside = ";
            for(int idx : outside) std::cout << " " << idx;
            std::cout << std::endl;
        }

    }
    */

    void FM2PC::categorizeTriangle(int &idx3, int &idx1, std::vector<int> &idxs, std::vector<std::pair<double, int>> &candidates)
    {
        candidates.clear();
        
        npoint3 pt1, pt3;
        PC->getNode(idx1,pt1);
        PC->getNode(idx3,pt3);

        npoint3 grad1;
        data->getGradient(idx1,grad1);

        npoint3 pt2, grad2, normalT;

        for(int idx : idxs){
            PC->getNode(idx,pt2);

            data->getGradient(idx,grad2);

            normalT = getNormal3pts(idx1,idx,idx3);

            npoint3 gradT1(grad1);

            rotateGradient(idx1, gradT1, normalT);
            rotateGradient(idx,grad2,normalT);

            // verif gradient dedans

            //
            double cosAngle = gradT1*grad2;
            double errorGrad = 1 - cosAngle;

            candidates.push_back({errorGrad,idx});
        }

        if(candidates.size() > 1) std::sort(candidates.begin(),candidates.end());
    }

    resultComputation FM2PC::computeTriangles(int &idx1, int &idx3, std::vector<int> &sndPoints)
    {
        // std::cout << " triangle " << idx1 << " " << idx3 << " et les pts ";
        // for(int idx : sndPoints) std::cout << idx << " ";
        // std::cout << std::endl;

        // std::vector<resultComputation> results(sndPoints.size());
        resultComputation resultLast, resultKeep, resultPrevious, resultMinSide;
        resultKeep.distance = __DBL_MAX__;
        resultPrevious.distance = __DBL_MAX__;
        resultMinSide.distance = __DBL_MAX__;

        // std::vector<int> states(sndPoints.size());
        int stateLast;
        double d3w1, d3w2, deltaD3w, medD3w, lastd3w = __DBL_MAX__;
        bool keepGoing = true;
        bool stopCriterion = false, followSide = true, first = true;
        int sndIdx = 0;
        int idxKeep = 0;

        bool minSideFound = false;

        while(keepGoing){
            // std::cout << " sndIdx = " << sndIdx << std::endl;
            resultComputation test1, test2;
            npoint3 normal = getNormal3pts(idx1,sndPoints[sndIdx],idx3);
            test1.normal = normal;
            test2.normal = normal;
            int state1, state2;
            double dTp1, dTp2;

            double cosNormal = min(normal*normals[idx1],normal*normals[sndPoints[sndIdx]]);

            // if idx1, idx2 are initiate points, normal is unknown
            if(normals[idx1].norm() == 0 && normals[sndPoints[sndIdx]].norm() == 0) cosNormal = 1;

            // std::cout << "normal : " << PC->printNode(normals[idx1]) << " " << PC->printNode(normals[sndIdx]) << std::endl;  

            d3w1 = computeDataTrig(idx1,sndPoints[sndIdx],idx3,test1,&dTp1,&state1);
            d3w2 = computeDataTrig(sndPoints[sndIdx],idx1,idx3,test2,&dTp2,&state2);

            double dist1 = test1.distance + d3w1;
            double dist2 = test2.distance + d3w2;

            deltaD3w = (dist1 > dist2) ? dist1 - dist2 : dist2 - dist1; 
            medD3w = (dist1 + dist2) / 2.;

            resultLast.distance = medD3w;
            resultLast.error = deltaD3w;///cosNormal;
            for(int j=0; j<3; j++) resultLast.grad[j] = (test1.grad[j] + test2.grad[j]) /2.;
            resultLast.grad.normalize();
            resultLast.normal = normal;
            double meanCurvature = test1.infos.frontCurvature /2. + test2.infos.frontCurvature /2.;
            resultLast.infos = infoComp({methodComp::marching,meanCurvature,std::vector<int>{idx1,sndPoints[sndIdx]}});

            // if(state1 != state2){
            //     std::cout << "STATE MISMATCH | diff = " << abs(dist1-dist2) << std::endl;
            //     std::cout << " d3w1 = " << d3w1 << " /dist1 = " << dist1 << std::endl;
            //     std::cout << " d3w2 = " << d3w2 << " /dist2 = " << dist2 << std::endl;
            // } 

            // stateLast = (state1 && state2);
            
            stateLast = min(state1,state2);
            if(stateLast) resultLast.infos.methodUsed = methodComp::marchingEdge;
            followSide = followSide && stateLast;

            // std::cout << "  cos Normal : " << cosNormal;
            // std::cout << "  state : " << stateLast << " delta : " << deltaD3w << std::endl;

            // std::cout << "  Delta D3w = " << deltaD3w << " / mean = " << medD3w << std::endl;

            if(followSide){
                // std::cout << " - follow side" << std::endl;
                // std::cout << " d3w1 = " << d3w1 - dTp1 << " /dist1 = " << dist1 << std::endl;
                // std::cout << " d3w2 = " << d3w2 - dTp2<< " /dist2 = " << dist2 << std::endl;
                // if(resultKeep.distance > dist1 && !minSideFound){
                //     resultKeep = resultLast;
                // } else {
                //     // if(resultPrevious.distance < resultLast.distance){
                //     //     minSideFound = true;
                //     // }
                // }
                if(resultKeep.distance > resultLast.distance && cosNormal > 0.1) resultKeep = resultLast;

                // std::cout << " followside : ";
                // switch (resultKeep.infos.methodUsed)
                // {
                // case methodComp::marchingEdge :
                //     std::cout << " marching edge";
                //     break;
                
                // case methodComp::marching :
                //     std::cout << " marching";
                // break;
                
                // case methodComp::undefined  :
                //     std::cout << " undefined";
                // break;

                // default:
                //     std::cout << " ?";
                //     break;
                // }

                // std::cout << " dist : " << resultKeep.distance << " / " << resultKeep.error << std::endl;

            } else { 

                if(stateLast==0){
                    bool rangeCovered = std::abs(resultKeep.distance - resultLast.distance) <= (resultKeep.error + resultLast.error)/2.;
                    if(rangeCovered){
                        if(resultKeep.error > resultLast.error){
                            resultKeep = resultLast;
                            // std::cout << "  kept distance : " << resultLast.distance << std::endl;
                        } 
                    } else {
                        if(resultKeep.distance > resultLast.distance){
                            resultKeep = resultLast;
                            // std::cout << "  kept distance : " << resultLast.distance << std::endl;
                        } 
                    }
                }
            }

            sndIdx++;
            resultPrevious = resultLast;
            keepGoing = !stopCriterion && sndPoints.size() - sndIdx > 0;
        }

        // std::cout << "result return :\n";
        // std::cout << " distance = " << resultKeep.distance << std::endl;
        return resultKeep;

        // BELOW : version < 3/3/26
        // if(curvCorrection){
        //     //TODO


        // } else {
        //     for(int i=0; i<sndPoints.size(); i++){
        //         resultComputation test1, test2;
        //         npoint3 normal = getNormal3pts(idx1,sndPoints[i],idx3);
        //         test1.normal = normal;
        //         test2.normal = normal;
        //         int state1, state2;

        //         d3w1 = computeDataTrig(idx1,sndPoints[i],idx3,test1,&state1);
        //         d3w2 = computeDataTrig(sndPoints[i],idx1,idx3,test2,&state2);

        //         double dist1 = test1.distance + d3w1;
        //         double dist2 = test2.distance + d3w2;

        //         deltaD3w = (dist1 > dist2) ? dist1 - dist2 : dist2 - dist1; 
        //         medD3w = (dist1 + dist2) / 2.;

        //         results[i].distance = medD3w;
        //         results[i].error = deltaD3w;
        //         for(int j=0; j<3; j++) results[i].grad[j] = (test1.grad[j] + test2.grad[j]) /2.;
        //         results[i].normal = normal;
        //         double meanCurvature = test1.infos.frontCurvature /2. + test2.infos.frontCurvature /2.;
        //         results[i].infos = infoComp({methodComp::marching,meanCurvature,std::vector<int>{idx1,sndPoints[i]}});

        //         states[i] = (state1 > state2)? state1 : state2;

        //         std::cout << setprecision(numeric_limits<double>::max_digits10);
        //         std::cout << " Trig " << i << " : " << results[i].distance << " +- " << deltaD3w << std::endl;
        //         std::cout << " P2 : " << PC->printNode(sndPoints[i]) << std::endl;
        //         std::cout << "    grad 1 = " << PC->printNode(test1.grad) << " / grad 2 = " << PC->printNode(test2.grad)
        //                 << " / grad = " << PC->printNode(results[i].grad) << std::endl;
        //         std::cout << "    K 1 = " << test1.infos.frontCurvature << "  K 2 = " << test2.infos.frontCurvature
        //                 << "  K = " << results[i].infos.frontCurvature << std::endl;
        //     }
        // }

        // int idxKeep = 0;
        // bool skipSideTriangle = false;

        // for(int i=0; i<results.size(); i++){

            // bool rangeCovered = std::abs(results[idxKeep].distance - results[i].distance) <= (results[idxKeep].error + results[i].error)/2.;

            // if(rangeCovered){
            //     idxKeep = (results[idxKeep].distance < results[i].distance) ? idxKeep : i;
            // } else {
            //     if(results[idxKeep].error < results[i].error + EPS_NUM && results[idxKeep].error > results[i].error - EPS_NUM){
            //         idxKeep = (results[idxKeep].distance < results[i].distance) ? idxKeep : i;
            //     } else {
            //         idxKeep = (results[idxKeep].error < results[i].error) ? idxKeep : i;
            //     }
            // }

            // if(results[idxKeep].error > results[i].error - EPS_NUM){
            //     idxKeep = i;
            // } else if (results[idxKeep].error < results[i].error + EPS_NUM && results[idxKeep].error > results[i].error - EPS_NUM) {
            //     idxKeep = (results[idxKeep].distance < results[i].distance) ? idxKeep : i;
            // }

        //     std::cout << "  " << i << "  State : " << states[i] << std::endl;

        //     npoint3 pt3;
        //     PC->getNode(idx3,pt3);

        //     if(states[i] == 0){
        //         if(!skipSideTriangle){
        //             skipSideTriangle = true;
        //             idxKeep = i;
        //         }

        //         // selection criteria below...

        //         bool rangeCovered = std::abs(results[idxKeep].distance - results[i].distance) <= (results[idxKeep].error + results[i].error);
                
        //         if(rangeCovered){
        //             // idxKeep = (results[idxKeep].distance > results[i].distance) ? i : idxKeep;
        //             idxKeep = (results[idxKeep].error > results[i].error) ? i : idxKeep;
        //         } else {
        //             // idxKeep = (results[idxKeep].error > results[i].error) ? i : idxKeep;
        //             idxKeep = (results[idxKeep].distance > results[i].distance) ? i : idxKeep;
                    
        //         }

        //     }
        //     else if(!skipSideTriangle) {
        //         idxKeep = (results[idxKeep].distance < results[i].distance) ? idxKeep : i;
        //     }
        // }

        // std::cout << "   chosen : " << idxKeep << std::endl;

        // return results[idxKeep];
    }

     // version < 10/02/26
    /*
    resultComputation FM2PC::computeTriangles(int &idx1, int &idx3, std::vector<int> &sndPoints)
    {
        std::vector<resultComputation> results(sndPoints.size()*2);
        double d3w, distance;

        if(curvCorrection){
            int idxResults = 0;
            for(; idxResults < sndPoints.size(); idxResults++){
                int state;
                results[idxResults].normal = getNormal3pts(idx1,sndPoints[idxResults],idx3);
                d3w = computeDataTrig(idx1,sndPoints[idxResults],idx3,results[idxResults],&state);
                if(state == 0){
                    distance = distanceCurvatureCorrection2(idx3,sndPoints[idxResults],results[idxResults].grad,d3w);
                } else {
                    distance = distanceCurvatureCorrection(idx3,results[idxResults].grad,d3w);
                }

                results[idxResults].distance += distance;
            }

            for(int i = 0; i < sndPoints.size(); i++){
                int state;
                results[i+idxResults].normal = getNormal3pts(idx1,sndPoints[i],idx3);
                d3w = computeDataTrig(sndPoints[i],idx1,idx3,results[i],&state);
                if(state == 0){
                    distance = distanceCurvatureCorrection2(idx3,sndPoints[i],results[i+idxResults].grad,d3w);
                } else {
                    distance = distanceCurvatureCorrection(idx3,results[i+idxResults].grad,d3w);
                }

                results[i+idxResults].distance += distance;
            }


        } else {
            int idxResults = 0;
            for(; idxResults < sndPoints.size(); idxResults++){
                results[idxResults].normal = getNormal3pts(idx1,sndPoints[idxResults],idx3);
                d3w = computeDataTrig(idx1,sndPoints[idxResults],idx3,results[idxResults]);
                results[idxResults].distance += d3w;
            }
            for(int i = 0; i < sndPoints.size(); i++){
                results[i+idxResults].normal = getNormal3pts(idx1,sndPoints[i],idx3);
                d3w = computeDataTrig(sndPoints[i],idx1,idx3,results[idxResults+i]);
                results[idxResults+i].distance += d3w;
            }
        }

        // std::cout << " length saved :";
        // for(auto& el : results) std::cout << " "  << el.distance;
        // std::cout << std::endl;

        estimateError(results);

        // std::cout << "  estimate errors :";
        // for(auto& el : results) std::cout << " "  << el.error;
        // std::cout << std::endl;

        int idxErrorMin = 0;
        double errorMin = results[0].error;

        for(int i = 1; i < results.size(); i++){
            if(errorMin > results[i].error){
                idxErrorMin = i;
                errorMin = results[i].error;
            }
        }
        
        return results[idxErrorMin];
    }
    */

    bool FM2PC::isFMpossible(int &idx1, int &idx2, int &idx3)
    {
        // filter distance
        npoint3 pt3, pt2, pt1;
        npoint3 grad1, grad2;
        PC->getNode(idx1,pt1);
        PC->getNode(idx2,pt2);
        PC->getNode(idx3,pt3);

        data->getGradient(idx1,grad1);
        data->getGradient(idx1,grad2);

        npoint3 normal = PC->getNormalTriangle(idx3,idx1,idx2);

        // --- check is triangle can be used
        bool okNormal = normal.norm() > 0.5;

        // --- forward limit in the neighbourhood

        // bool distLimit = (pt3-pt2).norm() < PC->getKeyRadius(idx2)*1.07;

        // bool distLimit = abs((pt3-pt2)*grad1) < PC->getKeyRadius(idx2)*1.05;

        // bool distLimit = abs((pt3-pt2)*grad1) < PC->getKeyRadius(idx3)*1.4; 

        // bool distLimit = abs((pt2-pt1)*grad1) < PC->getKeyRadius(idx2)*.8;

        // bool distLimit = (pt2-pt1).norm() < PC->getKeyRadius(idx1)*3; //1.5

        bool distLimit = abs((pt2-pt1)*grad1) < PC->getKeyRadius(idx1)*1.5; //1.5

        // bool distLimit = (pt3-pt2).norm() < PC->getKeyRadius(idx3)*1;


        // std::cout << " test " << idx1 << " " << idx2 << " " << idx3 << " : ";
        // std::cout << "ok normal : " << okNormal << " distLimit : " << distLimit;// << std::endl;
        // std::cout << " acute : " << PC->isAcute(pt1,pt2,pt3) << std::endl;

        // if(!distLimit) std::cout << "Filter " << idx2 <<" : point too far" << std::endl;
        // if(!okNormal) std::cout << "Filter " << idx2 <<": not triangle" << std::endl;
        // if(!PC->isAcute(pt1,pt2,pt3)) std::cout << "Filter " << idx2 <<": not acute triangle" << std::endl;

        // --- checks for the algorithm
        // 1. grad cannot be aligned with side 12 (K singulatity)
        npoint3 e12(pt2-pt1);
        e12.normalize();

        bool Ksafe1 = crossprod(e12,grad1).norm() > 0;
        bool Ksafe2 = crossprod(e12,grad2).norm() > 0;

        bool Ksafe = Ksafe1 && Ksafe2;

        // 2. avoid seek (gradient singularity)
        bool seekNotDetected = true;
        double tp1 = data->getDistance(idx1);
        double tp2 = data->getDistance(idx2);

        npoint3 deltaP2(pt1 - (tp1-tp2)*grad1 - pt2);
        npoint3 deltaP1(pt2 - (tp2-tp1)*grad2 - pt1);
        double K2 = 2*(deltaP2*grad1)/(deltaP2*deltaP2);
        double K1 = 2*(deltaP1*grad2)/(deltaP1*deltaP1);

        if(K2 < 0 || K1 < 0){
            double num2 = crossprod((pt1-pt3),(pt2-pt3))*normal;
            double deno2 = crossprod(grad1,(pt2-pt3))*normal;

            bool seekDetected2 = (-1 > K2*num2/deno2) && ( (pt1+(num2/deno2)*grad1-pt3)*(pt2-pt3) >= 0 && (pt1+(num2/deno2)*grad1-pt2)*(pt3-pt2) >= 0);

            double num1 = crossprod((pt2-pt3),(pt1-pt3))*normal;
            double deno1 = crossprod(grad2,(pt1-pt3))*normal;

            bool seekDetected1 = (-1 > K1*num1/deno1) && ( (pt2+(num1/deno1)*grad2-pt3)*(pt1-pt3) >= 0 && (pt2+(num1/deno1)*grad2-pt1)*(pt3-pt1) >= 0);

            seekNotDetected = !seekDetected1 && !seekDetected2;
        }


        return distLimit && okNormal && PC->isAcute(pt1,pt2,pt3) && Ksafe && seekNotDetected;

        // return true;
    }

    /* INTERPOLATION FUNCTIONS */

    double FM2PC::computeInterpolation(int &idx1, int &idx2, npoint3 &pt, npoint3 &grad)
    {
        /* npoint3 norm = getNormal3pts(idx1,idx2,pt);

        npoint3 grad1, grad2;
        double dist;

        double dist1 = computeDataTrig(idx1,idx2,pt,grad1,norm);

        double dist2 = computeDataTrig(idx2,idx1,pt,grad2,norm);

        if(dist1 < dist2){
            dist = dist1;
            grad = grad1;
        } else {
            dist = dist2;
            grad = grad2;
        }

        return dist; */
        return 0.;
    }

    /* DEBUG FUNCTIONS */

    void FM2PC::printNormals() const
    {
        const std::string fileName = "normalsExport.csv";

        ofstream csvFile;
        csvFile.open(fileName);
        if(csvFile.is_open()){
            csvFile << setprecision(numeric_limits<double>::max_digits10);
            int nbrN = PC->getNbrNodes();
            npoint3 nodeI;
            for(int i=0; i<nbrN; i++){
                PC->getNode(i,nodeI);
                for(int j=0; j<3; j++) csvFile << nodeI[j] << ";";
                for(int jj=0; jj<3; jj++) csvFile << approxNormals[i][jj] << ";";
                for(int jjj=0; jjj<3; jjj++) csvFile << normals[i][jjj] << ";";
                csvFile << std::endl;
            }
            csvFile.close();
        } else {
            std::cerr << "printNormals : failed to write " << fileName << std::endl;
        }
    }

    void FM2PC::displayApproximatedNormals(data_container &data, bool dispNode) const
    {
        if(dispNode) PC->displayNodes(data);

        data.setcolorlines(color(131, 171, 158,255)); // blueish green
        for(int i=0; i<approxNormals.size(); i++){
            npoint3 pt;
            PC->getNode(i,pt);
            data.add_line(line(pt,pt+.1*approxNormals[i]));
            //std::cout << "print normal = " << PC->printNode(normIs) << std::endl;
        }
    }

    void FM2PC::displayComputedNormals(data_container &data, bool dispNode) const
    {
        if(dispNode) PC->displayNodes(data);

        data.setcolorlines(color(255, 51, 204,255)); // magenta
        for(int i=0; i<normals.size(); i++){
            npoint3 pt;
            PC->getNode(i,pt);
            data.add_line(line(pt,pt+.1*normals[i]));
            //std::cout << "print normal = " << PC->printNode(normIs) << std::endl;
        }
    }

    void FM2PC::displayNormals(data_container &data) const
    {
        displayApproximatedNormals(data);
        displayComputedNormals(data,false);
    }

    void FM2PC::displayFrontCurvature(data_container &data) const
    {
        double Kmin = node_info[0].frontCurvature, Kmax = node_info[0].frontCurvature;

        for(int i=1; i<node_info.size(); i++){
            if(node_info[i].frontCurvature < Kmin) Kmin = node_info[i].frontCurvature;
            if(node_info[i].frontCurvature > Kmax) Kmax = node_info[i].frontCurvature;
        }

        std::cout << "Kmin = " << Kmin << " Kmax = " << Kmax << std::endl;

        double ratio;
        properties prop;
        prop.pointsize = 15.;

        for(int i=0; i<node_info.size(); i++){
            ratio = (node_info[i].frontCurvature - Kmin)/(Kmax-Kmin);
            
            prop.c = color(255*ratio,255*(1-ratio),0,255);
            data.setproppoints(prop);

            npoint3 pt;
            PC->getNode(i,pt);
            data.add_point(pt);
        }
    }
}