/*
 * To change this license header, choose License Headers in Project Properties.
 * To change this template file, choose Tools | Templates
 * and open the template in the editor.
 */

#include <assert.h>

#include <sstream>
#include <fstream>
#include <iomanip>
#include <algorithm>
#include <memory>
#include <tuple>
#include <vector>
#include <math.h>
#include <chrono>
#include <stdlib.h>
#include "matcher.h"
#include <Eigen/Dense>
//delete this probably
#include <ctime>
#include "boost/filesystem.hpp"
namespace fs = boost::filesystem;

using namespace std;
using namespace Eigen;

namespace PQ
{

Matcher::Matcher(string code_file)
{
    N = 200;
    description.push_back("minutiae similarity");

    description.push_back("obtaining corr");

    description.push_back("second order fast");

    description.push_back("second order original");

    dist_N = 50;
    table_dist.resize(dist_N*dist_N);
    max_nrof_templates = 0;
    int n = 0;
    for(int i=0;i<dist_N; ++i)
    {
        for(int j=i;j<dist_N;++j)
        {
            table_dist[i*dist_N+j] = sqrt((i*16.0)*(i*16.0) +  (j*16.0)*(j*16.0));
            table_dist[j*dist_N+i] = table_dist[i*dist_N+j];
        }
    }
    //    load code book
    ifstream is;
    is.open(code_file, ifstream::binary);
    // get length of file:
    is.seekg(0, ios::end);
    int length = is.tellg();

    if( length<=0 )
    {
        cout<<"codebook is empty!"<<endl;
    }
    is.seekg(0, ios::beg);

    nrof_subs = 0;
    nrof_clusters=0;
    sub_dim = 0;

    is.read(reinterpret_cast<char*>(&nrof_subs),sizeof(short));
    is.read(reinterpret_cast<char*>(&nrof_clusters),sizeof(short));
    is.read(reinterpret_cast<char*>(&sub_dim),sizeof(short));

    int len = nrof_subs*nrof_clusters*sub_dim;
    if(len<=0)
    {
        cout<<"codebook is empty!"<<endl;
    }

    codewords = std::vector<float>(len);
    float *pword = codewords.data();
    for(int i=0;i<nrof_subs; ++i)
    {
        for(int j=0; j<nrof_clusters; ++j)
        {
            is.read(reinterpret_cast<char*>(pword),sizeof(float)*sub_dim);
            pword += sub_dim;
        }
    }
}

int Matcher::List2List_matching(string latent_path, string rolled_path, string score_path)
{
    string template_file, score_file;

    int nrof_latents = 0;
    vector<fs::path> latent_template_files;
    fs::directory_iterator end_itr;
    for(fs::directory_iterator dir_itr(latent_path); dir_itr != end_itr; ++dir_itr)
    {
        if(dir_itr->path().extension() == ".dat")
        {
            latent_template_files.push_back(dir_itr->path());
            cout<<"latent template file"<<dir_itr->path()<<endl;
            ++nrof_latents;
        }
    }
    if(nrof_latents <= 0)
    {
        cout<<"No latent templates found in directory: "<<latent_path<<endl;
        return -1;
    }

    int i,j,k;

    int nrof_rolled = 0;
    vector<fs::path> rolled_template_files;
    for(fs::directory_iterator dir_itr(rolled_path); dir_itr != end_itr; ++dir_itr)
    {
    if(dir_itr->path().extension() == ".dat")
        {
            rolled_template_files.push_back(dir_itr->path());
            cout<<"rolled template file"<<dir_itr->path()<<endl;
            ++nrof_rolled;
        }
    }
    if(nrof_rolled <= 0)
    {
        cout<<"No rolled templates found in directory: "<<rolled_path<<endl;
        return -1;
    }
    cout<<"Gallery size: "<<nrof_rolled<<endl;
    {
        using namespace std::chrono;
        vector<high_resolution_clock::time_point> t(10);
        duration<double, std::milli> time_span;
        t[0] = high_resolution_clock::now();

        for(i=0;i<nrof_latents; ++i)
        {
            vector<float> scores(nrof_rolled, -1);
            cout<<latent_template_files[i]<<endl;

            //load latent original template and create a latent FP object
            LatentFPTemplate latent_FP{};
            try {
	            latent_FP = load_latent_template(latent_template_files[i].string());
	    } catch (const std::exception&) {
	    	cout<<"No minutiae or texture templates found"<<endl;
                ofstream output;
                output.open(score_path + latent_template_files[i].stem().string() + ".csv");

                output<<0<<endl;
                output.close();

                continue;
	    }
            cout<<"Latent minutiae templates: "<<latent_FP.m_minu_templates.size()<<endl;
            cout<<"Latent texture templates: "<<latent_FP.m_texture_templates.size()<<endl;
            if(latent_FP.m_minu_templates.size()<=0 && latent_FP.m_texture_templates.size()<=0)
            {
				cout<<"No minutiae or texture templates found"<<endl;
                ofstream output;
                output.open(score_path + latent_template_files[i].stem().string() + ".csv");

                output<<0<<endl;
                output.close();

                continue;
            }

            using namespace std::chrono;
            high_resolution_clock::time_point t_start = high_resolution_clock::now();
            int result = 0;
	        #pragma omp parallel for num_threads(8) schedule(static,16)
            for(j=0;j<nrof_rolled; ++j)
            {

                RolledFPTemplate rolled_FP;
                try {
                	rolled_FP = load_rolled_template(rolled_template_files[j].string());
                } catch (const std::exception&) {
                    rolled_FP.m_minu_templates.clear();
                    rolled_FP.m_texture_templates.clear();
                }

				vector<float> score;
				result = One2One_matching_selected_templates(latent_FP,rolled_FP,score);
                if(result == 1){
                    continue;
                }
                else if(result == 2){
                    cout<<"Comparison failed: rolled template is empty. Skipping."<<endl;
                    continue;
                }
				float final_score = score[0] + score[1] + score[2] + score[28]*0.3;
                scores[j] = final_score;
            }
            if(result == 1){
                cout<<"Matching failed: latent template is empty. Skipping."<<endl;
                continue;
            }
            auto t_end = high_resolution_clock::now();
            duration<double, std::milli> duration = (t_end - t_start);

			ofstream output;
			output.open(score_path + latent_template_files[i].stem().string() + ".csv");

			for(j=0;j<nrof_rolled; ++j)
			{
                output<<rolled_template_files[j]<<","<<std::setprecision(3)<<std::fixed<<scores[j]<<endl;
			}
			output.close();
        }
        t[1] = high_resolution_clock::now();
        time_span = t[1] - t[0];
        cout<<"Total matching duration (ms): "<<time_span.count()<<endl;

		auto timenow = chrono::system_clock::to_time_t(chrono::system_clock::now());
    }
    return 0;
}

int Matcher::One2List_matching(string latent_template_file_string, string rolled_list_file, string score_path)
{

    int i,j,k;

    fs::path latent_template_file(latent_template_file_string);
    string score_file = score_path + latent_template_file.stem().string() + ".csv";

    int nrof_rolled = 0;
    vector<fs::path> rolled_template_files;
    fs::directory_iterator end_itr;
    for(fs::directory_iterator dir_itr(rolled_list_file); dir_itr != end_itr; ++dir_itr)
    {
    if(dir_itr->path().extension() == ".dat")
        {
            rolled_template_files.push_back(dir_itr->path());
            ++nrof_rolled;
        }
    }
    if(nrof_rolled <= 0)
    {
        cout<<"No rolled templates found in directory: "<<rolled_list_file<<endl;
        return -1;
    }

    // Create a vector of indices
    // Allows us to output a sorted score list for use with the GUI
    vector<int> ind(rolled_template_files.size(), 0);
    for(int n = 0; n != rolled_template_files.size(); n++){
        ind[n] = n;
    }

    {
        using namespace std::chrono;
        vector<high_resolution_clock::time_point> t(10);
        duration<double, std::milli> time_span;
        t[0] = high_resolution_clock::now();

        vector<float> scores(nrof_rolled, -1);
        cout<<"Latent Query: "<<latent_template_file<<endl;
        cout<<"Gallery size: "<<nrof_rolled<<endl;

        LatentFPTemplate latent_FP;
        try {
        	latent_FP = load_latent_template(latent_template_file.string());
        } catch (const std::exception&) {
            ofstream output;
            output.open(score_file);

            output<<0<<endl;
            output.close();
        }
        if(latent_FP.m_minu_templates.size()<=0 && latent_FP.m_texture_templates.size()<=0)
        {
            ofstream output;
            output.open(score_file);

            output<<0<<endl;
            output.close();

        }

        using namespace std::chrono;
        high_resolution_clock::time_point t_start = high_resolution_clock::now();
        int result = 0;
        #pragma omp parallel for num_threads(8) schedule(static,16)
        for(j=0;j<nrof_rolled; ++j)
        {

            RolledFPTemplate rolled_FP;
            try {
            	rolled_FP = load_rolled_template(rolled_template_files[j].string());

            } catch (const std::exception&) {
                rolled_FP.m_minu_templates.clear();
                rolled_FP.m_texture_templates.clear();
            }

            vector<float> score;
            result = One2One_matching_selected_templates(latent_FP,rolled_FP,score);
            if(result == 1){
                continue;
            }
            else if(result == 2){
                cout<<"Comparison failed: rolled template is empty. Skipping."<<endl;
                continue;
            }
            float final_score = score[0] + score[1] + score[2] + score[28]*0.3;
            scores[j] = final_score;
        }
        if(result == 1){
            cout<<"Matching failed: latent template is empty. Exiting."<<endl;
            return 1;
        }
        auto t_end = high_resolution_clock::now();
        duration<double, std::milli> duration = (t_end - t_start);
        ofstream output;
        output.open(score_file);

        // Sort scores to create rank list
        sort(ind.begin(), ind.end(), [&](const int& a, const int& b){
                return (scores[a] > scores[b]);
            }
        );
        output<<"filename,score"<<endl;
        //generate correspondence files for top 24 only
        cout<<"Match Results"<<endl;
        cout<<"----------------"<<endl;
        cout<<"Rank     Filename      Score"<<endl;
        for(j=0; j<24; ++j)
        {
            if(j >= nrof_rolled){
                break;
            }
            output<<to_string(j+1)<<rolled_template_files[ind[j]]<<","<<scores[ind[j]]<<endl;
            RolledFPTemplate rolled_FP;
            try {
	            rolled_FP = load_rolled_template(rolled_template_files[ind[j]].string());
	    } catch (const std::exception&) {
		    rolled_FP.m_minu_templates.clear();
                    rolled_FP.m_texture_templates.clear();
	    }
            string latent_fname = latent_template_file.stem().string();
            string rolled_fname = rolled_template_files[ind[j]].stem().string();
            string corr_file = "/LatentAFIS/scores/corr" + latent_fname + "_" + rolled_fname;
            vector<float> score;
            One2One_matching_selected_templates(latent_FP,rolled_FP,score, true, corr_file);
            cout<<to_string(j+1)<<"        "<<rolled_template_files[ind[j]].filename()<<"       "<<scores[ind[j]]<<endl;
        }
        output.close();
        t[1] = high_resolution_clock::now();
        time_span = t[1] - t[0];
        cout<<"Total matching duration (ms): "<<time_span.count()<<endl;

    }
    return 0;
}

int Matcher::One2One_matching_all_templates(const LatentFPTemplate &latent_template, const RolledFPTemplate &rolled_template, vector<float> & score)
{

    score.resize(latent_template.m_minu_templates.size() + latent_template.m_texture_templates.size());
    std::fill(score.begin(), score.end(), 0);

   if(latent_template.m_minu_templates.size()<=0 && latent_template.m_texture_templates.size()<=0)
   {
        return 1;
    }

    if(rolled_template.m_minu_templates.size()<=0 && rolled_template.m_texture_templates.size()<=0)
    {
        return 2;
    }
    int i,j;

    using namespace std::chrono;
    vector<high_resolution_clock::time_point> t(10);

    t[0] = high_resolution_clock::now();

    for(i=0;i<latent_template.m_minu_templates.size() && rolled_template.m_minu_templates.size(); ++i)
    {
        float s = One2One_minutiae_matching(latent_template.m_minu_templates[i], rolled_template.m_minu_templates[0]);
        score[i] = s;
    }
    t[1] = high_resolution_clock::now();

    for(i=0;i<latent_template.m_texture_templates.size() && rolled_template.m_texture_templates.size()>0 ; ++i)
    {
        float s = One2One_texture_matching(latent_template.m_texture_templates[i], rolled_template.m_texture_templates[0]);
        score[i+latent_template.m_minu_templates.size()] = s;
    }

   return 0;
}

int Matcher::One2One_matching_selected_templates(const LatentFPTemplate &latent_template, const RolledFPTemplate &rolled_template, vector<float> & score, bool save_corr, string corr_file)
const
{
    score.resize(latent_template.m_minu_templates.size() + latent_template.m_texture_templates.size());
    std::fill(score.begin(), score.end(), 0);
    vector<int> selected_ind{27-1, 3-1, 12-1};

    if(latent_template.m_minu_templates.size()<=selected_ind[0] && latent_template.m_texture_templates.size()<=0)
    {
        return 1;
    }

    if(rolled_template.m_minu_templates.size()<=0 && rolled_template.m_texture_templates.size()<=0)
    {
        return 2;
    }
    int i,j;

    using namespace std::chrono;
    vector<high_resolution_clock::time_point> t(10);

    t[0] = high_resolution_clock::now();

    for(i=0;i<selected_ind.size() && rolled_template.m_minu_templates.size()>0; ++i)
    {
        int ind = selected_ind[i];
        if(latent_template.m_minu_templates.size()<=ind)
            continue;
        string one_corr_file = corr_file + "_" + to_string(i) + ".csv";
        float s = One2One_minutiae_matching(latent_template.m_minu_templates[ind], rolled_template.m_minu_templates[0], save_corr, one_corr_file);
        score[i] = s;
    }
    t[1] = high_resolution_clock::now();

    for(i=0;i<min(1ul,latent_template.m_texture_templates.size()) && rolled_template.m_texture_templates.size()>0 ; ++i)
    {
        float s = One2One_texture_matching(latent_template.m_texture_templates[i], rolled_template.m_texture_templates[0]);
        score[i+latent_template.m_minu_templates.size()] = s;
    }

    return 0;
}

float Matcher::One2One_minutiae_matching(const MinutiaeTemplate &latent_minu_template_in, const MinutiaeTemplate &rolled_minu_template_in, bool save_corr, string corr_file)
const
{
	// XXX: Can we avoid the manipulation from eigen below and not dup?
	MinutiaeTemplate latent_minu_template(latent_minu_template_in);
	MinutiaeTemplate rolled_minu_template(rolled_minu_template_in);

    // step 1: compute pairwise similarity between descriptors

    int n_time = 0;
    int i,j,k;

    int des_len = rolled_minu_template.des_length();
    if(des_len!=latent_minu_template.des_length()){
        cout<<latent_minu_template.des_length()<<endl;
	cout<<rolled_minu_template.des_length()<<endl;
	}
    assert(des_len == latent_minu_template.des_length());

    float simi = 0.0;

    using namespace std::chrono;
    vector<high_resolution_clock::time_point> t(10);

    Matrix<float, Eigen::Dynamic, Eigen::Dynamic> aa =  Map<Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>(latent_minu_template.m_des.data(),latent_minu_template.m_minutiae.size(),des_len);
    Matrix<float, Eigen::Dynamic, Eigen::Dynamic> bb =  Map<Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>(rolled_minu_template.m_des.data(),rolled_minu_template.m_minutiae.size(),des_len);

    MatrixXf  simi_matrix=aa*bb.transpose();

    for(i=0; i<latent_minu_template.m_minutiae.size(); ++i)
    {
        for(j = 0; j<rolled_minu_template.m_minutiae.size(); ++j)
        {
            if(simi_matrix(i,j)<0)
                simi_matrix(i,j) = 0;
        }
    }

    //  step 2:  similarity normalization
    VectorXf  rolled_simi_sum = simi_matrix.colwise().sum();
    VectorXf  latent_simi_sum = simi_matrix.rowwise().sum();

    int ind_1, ind_2 ;
    vector<float> norm_simi_matrix(latent_minu_template.m_minutiae.size()*rolled_minu_template.m_minutiae.size());
    float norm_simi=0.0;
    for(i=0; i<latent_minu_template.m_minutiae.size(); ++i)
    {
        ind_1 = i*rolled_minu_template.m_minutiae.size();
        for(j = 0; j<rolled_minu_template.m_minutiae.size(); ++j)
        {
            ind_2  = ind_1 + j;
            norm_simi = simi_matrix(i,j)/(latent_simi_sum(i) + rolled_simi_sum(j) - simi_matrix(i,j)+0.000001); // //simi_matrix[ind_2]*
            norm_simi_matrix[ind_2] = norm_simi;
        }
    }
    // step 3: find top N correspondences using norm_simi_matrix;
    // the sorting part can be replaced by a min-heap
    std::vector<int> y(norm_simi_matrix.size());
    std::iota(y.begin(), y.end(), 0);
    auto comparator = [&norm_simi_matrix](int a, int b){ return norm_simi_matrix[a] > norm_simi_matrix[b]; };
    std::sort(y.begin(), y.end(), comparator);

    std::vector<tuple<float, int, int>>corr;
    int topN = 120;
    if(rolled_minu_template.m_minutiae.size()*latent_minu_template.m_minutiae.size()<topN)
        topN = rolled_minu_template.m_minutiae.size()*latent_minu_template.m_minutiae.size();
    for(i=0; i<topN ; ++i)
    {
        ind_1 = y[i]/rolled_minu_template.m_minutiae.size(); // latent minutiae  index
        ind_2 = y[i] - ind_1*rolled_minu_template.m_minutiae.size(); // rolled minutiae index
        simi = simi_matrix(ind_1,ind_2);
        corr.push_back(make_tuple(simi,ind_1,ind_2));
    }

     // step 4: remove false correspondences using two graph matching
    int d_thr = 30;
    vector<tuple<float, int, int>> corr2 = LSS_R_Fast2_Dist_eigen(corr, latent_minu_template, rolled_minu_template, d_thr);

    vector<tuple<float, int, int>> corr3  = LSS_R_Fast2(corr2, latent_minu_template, rolled_minu_template, d_thr);

    if (save_corr){
        ofstream output;
        output.open(corr_file);
        for(int i = 0; i < corr3.size(); i++){
            output<<latent_minu_template.m_minutiae[get<1>(corr3[i])].x<<","<<latent_minu_template.m_minutiae[get<1>(corr3[i])].y
                <<","<<rolled_minu_template.m_minutiae[get<2>(corr3[i])].x<<","<<rolled_minu_template.m_minutiae[get<2>(corr3[i])].y<<endl;
        }
        output.close();
    }

    float score = 0.0;

    for(i=0; i<corr3.size(); ++i)
    {
        score += get<0>(corr3[i]);
    }
    return score;

}

int Matcher::One2One_matching(string latent_file, string rolled_file)
{
    LatentFPTemplate latent_template{};
    try {
    	load_latent_template(latent_file);
    } catch (const std::exception&) {
    	return (1);
    }

    RolledFPTemplate rolled_template{};
    try {
	    rolled_template = load_rolled_template(rolled_file);
    } catch (const std::exception&) {
    	return (2);
    }

    vector<float> score;
    int ret = One2One_matching_selected_templates(latent_template, rolled_template, score);

    return ret;
}

float Matcher::One2One_texture_matching(const LatentTextureTemplate &latent_texture_template, const RolledTextureTemplatePQ &rolled_texture_template)
const
{
    // step 1: compute pairwise similarity between descriptors
    int n_time = 0;
    int i,j,k;

    int des_len = rolled_texture_template.des_length();

   std::unique_ptr<float[]> simi_matrix{new float[MaxNRolledMinu*MaxNLatentMinu]};
   memset(simi_matrix.get(),0,MaxNRolledMinu*MaxNLatentMinu*sizeof(float));

    const int numLatentMinutiae = std::min(latent_texture_template.m_minutiae.size(), MaxNLatentMinu);
    const int numExemplarMinutiae = std::min(rolled_texture_template.m_minutiae.size(), MaxNRolledMinu);

    float simi = 0.0;
    float *p_latent_des, *p_latent_des0, *p_rolled_des;

    using namespace std::chrono;
    vector<high_resolution_clock::time_point> t(10);

    t[n_time++] = high_resolution_clock::now();
    float dist0=0.0, dist1= 0.0, dist2= 0, dist3=0.0, dist4 = 0.0; //, dist5, dist6,dist7, dist8;
    int code1 = 0, code2 = 0, code3 = 0, code4=0;
    float *p_dist_codewords0 = NULL, *p_dist_codewords1 = NULL, *p_dist_codewords2 =NULL;

    /* Make a copy of the codewords from the latent texture template */
    std::vector<float> latent_texture_template_m_dist_codewords_copy{latent_texture_template.m_dist_codewords};

    int n=0;
    int nrof_clusters3 = nrof_clusters*3, nrof_clusters2 = nrof_clusters*2;
    const int method = 1;
    if(method == 1)
    {
        for(i=0; i<numLatentMinutiae; ++i)
        {
            p_dist_codewords0 = latent_texture_template_m_dist_codewords_copy.data() + i*nrof_subs*nrof_clusters;
            for(j=0; j<numExemplarMinutiae; ++j)
            {
                dist1 = 6.;
                dist2 = 0.;
                dist3 = 0.;
                dist4 = 0.;
                p_dist_codewords1 = p_dist_codewords0;
                const unsigned char *p_des0 = rolled_texture_template.m_desPQ.data() + j* rolled_texture_template.des_length();
                for(k=0; k<nrof_subs; k+=4, p_dist_codewords1+=4*nrof_clusters)
                {
                    code1 = *(p_des0+k);
                    dist1 -= *(p_dist_codewords1 + code1);

                    code2 = *(p_des0+k+1);
                    dist2 -= *(p_dist_codewords1 + code2 + nrof_clusters);

                    code3 = *(p_des0+k+2);
                    dist3 -= *(p_dist_codewords1 + code3 + nrof_clusters2);

                    code4 = *(p_des0+k+3);
                    dist4 -= *(p_dist_codewords1 + code4 + nrof_clusters3);

                }
                simi_matrix[n++] = (dist1+dist2)+ (dist3+dist4);
            }
        }
    }
    else if (method==2)
    {
        int B1=64, B2 = 64;
        for(i=0; i<numLatentMinutiae-B1; i+=B1)
        {

            for(j=0; j<numExemplarMinutiae-B2; j += B2)
            {

                for(int ii=i; ii<i+B1; ++ii)
                {
                    p_dist_codewords0 = latent_texture_template_m_dist_codewords_copy.data() + ii*nrof_subs*nrof_clusters;
                    for(int jj=j; jj<j+B2; ++jj)
                    {
                       dist1 = 6.;
                       dist2 = 0.;
                       dist3 = 0.;
                       dist4 = 0.;
                       p_dist_codewords1 = p_dist_codewords0;
                       const unsigned char *p_des0 = rolled_texture_template.m_desPQ.data() + jj* rolled_texture_template.des_length();

                        for(k=0; k<nrof_subs; k+=4, p_dist_codewords1+=4*nrof_clusters)
                        {
                            code1 = *(p_des0+k);
                            dist1 -= *(p_dist_codewords1 + code1);

                            code2 = *(p_des0+k+1);
                            dist2 -= *(p_dist_codewords1 + code2 + nrof_clusters);

                            code3 = *(p_des0+k+2);
                            dist3 -= *(p_dist_codewords1 + code3 + nrof_clusters2);

                            code4 = *(p_des0+k+3);
                            dist4 -= *(p_dist_codewords1 + code4 + nrof_clusters3);

                        }
                        simi_matrix[ii*numExemplarMinutiae+jj] = (dist1+dist2)+ (dist3+dist4);
                    }
                }
            }
        }
    }
    else if(method == 3)
    {
         int B1=64, B2 = 64;
        for(i=0; i<numLatentMinutiae-B1; i+=B1)
        {
            p_dist_codewords0 = latent_texture_template_m_dist_codewords_copy.data() + i*nrof_subs*nrof_clusters;
            for(j=0; j<numExemplarMinutiae-B2; j += B2)
            {
                p_dist_codewords1 = p_dist_codewords0;
                const unsigned char *p_des0 = rolled_texture_template.m_desPQ.data() + j* rolled_texture_template.des_length();
                for(int ii=i; ii<i+B1; ++ii)
                {
                    const unsigned char *p_des1 = p_des0;
                    for(int jj=j; jj<j+B2; ++jj)
                    {
                       p_dist_codewords2 = p_dist_codewords1;
                       dist1 = 6.;
                       dist2 = 0.;
                       dist3 = 0.;
                       dist4 = 0.;

                        for(k=0; k<nrof_subs; k+=4, p_dist_codewords2+=4*nrof_clusters)
                        {
                            code1 = *(p_des1+k);
                            dist1 -= *(p_dist_codewords2 + code1);

                            code2 = *(p_des1+k+1);
                            dist2 -= *(p_dist_codewords2 + code2 + nrof_clusters);

                            code3 = *(p_des1+k+2);
                            dist3 -= *(p_dist_codewords2 + code3 + nrof_clusters2);

                            code4 = *(p_des1+k+3);
                            dist4 -= *(p_dist_codewords2 + code4 + nrof_clusters3);

                        }
                        simi_matrix[ii*numExemplarMinutiae+jj] = (dist1+dist2)+ (dist3+dist4);
                        p_des1 +=  rolled_texture_template.des_length();
                    }
                    p_dist_codewords1 += nrof_subs*nrof_clusters;
                }
            }
        }
    }
    else if(method==4)
    {
        for(i=0; i<numLatentMinutiae; ++i)
        {
            p_dist_codewords0 = latent_texture_template_m_dist_codewords_copy.data() + i*nrof_subs*nrof_clusters;
            for(k=0; k<nrof_subs; ++k)
            {
                for(j=0; j<numExemplarMinutiae-4; j+=4)
                {
                    n = i*numExemplarMinutiae + j;
                    const unsigned char *p_des0 = rolled_texture_template.m_desPQ.data() + j* rolled_texture_template.des_length() + k;
                    const unsigned char *p_des1 = p_des0 + rolled_texture_template.des_length();
                    const unsigned char *p_des2 = p_des1 + rolled_texture_template.des_length();
                    const unsigned char *p_des3 = p_des2 + rolled_texture_template.des_length();

                    dist0 -= *(p_dist_codewords0 + *p_des0);

                    dist1 -= *(p_dist_codewords0 + *p_des1);

                    dist2 -= *(p_dist_codewords0 + *p_des2);

                    dist3 -= *(p_dist_codewords0 + *p_des3);

                    simi_matrix[n] += dist0;
                    simi_matrix[n+1] += dist1;
                    simi_matrix[n+2] += dist2;
                    simi_matrix[n+3] += dist3;
                }
            }
        }
    }
    t[n_time] = high_resolution_clock::now();
    duration<double, std::milli> time_span = t[n_time] - t[n_time-1];

    vector<float> time(10);
    time[n_time-1]+=time_span.count() ;  // minutiae similarity
    n_time++;

//
    std::vector<tuple<float, int, int>>tmp_corr(numLatentMinutiae), corr(N);
    float max_val;
    float *psimi = simi_matrix.get();
    int max_index;
    for(i=0;i<numLatentMinutiae; ++i)
    {

        max_index = std::distance(psimi, std::max_element(psimi, psimi+numExemplarMinutiae));
        max_val = *(psimi + max_index);
        tmp_corr[i] = make_tuple(max_val,i,max_index);

        psimi += numExemplarMinutiae;
    }
    if(tmp_corr.size()>N)
    {
        std::vector<int> y(tmp_corr.size());
        std::iota(y.begin(), y.end(), 0);
        auto comparator = [&tmp_corr](int a, int b){ return get<0>(tmp_corr[a]) > get<0>(tmp_corr[b]); };
        std::sort(y.begin(), y.end(), comparator);

        for(i=0;i<N; ++i)
        {
            corr[i] = tmp_corr[y[i]];
        }
    }
    else
        corr = tmp_corr;
    t[n_time] = high_resolution_clock::now();

    time_span = t[n_time] - t[n_time-1];

    time[n_time-1]+=time_span.count() ;  // obtaining initial correspondences
    n_time++;

     // step 4: remove false correspondences using two graph matching
    int d_thr = 30;
    vector<tuple<float, int, int>> corr2 = LSS_R_Fast2_Dist_lookup(corr, latent_texture_template, rolled_texture_template, d_thr);

    t[n_time] = high_resolution_clock::now();
    time_span = t[n_time] - t[n_time-1];
    time[n_time-1]+=time_span.count() ;   // second order graph matching: distance
    n_time++;

    vector<tuple<float, int, int>> corr3  = LSS_R_Fast2(corr2, latent_texture_template, rolled_texture_template, d_thr);

    t[n_time] = high_resolution_clock::now();
    time_span = t[n_time] - t[n_time-1];
    time[n_time-1]+=time_span.count() ;   // second order graph matching: original
    n_time++;

    float score = 0.0;

    for(i=0; i<corr3.size(); ++i)
    {
        score += get<0>(corr3[i]);
    }
    return score;

}

LatentFPTemplate Matcher::load_latent_template(const std::string &tname) const
{
	std::ifstream is{tname, ifstream::binary};
	if (!is)
		throw std::runtime_error{"Could not open " + tname};

	is.seekg(0, std::ios_base::end);
	std::vector<uint8_t> buf(is.tellg());
	is.seekg(0, std::ios_base::beg);

	is.read(reinterpret_cast<char *>(buf.data()), buf.size());
	return (load_latent_template(buf));
}

LatentFPTemplate Matcher::load_latent_template(const std::vector<uint8_t> &buf) const
{
    LatentFPTemplate fp_template{};
    /*
     * FIXME: When using dynamic-sized containers, these maximums aren't
     *        necessary. Preserving them for historical reasons (we probably
     *        don't want to search when there's >2000 minutiae anyway).
     */
    static const short Max_Nrof_Minutiae = 2*1000; // including virtual minutiae. We only consider top 1000 minutiae including both real and virtual minutiae for each template.
    static const short Max_Des_Length = 192;
    static const short Max_BlkSize = 100;

    std::istringstream is{
        {reinterpret_cast<const char *>(buf.data()), buf.size()},
        std::istringstream::binary};

    // get length of file:
    is.seekg(0, ios::end);
    int length = is.tellg();

    if( length<=0 )
    {
            throw std::runtime_error{"Length of latent template is 0"};
    }
    is.seekg(0, ios::beg);
    short header[12];
    short h,w,blkH,blkW;
    unsigned char nrof_minu_template,nrof_texture_template;
    short nrof_minutiae;

    short nrof_minutiae_feature;
    short des_len;
    int i,j;

    std::vector<short> x{};
    std::vector<short> y{};
    std::vector<float> ori{};
    std::vector<float> des{};

    for(int i=0; i<12; i++){
        is.read(reinterpret_cast<char*>(&header[i]),sizeof(short));
    }
    /*
     * FIXME: Seen at least once where a template appears corrupt (possibly
     *        because an image was too large? it was a palm), so fail fast
     *        before we trigger a segfault allocating one of the internal
     *        structures.
     */
    static const short expectedHeader[12] = {
        1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
    if (memcmp(header, expectedHeader, 12 * sizeof(short)) != 0) {
    	throw std::runtime_error{"Template appears corrupt (invalid header)"};
    }

    is.read(reinterpret_cast<char*>(&h),sizeof(short));
    is.read(reinterpret_cast<char*>(&w),sizeof(short));
    is.read(reinterpret_cast<char*>(&blkH),sizeof(short));
    is.read(reinterpret_cast<char*>(&blkW),sizeof(short));
    is.read(reinterpret_cast<char*>(&nrof_minu_template),sizeof(unsigned char));
    if(blkH>50)
        blkH = 50;
    if(blkW>50)
        blkW = 50;
    for(i=0;i<nrof_minu_template; ++i)
    {
        is.read(reinterpret_cast<char*>(&nrof_minutiae),sizeof(short));
        if(nrof_minutiae<=0)
            continue;
        if(nrof_minutiae>Max_Nrof_Minutiae)
        {
            throw std::runtime_error{"Number of minutiae is larger than "
                "Max Number of Minutiae (latent): " +
                std::to_string(nrof_minutiae) + ">" +
                std::to_string(Max_Nrof_Minutiae)};
        }
        if(blkH>Max_BlkSize || blkW>Max_BlkSize)
        {
            throw std::runtime_error{"The size of the ridge flow is larger "
                "than maximum size: " + std::to_string(Max_BlkSize)};
        }
        x.resize(nrof_minutiae);
        is.read(reinterpret_cast<char*>(x.data()),sizeof(short)*nrof_minutiae);
        y.resize(nrof_minutiae);
        is.read(reinterpret_cast<char*>(y.data()),sizeof(short)*nrof_minutiae);
        ori.resize(nrof_minutiae);
        is.read(reinterpret_cast<char*>(ori.data()),sizeof(float)*nrof_minutiae);
        is.read(reinterpret_cast<char*>(&des_len),sizeof(short));

        des.resize(nrof_minutiae * des_len);
        is.read(reinterpret_cast<char*>(des.data()),sizeof(float)*nrof_minutiae*des_len);

        MinutiaeTemplate minu_template(nrof_minutiae,x,y,ori,des,blkH, blkW);
        fp_template.add_template(minu_template);
    }

    is.read(reinterpret_cast<char*>(&nrof_texture_template),sizeof(unsigned char));

   for(i=0;i<nrof_texture_template; ++i)
    {
        is.read(reinterpret_cast<char*>(&nrof_minutiae),sizeof(short));
        if(nrof_minutiae<=0)
            continue;
        if(nrof_minutiae>Max_Nrof_Minutiae)
        {
            throw std::runtime_error{"Number of minutiae is larger than Max "
                "Number of Minutiae: " + std::to_string(nrof_minutiae) + ">" +
                std::to_string(Max_Nrof_Minutiae)};
        }
        x.resize(nrof_minutiae);
        is.read(reinterpret_cast<char*>(x.data()),sizeof(short)*nrof_minutiae);
        y.resize(nrof_minutiae);
        is.read(reinterpret_cast<char*>(y.data()),sizeof(short)*nrof_minutiae);
        ori.resize(nrof_minutiae);
        is.read(reinterpret_cast<char*>(ori.data()),sizeof(float)*nrof_minutiae);
        is.read(reinterpret_cast<char*>(&des_len),sizeof(short));
        des.resize(nrof_minutiae * des_len);
        is.read(reinterpret_cast<char*>(des.data()),sizeof(float)*nrof_minutiae*des_len);

        LatentTextureTemplate texture_template(nrof_minutiae,x,y,ori,des);
        texture_template.compute_dist_to_codewords(this->codewords, nrof_subs,  sub_dim,  nrof_clusters);
        fp_template.add_texture_template(texture_template);
    }

    return (fp_template);
}

RolledFPTemplate Matcher::load_rolled_template(const string &tname) const
{
	std::ifstream is{tname, ifstream::binary};
	if (!is)
		throw std::runtime_error{"Could not open " + tname};

	is.seekg(0, std::ios_base::end);
	std::vector<uint8_t> buf(is.tellg());
	is.seekg(0, std::ios_base::beg);

	is.read(reinterpret_cast<char *>(buf.data()), buf.size());
	return (load_rolled_template(buf));
}

RolledFPTemplate Matcher::load_rolled_template(const std::vector<uint8_t> &buf) const
{
    RolledFPTemplate fp_template{};
    /*
     * FIXME: When using dynamic-sized containers, these maximums aren't
     *        necessary. Preserving them for historical reasons (we probably
     *        don't want to search when there's >2000 minutiae anyway).
     */
    static const short Max_Nrof_Minutiae = 2*1000; // including virtual minutiae. We only consider top 1000 minutiae including both real and virtual minutiae for each template.
    static const short Max_Des_Length = 192;
    static const short Max_BlkSize = 100;

    std::istringstream is{
        {reinterpret_cast<const char *>(buf.data()), buf.size()},
        std::istringstream::binary};

    // get length of file:
    is.seekg(0, ios::end);
    int length = is.tellg();

    if( length<=10 )
    {
        throw std::runtime_error{"Size of rolled template is " +
            std::to_string(length) + " (<= 10)"};
    }
    is.seekg(0, ios::beg);
    short header[12];
    short h,w,blkH,blkW;
    unsigned char nrof_minu_template,nrof_texture_template;
    short nrof_minutiae;

    short nrof_minutiae_feature;
    short des_len=96;
    int i,j;

    std::vector<short> x{};
    std::vector<short> y{};
    std::vector<float> ori{};

    std::vector<float> des{};

    for(int i=0; i<12; i++){
        is.read(reinterpret_cast<char*>(&header[i]),sizeof(short));
    }
    /*
     * FIXME: Seen at least once where a template appears corrupt (possibly
     *        because an image was too large? it was a palm), so fail fast
     *        before we trigger a segfault allocating one of the internal
     *        structures.
     */
    static const short expectedHeader[12] = {
        1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
    if (memcmp(header, expectedHeader, 12 * sizeof(short)) != 0) {
    	throw std::runtime_error{"Template appears corrupt (invalid header)"};
    }

    is.read(reinterpret_cast<char*>(&h),sizeof(short));
    is.read(reinterpret_cast<char*>(&w),sizeof(short));
    is.read(reinterpret_cast<char*>(&blkH),sizeof(short));
    is.read(reinterpret_cast<char*>(&blkW),sizeof(short));
    is.read(reinterpret_cast<char*>(&nrof_minu_template),sizeof(unsigned char));
    if(blkH>50)
        blkH = 50;
    if(blkW>50)
        blkW = 50;
    for(i=0;i<nrof_minu_template; ++i)
    {
        is.read(reinterpret_cast<char*>(&nrof_minutiae),sizeof(short));
        if(nrof_minutiae<=0)
            continue;
        if(nrof_minutiae>Max_Nrof_Minutiae)
        {
            throw std::runtime_error{"Number of minutiae is larger than Max "
                "Number of Minutiae: " + std::to_string(nrof_minutiae) + ">" +
                std::to_string(Max_Nrof_Minutiae)};
        }
        if(blkH>Max_BlkSize || blkW>Max_BlkSize)
        {
            throw std::runtime_error{"The size of the ridge flow is larger "
                "than maximum size: " + std::to_string(Max_BlkSize)};
        }
        x.resize(nrof_minutiae);
        is.read(reinterpret_cast<char*>(x.data()),sizeof(short)*nrof_minutiae);
        y.resize(nrof_minutiae);
        is.read(reinterpret_cast<char*>(y.data()),sizeof(short)*nrof_minutiae);
        ori.resize(nrof_minutiae);
        is.read(reinterpret_cast<char*>(ori.data()),sizeof(float)*nrof_minutiae);
        is.read(reinterpret_cast<char*>(&des_len),sizeof(short));

        des.resize(nrof_minutiae * des_len);
        is.read(reinterpret_cast<char*>(des.data()),sizeof(float)*nrof_minutiae*des_len);

        MinutiaeTemplate minu_template(nrof_minutiae,x,y,ori,des,blkH, blkW);
        fp_template.add_template(minu_template);
    }

    is.read(reinterpret_cast<char*>(&nrof_texture_template),sizeof(unsigned char));

   for(i=0;i<nrof_texture_template; ++i)
    {
        is.read(reinterpret_cast<char*>(&nrof_minutiae),sizeof(short));
        if(nrof_minutiae<=0)
            continue;
        if(nrof_minutiae>Max_Nrof_Minutiae)
        {
            throw std::runtime_error{"Number of minutiae is larger than Max "
                "Number of Minutiae: " + std::to_string(nrof_minutiae) + ">" +
                std::to_string(Max_Nrof_Minutiae)};
        }
        x.resize(nrof_minutiae);
        is.read(reinterpret_cast<char*>(x.data()),sizeof(short)*nrof_minutiae);
        y.resize(nrof_minutiae);
        is.read(reinterpret_cast<char*>(y.data()),sizeof(short)*nrof_minutiae);
        ori.resize(nrof_minutiae);
        is.read(reinterpret_cast<char*>(ori.data()),sizeof(float)*nrof_minutiae);
        is.read(reinterpret_cast<char*>(&des_len),sizeof(short));
        des.resize(nrof_minutiae * des_len);
        is.read(reinterpret_cast<char*>(des.data()),sizeof(float)*nrof_minutiae*des_len);

        RolledTextureTemplatePQ texture_template(nrof_minutiae,x,y,ori,des);
        fp_template.add_texture_template(texture_template);
    }

    return (fp_template);
}

vector<tuple<float, int, int>>  Matcher::LSS_R_Fast2_Dist(vector<tuple<float, int, int>> &corr, const SingleTemplate & latent_template, const SingleTemplate & rolled_template, float d_thr)
const
{
    int num = corr.size();
    vector<float> H(num*num);

    vector<short> flag_latent(latent_template.m_minutiae.size()),flag_rolled(rolled_template.m_minutiae.size());

    int i,j,k;

    float dist_1, dist_2, dist;
    float dx_1, dy_1, dx_2, dy_2;

    for(i=0; i<num-1; ++i)
    {
        const MinuPoint p_latent_minutia_1 = latent_template.m_minutiae[get<1>(corr[i])];
        const MinuPoint p_rolled_minutia_1 = rolled_template.m_minutiae[get<2>(corr[i])];
        for(j=i+1; j<num;++j)
        {
            const MinuPoint p_latent_minutia_2 = latent_template.m_minutiae[get<1>(corr[j])];
            const MinuPoint p_rolled_minutia_2 = rolled_template.m_minutiae[get<2>(corr[j])];

            dx_1 = p_latent_minutia_1.x-p_latent_minutia_2.x;
            dx_2 = p_rolled_minutia_1.x-p_rolled_minutia_2.x;

            dy_1 = p_latent_minutia_1.y-p_latent_minutia_2.y;
            dy_2 = p_rolled_minutia_1.y-p_rolled_minutia_2.y;

            dist_1 = (dx_1*dx_1)+(dy_1*dy_1);
            dist_1 = sqrt(dist_1);

            dist_2 = (dx_2*dx_2)+(dy_2*dy_2);
            dist_2 = sqrt(dist_2);

            dist = fabs(dist_1-dist_2);

            H[i*num+j] = (30-dist)/(25.0);
            if(H[i*num+j]>1)
                H[i*num+j] = 1.0;
            else if(H[i*num+j]<0)
                H[i*num+j] = 0.0;

            H[j*num+i] = H[i*num+j];
        }
    }

    vector<float> S(num),S1(num);

    float s0 = 1.0/num;
    for(i=0; i<num; ++i)
        S[i] = get<0>(corr[i]);

    float sum = 0.0;
    for(i=0;i<5 ; ++i)
    {
        sum = 0.0;
        for(j=0;j<num; ++j)
        {
            S1[j] = 0;
            for(k=0; k<num;++k)
            {
                //if(H[j*num+k])
                S1[j] += H[j*num+k]*S[k];
            }
            sum += S1[j];
        }
        sum = 1.0/(sum+0.0001);
        for(j=0;j<num; ++j)
        {
            S[j] = S1[j]*sum;
        }
    }

    // sort the S
    // the sorting part can be replaced by a min-heap
    std::vector<int> y(S.size());
    std::iota(y.begin(), y.end(), 0);
    auto comparator = [&S](int a, int b){ return S[a] > S[b]; };
    std::sort(y.begin(), y.end(), comparator);

    vector<tuple<float, int, int>>  new_corr;
    vector<int>  selected_ind;
    short ind;
    for(i=0; i<num; ++i)
    {
        ind = y[i];
        if(S[ind]<0.0001)
            break;
        if((flag_latent[get<1>(corr[ind])] == 1) || (flag_rolled[get<2>(corr[ind])] == 1))
            continue;

        if(i==0)
        {
            selected_ind.push_back(ind);
            new_corr.push_back(make_tuple(get<0>(corr[ind]),get<1>(corr[ind]),get<2>(corr[ind])));

            flag_latent[get<1>(corr[ind])] = 1;
            flag_rolled[get<2>(corr[ind])] = 1;
        }
        else
        {
            int found =0;
            for(j=0;j<selected_ind.size(); ++j)
            {
                if(H[ind*num+selected_ind[j]]<0.00001)
                {
                    found = 1;
                    break;
                }
            }
            if(found==0)
            {
                selected_ind.push_back(ind);
                new_corr.push_back(make_tuple(get<0>(corr[ind]),get<1>(corr[ind]),get<2>(corr[ind])));

                flag_latent[get<1>(corr[ind])] = 1;
                flag_rolled[get<2>(corr[ind])] = 1;
            }
        }
    }

    return new_corr;
}

vector<tuple<float, int, int>>  Matcher::LSS_R_Fast2_Dist_lookup(vector<tuple<float, int, int>> &corr, const SingleTemplate & latent_template, const SingleTemplate & rolled_template, float d_thr)
const
{
    int num = corr.size();
    std::unique_ptr<float[]> H{new float [num*num]()};
    vector<short> flag_latent(latent_template.m_minutiae.size()),flag_rolled(rolled_template.m_minutiae.size());

    int i,j,k;

    float dist_1, dist_2, dist;
    int  dx_1, dy_1, dx_2, dy_2;

    for(i=0; i<num-1; ++i)
    {
        const MinuPoint p_latent_minutia_1 = latent_template.m_minutiae[get<1>(corr[i])];
        const MinuPoint p_rolled_minutia_1 = rolled_template.m_minutiae[get<2>(corr[i])];
        for(j=i+1; j<num;++j)
        {
            const MinuPoint p_latent_minutia_2 = latent_template.m_minutiae[get<1>(corr[j])];
            const MinuPoint p_rolled_minutia_2 = rolled_template.m_minutiae[get<2>(corr[j])];

            dx_1 = p_latent_minutia_1.x-p_latent_minutia_2.x;
            dx_2 = p_rolled_minutia_1.x-p_rolled_minutia_2.x;

            dx_1 = abs(dx_1);
            dx_2 = abs(dx_2);
            dy_1 = p_latent_minutia_1.y-p_latent_minutia_2.y;
            dy_2 = p_rolled_minutia_1.y-p_rolled_minutia_2.y;

            dy_1 = abs(dy_1);
            dy_2 = abs(dy_2);

            if((dx_1>=dist_N) || (dx_2>=dist_N) || (dy_1>=dist_N) || (dy_2>=dist_N))
                continue;

            dist_1 = table_dist[dx_1*dist_N+dy_1];

            dist_2 = table_dist[dx_2*dist_N+dy_2];

            dist = fabs(dist_1-dist_2);
            if(dist>d_thr)
                continue;

            H[i*num+j] = (30-dist)/(25.0);
            if(H[i*num+j]>1)
                H[i*num+j] = 1.0;
            else if(H[i*num+j]<0)
                H[i*num+j] = 0.0;
            H[j*num+i] = H[i*num+j];
        }
    }

    Matrix<float, Eigen::Dynamic, Eigen::Dynamic> aa =  Map<Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>(H.get(),num,num);

    float sum = 0.0;
    VectorXf b(num);
    VectorXf c;
    for(i=0; i<num; ++i)
        b(i) = get<0>(corr[i]);
    for(i=0;i<3 ; ++i)
    {
        c = aa*b;
        sum = c.sum();
        b = c*(1./(sum+0.00001));
    }

    vector<float> S(num);
    for(i=0;i<num; ++i)
    {
        S[i] = b(i);
    }

    // sort S
    std::vector<int> y(S.size());
    std::iota(y.begin(), y.end(), 0);
    auto comparator = [&S](int a, int b){ return S[a] > S[b]; };
    std::sort(y.begin(), y.end(), comparator);

    vector<tuple<float, int, int>>  new_corr;
    vector<int>  selected_ind;
    short ind;
    for(i=0; i<num; ++i)
    {

        ind = y[i];
        if(S[ind]<0.0001)
            break;
        if((flag_latent[get<1>(corr[ind])] == 1) || (flag_rolled[get<2>(corr[ind])] == 1))
            continue;

        if(i==0)
        {
            selected_ind.push_back(ind);
            new_corr.push_back(make_tuple(get<0>(corr[ind]),get<1>(corr[ind]),get<2>(corr[ind])));

            flag_latent[get<1>(corr[ind])] = 1;
            flag_rolled[get<2>(corr[ind])] = 1;
        }
        else
        {
            int found = 0;
            for(j=0;j<selected_ind.size(); ++j)
            {
                if(H[ind*num+selected_ind[j]]<0.00001)
                {
                    found = 1;
                    break;
                }
            }
            if(found==0)
            {
                selected_ind.push_back(ind);
                new_corr.push_back(make_tuple(get<0>(corr[ind]),get<1>(corr[ind]),get<2>(corr[ind])));

                flag_latent[get<1>(corr[ind])] = 1;
                flag_rolled[get<2>(corr[ind])] = 1;
            }
        }
    }

    return new_corr;
}

vector<tuple<float, int, int>>  Matcher::LSS_R_Fast2_Dist_eigen(vector<tuple<float, int, int>> &corr, const SingleTemplate & latent_template, const SingleTemplate & rolled_template, float d_thr)
const
{
    int num = corr.size();
    std::unique_ptr<float[]> H{new float [num*num]()};

    vector<short> flag_latent(latent_template.m_minutiae.size()),flag_rolled(rolled_template.m_minutiae.size());

    int i,j,k;

    MinuPoint *p_latent_minutia_1, *p_latent_minutia_2, *p_rolled_minutia_1, *p_rolled_minutia_2;
    float dist_1, dist_2, dist;
    float dx_1, dy_1, dx_2, dy_2;

    for(i=0; i<num-1; ++i)
    {
        const MinuPoint p_latent_minutia_1 = latent_template.m_minutiae[get<1>(corr[i])];
        const MinuPoint p_rolled_minutia_1 = rolled_template.m_minutiae[get<2>(corr[i])];
        for(j=i+1; j<num;++j)
        {
            const MinuPoint p_latent_minutia_2 = latent_template.m_minutiae[get<1>(corr[j])];
            const MinuPoint p_rolled_minutia_2 = rolled_template.m_minutiae[get<2>(corr[j])];

            dx_1 = p_latent_minutia_1.x-p_latent_minutia_2.x;
            dx_2 = p_rolled_minutia_1.x-p_rolled_minutia_2.x;

            dy_1 = p_latent_minutia_1.y-p_latent_minutia_2.y;
            dy_2 = p_rolled_minutia_1.y-p_rolled_minutia_2.y;

            dist_1 = (dx_1*dx_1)+(dy_1*dy_1);
            dist_1 = sqrt(dist_1);

            dist_2 = (dx_2*dx_2)+(dy_2*dy_2);
            dist_2 = sqrt(dist_2);
            dist = fabs(dist_1-dist_2);
           if(dist>d_thr)
                continue;

            H[i*num+j] = (30-dist)/(25.0);
            if(H[i*num+j]>1)
                H[i*num+j] = 1.0;
            else if(H[i*num+j]<0)
                H[i*num+j] = 0.0;

            H[j*num+i] = H[i*num+j];
        }
    }

    Matrix<float, Eigen::Dynamic, Eigen::Dynamic> aa =  Map<Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>(H.get(),num,num);

    float sum = 0.0;
    VectorXf b(num);
    VectorXf c;
    for(i=0; i<num; ++i)
        b(i) = get<0>(corr[i]);
    for(i=0;i<5 ; ++i)
    {
        c = aa*b;
        sum = c.sum();
        b = c*(1./(sum+0.00001));
    }

    vector<float> S(num);
    for(i=0;i<num; ++i)
    {
        S[i] = b(i);
    }
    // sort the S
    // the sorting part can be replaced by a min-heap
    std::vector<int> y(S.size());
    std::iota(y.begin(), y.end(), 0);
    auto comparator = [&S](int a, int b){ return S[a] > S[b]; };
    std::sort(y.begin(), y.end(), comparator);

    vector<tuple<float, int, int>>  new_corr;
    vector<int>  selected_ind;
    short ind;
    for(i=0; i<num; ++i)
    {

        ind = y[i];
        if(S[ind]<0.0001)
            break;
        if((flag_latent[get<1>(corr[ind])] == 1) || (flag_rolled[get<2>(corr[ind])] == 1))
            continue;

        if(i==0)
        {
            selected_ind.push_back(ind);
            new_corr.push_back(make_tuple(get<0>(corr[ind]),get<1>(corr[ind]),get<2>(corr[ind])));

            flag_latent[get<1>(corr[ind])] = 1;
            flag_rolled[get<2>(corr[ind])] = 1;
        }
        else
        {
            int found =0;
            for(j=0;j<selected_ind.size(); ++j)
            {
                if(H[ind*num+selected_ind[j]]<0.00001)
                {
                    found = 1;
                    break;
                }
            }
            if(found==0)
            {
                selected_ind.push_back(ind);
                new_corr.push_back(make_tuple(get<0>(corr[ind]),get<1>(corr[ind]),get<2>(corr[ind])));

                flag_latent[get<1>(corr[ind])] = 1;
                flag_rolled[get<2>(corr[ind])] = 1;
            }
        }
    }

    return new_corr;
}

vector<tuple<float, int, int>>  Matcher::LSS_R_Fast2(vector<tuple<float, int, int>> &corr, const SingleTemplate & latent_template, const SingleTemplate & rolled_template, int d_thr)
const
{
    int num = corr.size();
    vector<bool> H(num*num);
    vector<short> flag_latent(latent_template.m_minutiae.size()),flag_rolled(rolled_template.m_minutiae.size());

    int i,j,k;

    float dist_1, dist_2;
    float angle_1, angle_2, angle_diff;
    float line_angle_1, line_angle_2;
    float dx_1,dx_2,dy_1,dy_2;

    for(i=0; i<num-1; ++i)
    {
        const MinuPoint p_latent_minutia_1 = latent_template.m_minutiae[get<1>(corr[i])];
        const MinuPoint p_rolled_minutia_1 = rolled_template.m_minutiae[get<2>(corr[i])];
        for(j=i+1; j<num;++j)
        {
            const MinuPoint p_latent_minutia_2 = latent_template.m_minutiae[get<1>(corr[j])];
            const MinuPoint p_rolled_minutia_2 = rolled_template.m_minutiae[get<2>(corr[j])];

            angle_1 = p_latent_minutia_1.ori-p_latent_minutia_2.ori;
            angle_1 = adjust_angle(angle_1);

            angle_2 = p_rolled_minutia_1.ori-p_rolled_minutia_2.ori;
            angle_2 = adjust_angle(angle_2);

            angle_diff = fabs(angle_1 - angle_2);

            if(angle_diff>PI)
               angle_diff = 2*PI - angle_diff;

            if(angle_diff>PI/4.)
                continue;

            dx_1 = p_latent_minutia_1.x-p_latent_minutia_2.x;
            dy_1 = p_latent_minutia_1.y-p_latent_minutia_2.y;

            line_angle_1 = -atan2(dy_1,dx_1);
            angle_1 = p_latent_minutia_1.ori - line_angle_1;
            angle_1 = adjust_angle(angle_1);

            dx_2 = p_rolled_minutia_1.x-p_rolled_minutia_2.x;
            dy_2 = p_rolled_minutia_1.y-p_rolled_minutia_2.y;

            line_angle_2 = -atan2(dy_2,dx_2);
            angle_2 = p_rolled_minutia_1.ori - line_angle_2;
            angle_2 = adjust_angle(angle_2);

            angle_diff = fabs(angle_1 - angle_2);

            if(angle_diff>PI)
               angle_diff = 2*PI - angle_diff;
            if(angle_diff>PI/6.)
                continue;

            angle_1 = p_latent_minutia_2.ori - line_angle_1;
            angle_1 = adjust_angle(angle_1);

            angle_2 = p_rolled_minutia_2.ori - line_angle_2;
            angle_2 = adjust_angle(angle_2);

            angle_diff = fabs(angle_1 - angle_2);

            if(angle_diff>PI)
               angle_diff = 2*PI - angle_diff;
            if(angle_diff>PI/6.)
                continue;

            H[i*num+j] = true;
            H[j*num+i] = true;
        }
    }

    vector<float> S(num),S1(num);

    float s0 = 1.0/num;
    for(i=0; i<num; ++i)
        S[i] = s0;

    float sum = 0.0;
    for(i=0;i<5 ; ++i)
    {
        sum = 0.0;
        for(j=0;j<num; ++j)
        {
            S1[j] = 0;
            for(k=0; k<num;++k)
            {
                if(H[j*num+k])
                   S1[j] += S[k];
            }
            sum += S1[j];
        }
        sum = 1.0/(sum+0.00001);
        for(j=0;j<num; ++j)
        {
            S[j] = S1[j]*sum;
        }
    }

    s0 = 0.0;

    // sort the S
    // the sorting part can be replaced by a min-heap
    std::vector<int> y(S.size());
    std::iota(y.begin(), y.end(), 0);
    auto comparator = [&S](int a, int b){ return S[a] > S[b]; };
    std::sort(y.begin(), y.end(), comparator);

    vector<tuple<float, int, int>>  new_corr;
    vector<int> selected_ind;
    short ind;
    for(i=0; i<num; ++i)
    {
        ind = y[i];
        if(S[ind]<0.001)
            break;
        if((flag_latent[get<1>(corr[ind])] == 1) || (flag_rolled[get<2>(corr[ind])] == 1))
            continue;

          if(i==0)
        {
            selected_ind.push_back(ind);
            new_corr.push_back(make_tuple(get<0>(corr[ind]),get<1>(corr[ind]),get<2>(corr[ind])));

            flag_latent[get<1>(corr[ind])] = 1;
            flag_rolled[get<2>(corr[ind])] = 1;
        }
        else
        {
            int found =0;
            for(j=0;j<selected_ind.size(); ++j)
            {
                if(!H[ind*num+selected_ind[j]])
                {
                    found = 1;
                    break;
                }
            }
            if(found==0)
            {
                selected_ind.push_back(ind);
                new_corr.push_back(make_tuple(get<0>(corr[ind]),get<1>(corr[ind]),get<2>(corr[ind])));

                flag_latent[get<1>(corr[ind])] = 1;
                flag_rolled[get<2>(corr[ind])] = 1;
            }

        }
    }

    return new_corr;
}

float Matcher::adjust_angle(float angle)
const
{
    if(angle>PI)
        angle -= 2*PI;
    else if (angle<-PI)
    {
        angle += 2*PI;
    }
    return angle;
}
}


// This function is used to conduct the Polyhedron Projection using Double Description Method

#include "RobotInfo.h"
#include "CommonHeader.h"
#include <setoper.h>
#include <cdd.h>
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <math.h>
#include <string.h>
#include <bits/stdc++.h>
#include <sstream>
#include <iterator>
#include <boost/algorithm/string.hpp>

static string cone_file_name = "CCV";

static double eps = 1e-8;

static void String2Coordinates(const string& str_line, std::vector<Vector3>& ConeVertices)
{
  // This function is used to transform string into coordinates
  std::istringstream buf(str_line);
  std::istream_iterator<std::string> beg(buf), end;
  std::vector<std::string> tokens(beg, end); // done!
  int Type = std::stoi (tokens[0]);
  switch (Type)
  {
    case 1:
    {
      double x = std::stod(tokens[1]);
      double y = std::stod(tokens[2]);
      Vector3 ConeVertex(x, y, 0.0);
      ConeVertices.push_back(ConeVertex);
    }
    break;
    default:
    break;
  }
}

static bool INEEmptyValidator()
{
  // This function is used to make sure that if the cone_file_name + ".ext" is empty then no need to read it anymore.
  string cone_file_path = cone_file_name + ".ext";
  ifstream ConeInfofile (cone_file_path);
  string str_line;
  string keyword="region found empty";
  bool ValidFlag = true;
  if (ConeInfofile.is_open())
  {
    while (getline (ConeInfofile, str_line))
    {
      int EmptyPos = str_line.find(keyword);
      if (EmptyPos != string::npos)
      {
        ValidFlag =  false;
      }
    }
    ConeInfofile.close();
  }
  else std::cerr << "Unable to open" + cone_file_name + ".ext for Empty Validation"<<std::endl;
  return ValidFlag;
}

static bool INEZeroValidator()
{
  // This function is used to make sure that if the cone_file_name + ".ext" is empty then no need to read it anymore.
  string cone_file_path = cone_file_name + ".ext";
  ifstream ConeInfofile (cone_file_path);
  string str_line;
  string keyword="begin";
  bool ValidFlag = true;
  bool StartFlag = false;
  if (ConeInfofile.is_open())
  {
    while (getline (ConeInfofile, str_line))
    {
      switch (StartFlag)
      {
        case true:
        {
          vector<string> strs;
          boost::split(strs,str_line, boost::is_any_of("  "));
          if(strs[0]=="0")
          {
            ValidFlag = false;
          }
          StartFlag = false;
        }
        break;
        default:
        {
        }
        break;
      }
      int ZeroPos = str_line.find(keyword);
      if (ZeroPos != string::npos)
      {
        StartFlag =  true;;
      }
    }
    ConeInfofile.close();
  }
  else std::cerr << "Unable to open" + cone_file_name + ".ext for Zero Validation"<<std::endl;
  return ValidFlag;
}

static std::vector<Vector3> INEReader()
{
  std::vector<Vector3>  ConeVertices;

  bool EmptyFlag = INEEmptyValidator();
  bool ZeroFlag = INEZeroValidator();

  switch (EmptyFlag)
  {
    case false:
    {
      std::printf("INE File Empty Failure!\n");
      return ConeVertices;
    }
    break;
    default:
    {

    }
    break;
  }

  switch (ZeroFlag)
  {
    case false:
    {
      std::printf("INE File Zero Failure!\n");
      return ConeVertices;
    }
    break;
    default:
    {

    }
    break;
  }

  // This function is used to read in the cone file.
  string cone_file_path = cone_file_name + ".ext";
  ifstream ConeInfofile (cone_file_path);

  string str_line;
  string start_keyword="real";
  string end_keyword="end";

  int valid_flag = 0;
  if (ConeInfofile.is_open())
  {
    while (getline (ConeInfofile, str_line))
    {
      int start_pos = str_line.find(start_keyword);
      int end_pos = str_line.find(end_keyword);
      if(end_pos != string::npos)
      {
        valid_flag = 0;
      }
      switch (valid_flag)
      {
        case 1:
        {
          String2Coordinates(str_line, ConeVertices);
        }
        break;
        default:
        break;
      }
      if (start_pos != string::npos)
      {
        valid_flag = 1;
      }
    }
    ConeInfofile.close();
  }
  else std::cerr << "Unable to open" + cone_file_name + ".ext"<<std::endl;

  return ConeVertices;
}

static void INEWriter(const std::vector<Vector3>& ConeInequalities)
{
  // It has been noticed that based on the pure cddlib, the generated vertices are not correct.
  // As a result, a file write/read process has to be conducted.
  string cone_file_path = cone_file_name + ".ine";
  ofstream ConeInfofile (cone_file_path);
  /*
      Header
  */
  ConeInfofile<<"H-representation"<<endl;
  ConeInfofile<<"begin"<<endl;
  ConeInfofile<<" "<<std::to_string(ConeInequalities.size())<<" 3 real"<<endl;

  for (int i = 0; i < ConeInequalities.size(); i++)
  {
    Vector3 ConeInequality = ConeInequalities[i];
    ConeInfofile<<"\t"<<std::to_string(ConeInequality.x)<<" "<<std::to_string(ConeInequality.y)<<" "<<std::to_string(ConeInequality.z)<<endl;
  }
  ConeInfofile<<"end"<<endl;
  ConeInfofile.close();
}

static Matrix PolyhedralProjection(const std::vector<Vector3> & ActContactPositions, const std::vector<Vector3> & ConeUnit, const int & EdgeNo, int &FailureFlag)
{
  // This function is used to project the feasible contact wrench cone
  /*
    For each of the vector in ConeUnit, a new projected Conic Vector should be computed.
  */
  dd_PolyhedraPtr poly;
  dd_MatrixPtr A, B, G;
  dd_rowrange m;
  dd_colrange d;
  dd_ErrorType err;

  FailureFlag = 0;

  dd_set_global_constants();                  /* First, this must be called to use cddlib. */

  m = ConeUnit.size();
  d = 6 + 1;        // Here 6 means the force and momentum balance

  A = dd_CreateMatrix(m,d);
  for (int i = 0; i < ConeUnit.size(); i++)
  {
    for (int j = 0; j < d; j++)
    {
      dd_set_d(A->matrix[i][j], 0.0);
    }
    int EleIndex = floor(i/EdgeNo);
    Vector3 ContactPos = ActContactPositions[EleIndex];
    // Give the value into A matrix.
    Vector3 ConeUnit_i = ConeUnit[i] - ContactPos;
    dd_set_d(A->matrix[i][1], ConeUnit_i.x);
    dd_set_d(A->matrix[i][2], ConeUnit_i.y);
    dd_set_d(A->matrix[i][3], ConeUnit_i.z);

    Vector3 pcrossf = cross(ContactPos, ConeUnit_i);
    dd_set_d(A->matrix[i][4], pcrossf.x);
    dd_set_d(A->matrix[i][5], pcrossf.y);
    dd_set_d(A->matrix[i][6], pcrossf.z);
  }

  A->representation = dd_Generator;
  poly=dd_DDMatrix2Poly(A, &err);

  G=dd_CopyGenerators(poly);
  // dd_WriteMatrix(stdout,G);   printf("\n");
  B=dd_CopyInequalities(poly);
  // dd_WriteMatrix(stdout,B);   printf("\n");

  Matrix H;
  if (err!=dd_NoError)
  {
    // printf("PolyhedralProjection: Given Matrix is not compatiable!\n");
    FailureFlag = 1;
    dd_FreeMatrix(A);
    dd_FreeMatrix(B);
    dd_FreeMatrix(G);
    dd_FreePolyhedra(poly);

    dd_free_global_constants();  /* At the end, this must be called. */

    return H;
  }

  H.resize(B->rowsize, B->colsize-1);

  for (int i = 0; i < B->rowsize; i++)
  {
    for (int j = 0; j < B->colsize-1; j++)
    {
      H(i,j) = *B->matrix[i][j+1];
    }
  }
  dd_FreeMatrix(A);
  dd_FreeMatrix(B);
  dd_FreeMatrix(G);
  dd_FreePolyhedra(poly);

  dd_free_global_constants();  /* At the end, this must be called. */
  H.setNegative(H);
  return H;
}

static std::vector<Vector3> CentroidalCone(const Matrix& H, const Vector3& COMPos, const Vector3& COMVel)
{
  // This function is used to calcualte the centroidal Cone based on the H matrix
  Vector3 g(0.0, 0.0, -9.81);
  // According to the Zero-Step Capturability Paper there are three terms to be computed.
  Vector a_lhs(6);
  Vector3 v = COMVel;
  v.setNormalized(v);

  // For term a,
  a_lhs[0] = 1.0;
  a_lhs[1] = 1.0;
  a_lhs[2] = 1.0;
  Vector3 mczerocrossv = 1.0 * cross(COMPos, v);
  a_lhs[3] = mczerocrossv.x;
  a_lhs[4] = mczerocrossv.y;
  a_lhs[5] = mczerocrossv.z;

  // For term b,
  Vector b_lhs(6);
  Vector3 mgcrossv = 1.0 * cross(g, v);
  b_lhs[0] = 0.0;
  b_lhs[1] = 0.0;
  b_lhs[2] = 0.0;
  b_lhs[3] = mgcrossv.x;
  b_lhs[4] = mgcrossv.y;
  b_lhs[5] = mgcrossv.z;

  // For term d,
  Vector d_rhs(6);
  Vector3 mczerocrossg = 1.0 * cross(COMPos, g);
  d_rhs[0] = 0.0;
  d_rhs[1] = 0.0;
  d_rhs[2] = 1.0 * -9.81;
  d_rhs[3] = mczerocrossg.x;
  d_rhs[4] = mczerocrossg.y;
  d_rhs[5] = mczerocrossg.z;

  Vector Ha, Hb, Hd;
  H.mul(a_lhs,Ha);
  H.mul(b_lhs,Hb);
  H.mul(d_rhs,Hd);

  /*
    For each of the vector in ConeUnit, a new projected Conic Vector should be computed.
  */
  dd_PolyhedraPtr poly;
  dd_MatrixPtr A, B, G;
  dd_rowrange m;
  dd_colrange d;
  dd_ErrorType err;

  dd_set_global_constants();                  /* First, this must be called to use cddlib. */

  m = Ha.size();
  d = 2 + 1;

  A = dd_CreateMatrix(m,d);
  for (int i = 0; i < m; i++)
  {
    dd_set_d(A->matrix[i][0], Hd[i]);
    dd_set_d(A->matrix[i][1], -1.0 * Hb[i]);
    dd_set_d(A->matrix[i][2], -1.0 * Ha[i]);
  }
  A->representation = dd_Inequality;
  poly=dd_DDMatrix2Poly(A, &err);  /* compute the second (generator) representation */
  std::vector<Vector3> ConeVertices;
  if (err!=dd_NoError)
  {
    printf("CentroidalCone: Given Matrix is not compatiable!\n");
    dd_FreeMatrix(A);
    dd_FreePolyhedra(poly);
    dd_free_global_constants();  /* At the end, this must be called. */
    return ConeVertices;
  }
  G=dd_CopyGenerators(poly);
  // dd_WriteMatrix(stdout,G);   printf("\n");
  B=dd_CopyInequalities(poly);
  // dd_WriteMatrix(stdout,B);   printf("\n");

  std::vector<Vector3> ConeInequalities;
  ConeInequalities.reserve(B->rowsize);
  for (int i = 0; i < B->rowsize; i++)
  {
    Vector3 ConeInequality(*B->matrix[i][0], *B->matrix[i][1], *B->matrix[i][2]);
    ConeInequalities.push_back(ConeInequality);
  }
  dd_FreeMatrix(A);
  dd_FreeMatrix(B);
  dd_FreeMatrix(G);
  dd_FreePolyhedra(poly);

  dd_free_global_constants();  /* At the end, this must be called. */

  string del_command = "rm -f " + cone_file_name + ".*";
  const char *delete_command = del_command.c_str();
  std::system(delete_command);

  switch (ConeInequalities.size())
  {
    case 0:
    {
      return ConeVertices;
    }
    break;
    case 1:
    {
      return ConeVertices;
    }
    break;
    case 2:
    {
      return ConeVertices;
    }
    break;
    case 3:
    {
      return ConeVertices;
    }
    break;
    default:
    {
    }
    break;
  }

  INEWriter(ConeInequalities);

  string cone_command = "./../../../cddplus/cddf+ " + cone_file_name + ".ine";

  // Convert string to const char * as system requires
  // parameter of type const char *
  const char *command = cone_command.c_str();
  std::system(command);

  ConeVertices = INEReader();

  // string del_command = "rm -f " + cone_file_name + ".*";
  // const char *delete_command = del_command.c_str();
  // std::system(delete_command);

  return ConeVertices;
}

static std::pair<double, double> IntegratorLDS(const double & alpha, const double & alphadot, double & alphaddot, const double & beta, const double & gamma, const double & alphaupp)
{
  // This function is used to integrate the system dynamics according to the a-addot figure
  /*
  Here: alphaddot = beta + gamma * alpha,         alphalow<=alpha<=alphaupp
  */
  std::pair <double, double> ResPair;

  if (gamma>eps)
  {
    double omega = sqrt(gamma);
    double arg = -1.0 * alphadot/(omega * (alpha + beta/gamma));
    if((arg * arg)<=1.0)
    {
      double t = 1.0/omega * atanh(arg);
      double alpha_t = cosh(omega * t) * alpha + sinh(omega * t) * alphadot/omega + (cosh(omega * t) - 1.0) * (beta/gamma);
      if(alpha_t<=alphaupp)
      {
        alphaddot = beta + (alpha_t - alpha) * gamma;
        ResPair = std::make_pair(alpha_t, 0.0);
        return ResPair;
      }
    }
    double A = alpha + beta/gamma;
    double B = alphadot/omega;
    double C = -alphaupp - beta/gamma;
    double InLog = (sqrt(B * B + C * C - A * A) - C)/(A + B);
    double t = 1.0/omega * log(InLog);
    double alpha_t = cosh(omega * t) * alpha + sinh(omega*t)/omega * alphadot + (cosh(omega * t)- 1.0) * beta/gamma;
    double alphadot_t = omega * sinh(omega * t) * alpha + cosh(omega*t) * alphadot + omega * sinh(omega * t) * beta/gamma;
    alphaddot = beta + (alpha_t - alpha) * gamma;
    ResPair = std::make_pair(alphaupp, alphadot_t);
    return ResPair;
  }
  else
  {
    if (gamma<-eps)
    {
      // std::cout<<"Needs to be careful since gamma is negative!"<<std::endl;
      // system("pause");
      std::complex<double> omega(0.0, sqrt(-1.0 * gamma));                          // This is an imaginary number.
      std::complex<double> arg = -1.0 * alphadot/(omega * (alpha + beta/gamma));    // This is an imaginary number.
      std::complex<double> t = 1.0/omega * atanh(arg);                              // This is a real number.
      std::complex<double> alpha_t = cosh(omega * t) + sinh(omega * t) * alphadot/omega + (cosh(omega * t)-1.0) * (beta/gamma); // This is also a real number.
      double alpha_t_real = real(alpha_t);
      if(alpha_t_real <= alphaupp)
      {
        alphaddot = beta + (alpha_t_real - alpha) * gamma;
        ResPair = std::make_pair(alpha_t_real, 0.0);
        return ResPair;
      }
      std::complex<double> A = alpha + beta/gamma;
      std::complex<double> B = alphadot/omega;
      std::complex<double> C = -alphaupp - beta/gamma;
      std::complex<double> InLog = (-sqrt(B * B + C * C - A * A) - C)/(A + B);
      t = 1.0/omega * log(InLog);
      std::complex<double> alphadot_t_complex = omega * sinh(omega * t) * alpha + cosh(omega*t) * alphadot + omega * sinh(omega * t) * beta/gamma;
      std::complex<double> alpha_t_complex = cosh(omega * t) * alpha + sinh(omega*t)/omega * alphadot + (cosh(omega * t)- 1.0) * beta/gamma;
      alpha_t_real = real(alpha_t_complex);
      double alphadot_t = real(alphadot_t_complex);
      alphadot_t = sqrt(alphadot_t * alphadot_t);
      alphaddot = beta + (alpha_t_real - alpha) * gamma;
      ResPair = std::make_pair(alphaupp, alphadot_t);
      return ResPair;
    }
    else
    {
      double alpha_t = alpha - 0.5 * alphadot * alphadot/beta;
      if(alpha_t<=alphaupp)
      {
        alphaddot = beta + (alpha_t - alpha) * gamma;
        ResPair = std::make_pair(alpha_t, 0.0);
        return ResPair;
      }
      double t = (-alphadot + sqrt(alphadot * alphadot - 2.0 * beta * (alpha - alphaupp)))/beta;
      alpha_t = alphaupp;
      double alphadot_t = alphadot + t * beta;
      alphaddot = beta + (alpha_t - alpha) * gamma;
      ResPair = std::make_pair(alpha_t, alphadot_t);
      return ResPair;
    }
  }
  return ResPair;
}

static double LinearInter(const Vector2& LeftPt, const Vector2& RightPt, const double& x_new)
{
  // This function is used to calculate the value for y_new based on the LeftPt and Rightpt.
  double x_offset = x_new - LeftPt.x;
  double k = (RightPt.y - LeftPt.y)/(RightPt.x - LeftPt.x);
  double y_new = LeftPt.y + k * x_offset;
  return y_new;
}

static void AccLinearCoeff(const double & Alpha, const std::vector<Vector2> & Path, double & Beta, double & Gamma, double & AlphaUpp, const double & AlphaFeasMax)
{
  // This function is used to find out the linear coefficients for acceleration function.
  for (int i = 0; i < Path.size()-1; i++)
  {
    if((Alpha>=Path[i].x)&&(Alpha<=Path[i+1].x))
    {
      // This is used to bound the range of alpha.
      Vector2 LeftPt = Path[i];
      Vector2 RightPt = Path[i+1];

      Gamma = (RightPt.y - LeftPt.y)/(RightPt.x - LeftPt.x);
      Beta = LeftPt.y - Gamma * LeftPt.x;

      if(RightPt.y<0)
      {
        AlphaUpp = RightPt.x;
      }
      else
      {
        AlphaUpp = LeftPt.x - LeftPt.y/Gamma;
      }
    }
  }
  if(AlphaUpp>=AlphaFeasMax)
  {
    AlphaUpp = AlphaFeasMax;
  }
}

static int LinearCoeff(const double & alpha, const FacetInfo & FacetObj, const int & Flag, double & beta_i, double & gamma_i, double & alphaupp_i)
{
  // This function is used to calculate the linear coefficient: alphaddot = beta_i + gamma_i * alpha.
  std::vector<int> EdgeIndices;
  std::vector<Vector2> LeftPts, RightPts;
  std::vector<double> InterAlphaddot;
  for (int i = 0; i < FacetObj.FacetEdges.size(); i++)
  {
    double alphaleft = FacetObj.FacetEdges[i].first.x - alpha;
    double alpharight = FacetObj.FacetEdges[i].second.x - alpha;
    if(((alphaleft<=0)&&(alpharight>=0))||((alphaleft >=0)&&(alpharight<=0)))
    {
      // Then we switch the order of first, second to make sure that the second point is on the right.
      Vector2 LeftPt, RightPt;
      if(FacetObj.FacetEdges[i].second.x>FacetObj.FacetEdges[i].first.x)
      {
        LeftPt.x = FacetObj.FacetEdges[i].first.x;
        LeftPt.y = FacetObj.FacetEdges[i].first.y;

        RightPt.x = FacetObj.FacetEdges[i].second.x;
        RightPt.y = FacetObj.FacetEdges[i].second.y;
      }
      else
      {
        LeftPt.x = FacetObj.FacetEdges[i].second.x;
        LeftPt.y = FacetObj.FacetEdges[i].second.y;

        RightPt.x = FacetObj.FacetEdges[i].first.x;
        RightPt.y = FacetObj.FacetEdges[i].first.y;
      }
      if((RightPt.x - alpha)>0)
      {
        // This means that we only choose the point on the right side
        EdgeIndices.push_back(i);
        LeftPts.push_back(LeftPt);
        RightPts.push_back(RightPt);
        double alphaddot = LinearInter(LeftPt, RightPt, alpha);
        InterAlphaddot.push_back(alphaddot);
      }
    }
  }
  if(InterAlphaddot.size() ==0)
  {
    // This means that the current region does not contain the origin to search for the feasible solution.
    return 2;
  }

  // In this case, we should sort alpha values according to the linear splines.
  int AlphaIndex;
  switch (Flag)
  {
    case 0:
    {
      AlphaIndex = std::max_element(InterAlphaddot.begin(), InterAlphaddot.end()) - InterAlphaddot.begin();
    }
    break;
    case 1:
    {
      AlphaIndex = std::min_element(InterAlphaddot.begin(), InterAlphaddot.end()) - InterAlphaddot.begin();
    }
    break;
    default:
    break;
  }
  Vector2 LeftPt = LeftPts[AlphaIndex];
  Vector2 RightPt = RightPts[AlphaIndex];

  gamma_i = (RightPt.y - LeftPt.y)/(RightPt.x - LeftPt.x);
  beta_i = InterAlphaddot[AlphaIndex];
  if(RightPt.y<0)
  {
    alphaupp_i = RightPt.x;
  }
  else
  {
    alphaupp_i = LeftPt.x - LeftPt.y/gamma_i;
  }
  return 1;
}

static Vector2 AppendPoint(const Vector2 & Point, const FacetInfo & FacetObj, bool & PointAdditionFlag)
{
  Vector2 NextPoint;
  for (int i = 0; i < FacetObj.FacetEdges.size(); i++)
  {
    double FirstCom_x = FacetObj.FacetEdges[i].first.x - Point.x;
    double FirstCom_y = FacetObj.FacetEdges[i].first.y - Point.y;
    double FirstComVal = FirstCom_x * FirstCom_x + FirstCom_y * FirstCom_y;

    double SecondCom_x = FacetObj.FacetEdges[i].second.x - Point.x;
    double SecondCom_y = FacetObj.FacetEdges[i].second.y - Point.y;
    double SecondComVal = SecondCom_x * SecondCom_x + SecondCom_y * SecondCom_y;

    if(FirstComVal<eps)
    {
      // This means that First Point is the Point
      if(SecondCom_x>0)
      {
        NextPoint.x = FacetObj.FacetEdges[i].second.x;
        NextPoint.y = FacetObj.FacetEdges[i].second.y;

        PointAdditionFlag = true;
        return NextPoint;
      }
    }

    if(SecondComVal<eps)
    {
      if(FirstCom_x>0)
      {
        NextPoint.x = FacetObj.FacetEdges[i].first.x;
        NextPoint.y = FacetObj.FacetEdges[i].first.y;

        PointAdditionFlag = true;
        return NextPoint;
      }
    }
  }
  PointAdditionFlag = false;
  return NextPoint;
}

static void FindPathToTheEnd(std::vector<Vector2> & Path, const FacetInfo & FacetObj)
{
  bool PointAdditionFlag = true;
  while(PointAdditionFlag)
  {
    // try to find the path connecting to the last point from path
    Vector2 NextPoint;
    NextPoint = AppendPoint(Path[Path.size()-1], FacetObj, PointAdditionFlag);
    switch (PointAdditionFlag)
    {
      case true:
      {
        Path.push_back(NextPoint);
      }
      break;
      default:
      {
      }
      break;
    }
  }
}

static bool PathToTheEnd(const FacetInfo & FacetObj, std::vector<Vector2> & UpperPath, std::vector<Vector2> & LowerPath)
{
  // This function is used to find the path from 0 to the end.
  bool PathFlag = false;
  std::vector<Vector2> StartPointsLeft, StartPointsRight;
  std::vector<int> InterSegIndices;
  for (int i = 0; i < FacetObj.FacetEdges.size(); i++)
  {
    // The first element is the alpha while the second element is the alpha ddot.
    double Sign = FacetObj.FacetEdges[i].first.x * FacetObj.FacetEdges[i].second.x;
    if (Sign<0)
    {
      double delta_y = FacetObj.FacetEdges[i].first.y - FacetObj.FacetEdges[i].second.y;
      double delta_x = FacetObj.FacetEdges[i].first.x - FacetObj.FacetEdges[i].second.x;
      double k = delta_y/delta_x;

      double StartPointLeft_y = FacetObj.FacetEdges[i].first.y - k * FacetObj.FacetEdges[i].first.x;
      Vector2 StartPointLeft(0.0, StartPointLeft_y);
      StartPointsLeft.push_back(StartPointLeft);
      if(FacetObj.FacetEdges[i].first.x>FacetObj.FacetEdges[i].second.x)
      {
        Vector2 StartPointRight(FacetObj.FacetEdges[i].first.x, FacetObj.FacetEdges[i].first.y);
        StartPointsRight.push_back(StartPointRight);
      }
      else
      {
        Vector2 StartPointRight(FacetObj.FacetEdges[i].second.x, FacetObj.FacetEdges[i].second.y);
        StartPointsRight.push_back(StartPointRight);
      }
      InterSegIndices.push_back(i);
    }
    else
    {
      if(Sign == 0)
      {
        double FirstAtZero = FacetObj.FacetEdges[i].first.x * FacetObj.FacetEdges[i].first.x;
        if(FirstAtZero<eps)
        {
          if(FacetObj.FacetEdges[i].first.x<FacetObj.FacetEdges[i].second.x)
          {
            Vector2 StartPointLeft(FacetObj.FacetEdges[i].first.x, FacetObj.FacetEdges[i].first.y);
            Vector2 StartPointRight(FacetObj.FacetEdges[i].second.x, FacetObj.FacetEdges[i].second.y);
            StartPointsLeft.push_back(StartPointLeft);
            StartPointsRight.push_back(StartPointRight);
            InterSegIndices.push_back(i);
          }
        }
        else
        {
          if(FacetObj.FacetEdges[i].first.x>FacetObj.FacetEdges[i].second.x)
          {
            Vector2 StartPointLeft(FacetObj.FacetEdges[i].second.x, FacetObj.FacetEdges[i].second.y);
            Vector2 StartPointRight(FacetObj.FacetEdges[i].first.x, FacetObj.FacetEdges[i].first.y);
            StartPointsLeft.push_back(StartPointLeft);
            StartPointsRight.push_back(StartPointRight);
            InterSegIndices.push_back(i);
          }
        }
      }
    }
  }

  switch (StartPointsLeft.size())
  {
    case 0:
    {
      return false;
    }
    break;
    case 3:
    {
      std::cerr<<"Intersections cannot have more than 2 segments!"<<endl;
      return false;
    }
    break;
    default:
    {
    }
    break;
  }

  // Now the job is to figure out which one is at top and which one is at bottom.
  if(StartPointsLeft[0].y > StartPointsLeft[1].y)
  {
    UpperPath.push_back(StartPointsLeft[0]);
    UpperPath.push_back(StartPointsRight[0]);

    LowerPath.push_back(StartPointsLeft[1]);
    LowerPath.push_back(StartPointsRight[1]);
  }
  else
  {
    UpperPath.push_back(StartPointsLeft[1]);
    UpperPath.push_back(StartPointsRight[1]);

    LowerPath.push_back(StartPointsLeft[0]);
    LowerPath.push_back(StartPointsRight[0]);
  }

  // Alright, let's find the path from this point to the most right end.
   FindPathToTheEnd(UpperPath, FacetObj);
   FindPathToTheEnd(LowerPath, FacetObj);

  return true;
}

static bool TooFastCaseValidation(const double & AlphadotInit, const std::vector<Vector2> & UppPath, const double & AlphaFeasMin, const double & AlphaFeasMax)
{
  // This main purpose of this function is to validate whether robot can reach feasible alpha region.
  // This computation will be terminated if any of these two conditions has been triggered.
  // 1. alpha reaches feasible region.
  // 2. alphadot has been declined to zero.
  double Alpha = 0.0;
  double Alphadot = AlphadotInit;
  double Beta = 0.0;
  double Gamma = 0.0;
  double AlphaUpp = 0.0;
  double Alphaddot = 0.0;

  int IterLimit = 20;
  for (int i = 0; i < IterLimit; i++)
  {
    AccLinearCoeff(Alpha, UppPath, Beta, Gamma, AlphaUpp, AlphaFeasMax);
    std::pair<double, double> AlphaNAlphadot = IntegratorLDS(Alpha, Alphadot, Alphaddot, Beta, Gamma, AlphaUpp);
    Alpha = AlphaNAlphadot.first;
    Alphadot = AlphaNAlphadot.second;

    // Condition 1
    if(Alpha>=AlphaFeasMin)
    {
      return true;
    }

    // Condition 2
    if((Alphadot==0.0)&&(Alpha<AlphaFeasMin))
    {
      return false;
    }
  }
  return false;
}

static bool ZSCEvaluation(const double & AlphadotInit, const std::vector<Vector2> & LowPath, const std::vector<Vector2> & UppPath, const double & AlphaFeasMin, const double & AlphaFeasMax, const double & AlphaMax)
{
  // This function is used to conduct the evaluation of ZSC.
  int MaxIter = 100;
  int CurIter = 0;
  double Alpha = 0.0;
  double Alphadot = AlphadotInit;
  double Alphaddot = 0.0;

  double Beta = 0.0;
  double Gamma = 0.0;
  double AlphaUpp = 0.0;          // This is the Upper Bound for a certain integration.

  bool StabilizedFlag = false;
  while (CurIter<MaxIter)
  {
    if((Alphadot>-eps)&&(Alphadot<eps))
    {
      // In this case, Alphadot vanishes!
      if((Alpha=AlphaFeasMin)&&(Alpha<=AlphaFeasMax))
      {
        // C3.1
        // Tested!
        StabilizedFlag = true;
        // std::printf("C3.1\n");
        return StabilizedFlag;
      }
      if(Alpha<AlphaFeasMin)
      {
        // C3.2
        // This is an annoying case.
        StabilizedFlag = TooFastCaseValidation(AlphadotInit, UppPath, AlphaFeasMin, AlphaFeasMax);
        // std::printf("C3.2\n");
        return StabilizedFlag;
      }
      if(Alpha>AlphaFeasMax)
      {
        // C3.3
        StabilizedFlag = false;
        // std::printf("C3.3\n");
        return StabilizedFlag;
      }
    }
    else
    {
      // In this case, robot's centroidal velocity is not zero.
      if(Alpha>=AlphaFeasMax)
      {
        // C1: Velocity is too large to be compensated!
        // Tested!
        StabilizedFlag = false;
        // std::printf("C1\n");
        return StabilizedFlag;
      }
      if(Alpha>=AlphaMax)
      {
        // C2: Position arrives at the position where constraints will be violated!
        // Tested!
        StabilizedFlag = false;
        // std::printf("C2\n");
        return StabilizedFlag;
      }
    }
    AccLinearCoeff(Alpha, LowPath, Beta, Gamma, AlphaUpp, AlphaFeasMax);
    std::pair<double, double> AlphaNAlphadot = IntegratorLDS(Alpha, Alphadot, Alphaddot, Beta, Gamma, AlphaUpp);
    Alpha = AlphaNAlphadot.first;
    Alphadot = AlphaNAlphadot.second;

    // std::printf("Alpha: %f and Alphadot: %f\n", Alpha, Alphadot);
    CurIter = CurIter + 1;
  }
  return StabilizedFlag;
}

static bool ZSCInner(const std::vector<Vector3> & ActContactPositions, const std::vector<Vector3> & ConeUnit, const int & EdgeNo, const Vector3& COMPos, const Vector3& COMVel)
{
  int HFailureFlag;
  Matrix H = PolyhedralProjection(ActContactPositions, ConeUnit, EdgeNo, HFailureFlag);
  switch (HFailureFlag)
  {
    case 1:
    {
      // This means that Zero-step capturability method does not work
      return false;
    }
    break;
    default:
    break;
  }

  std::vector<Vector3> ConeVertices = CentroidalCone(H, COMPos, COMVel);
  if(ConeVertices.size()<3)
  {
    // In this case, there is no need to conduct further computation.
    return false;
  }

  int CollinearFlag = CollinearTest(ActContactPositions);
  switch (CollinearFlag)
  {
    case 1:
    {
      return false;
    }
    break;
    default:
    break;
  }

  int FacetFlag = 0;
  FacetInfo FacetObj = FlatContactHullGeneration(ConeVertices, FacetFlag);
  switch (FacetFlag)
  {
    case 0:
    {
      return false;
    }
    break;
    default:
    break;
  }

  // Now let's compute the maximum deaccelerated velocity.
  bool StabilizedFlag = true;
  /*
    The first job is to compute the intersection where the acceleration vanishes.
  */
  std::vector<double> AlphaFeasible;
  std::vector<double> AlphaTotal(FacetObj.FacetEdges.size());
  for (int i = 0; i < FacetObj.FacetEdges.size(); i++)
  {
    // The first element is the alpha while the second element is the alpha ddot.
    double Sign = FacetObj.FacetEdges[i].first.y * FacetObj.FacetEdges[i].second.y;
    if (Sign<0)
    {
      double delta_y = FacetObj.FacetEdges[i].first.y - FacetObj.FacetEdges[i].second.y;
      double delta_x = FacetObj.FacetEdges[i].first.x - FacetObj.FacetEdges[i].second.x;
      double k = delta_y/delta_x;
      double Alpha = FacetObj.FacetEdges[i].first.x - FacetObj.FacetEdges[i].first.y/k;
      AlphaFeasible.push_back(Alpha);
    }
    else
    {
      if(Sign == 0)
      {
        double FirstAtZero = FacetObj.FacetEdges[i].first.y * FacetObj.FacetEdges[i].first.y;
        if(FirstAtZero<eps)
        {
          AlphaFeasible.push_back(FacetObj.FacetEdges[i].first.x);
        }
        else
        {
          AlphaFeasible.push_back(FacetObj.FacetEdges[i].second.x);
        }
      }
    }
    AlphaTotal[i] = FacetObj.FacetEdges[i].first.x;
  }
  switch (AlphaFeasible.size())
  {
    case 0:
    {
      return false;
    }
    break;
    default:
    break;
  }

  double AlphaFeasMin = *std::min_element(AlphaFeasible.begin(), AlphaFeasible.end());
  double AlphaFeasMax = *std::max_element(AlphaFeasible.begin(), AlphaFeasible.end());

  double AlphaTotalMin = *std::min_element(AlphaTotal.begin(), AlphaTotal.end());
  double AlphaTotalMax = *std::max_element(AlphaTotal.begin(), AlphaTotal.end());

  std::vector<Vector2> UpperPath, LowerPath;
  PathToTheEnd(FacetObj, UpperPath, LowerPath);

  switch (UpperPath.size())
  {
    case 0:
    {
      return false;
    }
    break;
    case 1:
    {
      return false;
    }
    break;
    default:
    {
    }
    break;
  }

  switch (LowerPath.size())
  {
    case 0:
    {
      return false;
    }
    break;
    case 1:
    {
      return false;
    }
    break;
    default:
    {
    }
    break;
  }

  double AlphadotInit = sqrt(COMVel.x * COMVel.x + COMVel.y * COMVel.y + COMVel.z * COMVel.z);

  StabilizedFlag = ZSCEvaluation(AlphadotInit, LowerPath, UpperPath, AlphaFeasMin, AlphaFeasMax, AlphaTotalMax);

  return StabilizedFlag;
}

double ZeroStepCapturabilityGenerator(const std::vector<Vector3> & ActContactPositions, const std::vector<Vector3> & ConeUnit, const int & EdgeNo, const Vector3& COMPos, const Vector3& COMVel)
{
  // This function is used to generate the solution for zero capturability failure metric.
  bool StabilizedFlag;
  try
  {
    StabilizedFlag = ZSCInner(ActContactPositions, ConeUnit, EdgeNo, COMPos, COMVel);
  }
  catch(...)
  {
    StabilizedFlag = false;
  }
  double ZSCObj = 1.0;
  switch (StabilizedFlag)
  {
    case true:
    {
      // This means that the robot can be stabilized using this method.
      ZSCObj = 0.0;
    }
    break;
    default:
    {
      // This means that the robot cannot be stabilized using this metric.
      ZSCObj = 1.0;
    }
    break;
  }
  return ZSCObj;
}