#include "rbfcore.h"
#include "mymesh/utility.h"
#include "Solver.h"
#include <armadillo>
#include <fstream>
#include <limits>
#include <unordered_map>
#include <ctime>
#include <chrono>
#include <iomanip>
#include <algorithm>
#include <queue>
#include "mymesh/readers.h"
#include "mymesh/UnionFind.h"
//#include "mymesh/tinyply.h"

typedef std::chrono::high_resolution_clock Clock;
double randomdouble() {return static_cast <double> (rand()) / static_cast <double> (RAND_MAX);}
double randomdouble(double be,double ed) {return be + randomdouble()*(ed-be);	}

void RBF_Core::NormalRecification(double maxlen, vector<double>&nors){


    double maxlen_r = -1;
    auto p_vn = nors.data();
    int  np = nors.size()/3;
    if(1){
        for(int i=0;i<np;++i){
            maxlen_r = max(maxlen_r,MyUtility::normVec(p_vn+i*3));
        }

        cout<<"maxlen_r: "<<maxlen_r<<endl;
        double ratio = maxlen / maxlen_r;
        for(auto &a:nors)a*=ratio;
    }else{
        for(int i=0;i<np;++i){
            MyUtility::normalize(p_vn+i*3);
        }

    }




}

bool RBF_Core::Write_Hermite_NormalPrediction(string fname, int mode){


    vector<uchar>labelcolor(npt*4);
    vector<uint>f2v;
    uchar red[] = {255,0,0, 255};
    uchar green[] = {0,255,0, 255};
    uchar blue[] = {0,0,255, 255};
//    for(int i=0;i<labels.size();++i){
//        uchar *pcolor;
//        if(labels[i]==0)pcolor = green;
//        else if(labels[i]==-1)pcolor = blue;
//        else if(labels[i]==1)pcolor = red;
//        for(int j=0;j<4;++j)labelcolor[i*4+j] = pcolor[j];
//    }
    //fname += mp_RBF_METHOD[curMethod];

    for(int i=0;i<npt;++i){
        uchar *pcolor = green;
        for(int j=0;j<4;++j)labelcolor[i*4+j] = pcolor[j];
    }

    vector<double>nors;
    if(mode ==0)nors=initnormals;
    else if(mode == 1)nors=newnormals;
    else if(mode == 2)nors = initnormals_uninorm;
    NormalRecification(1.,nors);

    //for(int i=0;i<npt;++i)if(randomdouble()<0.5)MyUtility::negVec(nors.data()+i*3);

    //cout<<pts.size()<<' '<<f2v.size()<<' '<<nors.size()<<' '<<labelcolor.size()<<endl;
    writePLYFile(fname,pts,f2v,nors,labelcolor);

    return 1;
}

bool RBF_Core::Write_Hermite_MST(string fname){

    return writeObjFile_line(fname,pts,MST_edges);

}

void RBF_Core::Set_HermiteRBF(vector<double>&pts){

    cout<<"Set_HermiteRBF"<<endl;
    //for(auto a:pts)cout<<a<<' ';cout<<endl;
    isHermite = true;

    a.set_size(npt*4);
    M.set_size(npt*4,npt*4);
    double *p_pts = pts.data();
    for(int i=0;i<npt;++i){
        for(int j=i;j<npt;++j){
            M(i,j) = M(j,i) = Kernal_Function_2p(p_pts+i*3, p_pts+j*3);
        }
    }


    //if(User_Lamnbda!=0)for(int i=0;i<npt;++i)M(i,i) += User_Lamnbda;


    double G[3];
    for(int i=0;i<npt;++i){
        for(int j=0;j<npt;++j){

            Kernal_Gradient_Function_2p(p_pts+i*3, p_pts+j*3, G);
            //            int jind = j*3+npt;
            //            for(int k=0;k<3;++k)M(i,jind+k) = -G[k];
            //            for(int k=0;k<3;++k)M(jind+k,i) = G[k];

            for(int k=0;k<3;++k)M(i,npt+j+k*npt) = G[k];
            for(int k=0;k<3;++k)M(npt+j+k*npt,i) = G[k];

        }
    }

    double H[9];
    for(int i=0;i<npt;++i){
        for(int j=i;j<npt;++j){

            Kernal_Hessian_Function_2p(p_pts+i*3, p_pts+j*3, H);
            //            int iind = i*3+npt;
            //            int jind = j*3+npt;
            //            for(int k=0;k<3;++k)
            //                for(int l=0;l<3;++l)
            //                    M(jind+l,iind+k) = M(iind+k,jind+l) = -H[k*3+l];

            for(int k=0;k<3;++k)
                for(int l=0;l<3;++l)
                    M(npt+j+l*npt,npt+i+k*npt) = M(npt+i+k*npt,npt+j+l*npt) = -H[k*3+l];
        }
    }

    //cout<<std::setprecision(5)<<std::fixed<<M<<endl;

    bsize= 4;
    N.zeros(npt*4,4);
    b.set_size(4);

    for(int i=0;i<npt;++i){
        N(i,0) = 1;
        for(int j=0;j<3;++j)N(i,j+1) = pts[i*3+j];
    }
    for(int i=0;i<npt;++i){
        //        int ind = i*3+npt;
        //        for(int j=0;j<3;++j)N(ind+j,j+1) = 1;

        for(int j=0;j<3;++j)N(npt+i+j*npt,j+1) = -1;
    }

    //cout<<N<<endl;
    //arma::vec eigval = eig_sym( M ) ;
    //cout<<eigval.t()<<endl;


    if(!isnewformula){
        cout<<"start solve M: "<<endl;
        auto t1 = Clock::now();
        if(isinv)Minv = inv(M);
        else {
            arma::mat Eye;
            Eye.eye(npt*4,npt*4);
            Minv = solve(M,Eye);
        }
        cout<<"solved M: "<<(invM_time = std::chrono::nanoseconds(Clock::now() - t1).count()/1e9)<<endl;

        t1 = Clock::now();
        if(isinv)bprey = inv_sympd(N.t() * Minv * N) * N.t() * Minv;
        else {
            arma::mat Eye2;
            Eye2.eye(bsize,bsize);
            bprey = solve(N.t() * Minv * N, Eye2) * N.t() * Minv;
        }
        cout<<"solved bprey "<<std::chrono::nanoseconds(Clock::now() - t1).count()/1e9<<endl;
    }else{



    }
}



int RBF_Core::Solve_HermiteRBF(vector<double>&vn){

    cout<<"Solve_HermiteRBF "<<npt<<' '<<vn.size()<<endl;
    arma::vec y(npt*4);
    for(int i=0;i<npt;++i)y(i) = 0;
    //for(int i=0;i<npt*3;++i)y(i+npt) = normals[i];
    for(int i=0;i<npt;++i)for(int j=0;j<3;++j)y(npt+i+j*npt) = -vn[i*3+j];

    //cout<<y<<endl;

    b = bprey * y;
    a = Minv * (y - N*b);
    //    cout<<a.t()<<endl;
    //    cout<<b.t()<<endl;
    sol.energy = arma::dot(a,M*a);
    cout<<"Solve_HermiteRBF"<<endl;
    return 1;
}


double Gaussian_2p(const double *p1, const double *p2, double sigma){

    return exp(-MyUtility::vecSquareDist(p1,p2)/(2*sigma*sigma));
}


void MinandAveDist(vector<double>&pts, double &mindist, double &avedist){

    int n = pts.size()/3;
    if(n==0)return;
    mindist = 1e30;
    avedist = 0;
    auto p_v = pts.data();
    for(int i=0;i<n;++i){
        for(int j=i+1;j<n;++j){

            double dist = MyUtility::_VerticesDistance(p_v+i*3,p_v+j*3);
            mindist = min(mindist, dist);
            avedist += dist;




        }
    }
    avedist/=n;



}


void RBF_Core::Set_Actual_User_LSCoef(double user_ls){

    User_Lamnbda = User_Lamnbda_inject = user_ls > 0 ?  user_ls : 0;

}

void RBF_Core::Set_Actual_Hermite_LSCoef(double hermite_ls){

    ls_coef = Hermite_ls_weight_inject = hermite_ls > 0?hermite_ls:0;
}

void RBF_Core::Set_SparsePara(double spa){

    sparse_para = spa;
    if(isuse_sparse){
        SparsifyKH(finalH, sp_H);
        sp_K = sp_H;
    }
}

void RBF_Core::SparsifyKH(arma::mat &KH, arma::sp_mat &sp_KH){

    if(isuse_sparse){
        arma::mat spKH(KH);
        double cutele;
        int spmethod = 3;
        if(spmethod == 0){
            cout<<"spmethod 0"<<endl;
            cutele = max(fabs(spKH.max()),fabs(spKH.min())) * sparse_para;
            for(int i=0;i<npt*3;++i)for(int j=0;j<npt*3;++j)if(fabs(spKH(i,j))<cutele){
                spKH(i,j) = 0;
            }
        }else if(spmethod == 1){
            cout<<"spmethod 1"<<endl;
            Coherence_M.set_size(npt,npt);
            cutele = max(fabs(spKH.max()),fabs(spKH.min())) * sparse_para;
            //cutele =  1e-2;
            for(int p1=0; p1<npt;++p1)for(int p2=p1+1; p2<npt;++p2){
                arma::mat aa(3,3);
                for(int j=0;j<3;++j)for(int k=0;k<3;++k)aa(j,k) = spKH(p1+j*npt,p2+k*npt);
                double ll  = Coherence_M(p1,p2) = Coherence_M(p2,p1) =  arma::norm(aa);
                if(ll<cutele)for(int j=0;j<3;++j)for(int k=0;k<3;++k){spKH(p1+j*npt,p2+k*npt) = spKH(p2+j*npt,p1+k*npt) = 0;}
            }
        }else if(spmethod == 2){
            cout<<"spmethod 2"<<endl;
            Coherence_M.set_size(npt,npt);

            for(int p1=0; p1<npt;++p1)Coherence_M(p1,p1) = 0;
            for(int p1=0; p1<npt;++p1)for(int p2=p1+1; p2<npt;++p2){
                arma::mat aa(3,3);
                for(int j=0;j<3;++j)for(int k=0;k<3;++k)aa(j,k) = spKH(p1+j*npt,p2+k*npt);
                Coherence_M(p1,p2) = Coherence_M(p2,p1) =  arma::norm(aa);
            }
            cutele = Coherence_M.max() * sparse_para;
            for(int p1=0; p1<npt;++p1)for(int p2=p1+1; p2<npt;++p2){
                if(Coherence_M(p1,p2)<cutele)for(int j=0;j<3;++j)for(int k=0;k<3;++k){spKH(p1+j*npt,p2+k*npt) = spKH(p2+j*npt,p1+k*npt) = 0;}
            }
        }else if(spmethod == 3){
            cout<<"spmethod 3"<<endl;
            Coherence_M.set_size(npt,npt);
            for(int p1=0; p1<npt;++p1)Coherence_M(p1,p1) = 0;
            for(int p1=0; p1<npt;++p1)for(int p2=p1+1; p2<npt;++p2){
                arma::mat aa(3,3);
                for(int j=0;j<3;++j)for(int k=0;k<3;++k)aa(j,k) = spKH(p1+j*npt,p2+k*npt);
                Coherence_M(p1,p2) = Coherence_M(p2,p1) =  arma::norm(aa);
            }
            vector<double>rowmax(npt),colmax(npt);
            for(int p1=0; p1<npt;++p1){
                arma::rowvec vv_r = Coherence_M.row(p1);
                rowmax[p1] = vv_r.max();
                arma::vec vv_c = Coherence_M.col(p1);
                colmax[p1] = vv_c.max();
            }
            for(int p1=0; p1<npt;++p1)for(int p2=p1+1; p2<npt;++p2){
                cutele = min(rowmax[p1],colmax[p2]) * sparse_para;
                if(Coherence_M(p1,p2)<cutele)for(int j=0;j<3;++j)for(int k=0;k<3;++k){spKH(p1+j*npt,p2+k*npt) = spKH(p2+j*npt,p1+k*npt) = 0;}
            }

        }
       // spKH.for_each( [&cutele](arma::mat::elem_type& val) { if(fabs(val)<cutele)val=0; } );
        sp_KH = arma::sp_mat(spKH);
        int nsparse = sp_KH.n_nonzero;
        cout<<nsparse<<' '<<double(nsparse)/sp_KH.n_elem<<' '<<sp_KH.max()<<' '<<cutele<<endl;
    }
}

void RBF_Core::Set_User_Lamnda_ToMatrix(double user_ls){


    {
        Set_Actual_User_LSCoef(user_ls);
        auto t1 = Clock::now();
        cout<<"setting K, HermiteApprox_Lamnda"<<endl;
        if(User_Lamnbda>0){
            arma::sp_mat eye;
            eye.eye(npt,npt);

            dI = inv(eye + User_Lamnbda*K00);
            saveK_finalH = K = K11 - (User_Lamnbda)*(K01.t()*dI*K01);

        }else saveK_finalH = K = K11;
        cout<<"solved: "<<(std::chrono::nanoseconds(Clock::now() - t1).count()/1e9)<<endl;
    }

    finalH = saveK_finalH;

    SparsifyKH(finalH, sp_H);
    if(isuse_sparse)sp_K = sp_H;

}

void RBF_Core::Set_HermiteApprox_Lamnda(double hermite_ls){


    {
        Set_Actual_Hermite_LSCoef(hermite_ls);
        auto t1 = Clock::now();
        cout<<"setting K, HermiteApprox_Lamnda"<<endl;
        if(ls_coef>0){
            arma::sp_mat eye;
            eye.eye(npt,npt);

            if(ls_coef > 0){
                arma:: mat tmpdI = inv(eye + (ls_coef+User_Lamnbda)*K00);
                K = K11 - (ls_coef+User_Lamnbda)*(K01.t()*tmpdI*K01);
            }else{
                K = saveK_finalH;
            }
        }
        cout<<"solved: "<<(std::chrono::nanoseconds(Clock::now() - t1).count()/1e9)<<endl;    
    }

    SparsifyKH(K,sp_K);

}



void RBF_Core::Set_Hermite_PredictNormal(vector<double>&pts){



    Set_HermiteRBF(pts);

    auto t1 = Clock::now();
    cout<<"setting K"<<endl;


    if(!isnewformula){
        arma::mat D = N.t()*Minv;
        K = Minv - D.t()*inv(D*N)*D;
        K = K.submat( npt, npt, npt*4-1, npt*4-1 );
        finalH = saveK_finalH = K;
        SparsifyKH(K,sp_K);
        SparsifyKH(finalH, sp_H);
    }else{
        cout<<"using new formula"<<endl;
        bigM.zeros((npt+1)*4,(npt+1)*4);
        bigM.submat(0,0,npt*4-1,npt*4-1) = M;
        bigM.submat(0,npt*4,(npt)*4-1, (npt+1)*4-1) = N;
        bigM.submat(npt*4,0,(npt+1)*4-1, (npt)*4-1) = N.t();

        //for(int i=0;i<4;++i)bigM(i+(npt)*4,i+(npt)*4) = 1;

        auto t2 = Clock::now();
        bigMinv = inv(bigM);
        cout<<"bigMinv: "<<(setK_time= std::chrono::nanoseconds(Clock::now() - t2).count()/1e9)<<endl;
		bigM.clear();
        Minv = bigMinv.submat(0,0,npt*4-1,npt*4-1);
        Ninv = bigMinv.submat(0,npt*4,(npt)*4-1, (npt+1)*4-1);

        bigMinv.clear();
        //K = Minv - Ninv *(N.t()*Minv);
        K = Minv;
        K00 = K.submat(0,0,npt-1,npt-1);
        K01 = K.submat(0,npt,npt-1,npt*4-1);
        K11 = K.submat( npt, npt, npt*4-1, npt*4-1 );

        M.clear();N.clear();
        cout<<"K11: "<<K11.n_cols<<endl;


        Set_Hermite_DesignedCurve();

        Set_User_Lamnda_ToMatrix(User_Lamnbda_inject);

		
//		arma::vec eigval, ny;
//		arma::mat eigvec;
//		ny = eig_sym( eigval, eigvec, K);
//		cout<<ny<<endl;

        cout<<"K: "<<K.n_cols<<endl;
    }




    //K = ( K.t() + K )/2;
    cout<<"solve K total: "<<(setK_time= std::chrono::nanoseconds(Clock::now() - t1).count()/1e9)<<endl;
    return;

}


void RBF_Core::KMatrixAnalysis(){

    int pickpnt = 60;

    //cout<<"sum: "<<arma::sum(K.col(0))<<endl;


    vector<double>weights(npt);
    for(int i=0;i<npt;++i){
        if(i!=pickpnt){
            weights[i] = 0;
            for(int j=0;j<3;++j)for(int k=0;k<3;++k)weights[i]+=pow(K(pickpnt+j*npt,i+k*npt),2);
        }else{
            weights[i] = -1;
        }
    }
    auto sortweights = weights;
    sort(sortweights.begin(),sortweights.end());
    for(auto a:sortweights)cout<<a<<' ';cout<<endl;
    int pickmaxNei = max_element(weights.begin(),weights.end()) - weights.begin();

    arma::vec eigval, ny;
    arma::mat eigvec;


    vector<arma::mat>eigenmatrix(npt);
    vector<arma::vec>eigenval(npt);
    for(int i=0;i<npt;++i){
        arma::mat aa(3,3);
        for(int j=0;j<3;++j)for(int k=0;k<3;++k)aa(j,k) = K(i+j*npt,i+k*npt);
        eig_sym( eigenval[i], eigenmatrix[i], aa);
        //cout<<eigenval[i].t();
    }


    arma::mat aa(3,3);
    for(int j=0;j<3;++j)for(int k=0;k<3;++k)aa(j,k) = K(pickpnt+j*npt,pickmaxNei+k*npt);



    cout<<eigenmatrix[pickpnt]<<endl;
    cout<<eigenval[pickpnt].t()<<endl;
    cout<<eigenmatrix[pickmaxNei]<<endl;
    cout<<eigenval[pickmaxNei].t()<<endl;
    cout<<normalise( aa*eigenmatrix[pickmaxNei] )<<endl;
    cout<<aa<<endl;
    //cout<<aa*aa.t()<<endl;


    exit(0);

}

class Triple{
public:
    double val;
    int i,j;
    Triple();
    Triple(double val, int i, int j):val(val),i(i),j(j){}
};
bool operator <(const Triple& l, const Triple& r){return l.val < r.val;}
bool operator >(const Triple& l, const Triple& r){return l.val > r.val;}


void BuildMST(vector<uint>&MST_edges, vector<vector<int>>&MST_v2v, vector<double>&MST_edgesWeight, vector<Triple>&eweights, int n_v){


    MST_edges.clear();
    MST_v2v.clear();
    MST_edgesWeight.clear();
    MST_v2v.resize(n_v);

    sort(eweights.begin(),eweights.end(),greater<Triple>());

    int n_neg = 0;
    vector<int>ele(n_v);
    for(int i=0;i<n_v;++i)ele[i]=i;
    UnionFind unifind;
    unifind.SetElements(ele);
    double lastval;

    //for(auto &a:eweights)cout<<a.val<<' ';cout<<endl;
    for(auto &a:eweights){
        //cout<<a.val<<' ';
        if(!unifind.Is_Connected(a.i,a.j)){
            lastval=a.val;
            unifind.Union(a.i,a.j);
            MST_edges.push_back(a.i);MST_edges.push_back(a.j);
            MST_v2v[a.i].push_back(a.j);MST_v2v[a.j].push_back(a.i);
            MST_edgesWeight.push_back(a.val);
            ++n_neg;
        }
    }
    //cout<<"lastval: "<<lastval<<endl;
    //cout<<"n_neg: "<<n_neg<<endl;
    assert(unifind.GetNumOfComponents()==1);


}
void MST_Orientate(vector<bool>&isflip,vector<vector<double>>&edgelength, vector<vector<int>>&MST_v2v, int npt, int startpnt){

    int nflip = 0;
    vector<bool>isvisited(npt,false);
    isflip.clear();
    isflip.resize(npt,false);
    queue<pair<int,int>>q;
    for(auto a:MST_v2v[startpnt])q.emplace(a,startpnt);
    isvisited[startpnt]=true;
    while(!q.empty()){
        pair<int,int> aa = q.front();
        int curpnt = aa.first;
        q.pop();
        if(isvisited[curpnt])continue;
        isvisited[curpnt] = true;
        if((edgelength[curpnt][aa.second]<0 && isflip[aa.second]) || (edgelength[curpnt][aa.second]>0 && !isflip[aa.second])){
            isflip[curpnt] = true;++nflip;
        }
        for(auto a:MST_v2v[curpnt])q.emplace(a,curpnt);
    }

    //cout<<"nflip: "<<nflip<<endl;


}


void RBF_Core::NormalReOrientation(vector<double>&nors, bool isnormalized){


    cout<<"NormalReOrientation"<<endl;
    vector<double>nors_strength(npt);
    //arma::mat edgelength(npt,npt);
    vector<vector<double>>edgelength(npt,vector<double>(npt));

    vector<arma::vec>arma_normals(npt,arma::vec(3));
    auto p_nor = nors.data();


    for(int p1=0; p1<npt;++p1){
        for(int j=0;j<3;++j)arma_normals[p1](j) = p_nor[p1*3+j];
        nors_strength[p1] = arma::norm(arma_normals[p1] );
        arma_normals[p1] = arma::normalise( arma_normals[p1] );
    }

    vector<Triple>eabslength;
    int n_neg = 0;
    for(int p1=0; p1<npt;++p1)for(int p2=p1+1; p2<npt;++p2){
        arma::mat aa(3,3);
        for(int j=0;j<3;++j)for(int k=0;k<3;++k)aa(j,k) = K(p1+j*npt,p2+k*npt);
        edgelength[p1][p2] = edgelength[p2][p1]  = arma::dot(arma_normals[p1], aa*arma_normals[p2]);
        if(edgelength[p1][p2]<0)n_neg++;
        if(  fabs(edgelength[p1][p2]) > 1e-4 )eabslength.emplace_back(fabs(edgelength[p1][p2]),p1,p2);
    }

    vector<vector<int>>MST_v2v;
    BuildMST(MST_edges, MST_v2v,MST_edgesWeight, eabslength, npt);



    vector<bool>isflip(npt,false);
    int startpnt = max_element(nors_strength.begin(),nors_strength.end())-nors_strength.begin();
    MST_Orientate(isflip,edgelength,MST_v2v,npt,startpnt);

    for(int p1=0; p1<npt;++p1)if(isflip[p1]){
        arma_normals[p1] = -arma_normals[p1];
    }

    for(int p1=0; p1<npt;++p1){
        for(int j=0;j<3;++j)p_nor[p1*3+j] = arma_normals[p1](j);
    }
    if(!isnormalized){
        for(int p1=0; p1<npt;++p1){
            for(int j=0;j<3;++j)p_nor[p1*3+j] = nors_strength[p1] * p_nor[p1*3+j];
        }
    }

}

void RBF_Core::NormalReOrientation_WithGT(vector<double> &nors, bool isnormalized){

    for(int i=0;i<npt;++i){
        auto p_gt = normals.data()+i*3;
        auto p_nor = nors.data()+i*3;

        if(MyUtility::dot(p_gt,p_nor)<0)for(int j=0;j<3;++j)p_nor[j] = -p_nor[j];
    }

    if(isnormalized)for(int i=0;i<npt;++i){
        MyUtility::normalize(nors.data()+i*3);
    }

}

void RBF_Core::BuildCoherentGraph(){

    cout<<"BuildCoherentGraph"<<endl;
    vector<Triple>eabslength;
    Coherence_M.set_size(npt,npt);


    for(int p1=0; p1<npt;++p1)for(int p2=p1; p2<npt;++p2){
        arma::mat aa(3,3);
        for(int j=0;j<3;++j)for(int k=0;k<3;++k)aa(j,k) = K(p1+j*npt,p2+k*npt);
        double ll  = Coherence_M(p1,p2) = Coherence_M(p2,p1) =  arma::norm(aa);
        if(p2!=p1)
            if(  fabs(ll) > 1e-4 )
                eabslength.emplace_back(fabs(ll),p1,p2);
    }

    sort(eabslength.begin(),eabslength.end(),greater<Triple>());

    Coherence_Graph_Weight.clear();
    Coherence_Graph.clear();
    for(auto &a:eabslength){
        Coherence_Graph_Weight.push_back(a.val);
        Coherence_Graph.push_back(a.i);
        Coherence_Graph.push_back(a.j);
    }
}


double stdvar(vector<double>&v){

    int nv = v.size();
    double ave = 0;
    for(auto a:v)ave+=a;
    ave/=nv;

    double re = 0;
    for(auto a:v)re+= pow(ave-a,2);
    return sqrt(re/nv);


}


void RBF_Core::LocalEigenInit_PredictNormal(){


    cout<<"LocalEigenInit_PredictNormal"<<endl;
    arma::mat eigenmatrix;
    arma::vec eigenval;

    initnormals.resize(npt*3);

    vector<double>eva(3);
    for(int i=0;i<npt;++i){
        arma::mat aa(3,3);
        for(int j=0;j<3;++j)for(int k=0;k<3;++k)aa(j,k) = K(i+j*npt,i+k*npt);
        eig_sym( eigenval, eigenmatrix, aa);
        for(int j=0;j<3;++j)eva[j] = eigenval(j);
        //double stdv = stdvar(eva);
        double stdv = eva[1] - eva[0];
        //cout<<stdv<<' ';
        for(int j=0;j<3;++j)initnormals[i*3+j] = stdv*eigenmatrix(j,0);
    }

    NormalReOrientation(initnormals,false);
    SetInitnormal_Uninorm();

}

void RBF_Core::SetInitnormal_Uninorm(){

    initnormals_uninorm = initnormals;
    for(int i=0;i<npt;++i)MyUtility::normalize(initnormals_uninorm.data()+i*3);

}

int RBF_Core::Solve_Hermite_PredictNormal_UnitNorm(){

    arma::vec eigval, ny;
    arma::mat eigvec;

    if(!isuse_sparse){
        ny = eig_sym( eigval, eigvec, K);
    }else{
		cout<<"use sparse eigen"<<endl;
        int k = 4;
        do{
            ny = eigs_sym( eigval, eigvec, sp_K, k, "sa" );
            k+=4;
        }while(ny(0)==0);
    }




    cout<<"eigval(0): "<<eigval(0)<<endl;

    int smalleig = 0;
    //    for(int i=0;i<eigval.size();++i)if(eigval(i)>0){
    //        smalleig = i;
    //        break;
    //    }
    //    if(smalleig!=0)if( fabs(eigval(smalleig-1)) < fabs(eigval(smalleig)) )smalleig-=1;

    //smalleig = 9;

    initnormals.resize(npt*3);
    arma::vec y(npt*4);
    for(int i=0;i<npt;++i)y(i) = 0;
    for(int i=0;i<npt*3;++i)y(i+npt) = eigvec(i,smalleig);
    for(int i=0;i<npt;++i){
        initnormals[i*3]   = y(npt+i);
        initnormals[i*3+1] = y(npt+i+npt);
        initnormals[i*3+2] = y(npt+i+npt*2);
        //MyUtility::normalize(normals.data()+i*3);
    }


    SetInitnormal_Uninorm();

    //b = bprey * y;
    //a = Minv * (y - N*b);

    //sol.energy = arma::dot(a,M*a);

    cout<<"Solve_Hermite_PredictNormal_UnitNorm finish"<<endl;
    return 1;
}


void WritePointPLY(string fname, vector<double>&out_Vs, vector<double>&normals, vector<int>&labels);
void RBF_Core::WriteSeletivePLY(string fname, vector<double>&allnormals, vector<int>&pickInd){
    vector<double>npts;
    vector<double>nnor;
    vector<int>labels(pickInd.size(),0);
    for(auto ind:pickInd){
        for(int j=0;j<3;++j)npts.push_back(pts[ind*3+j]);
        for(int j=0;j<3;++j)nnor.push_back(allnormals[ind*3+j]);
    }
    WritePointPLY(fname,npts,nnor,labels);
}

void RBF_Core::ClearSavedIterBatchInit(){

    InitBatch_Vs.clear();
    InitBatch_VNs.clear();
    InitBatch_VUNs.clear();
    InitBatch_VInds.clear();
    InitBatch_Coherence_Graph.clear();
    InitBatch_Coherence_Graph_Weight.clear();
}


void RBF_Core::SaveIterBatchInit(vector<double>&allnormals, vector<double>&allnormals_uninorm, vector<int>&pickInd){
    int nnInd = InitBatch_Vs.size();
    InitBatch_Vs.resize(nnInd+1);
    InitBatch_VNs.resize(nnInd+1);
    InitBatch_VUNs.resize(nnInd+1);
    InitBatch_Coherence_Graph.resize(nnInd+1);
    InitBatch_Coherence_Graph_Weight.resize(nnInd+1);

    InitBatch_VInds.emplace_back(pickInd);
    vector<double>&npts = InitBatch_Vs[nnInd];
    vector<double>&nor = InitBatch_VNs[nnInd];
    vector<double>&nnor = InitBatch_VUNs[nnInd];
    vector<uint>&gg = InitBatch_Coherence_Graph[nnInd];
    vector<double>&ggw = InitBatch_Coherence_Graph_Weight[nnInd];

    vector<bool>ispick(npt,false);
    for(auto ind:pickInd){
        for(int j=0;j<3;++j)npts.push_back(pts[ind*3+j]);
        for(int j=0;j<3;++j)nor.push_back(allnormals[ind*3+j]);
        for(int j=0;j<3;++j)nnor.push_back(allnormals_uninorm[ind*3+j]);
        ispick[ind]=true;
    }

    vector<int>sortInd(pickInd);
    sort(sortInd.begin(),sortInd.end());
    vector<int>invertInd(npt,-1);
    for(int p1=0; p1<pickInd.size();++p1)invertInd[pickInd[p1]] = p1;
    vector<Triple>eeT;
    for(int p1=0; p1<sortInd.size();++p1)for(int p2=p1+1; p2<sortInd.size();++p2){
        int i=sortInd[p1],j=sortInd[p2];
        eeT.emplace_back(Coherence_M(i,j),invertInd[i],invertInd[j]);
    }

    sort(eeT.begin(),eeT.end(),greater<Triple>());

    gg.clear();ggw.clear();
    gg.reserve(eeT.size()*2);ggw.reserve(eeT.size());
    for(auto &a:eeT){
        ggw.push_back(a.val);
        gg.push_back(a.i);
        gg.push_back(a.j);
    }

}

int RBF_Core::Solve_Hermite_PredictNormal_UnitNorm_Iterative(){

    vector<int>labels(npt,0);
    double thres_coef = 0.5;

    arma::vec eigval, ny;
    arma::mat eigvec;

    int iter = 0;
    int maxiter = 20;
    vector<int>activeInd(npt);
    vector<int>fixedInd;
    for(int i=0;i<npt;++i)activeInd[i] = i;

    arma::mat subactiveMat;
    arma::vec fixVec,addVec;

    initnormals.clear();
    initnormals.resize(npt*3,0);
    vector<double>norm_normals(npt*3,0);
    ClearSavedIterBatchInit();
    while(true){

        ++iter;
        int n_Active = activeInd.size();
        int n_Fixed = fixedInd.size();
        bool prebreak = (iter>=maxiter) /*|| (n_Active < npt * 0.05)*/;

        arma::uvec row_ind(n_Active*3);
        for(int i=0;i<n_Active;++i){
            for(int j=0;j<3;++j)row_ind(i*3+j) = activeInd[i]+j*npt;
        }
        //cout<<row_ind.t()<<endl;

        subactiveMat = K.submat(row_ind,row_ind);
        //cout<<K.n_rows<<" "<<K.n_cols<<endl;

        ny = eig_sym( eigval, eigvec, subactiveMat);
        arma::vec firstEigvec(eigvec.col(0));
        //cout<<firstEigvec.n_elem<<endl;

        double thres = sqrt(1./npt) * thres_coef ;
        vector<int>newactive,addfixInd;
        //cout<<"thres: "<<thres<<endl;
        for(int i=0;i<n_Active;++i){
            auto p_nor = initnormals.data()+activeInd[i]*3;
            auto p_nor_n = norm_normals.data()+activeInd[i]*3;
            for(int j=0;j<3;++j)p_nor_n[j] = p_nor[j] = firstEigvec(i*3+j);
            //cout<<i<<endl;
            if(!prebreak && MyUtility::normVec(p_nor)<thres){
                newactive.push_back(activeInd[i]);
                for(int j=0;j<3;++j)p_nor_n[j] = 0;
            }else{
                //cout<<MyUtility::normVec(p_nor)<<' ';
                addfixInd.push_back(activeInd[i]);
                fixedInd.push_back(activeInd[i]);
                MyUtility::normalize(p_nor_n);
            }
        }



        cout<<iter<<" addfix: "<<addfixInd.size()<<' '<<fixedInd.size()<<endl;
        if(n_Fixed>0){
            int n_Addfix = fixedInd.size()-n_Fixed;
            arma::uvec addfix_row_ind(n_Addfix*3);
            addVec.set_size(n_Addfix*3);
            for(int i=0;i<n_Addfix;++i){
                for(int j=0;j<3;++j)addVec(i*3+j) = norm_normals[addfixInd[i]*3+j];
                for(int j=0;j<3;++j)addfix_row_ind(i*3+j) = addfixInd[i]+j*npt;
            }

            arma::uvec fix_row_ind(n_Fixed*3);
            fixVec.set_size(n_Fixed*3);
            for(int i=0;i<n_Fixed;++i){
                for(int j=0;j<3;++j)fix_row_ind(i*3+j) = fixedInd[i]+j*npt;
                for(int j=0;j<3;++j)fixVec(i*3+j) = norm_normals[fixedInd[i]*3+j];
            }


            arma::mat subInteractiveMat = K.submat(addfix_row_ind,fix_row_ind);
            double inter_en = arma::dot(addVec,subInteractiveMat*fixVec);

            cout<<"inter_en: "<<inter_en<<endl;
            if(inter_en>0){
                cout<<"flip"<<endl;
                for(int i=0;i<n_Addfix;++i){
                    int ind = addfixInd[i]*3;
                    for(int j=0;j<3;++j)initnormals[ind+j] = -initnormals[ind+j];
                    for(int j=0;j<3;++j)norm_normals[ind+j] = -norm_normals[ind+j];
                }
            }
        }
        //        WritePointPLY(string("/Users/Research/Geometry/RBF/data/iter/nn_")+to_string(iter),pts,norm_normals,labels);
        //        WritePointPLY(string("/Users/Research/Geometry/RBF/data/iter/un_")+to_string(iter),pts,newnormals,labels);
        WriteSeletivePLY(string("/Users/Research/Geometry/RBF/data/iter/nn_")+to_string(iter),norm_normals,activeInd);
        WriteSeletivePLY(string("/Users/Research/Geometry/RBF/data/iter/un_")+to_string(iter),initnormals,activeInd);

        SaveIterBatchInit(initnormals,norm_normals,activeInd);
        activeInd = newactive;
        if(activeInd.size()==0)break;
        if(prebreak)break;

    }

    initnormals_uninorm = initnormals = norm_normals;

    cout<<"Solve_Hermite_PredictNormal_UnitNorm_Iterative"<<endl;
    return 1;



}

int ExhaustedCombination(vector<bool>&isflip, vector<vector<double>>&en_mat, int n_v){
    isflip.clear();
    isflip.resize(n_v,false);
    if(n_v<2)return 1;
    cout<<"ExhaustedCombination"<<endl;

    arma::mat arma_enmat(n_v,n_v);
    for(int i=0;i<n_v;++i)for(int j=0;j<n_v;++j)arma_enmat(i,j) = en_mat[i][j];
    for(int i=0;i<n_v;++i)arma_enmat(i,i) = 0;

    double min_en = DBL_MAX;
    int min_ind = -1;
    arma::vec fvec;
    fvec.ones(n_v);
    int npow = pow(2,n_v-1);
    for(int i=0;i<npow;++i){
        //cout<<i<<' ';
        int maker = 1;
        for(int j=0;j<n_v-1 && maker<=i+1;++j){
            cout<<j<<' ';
            fvec(j+1) = i & maker ? -1:1;
            maker<<=1;
        }
        //cout<<endl;
        //if(i==63)cout<<fvec.t()<<endl;
        double cur_en = arma::dot(fvec,arma_enmat*fvec);
        if(cur_en<min_en){
            min_ind = i;min_en = cur_en;
        }
    }

    int maker = 1;
    for(int j=0;maker<=min_ind+1;++j){
        isflip[j+1] = min_ind & maker ? true : false;
        maker<<=1;
    }
	return 1;
}


int RBF_Core::Solve_Hermite_PredictNormal_UnitNorm_Iterative_MST(){

    vector<int>labels(npt,0);
    double thres_coef = 0.9;

    arma::vec eigval, ny;
    arma::mat eigvec;

    int iter = 0;
    int maxiter = 100;
    vector<int>activeInd(npt);
    vector<int>fixedInd;
    for(int i=0;i<npt;++i)activeInd[i] = i;

    arma::mat subactiveMat;
    arma::vec fixVec,addVec;

    initnormals.clear();
    initnormals.resize(npt*3,0);
    vector<double>norm_normals(npt*3,0);
    vector<vector<int>>hyper_patch;

    ClearSavedIterBatchInit();
    while(true){

        ++iter;
        int n_Active = activeInd.size();
        int n_Fixed = fixedInd.size();
        bool prebreak = (iter>=maxiter) /*|| (n_Active < npt * 0.05)*/;

        arma::uvec row_ind(n_Active*3);
        for(int i=0;i<n_Active;++i){
            for(int j=0;j<3;++j)row_ind(i*3+j) = activeInd[i]+j*npt;
        }
        //cout<<row_ind.t()<<endl;

        subactiveMat = K.submat(row_ind,row_ind);
        //cout<<K.n_rows<<" "<<K.n_cols<<endl;

        ny = eig_sym( eigval, eigvec, subactiveMat);
        arma::vec firstEigvec(eigvec.col(0));
        //cout<<firstEigvec.n_elem<<endl;

        double thres = sqrt(1./npt) * thres_coef ;
        //double thres = sqrt(1./n_Active) * thres_coef ;
        hyper_patch.resize(hyper_patch.size()+1);
        vector<int>newactive;
        vector<int>&addfixInd = hyper_patch[hyper_patch.size()-1];
        //cout<<"thres: "<<thres<<endl;
        for(int i=0;i<n_Active;++i){
            auto p_nor = initnormals.data()+activeInd[i]*3;
            auto p_nor_n = norm_normals.data()+activeInd[i]*3;
            for(int j=0;j<3;++j)p_nor_n[j] = p_nor[j] = firstEigvec(i*3+j);
            //cout<<i<<endl;
            if(!prebreak && MyUtility::normVec(p_nor)<thres){
                newactive.push_back(activeInd[i]);
                for(int j=0;j<3;++j)p_nor_n[j]  = 0;
            }else{
                //cout<<MyUtility::normVec(p_nor)<<' ';
                addfixInd.push_back(activeInd[i]);
                fixedInd.push_back(activeInd[i]);
                MyUtility::normalize(p_nor_n);
            }
        }

        cout<<iter<<" addfix: "<<addfixInd.size()<<' '<<fixedInd.size()<<endl;
        //        WritePointPLY(string("/Users/Research/Geometry/RBF/data/iter/nn_")+to_string(iter),pts,norm_normals,labels);
        //        WritePointPLY(string("/Users/Research/Geometry/RBF/data/iter/un_")+to_string(iter),pts,newnormals,labels);
        //WriteSeletivePLY(string("/Users/Research/Geometry/RBF/data/iter/nn_")+to_string(iter),norm_normals,activeInd);
        //WriteSeletivePLY(string("/Users/Research/Geometry/RBF/data/iter/un_")+to_string(iter),initnormals,activeInd);
        SaveIterBatchInit(initnormals,norm_normals,activeInd);
        activeInd = newactive;
        if(activeInd.size()==0)break;
        if(prebreak)break;
    }


    vector<uint>MST_edges;
    vector<vector<int>>MST_v2v;
    vector<Triple>eweights;
    int n_hyper = hyper_patch.size();

    vector<arma::vec>hyperb_vec(hyper_patch.size());
    vector<arma::uvec>hyperb_rowind(hyper_patch.size());
    for(int i=0;i<hyper_patch.size();++i){
        int b_nv = hyper_patch[i].size();
        auto &hyperVec = hyperb_vec[i];
        auto &row_ind = hyperb_rowind[i];
        auto &hyper_ind = hyper_patch[i];
        hyperVec.set_size(b_nv*3);
        row_ind.set_size(b_nv*3);

        for(int k=0;k<b_nv;++k){
            for(int j=0;j<3;++j)hyperVec(k*3+j) = norm_normals[hyper_ind[k]*3+j];
            for(int j=0;j<3;++j)row_ind(k*3+j) = hyper_ind[k]+j*npt;
        }
    }
    cout<<"adsada"<<endl;

    vector<vector<double>>interactiveEn_mat(n_hyper,vector<double>(n_hyper));
    for(int i=0;i<hyper_patch.size();++i){
        for(int j=i+1;j<hyper_patch.size();++j){
            arma::mat subInteractiveMat = K.submat(hyperb_rowind[i],hyperb_rowind[j]);
            //cout<<"aaa"<<endl;
            interactiveEn_mat[j][i] = interactiveEn_mat[i][j] = arma::dot(hyperb_vec[i],subInteractiveMat*hyperb_vec[j]);
            //cout<<i<<" "<<j<<" "<<interactiveEn_mat[i][j]<<endl;
            eweights.emplace_back(fabs(interactiveEn_mat[i][j]),i,j);
        }
    }


    vector<bool>isflip(n_hyper,false);

    if(1){
        cout<<"BuildMST"<<endl;
        BuildMST(MST_edges, MST_v2v,MST_edgesWeight, eweights, n_hyper);
        MST_Orientate(isflip,interactiveEn_mat,MST_v2v,n_hyper,0);

    }else{
        ExhaustedCombination(isflip,interactiveEn_mat,n_hyper);
    }


    for(int i=0;i<n_hyper;++i)if(isflip[i]){
        int b_nv = hyper_patch[i].size();
        auto &hyper_ind = hyper_patch[i];
        for(int k=0;k<b_nv;++k){
            for(int j=0;j<3;++j)norm_normals[hyper_ind[k]*3+j] = -norm_normals[hyper_ind[k]*3+j];
        }
    }


    initnormals_uninorm = initnormals = norm_normals;
    this->MST_edges.clear();
    this->MST_edgesWeight.clear();
    cout<<"Solve_Hermite_PredictNormal_UnitNorm_Iterative"<<endl;
    return 1;



}

/***************************************************************************************************/
/***************************************************************************************************/
double acc_time;

static int countopt = 0;
double optfunc_Hermite(const vector<double>&x, vector<double>&grad, void *fdata){

    auto t1 = Clock::now();
    RBF_Core *drbf = reinterpret_cast<RBF_Core*>(fdata);
    int n = drbf->npt;
    arma::vec arma_x(n*3);

    //(  sin(a)cos(b), sin(a)sin(b), cos(a)  )  a =>[0, pi], b => [-pi, pi];
    vector<double>sina_cosa_sinb_cosb(n * 4);
    for(int i=0;i<n;++i){
        int ind = i*4;
        sina_cosa_sinb_cosb[ind] = sin(x[i*2]);
        sina_cosa_sinb_cosb[ind+1] = cos(x[i*2]);
        sina_cosa_sinb_cosb[ind+2] = sin(x[i*2+1]);
        sina_cosa_sinb_cosb[ind+3] = cos(x[i*2+1]);
    }

    for(int i=0;i<n;++i){
        auto p_scsc = sina_cosa_sinb_cosb.data()+i*4;
        //        int ind = i*3;
        //        arma_x(ind) = p_scsc[0] * p_scsc[3];
        //        arma_x(ind+1) = p_scsc[0] * p_scsc[2];
        //        arma_x(ind+2) = p_scsc[1];
        arma_x(i) = p_scsc[0] * p_scsc[3];
        arma_x(i+n) = p_scsc[0] * p_scsc[2];
        arma_x(i+n*2) = p_scsc[1];
    }

    arma::vec a2;
    if(drbf->isuse_sparse)a2 = drbf->sp_H * arma_x;
    else a2 = drbf->finalH * arma_x;


    if (!grad.empty()) {

        grad.resize(n*2);

        for(int i=0;i<n;++i){
            auto p_scsc = sina_cosa_sinb_cosb.data()+i*4;

            //            int ind = i*3;
            //            grad[i*2] = a2(ind) * p_scsc[1] * p_scsc[3] + a2(ind+1) * p_scsc[1] * p_scsc[2] - a2(ind+2) * p_scsc[0];
            //            grad[i*2+1] = -a2(ind) * p_scsc[0] * p_scsc[2] + a2(ind+1) * p_scsc[0] * p_scsc[3];

            grad[i*2] = a2(i) * p_scsc[1] * p_scsc[3] + a2(i+n) * p_scsc[1] * p_scsc[2] - a2(i+n*2) * p_scsc[0];
            grad[i*2+1] = -a2(i) * p_scsc[0] * p_scsc[2] + a2(i+n) * p_scsc[0] * p_scsc[3];

        }
    }

    double re = arma::dot( arma_x, a2 );
    countopt++;

    acc_time+=(std::chrono::nanoseconds(Clock::now() - t1).count()/1e9);

    //cout<<countopt++<<' '<<re<<endl;
    return re;

}


int RBF_Core::Solve_Hermite_PredictNormal_UnitNormal(){

    cout<<"ads"<<endl;
    sol.solveval.resize(npt * 2);

    //Solve_Hermite_PredictNormal_UnitNorm();
    //NormalReOrientation();


    for(int kk=2;kk<3;++kk){
        auto t1 = Clock::now();
        if(kk==1){
            //Solve_Hermite_PredictNormal_UnitNorm();
            //Solve_Hermite_PredictNormal_UnitNorm_Iterative();
            Solve_Hermite_PredictNormal_UnitNorm_Iterative_MST();
            //NormalReOrientation();
            //initnormals = newnormals;
        }else if(kk==2){
            LocalEigenInit_PredictNormal();

            //for(int i=0;i<npt;++i)MyUtility::normalize(newnormals.data()+i*3);
        }else if(kk==0){
            initnormals = normals;
            cout<<"initnormals: "<<normals.size()<<endl;
        }
        //
        init_time = std::chrono::nanoseconds(Clock::now() - t1).count()/1e9;

        for(int i=0;i<npt;++i){
            double veccc[3];
            for(int j=0;j<3;++j)veccc[j] = initnormals[i*3+j];
            {
                //MyUtility::normalize(veccc);
                sol.solveval[i*2] = atan2(sqrt(veccc[0]*veccc[0]+veccc[1]*veccc[1]),veccc[2] );
                sol.solveval[i*2 + 1] = atan2( veccc[1], veccc[0]   );
            }

        }
        //cout<<"smallvec: "<<smallvec<<endl;

        if(1){
            vector<double>upper(npt*2);
            vector<double>lower(npt*2);
            for(int i=0;i<npt;++i){
                upper[i*2] = 2 * my_PI;
                upper[i*2 + 1] = 2 * my_PI;

                lower[i*2] = -2 * my_PI;
                lower[i*2 + 1] = -2 * my_PI;
            }


            countopt = 0;
            acc_time = 0;

            //LocalIterativeSolver(sol,kk==0?normals:newnormals,300,1e-7);


            Solver::nloptwrapper(lower,upper,optfunc_Hermite,this,1e-7,3000,sol);
            cout<<"number of call: "<<countopt<<" t: "<<acc_time<<" ave: "<<acc_time/countopt<<endl;
            callfunc_time = acc_time;
            solve_time = sol.time;
            //for(int i=0;i<npt;++i)cout<< sol.solveval[i]<<' ';cout<<endl;

        }
    }

    //exit(-1);

    newnormals.resize(npt*3);
    arma::vec y(npt*4);
    for(int i=0;i<npt;++i)y(i) = 0;
    for(int i=0;i<npt;++i){

        double a = sol.solveval[i*2], b = sol.solveval[i*2+1];
        //        int ind = i*3 + npt;
        //        y(ind) = sin(a) * cos(b);
        //        y(ind+1) = sin(a) * sin(b);
        //        y(ind+2) = cos(a);
        newnormals[i*3]   = y(npt+i) = sin(a) * cos(b);
        newnormals[i*3+1] = y(npt+i+npt) = sin(a) * sin(b);
        newnormals[i*3+2] = y(npt+i+npt*2) = cos(a);
        MyUtility::normalize(newnormals.data()+i*3);
    }




    Set_RBFCoef(y);

    //sol.energy = arma::dot(a,M*a);
    cout<<"Solve_Hermite_PredictNormal_UnitNormal"<<endl;
	
	return 1;
}





int RBF_Core::Opt_Hermite_PredictNormal_UnitNormal(){


    sol.solveval.resize(npt * 2);

    for(int i=0;i<npt;++i){
        double *veccc = initnormals.data()+i*3;
        {
            //MyUtility::normalize(veccc);
            sol.solveval[i*2] = atan2(sqrt(veccc[0]*veccc[0]+veccc[1]*veccc[1]),veccc[2] );
            sol.solveval[i*2 + 1] = atan2( veccc[1], veccc[0]   );
        }

    }
    //cout<<"smallvec: "<<smallvec<<endl;

    if(1){
        vector<double>upper(npt*2);
        vector<double>lower(npt*2);
        for(int i=0;i<npt;++i){
            upper[i*2] = 2 * my_PI;
            upper[i*2 + 1] = 2 * my_PI;

            lower[i*2] = -2 * my_PI;
            lower[i*2 + 1] = -2 * my_PI;
        }

        countopt = 0;
        acc_time = 0;

        //LocalIterativeSolver(sol,kk==0?normals:newnormals,300,1e-7);
        Solver::nloptwrapper(lower,upper,optfunc_Hermite,this,1e-7,3000,sol);
        cout<<"number of call: "<<countopt<<" t: "<<acc_time<<" ave: "<<acc_time/countopt<<endl;
        callfunc_time = acc_time;
        solve_time = sol.time;
        //for(int i=0;i<npt;++i)cout<< sol.solveval[i]<<' ';cout<<endl;

    }
    newnormals.resize(npt*3);
    arma::vec y(npt*4);
    for(int i=0;i<npt;++i)y(i) = 0;
    for(int i=0;i<npt;++i){

        double a = sol.solveval[i*2], b = sol.solveval[i*2+1];
        newnormals[i*3]   = y(npt+i) = sin(a) * cos(b);
        newnormals[i*3+1] = y(npt+i+npt) = sin(a) * sin(b);
        newnormals[i*3+2] = y(npt+i+npt*2) = cos(a);
        MyUtility::normalize(newnormals.data()+i*3);
    }

    Set_RBFCoef(y);

    //sol.energy = arma::dot(a,M*a);
    cout<<"Opt_Hermite_PredictNormal_UnitNormal"<<endl;
    return 1;
}

void RBF_Core::Set_RBFCoef(arma::vec &y){
    cout<<"Set_RBFCoef"<<endl;
    if(curMethod==HandCraft){
        cout<<"HandCraft, not RBF"<<endl;
        return;
    }
    if(!isnewformula){
        b = bprey * y;
        a = Minv * (y - N*b);
    }else{

        if(User_Lamnbda>0)y.subvec(0,npt-1) = -User_Lamnbda*dI*K01*y.subvec(npt,npt*4-1);

        a = Minv*y;
        b = Ninv.t()*y;

    }


}



int RBF_Core::Solve_Hermite_PredictNormal_UnitNormal_InitializationTest(){

    sol.solveval.resize(npt * 2);

    int global_eigenIndex  = 1;
    int local_eigenIndex  = 2;
    int gt_Index = 0;



    for(int kk=0;kk<2;++kk){

        if(kk==global_eigenIndex){
            Solve_Hermite_PredictNormal_UnitNorm();
            NormalReOrientation_WithGT(initnormals,false);
        }else if(kk==local_eigenIndex){
            LocalEigenInit_PredictNormal();
        }
        double veccc[3];
        for(int i=0;i<npt;++i){
            if(kk!=gt_Index)for(int j=0;j<3;++j)veccc[j] = newnormals[i*3+j];
            else for(int j=0;j<3;++j)veccc[j] = normals[i*3+j];

            {
                //MyUtility::normalize(veccc);
                sol.solveval[i*2] = atan2(sqrt(veccc[0]*veccc[0]+veccc[1]*veccc[1]),veccc[2] );
                sol.solveval[i*2 + 1] = atan2( veccc[1], veccc[0]   );
            }
        }

        if(1){
            vector<double>upper(npt*2);
            vector<double>lower(npt*2);
            for(int i=0;i<npt;++i){
                upper[i*2] = 2 * my_PI;
                upper[i*2 + 1] = 2 * my_PI;

                lower[i*2] = -2 * my_PI;
                lower[i*2 + 1] = -2 * my_PI;
            }


            countopt = 0;

            Solver::nloptwrapper(lower,upper,optfunc_Hermite,this,1e-7,3000,sol);
            cout<<"number of call: "<<countopt<<endl;
            //for(int i=0;i<npt;++i)cout<< sol.solveval[i]<<' ';cout<<endl;

        }

        if(kk==global_eigenIndex){
            eigenBe.push_back(sol.init_energy);
            eigenEd.push_back(sol.energy);
        }else if(kk==local_eigenIndex){
            local_eigenBe.push_back(sol.init_energy);
            local_eigenEd.push_back(sol.energy);
        }else if(kk==gt_Index){
            gtBe.push_back(sol.init_energy);
            gtEd.push_back(sol.energy);
        }

        continue;

    }
    //sol.energy = arma::dot(a,M*a);
    cout<<"Solve_Hermite_PredictNormal_UnitNormal"<<endl;
    return 1;
}


int RBF_Core::Solve_Hermite_PredictNormal_UnitNormal_SolverTest(){



    int global_solver  = 1;
    int local_solver  = 0;



    LocalEigenInit_PredictNormal();
    for(int kk=0;kk<2;++kk){

        if(kk==global_solver){
            sol.solveval.resize(npt * 2);
            double veccc[3];
            for(int i=0;i<npt;++i){
                for(int j=0;j<3;++j)veccc[j] = newnormals[i*3+j];
                {
                    //MyUtility::normalize(veccc);
                    sol.solveval[i*2] = atan2(sqrt(veccc[0]*veccc[0]+veccc[1]*veccc[1]),veccc[2] );
                    sol.solveval[i*2 + 1] = atan2( veccc[1], veccc[0]   );
                }
            }

            {
                vector<double>upper(npt*2);
                vector<double>lower(npt*2);
                for(int i=0;i<npt;++i){
                    upper[i*2] = 2 * my_PI;
                    upper[i*2 + 1] = 2 * my_PI;

                    lower[i*2] = -2 * my_PI;
                    lower[i*2 + 1] = -2 * my_PI;
                }

                countopt = 0;

                Solver::nloptwrapper(lower,upper,optfunc_Hermite,this,1e-7,3000,sol);
                cout<<"number of call: "<<countopt<<endl;
                //for(int i=0;i<npt;++i)cout<< sol.solveval[i]<<' ';cout<<endl;

            }
        }else if(kk == local_solver){
            LocalIterativeSolver(sol,newnormals,300,1e-7);
        }

        if(kk==local_solver){
            local_eigenBe.push_back(sol.init_energy);
            local_eigenEd.push_back(sol.energy);
        }else if(kk==global_solver){
            gtBe.push_back(sol.init_energy);
            gtEd.push_back(sol.energy);
        }

        continue;

    }
    //sol.energy = arma::dot(a,M*a);
    cout<<"Solve_Hermite_PredictNormal_UnitNormal"<<endl;
    return 1;
}


void RBF_Core::ClearInitializationTest(){

    lamnbdaGlobal_Be.clear();
    lamnbdaGlobal_Ed.clear();
    local_eigenBe.clear();
    local_eigenEd.clear();
    eigenEd.clear();
    eigenBe.clear();
    eigenEd.clear();
    gtBe.clear();
    gtEd.clear();
}

void RBF_Core::Print_InitializationTest(string fname){

    bool isglobalEigen = true;
    cout<<setprecision(5);
    cout<<"Eigen \t\t\t Gt"<<endl;
    if(isglobalEigen){ for(int i=0;i<gtBe.size();++i){
            cout<<eigenBe[i]<<" -> "<<eigenEd[i]<<"\t"<<gtBe[i]<<" -> "<<gtEd[i]<<endl;
        }
    }else{
        for(int i=0;i<gtBe.size();++i){
            cout<<local_eigenBe[i]<<" -> "<<local_eigenEd[i]<<"\t"<<gtBe[i]<<" -> "<<gtEd[i]<<endl;
        }
    }

    //    int nclose=0;
    //    for(int i=0;i<gtBe.size();++i){
    //        if(fabs(gtEd[i]-local_eigenEd[i]) < 10)nclose++;
    //    }
    //cout<<"nclose: "<<nclose<<endl;

    ofstream fout(fname);
    fout<<setprecision(5);
    if(!fout.fail()){
        if(isglobalEigen){  for(int i=0;i<eigenBe.size();++i){
                fout<<eigenBe[i]<<"\t"<<eigenEd[i]<<"\t"<<gtBe[i]<<"\t"<<gtEd[i]<<endl;
            }
        }else{
            for(int i=0;i<gtBe.size();++i){
                fout<<local_eigenBe[i]<<"\t"<<local_eigenEd[i]<<"\t"<<gtBe[i]<<"\t"<<gtEd[i]<<endl;
            }
        }
    }
    fout.close();

}



int RBF_Core::Solve_Hermite_PredictNormal_UnitNorm_Cluster_MST(double graph_cut_percentage, double localstv_maxcut_percentage, int visualmethod){


    double thres_coef = 0.9;

    arma::vec eigval, ny;
    arma::mat eigvec;

    int iter = 0;
    int maxiter = 100;

    vector<int>fixedInd;

    arma::mat subactiveMat;

    vector<vector<int>>hyper_patch;

    ClearSavedIterBatchInit();


    LocalEigenInit_PredictNormal();
    vector<double>norm_normals(initnormals);
    vector<double>nors_strength(npt);


    auto p_nor = initnormals.data();

    for(int p1=0; p1<npt;++p1){
        nors_strength[p1] = MyUtility::normVec(p_nor+p1*3);
    }

    double maxstrength = *max_element(nors_strength.begin(),nors_strength.end()) * localstv_maxcut_percentage;

    vector<int>headInd;

    vector<int>ccInd;
    vector<bool>headpick(npt,false);
    for(int p1=0; p1<npt;++p1){
        if(nors_strength[p1]>maxstrength){
            headInd.push_back(p1);
            headpick[p1] = true;
        }
        else ccInd.push_back(p1);
    }

    UnionFind uni_find_head;
    UnionFind uni_find_cc;
    uni_find_cc.SetElements(ccInd);
    uni_find_head.SetElements(headInd);

    double westrength = Coherence_Graph_Weight[0]*graph_cut_percentage;
    double n_reserve = 2 * npt * graph_cut_percentage;
    if(0){

        for(int i=0;i<Coherence_Graph_Weight.size();++i){
            if(Coherence_Graph_Weight[i]>westrength){
                int a = Coherence_Graph[i*2],b = Coherence_Graph[i*2+1];
                if(!headpick[a] && !headpick[b]){
                    uni_find_cc.Union(a,b);
                }
                if(headpick[a] && headpick[b]){
                    uni_find_head.Union(a,b);
                }
            }else break;
        }
    }else{

        for(int i=0;i<n_reserve;++i){
            int a = Coherence_Graph[i*2],b = Coherence_Graph[i*2+1];
            if(!headpick[a] && !headpick[b]){
                uni_find_cc.Union(a,b);
            }
            if(headpick[a] && headpick[b]){
                uni_find_head.Union(a,b);
            }
        }
    }


    vector<vector<int>>ccHyper,headHyper;
    uni_find_head.ExtractComponents(headHyper);
    uni_find_cc.ExtractComponents(ccHyper);
    cout<<"Hyper: "<<headHyper.size()<<' '<<ccHyper.size()<<endl;

    for(auto &a:headHyper)hyper_patch.emplace_back(a);
    for(auto &a:ccHyper)hyper_patch.emplace_back(a);


    for(auto &activeInd:headHyper){
        int n_Active = activeInd.size();
        for(int i=0;i<n_Active;++i)MyUtility::normalize(norm_normals.data()+activeInd[i]*3);
        //SaveIterBatchInit(initnormals,norm_normals,activeInd);
    }
    SaveIterBatchInit(initnormals,norm_normals,headInd);
    //    for(auto &activeInd:headHyper){
    //        SaveIterBatchInit(initnormals,norm_normals,activeInd);
    //    }
    cout<<"n_Active: ";
    for(auto &activeInd:headHyper){
        int n_Active = activeInd.size();

        vector<uint>MST_edges;
        vector<vector<int>>MST_v2v;
        vector<double>MST_edgesWeight;
        vector<int>ind(npt,-10000);
        for(int i=0;i<n_Active;++i)ind[activeInd[i]] = i;
        vector<vector<double>>edgelength(npt,vector<double>(npt));
        vector<arma::vec>arma_normals(n_Active,arma::vec(3));
        auto p_nor = norm_normals.data();
        for(int p1=0; p1<n_Active;++p1){
            for(int j=0;j<3;++j)arma_normals[p1](j) = p_nor[activeInd[p1]*3+j];
            arma_normals[p1] = arma::normalise( arma_normals[p1] );
        }

        vector<Triple>eabslength;
        int n_neg = 0;
        for(int m=0; m<n_Active;++m)for(int n=m+1; n<n_Active;++n){
            int p1 = activeInd[m], p2 = activeInd[n];
            arma::mat aa(3,3);
            for(int j=0;j<3;++j)for(int k=0;k<3;++k)aa(j,k) = K(p1+j*npt,p2+k*npt);
            edgelength[m][n] = edgelength[m][n]  = arma::dot(arma_normals[m], aa*arma_normals[n]);
            /*if(  fabs(edgelength[m][n]) > 1e-4 )*/eabslength.emplace_back(fabs(edgelength[m][n]),m,n);
        }
        vector<bool>isflip(n_Active);
        BuildMST(MST_edges, MST_v2v,MST_edgesWeight, eabslength, n_Active);
        MST_Orientate(isflip,edgelength,MST_v2v,n_Active,0);
        for(int i=0;i<n_Active;++i)if(isflip[i]){
            for(int j=0;j<3;++j)initnormals[activeInd[i]*3+j] = -initnormals[activeInd[i]*3+j];
            for(int j=0;j<3;++j)norm_normals[activeInd[i]*3+j] = -norm_normals[activeInd[i]*3+j];
        }



        if(0){
            arma::uvec row_ind(n_Active*3);
            for(int i=0;i<n_Active;++i){
                for(int j=0;j<3;++j)row_ind(i*3+j) = activeInd[i]+j*npt;
            }
            //cout<<row_ind.t()<<endl;
            subactiveMat = K.submat(row_ind,row_ind);
            //cout<<K.n_rows<<" "<<K.n_cols<<endl;
            ny = eig_sym( eigval, eigvec, subactiveMat);
            arma::vec firstEigvec(eigvec.col(0));
            for(int i=0;i<n_Active;++i){
                auto p_nor = initnormals.data()+activeInd[i]*3;
                auto p_nor_n = norm_normals.data()+activeInd[i]*3;
                for(int j=0;j<3;++j)p_nor_n[j] = p_nor[j] = firstEigvec(i*3+j);
                MyUtility::normalize(p_nor_n);
            }
        }
        cout<<n_Active<<' ';
        SaveIterBatchInit(initnormals,norm_normals,activeInd);
    }
    cout<<" || ";
    vector<double>saveInit(initnormals),save_norm(norm_normals);



    for(auto &ccInd:ccHyper){
        UnionFind uni_find;
        vector<int>ele;
        for(auto &b:headHyper)for(auto a:b)ele.push_back(a);
        for(auto b:ccInd)ele.push_back(b);
        uni_find.SetElements(ele);
        for(auto &b:headHyper)for(int i=1;i<b.size();++i)uni_find.Union(b[i],b[i-1]);
        for(int i=1;i<ccInd.size();++i)uni_find.Union(ccInd[i],ccInd[i-1]);
        vector<bool>ccpick(npt,false);
        for(auto b:ccInd)ccpick[b]=true;
        for(int i=0;i<n_reserve;++i){
            int a = Coherence_Graph[i*2],b = Coherence_Graph[i*2+1];
            if((headpick[a] && ccpick[b])|| (headpick[b] && ccpick[a])){
                uni_find.Union(a,b);
            }
        }
        vector<int>activeInd(ccInd);
        for(auto &b:headHyper)if(uni_find.Is_Connected(b[0],ccInd[0]))for(auto a:b)activeInd.push_back(a);

        int n_Active = activeInd.size();
        arma::uvec row_ind(n_Active*3);
        for(int i=0;i<n_Active;++i){
            for(int j=0;j<3;++j)row_ind(i*3+j) = activeInd[i]+j*npt;
        }
        //cout<<row_ind.t()<<endl;
        subactiveMat = K.submat(row_ind,row_ind);
        //cout<<K.n_rows<<" "<<K.n_cols<<endl;
        ny = eig_sym( eigval, eigvec, subactiveMat);
        arma::vec firstEigvec(eigvec.col(0));
        vector<int>*drawInd;
        if(visualmethod==0)drawInd = &activeInd;//activeInd ccInd
        else if(visualmethod==1)drawInd = &ccInd;
        for(int i=0;i<drawInd->size();++i){
            auto p_nor = initnormals.data()+activeInd[i]*3;
            auto p_nor_n = norm_normals.data()+activeInd[i]*3;
            for(int j=0;j<3;++j)p_nor_n[j] = p_nor[j] = firstEigvec(i*3+j);
            MyUtility::normalize(p_nor_n);
        }
        cout<<n_Active<<' ';
        SaveIterBatchInit(initnormals,norm_normals,*drawInd);
    }
    cout<<endl;
    for(auto &activeInd:headHyper){
        int n_Active = activeInd.size();
        for(int i=0;i<n_Active;++i){
            int offset = activeInd[i]*3;
            MyUtility::copyVec(saveInit.data()+offset, initnormals.data()+offset);
            MyUtility::copyVec(save_norm.data()+offset, norm_normals.data()+offset);
        }
    }



    vector<Triple>eweights;
    int n_hyper = hyper_patch.size();

    vector<arma::vec>hyperb_vec(hyper_patch.size());
    vector<arma::uvec>hyperb_rowind(hyper_patch.size());
    for(int i=0;i<hyper_patch.size();++i){
        int b_nv = hyper_patch[i].size();
        auto &hyperVec = hyperb_vec[i];
        auto &row_ind = hyperb_rowind[i];
        auto &hyper_ind = hyper_patch[i];
        hyperVec.set_size(b_nv*3);
        row_ind.set_size(b_nv*3);

        for(int k=0;k<b_nv;++k){
            for(int j=0;j<3;++j)hyperVec(k*3+j) = norm_normals[hyper_ind[k]*3+j];
            for(int j=0;j<3;++j)row_ind(k*3+j) = hyper_ind[k]+j*npt;
        }
    }
    cout<<"adsada"<<endl;

    vector<vector<double>>interactiveEn_mat(n_hyper,vector<double>(n_hyper));
    for(int i=0;i<hyper_patch.size();++i){
        for(int j=i+1;j<hyper_patch.size();++j){
            arma::mat subInteractiveMat = K.submat(hyperb_rowind[i],hyperb_rowind[j]);
            //cout<<"aaa"<<endl;
            interactiveEn_mat[j][i] = interactiveEn_mat[i][j] = arma::dot(hyperb_vec[i],subInteractiveMat*hyperb_vec[j]);
            //cout<<i<<" "<<j<<" "<<interactiveEn_mat[i][j]<<endl;
            eweights.emplace_back(fabs(interactiveEn_mat[i][j]),i,j);
        }
    }


    vector<bool>isflip(n_hyper,false);

    if(1){
        cout<<"BuildMST"<<endl;
        vector<uint>MST_edges;
        vector<vector<int>>MST_v2v;
        vector<double>MST_edgesWeight;
        BuildMST(MST_edges, MST_v2v,MST_edgesWeight, eweights, n_hyper);
        MST_Orientate(isflip,interactiveEn_mat,MST_v2v,n_hyper,0);

    }else{
        ExhaustedCombination(isflip,interactiveEn_mat,n_hyper);
    }


    for(int i=0;i<n_hyper;++i)if(isflip[i]){
        int b_nv = hyper_patch[i].size();
        auto &hyper_ind = hyper_patch[i];
        for(int k=0;k<b_nv;++k){
            for(int j=0;j<3;++j)norm_normals[hyper_ind[k]*3+j] = -norm_normals[hyper_ind[k]*3+j];
        }
    }


    initnormals_uninorm = initnormals = norm_normals;
    this->MST_edges.clear();
    this->MST_edgesWeight.clear();
    cout<<"Solve_Hermite_PredictNormal_UnitNorm_Cluster_MST"<<endl;
    return 1;


}






int RBF_Core::Lamnbda_Search_GlobalEigen(){

    vector<double>lamnbda_list({0, 0.001, 0.01, 0.1, 1});
    //vector<double>lamnbda_list({  0.5,0.6,0.7,0.8,0.9,1,1.1,1.5,2,3});
    //lamnbda_list.clear();
    //for(double i=1.5;i<2.5;i+=0.1)lamnbda_list.push_back(i);
    //vector<double>lamnbda_list({0});
    vector<double>initen_list(lamnbda_list.size());
    vector<double>finalen_list(lamnbda_list.size());
    vector<vector<double>>init_normallist;
    vector<vector<double>>opt_normallist;

    lamnbda_list_sa = lamnbda_list;
    for(int i=0;i<lamnbda_list.size();++i){

        Set_HermiteApprox_Lamnda(lamnbda_list[i]);

        if(curMethod==Hermite_UnitNormal){
            Solve_Hermite_PredictNormal_UnitNorm();
        }else if(curMethod==Hermite_Tangent_UnitNormal){
            Solve_Hermite_PredictNormal_TangentConstraint_UnitNorm();
        }else if(curMethod==HandCraft){
            Solve_Hermite_PredictNormal_UnitNorm();
        }

        //Solve_Hermite_PredictNormal_UnitNorm();
        OptNormal(1);

        initen_list[i] = sol.init_energy;
        finalen_list[i] = sol.energy;

        init_normallist.emplace_back(initnormals);
        opt_normallist.emplace_back(newnormals);
    }

    lamnbdaGlobal_Be.emplace_back(initen_list);
    lamnbdaGlobal_Ed.emplace_back(finalen_list);

    cout<<std::setprecision(8);
    for(int i=0;i<initen_list.size();++i){
        cout<<lamnbda_list[i]<<": "<<initen_list[i]<<" -> "<<finalen_list[i]<<endl;
    }

    int minind = min_element(finalen_list.begin(),finalen_list.end()) - finalen_list.begin();
    cout<<"min energy: "<<endl;
    cout<<lamnbda_list[minind]<<": "<<initen_list[minind]<<" -> "<<finalen_list[minind]<<endl;


    initnormals = init_normallist[minind];
    SetInitnormal_Uninorm();
    newnormals = opt_normallist[minind];
	return 1;
}


int RBF_Core::Solve_Hermite_PredictNormal_UnitNormal_LamnbdaSearchTest(){

    cout<<"Solve_Hermite_PredictNormal_UnitNormal_LamnbdaSearchTest"<<endl;
    curMethod = Hermite_UnitNormal;
    Set_Hermite_PredictNormal(pts);
    Lamnbda_Search_GlobalEigen();

    RBF_Paras para;
    para.InitMethod = GT_NORMAL;
    InitNormal(para);
    OptNormal(1);

    gtBe.push_back(sol.init_energy);
    gtEd.push_back(sol.energy);

	return 1;
}


void RBF_Core::Print_LamnbdaSearchTest(string fname){


    cout<<setprecision(7);
    cout<<"Print_LamnbdaSearchTest"<<endl;
    for(int i=0;i<lamnbda_list_sa.size();++i)cout<<lamnbda_list_sa[i]<<' ';cout<<endl;
    cout<<lamnbdaGlobal_Be.size()<<endl;
    for(int i=0;i<lamnbdaGlobal_Be.size();++i){
        for(int j=0;j<lamnbdaGlobal_Be[i].size();++j){
            cout<<lamnbdaGlobal_Be[i][j]<<"\t"<<lamnbdaGlobal_Ed[i][j]<<"\t";
        }
        cout<<gtBe[i]<<"\t"<<gtEd[i]<<endl;
    }

    ofstream fout(fname);
    fout<<setprecision(7);
    if(!fout.fail()){
        for(int i=0;i<lamnbda_list_sa.size();++i)fout<<lamnbda_list_sa[i]<<' ';fout<<endl;
        fout<<lamnbdaGlobal_Be.size()<<endl;
        for(int i=0;i<lamnbdaGlobal_Be.size();++i){
            for(int j=0;j<lamnbdaGlobal_Be[i].size();++j){
                fout<<lamnbdaGlobal_Be[i][j]<<"\t"<<lamnbdaGlobal_Ed[i][j]<<"\t";
            }
            fout<<gtBe[i]<<"\t"<<gtEd[i]<<endl;
        }
    }
    fout.close();

}