#include <stdlib.h>
#include <assert.h>
#include <math.h>
#include <complex.h>
#include <time.h>
#include <NTL/ZZ.h>
#include <NTL/ZZX.h>
#include <NTL/mat_ZZ.h>
#include <gmp.h>
#include "blake2.h"
#include "blake2-kat.h"

#include "Sampling.h"
#include "params.h"
#include "FFT.h"
#include "Random.h"
#include "Algebra.h"
#define bitRead(value, bit) (((value) >> (bit)) & 0x01)
using namespace std;
using namespace NTL;

const ZZX phi = Cyclo();

//==============================================================================
//Generates from parameters N and q :
// - a public key : polynomial h
// - a private key : polynomials f,g,F,G
//==============================================================================
void Keygen(ZZ_pX& PublicKey, ZZX* PrivateKey)
{
    ZZ SqNorm;
    ZZX f,g,F,G;

    SqNorm = conv<ZZ>(1.36*q0/2);

    GenerateBasis(f, g, F, G, SqNorm);

	//added by me ============

	//added by me ==============

    PrivateKey[0] = f;
    PrivateKey[1] = g;
    PrivateKey[2] = F;
    PrivateKey[3] = G;

    for(unsigned int i=0; i<4; i++)
    {
            PrivateKey[i].SetLength(N0);
    }

    PublicKey = Quotient(f, g);
}

void myKeygen(ZZ_pX& PublicKey, ZZX* PrivateKey)
{
    ZZ SqNorm;
    ZZX f,g,F,G;

    SqNorm = conv<ZZ>(1.36*q0/2);

    GenerateBasis(f, g, F, G, SqNorm);

	//added by me ============

	//added by me ==============

    PrivateKey[0] = f;
    PrivateKey[1] = g;
    PrivateKey[2] = F;
    PrivateKey[3] = G;

    for(unsigned int i=0; i<4; i++)
    {
            PrivateKey[i].SetLength(N0);
    }

    PublicKey = Quotient(f, g);
}

//==============================================================================
//Computes the private basis B from private key PrivateKey and parameter N
//==============================================================================
void CompletePrivateKey(mat_ZZ& B, const ZZX * const PrivateKey)
{
    ZZX f,g,F,G;
    f = PrivateKey[0];
    g = PrivateKey[1];
    F = PrivateKey[2];
    G = PrivateKey[3];

    f = -f;
    F = -F;

    B = BasisFromPolynomials(g, f, G, F);
}

void GPV(RR_t * v, const RR_t * const c, const RR_t s, const MSK_Data * const MSKD)
{

    int i;
    unsigned j;
    RR_t ci[2*N0], zi, cip, sip, aux;

    for(j=0; j<2*N0;j++)
    {
        ci[j] = c[j];
    }

//    for(j=0; j<2*N0; j++)
//    {
//
//    }

    for(i=2*N0-1; i>=0; i--)
    {
        aux = (MSKD->GS_Norms)[i];
        cip = DotProduct(ci, MSKD->Bstar[i])/(aux*aux);
        sip = s/aux;
        zi = Sample4(cip, sip*PiPrime);

        for(j=0; j<2*N0; j++)
        {
            ci[j] -= zi*(MSKD->B)[i][j];
        }
    }

    for(j=0; j<2*N0; j++)
    {
        v[j] = c[j] - ci[j];
    }

}

//==============================================================================
//==============================================================================
//                            KeyGen
//==============================================================================
//==============================================================================

void CompleteMSK(MSK_Data * MSKD, ZZX * MSK)
{
    unsigned int i, j;
    mat_ZZ B0;

    for(i=0; i<4; i++)
    {
        MSKD->PrK[i] = MSK[i];
        ZZXToFFT(MSKD->PrK_fft[i], MSK[i]);
    }

    CompletePrivateKey(B0, MSK);

    for(i=0; i<2*N0; i++)
    {
        for(j=0; j<2*N0; j++)
        {
            MSKD->B[i][j] = ( (RR_t) conv<double>(B0[i][j]) );
        }
    }

    for(i=0; i<1; i++)
    {
        FastMGS(MSKD->Bstar, MSKD->B);
    }

    for(i=0; i<2*N0; i++)
    {
        MSKD->GS_Norms[i] = sqrt( DotProduct(MSKD->Bstar[i], MSKD->Bstar[i]) );
    }

    MSKD->sigma = 2*MSKD->GS_Norms[0];

}

void CompleteMPK(MPK_Data * MPKD, ZZ_pX MPK)
{
    MPKD->h = MPK;
    ZZXToFFT(MPKD->h_FFT, conv<ZZX>(MPK));
}

//==============================================================================
//==============================================================================
//                            Trapdoor
//==============================================================================
//==============================================================================

void PEKS_Trapdoor(ZZX SK_id[2], vec_ZZ id, const MSK_Data * const MSKD)
{
    unsigned int i;
    RR_t c[2*N0], sk[2*N0], sigma;
    ZZX f,g,aux;

/*	id[0] = q0-1;
	id[1] = q0-3;
	id[2] = q0-5;
	id[3] = q0-5;
	id[4] = q0-8;
	id[5] = q0/2;
	id[6] = q0-10;
	id[7] = q0-11;*/
//id = RandomVector();

    f = MSKD -> PrK[0];
    g = MSKD -> PrK[1];
    sigma = MSKD->sigma;
    SK_id[0].SetLength(N0);
    SK_id[1].SetLength(N0);

    for(i=0;i<N0;i++)
    {
        c[i] = ((RR_t) conv<double>(id[i])) ;
        c[i+N0] = 0;
    }
/*		cout<< "ID in trapdoor: ";
	for (int j = 0;j<N0;j++)
		cout << id[j]<< "   ";*/
	cout << endl;

    GPV(sk, c, sigma, MSKD);

    for(i=0; i<N0; i++)
    {
        sk[i] = c[i] - sk[i];
        sk[i+N0] = - sk[i+N0];
    }

    for(i=0; i<N0; i++)
    {
        SK_id[0][i] = sk[i];
        SK_id[1][i] = sk[i+N0];
    }

}

//==============================================================================
//==============================================================================
//                          Verify  Trapdoor
//==============================================================================
//==============================================================================

unsigned long  PEKS_Verify_Trapdoor( const ZZX SK_td[2], const vec_ZZ kw, const MSK_Data * const MSKD)
{
    unsigned int i;
    ZZX f,g,t,aux;

    f = MSKD -> PrK[0];
    g = MSKD -> PrK[1];

    t = conv<ZZX>(kw);
    aux = ((SK_td[0] - t)*f + g*SK_td[1])%phi;

    for(i=0; i<N0; i++)
    {
        aux[i] %= q1;
    }

    if( IsZero(aux) != 0)
    {
        cout << "The signature (s1,s2) doesn't verify the required equality [ (s1 - t)*f + g*s2 = 0 ] !\nActually, (s1 - t)*f + g*s2 = " << aux << endl << endl;
    }
    return IsZero(aux);
}

//==============================================================================
//==============================================================================
//                          PEKS
//==============================================================================
//==============================================================================

void PEKS_Enc(long C[2][N0],  long C2[N0],  long id0[N0], const MPK_Data * const MPKD)
{

    unsigned long i;
//	srand(3);

/*cout << "The ID in PEKS_ENC  :   ";
		for (int i = 0; i<N0 ; i++){

			cout << id0[i]<<"  ";
			}*/

/*	unsigned char z[32] = {0x54, 0xa2, 0xf8, 0x03, 0x1d, 0x18, 0xac, 0x77, 0xd2, 0x53, 0x92, 0xf2, 0x80, 0xb4, 0xb1, 0x2f, 0xac, 0xf1, 0x29, 0x3f, 0x3a, 0xe6, 0x77, 0x7d, 0x74, 0x15, 0x67, 0x91, 0x99, 0x53, 0x69, 0xc5};
	uint8_t hashed[64];
	long hashed2[64];
	uint8_t hash[2] = {0};
	blake2b(hashed, hash, z, 64, 2, 32);
	cout << "the hashed values are as follows";
	for (int i = 0; i<64 ; i++){
		hashed2[i]= hashed[i];
	 hashed2[i] << 17;}
	for (int i = 0; i<64 ; i++)
		cout << hashed2[i] << "  ";
		*/
/*	id0[0] =  100;
	id0[1] = 234;
	id0[2] = 12;
	id0[3] = 546;
	id0[4] = 98;
	id0[5] = 0;
	id0[6] = 0;
	id0[7] = q0-0;
	id0[8] =  q0 + 100023 - 6619531;
	id0[9] = 0;
	id0[10] = 0;
	id0[11] = 0;
	id0[12] = 0;
	id0[13] = 0;
	id0[14] = 500;
	id0[15] = 0;*/

    long r[N0], e1[N0], e2[N0];
    CC_t r_FFT[N0], t_FFT[N0], aux1_FFT[N0], aux2_FFT[N0];
	for(i=0; i<N0; i++)
        {
            C2[i] = (rand()%2);
        }
    for(i=0; i<N0; i++)
    {
        e1[i] = (rand()%3) - 1;
        e2[i] = (rand()%3) - 1;
        r[i] = (rand()%3) - 1;
    }

	//============================

		//----------------
    MyIntFFT(r_FFT, r);
    MyIntFFT(t_FFT, id0);

    for(i=0; i<N0; i++)
    {
        aux1_FFT[i] = r_FFT[i]*((MPKD->h_FFT)[i]);
        aux2_FFT[i] = r_FFT[i]*t_FFT[i];
    }

    MyIntReverseFFT(C[0], aux1_FFT);
    MyIntReverseFFT(C[1], aux2_FFT);

    for(i=0; i<N0; i++)
    {
        C[0][i] = (C[0][i] + e1[i]               + q0/2)%q0 - (q0/2);
        C[1][i] = (C[1][i] + e2[i] + (q0/2)*C2[i] + q0/2)%q0 - (q0/2);
    }

}

//==============================================================================
//==============================================================================
//                          TEST
//==============================================================================
//==============================================================================

void PEKS_Test(long message[N0],  long C[2][N0], const CC_t * const SKid_FFT)
{
    unsigned int i;
    CC_t c0_FFT[N0], aux_FFT[N0];
	/*        for(int j=0; j<N0; j++)
        {
            C[1][j] = (rand()%q0);
        }*/

    MyIntFFT(c0_FFT, C[0]);

    for(i=0; i<N0; i++)
    {
        aux_FFT[i] = c0_FFT[i]*SKid_FFT[i];
    }

/*		cout << "MESSAGE b4 DECRYPT: "<<endl;
		for(int j=0; j<N0; j++)
        {
            cout << message[j] << " ";
        }*/

    MyIntReverseFFT(message, aux_FFT);

    for(i=0; i<N0; i++)
    {
        message[i] = C[1][i] - message[i];
        message[i] = ((unsigned long)(message[i] ))%q0;
        message[i] = (message[i] + (q0>>2) )/(q0>>1);
        message[i] %= 2;
    }

}

//==============================================================================
//==============================================================================
//                             BENCHES AND TESTS
//                   FOR EXTRACTION AND ENCRYPTION/DECRYPTION
//==============================================================================
//==============================================================================

void Trapdoor_Bench(const unsigned int nb_extr, MSK_Data * MSKD)
{
    clock_t t1, t2;
    float diff;
    unsigned int i;
    vec_ZZ kw;
    ZZX SK_td[2];

    t1 = clock();

    cout << "0%" << flush;
    for(i=0; i<nb_extr; i++)
    {
        kw = RandomVector();

        PEKS_Trapdoor(SK_td, kw, MSKD);
        if((i+1)%(nb_extr/10)==0)
        {
            cout << "..." << (i+1)/(nb_extr/10) << "0%" << flush;
        }
    }

    t2 = clock();
    diff = ((float)t2 - (float)t1)/1000000.0F;
    cout << "\n\nIt took " << diff << " seconds to create  " << nb_extr << " trapdoors." << endl;
    cout << "That's " << (diff/nb_extr)*1000 << " milliseconds per trapdoor." << endl << endl;
}

void Encrypt_Bench(const unsigned int nb_cryp, MPK_Data * MPKD, MSK_Data * MSKD)
{
    clock_t te1, te2, td1, td2;
    float diffe, diffd;
    unsigned int i,j;
    vec_ZZ kw;
    ZZX SK_td[2], w;
    CC_t SKid_FFT[N0];
    long int message[N0], decrypted[N0];
    long int keyword[N0], Ciphertext[2][N0];

    kw = RandomVector();
    PEKS_Trapdoor(SK_td, kw, MSKD);
    PEKS_Verify_Trapdoor(SK_td, kw, MSKD);
    ZZXToFFT(SKid_FFT, SK_td[1]);

    for(i=0; i<N0; i++)
    {
        keyword[i] = conv<long int>(kw[i]);
    }

    cout << "0%" << flush ;
    for(i=0; i<nb_cryp; i++)
    {

	te1 = clock();
		PEKS_Enc(Ciphertext, message, keyword, MPKD);
	te2 = clock();

	td1 = clock();
	/*	if (!PEKS_Test(decrypted, Ciphertext, SKid_FFT)){
			 cout << "TEST FAILED --- Exiting..."<<endl;
			break;
			}*/

	td2 = clock();

    //    if((i+1)%(nb_cryp/10)==0)
        {
            cout << "..." << (i+1)/(nb_cryp/10) << "0%" << flush;
        }

	diffe += ((float)te2 - (float)te1)/1000000.0l;
	diffd += ((float)td2 - (float)td1)/1000000.0l;

    }

    cout << "\n\nIt took " << diffe << " seconds to do " << nb_cryp << " encryptions." << endl;
    cout << "That's " << (diffe/nb_cryp)*1000 << " milliseconds per PEKS generation." << endl;
    cout << "That's " << (diffe/nb_cryp)*1000*1024/N0 << " milliseconds per PEKS per Kilobit." << endl << endl;

    cout << "\n\nIt took " << diffd << " seconds to do " << nb_cryp << " Tests." << endl;
    cout << "That's " << (diffd/nb_cryp)*1000 << " milliseconds per Tests." << endl;
    cout << "That's " << (diffd/nb_cryp)*1000*1024/N0 << " milliseconds per Tests per Kilobit." << endl << endl;

}

void Trapdoor_Test(const unsigned int nb_extr, MSK_Data * MSKD)
{
    unsigned int i, rep;
    vec_ZZ kw;
    ZZX SK_kw[2];

    rep = 0;

    cout << "0%" << flush;
    for(i=0; i<nb_extr; i++)
    {
        kw = RandomVector();

        PEKS_Trapdoor(SK_kw, kw, MSKD);
        rep += PEKS_Verify_Trapdoor(SK_kw, kw, MSKD);
      //  if((i+1)%(nb_extr/10)==0)
        {
            cout << "..." << (i+1)/(nb_extr/10) << "0%" << flush;
        }
    }

    cout << endl;
    if(rep == 0)
    {    cout << endl << nb_extr << " Trapdoor successfully performed!" << endl << endl;    }
    else
    {    cout << endl << rep << " out of " << nb_extr << " extractions failed miserabily!" << endl << endl;    }
}

void Encrypt_Test(const unsigned int nb_cryp, MPK_Data * MPKD, MSK_Data * MSKD)
{
    unsigned int i, j, rep;
    vec_ZZ kw;
    ZZX SK_td[2], m;
    CC_t SKtd_FFT[N0];
    long int kw0[N0], Ciphertext[2][N0], Ciphertext2[N0];
    long int message[N0], decrypted[N0];
	long keywordConv [N0];

/*		keywordConv[0] =  878974;
	keywordConv[1] = 11134;
	keywordConv[2] = 85012;
	keywordConv[3] = 546;
	keywordConv[4] = 1198876;
	keywordConv[5] = 171110;
	keywordConv[6] = 881000;
	keywordConv[7] = 0;
	keywordConv[8] =  100023 - 6619531;
	keywordConv[9] = 1000;
	keywordConv[10] = 0;
	keywordConv[11] =0;
	keywordConv[12] = 100257;
	keywordConv[13] = 0;
	keywordConv[14] = 0;
	keywordConv[15] = q0-55555;*/

	/*long jim[N0];*/
    //kw = RandomVector();

	//================
	int sum = 0;
	string word1; char ch1;
	char keywordAux1[N0];
	cout << "Keyin a word";
	cin >> word1;
	for  (int i=0; i < word1.length();i++){
		keywordAux1[i] = word1[i];
		ch1  = keywordAux1[i];
		keywordConv[i] = (ch1);
	}
	for (int i =word1.length(); i<N0;i++){
		keywordConv[i] = 0;
	}
		for (int i =0; i<N0;i++){
		sum +=keywordConv[i];
	}
	srand(sum);
			for (int i = 0; i<N0 ; i++){

	keywordConv[i] = (rand())%q0;
		}
	//==============

	kw = keywordVector(keywordConv);
  //kw = RandomVector();

    PEKS_Trapdoor(SK_td, kw, MSKD);
    PEKS_Verify_Trapdoor(SK_td, kw, MSKD);
    ZZXToFFT(SKtd_FFT, SK_td[1]);

    rep = 0;
	sum = 0;
	long keywordConverted[N0] ={0};
	string word; char ch;
	char keywordAux[N0];
	cout << "Keyin a word";
	cin >> word;
	for  (int i=0; i < word.length();i++){
		keywordAux[i] = word[i];
		ch  = keywordAux[i];
		keywordConverted[i] = (ch);
	}
	for (int i =word.length(); i<N0;i++){
		keywordConverted[i] = 0;
	}
	for (int i =word1.length(); i<N0;i++){
		keywordConverted[i] = 0;
	}
		for (int i =0; i<N0;i++){
		sum +=keywordConverted[i];
	}
	srand(sum);
			for (int i = 0; i<N0; i++){

	keywordConverted[i] = (rand())%q0;
		}

  /*  for(i=0; i<N0; i++)
    {
        kw0[i] = conv<long int>(kw[i]);

    }

    cout << "0%" << flush;*/

    for(i=0; i<nb_cryp; i++)
    {

		PEKS_Enc(Ciphertext,Ciphertext2,keywordConverted, MPKD);

		PEKS_Test(decrypted,Ciphertext, SKtd_FFT);

		for(j=0; j<N0; j++){
			if( Ciphertext2[j] != decrypted[j])
			{
				cout << "ERROR : Dec(Enc(m)) != m " << endl;
				rep++;
				break;
			}
		}

		  //  if((i+1)%(nb_cryp/10)==0)
			{
		   //     cout << "..." << (i+1)/(nb_cryp/10) << "0%" << flush;
			}
    }

    cout << endl;
    if(rep == 0)
    {    cout << endl << nb_cryp << " PEKS+TEST successfully performed!" << endl << endl;    }
    else
    {    cout << endl << rep << " out of " << nb_cryp << " PEKS+TEST failed miserabily!" << endl << endl;    }
}


//
//  poly_grav.cpp
//
//
//  Created by Protoss Probe on 2017/05/09.
//  Copyright © 2016-2017年 probe. All rights reserved.
//

#include <boost/array.hpp>
#include <cmath>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Eigenvalues>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <string>

#include "poly_grav.hpp"

using namespace std;
using namespace Eigen;

vec3 operator+(const vec3 &a1, const vec3 &a2) {
    vec3 a;
    for (size_t i = 0; i < 3; i++)
        a[i] = a1[i] + a2[i];
    return a;
}

vec3 operator-(const vec3 &a1, const vec3 &a2) {
    vec3 a;
    for (size_t i = 0; i < 3; i++)
        a[i] = a1[i] - a2[i];
    return a;
}

vec3 operator-(const vec3 &a1) {
    vec3 a;
    for (size_t i = 0; i < 3; i++)
        a[i] = -a1[i];
    return a;
}

vec3 operator*(const vec3 &a1, const double &a2) {
    vec3 a;
    for (size_t i = 0; i < 3; i++)
        a[i] = a1[i] * a2;
    return a;
}

vec3 operator/(const vec3 &a1, const double &a2) {
    vec3 a;
    for (size_t i = 0; i < 3; i++)
        a[i] = a1[i] / a2;
    return a;
}

mat3 operator+(const mat3 &a1, const mat3 &a2) {
    mat3 a;
    for (size_t i = 0; i < 3; i++) {
        for (size_t j = 0; j < 3; j++) {
            a[i][j] = a1[i][j] + a2[i][j];
        }
    }
    return a;
}

mat3 operator-(const mat3 &a1, const mat3 &a2) {
    mat3 a;
    for (size_t i = 0; i < 3; i++) {
        for (size_t j = 0; j < 3; j++) {
            a[i][j] = a1[i][j] - a2[i][j];
        }
    }
    return a;
}

PolyGrav::PolyGrav() = default;
PolyGrav::~PolyGrav() = default;
PolyGrav::PolyGrav(string dir) : dir(dir){};

void PolyGrav::import_3d_obj(string dir) {
    // parse .obj file to get vertexs and polygon data
    string data;
    double edge_len;
    vec3 temp, normal, vector1, vector2;
    mat3 F_f;
    connect3 temp_c;
    ifstream objfile(dir);
    if (objfile.is_open()) {
        while (!objfile.eof()) {
            objfile >> data;
            if (data == "v") {
                for (size_t i = 0; i < 3; i++) {
                    objfile >> data;
                    temp[i] = stof(data);
                }
                points.push_back(temp);
            } else if (data == "f") {
                for (size_t i = 0; i < 3; i++) {
                    objfile >> data;
                    temp_c[i] = stoi(data) - 1;
                }
                vector1 = points[temp_c[1]] - points[temp_c[0]];
                vector2 = points[temp_c[2]] - points[temp_c[1]];
                edge_len = PolyGrav::norm(vector1);
                vector1 = vector1 / edge_len;

                normal = PolyGrav::cross(vector1, vector2);
                normal = normal / PolyGrav::norm(normal);
                F_f = PolyGrav::outer(normal, normal);

                polygons.push_back(temp_c);
                normals.push_back(normal);
                F_fs.push_back(F_f);
            }
        }
        objfile.close();
        vert_n = points.size();
        face_n = polygons.size();
        edge_n = vert_n + face_n - 2;
    };
}

void PolyGrav::get_edges_info() {
    connect2 con;
    double edge_len;
    vec3 edge_vec, p0, p1;
    mat3 E_e;
    int common_edge;
    size_t ii;

    for (size_t i = 0; i < face_n; i++) {
        for (size_t j = 0; j < 3; j++) {
            con[0] = polygons[i][j];
            con[1] = polygons[i][(j + 1) % 3];
            common_edge = PolyGrav::find_common_edge(con, i);
            if (common_edge != -1) {
                ii = (size_t)common_edge;
                p0 = points[con[0]];
                p1 = points[con[1]];
                edge_vec = p1 - p0;
                edge_len = PolyGrav::norm(edge_vec);
                edge_vec = edge_vec / edge_len;
                E_e = PolyGrav::outer(normals[i],
                                      PolyGrav::cross(edge_vec, normals[i])) +
                      PolyGrav::outer(normals[ii],
                                      PolyGrav::cross(-edge_vec, normals[ii]));

                edges.push_back({{con[0], con[1], i, ii}});
                edges_len.push_back(edge_len);
                E_es.push_back(E_e);
            }
        }
    }
}

int PolyGrav::find_common_edge(connect2 con, size_t i) {
    size_t n = i + 1;
    for (size_t i = n; i < face_n; i++) {
        for (size_t j = 0; j < 3; j++) {
            if (con[1] == polygons[i][j] and con[0] == polygons[i][(j + 1) % 3])
                return i;
        }
    }
    return -1;
}

void PolyGrav::export_3d_txt(string dir, char acc = 'f') {
    ofstream txtfile;
    txtfile.open(dir);
    txtfile << vert_n << endl;
    txtfile << endl;
    for (size_t i = 0; i < vert_n; i++) {
        for (size_t j = 0; j < 3; j++) {
            if (acc == 'f') {
                txtfile << float(points[i][j]) << ' ';
            } else if (acc == 'd') {
                txtfile << setprecision(8) << points[i][j] << ' ';
            }
        }
        txtfile << endl;
    }
    txtfile << endl;
    txtfile << face_n << endl;
    txtfile << endl;
    for (size_t i = 0; i < face_n; i++) {
        txtfile << 3 << ' ';
        for (size_t j = 0; j < 3; j++) {
            txtfile << polygons[i][j] << ' ';
        }
        txtfile << endl;
    }

    txtfile << endl;
    txtfile << edge_n << endl;
    txtfile << endl;
    for (size_t i = 0; i < edge_n; i++) {
        for (size_t j = 0; j < 4; j++) {
            txtfile << edges[i][j] << ' ';
        }
        txtfile << endl;
    }
}

void PolyGrav::import_info(string dir) {
    ifstream txtfile(dir);
    if (txtfile.is_open()) {
        while (!txtfile.eof()) {
            for (size_t i = 0; i < 3; i++) {
                txtfile >> mc[i];
            }
            for (size_t i = 0; i < 3; i++) {
                for (size_t j = 0; j < 3; j++) {
                    txtfile >> jj[i][j];
                }
            }
        }
    }
    cout << "Mass and inertia tensor are imported!!" << endl;
}

void PolyGrav::calexec(string dir) {
    string exe = "./bin/volInt ";
    exe += dir;
    const char *input = exe.c_str();
    cout << input << endl;
    // execl(executable, executable, input, (char *)NULL);

    system(input);
}

void PolyGrav::init() {
    string filename = dir;
    PolyGrav::import_3d_obj("assets/" + filename + ".obj");
    PolyGrav::get_edges_info();
    PolyGrav::export_3d_txt("assets/" + filename + ".txt");
    PolyGrav::calexec("assets/" + filename + ".txt");
    PolyGrav::import_info("assets/info.txt");
    // cout << "Initialization Completed!\n\n";
    // cout << "Vertex Number: " << vert_n << "\n\n"
    //      << "Faces Number: " << face_n << "\n\n";
    // cout << "Center of Mass: \n"
    //      << mc << "\n\n"
    //      << "Inertia Tensor: \n"
    //      << jj << "\n\n";
}

void PolyGrav::principle_axes() {
    Matrix3d temp_mat;
    temp_mat = boost2eigen_mat(jj);
    EigenSolver<MatrixXd> es(temp_mat);
    abc = eigen2boost_vec(es.eigenvalues().real());
    mat3 rotmat = eigen2boost_mat(es.eigenvectors().real());
    for (auto it = points.begin(); it != points.end(); ++it) {
        *it = *it - mc;
        *it = PolyGrav::mul(PolyGrav::transpose(rotmat), *it);
    }
}

double PolyGrav::L_e(double a, double b, double e) {
    return log((a + b + e) / (a + b - e));
}

double PolyGrav::omega_f(mat3 r, vec3 r_len) {
    double y = PolyGrav::dot(r[0], PolyGrav::cross(r[1], r[2]));
    double x = 0;
    x += r_len[0] * r_len[1] * r_len[2];
    for (size_t i = 0; i < 3; i++) {
        x += r_len[i] * PolyGrav::dot(r[(i + 1) % 3], r[(i + 2) % 3]);
    }

    return 2 * atan2(y, x);
}

double PolyGrav::potential(vec3 field_p) {
    double result = 0, E_term = 0, F_term = 0;
    vec_data r_vec;
    val_data r_len;
    for (size_t i = 0; i < vert_n; i++) {
        r_vec.push_back(points[i] - field_p);
        r_len.push_back(PolyGrav::norm(r_vec[i]));
    }
    // faces loop
    connect3 polygon;
    size_t a, b, c;
    mat3 F_f;
    vec3 normal;
    double omega_f;
    for (size_t n = 0; n < face_n; n++) {
        polygon = polygons[n];
        a = polygon[0];
        b = polygon[1];
        c = polygon[2];
        F_f = F_fs[n];
        normal = normals[n];
        omega_f = PolyGrav::omega_f({{r_vec[a], r_vec[b], r_vec[c]}},
                                    {{r_len[a], r_len[b], r_len[c]}});
        F_term +=
            PolyGrav::dot(r_vec[a], PolyGrav::mul(F_f, r_vec[a])) * omega_f;
    }
    // edges loop
    double edge_len;
    mat3 E_e;
    connect4 edge;
    for (size_t n = 0; n < edge_n; n++) {
        edge_len = edges_len[n];
        E_e = E_es[n];
        edge = edges[n];
        a = edge[0], b = edge[1];
        E_term += PolyGrav::dot(r_vec[a], PolyGrav::mul(E_e, r_vec[a])) *
                  PolyGrav::L_e(r_len[a], r_len[b], edge_len);
    }
    result = co * (E_term - F_term);
    return result;
}

Vector3d PolyGrav::boost2eigen_vec(vec3 vec) {
    Vector3d output;
    for (size_t i = 0; i < 3; i++) {
        output(i) = vec[i];
    }
    return output;
}

Matrix3d PolyGrav::boost2eigen_mat(mat3 mat) {
    Matrix3d output;
    for (size_t i = 0; i < 3; i++) {
        for (size_t j = 0; j < 3; j++) {
            output(i, j) = mat[i][j];
        }
    }
    return output;
}

vec3 PolyGrav::eigen2boost_vec(Vector3d vec) {
    vec3 output;
    for (size_t i = 0; i < 3; i++) {
        output[i] = vec(i);
    }
    return output;
}

mat3 PolyGrav::eigen2boost_mat(Matrix3d mat) {
    mat3 output;
    for (size_t i = 0; i < 3; i++) {
        for (size_t j = 0; j < 3; j++) {
            output[i][j] = mat(i, j);
        }
    }
    return output;
}

double PolyGrav::norm(const vec3 &vec) {
    return sqrt(vec[0] * vec[0] + vec[1] * vec[1] + vec[2] * vec[2]);
}

double PolyGrav::dot(const vec3 &vec1, const vec3 &vec2) {
    return vec1[0] * vec2[0] + vec1[1] * vec2[1] + vec1[2] * vec2[2];
}

vec3 PolyGrav::cross(const vec3 &vec1, const vec3 &vec2) {
    vec3 output;
    output[0] = -vec1[2] * vec2[1] + vec1[1] * vec2[2];
    output[1] = vec1[2] * vec2[0] - vec1[0] * vec2[2];
    output[2] = -vec1[1] * vec2[0] + vec1[0] * vec2[1];
    return output;
}

vec3 PolyGrav::mul(const mat3 &mat, const vec3 &vec) {
    vec3 output;
    for (size_t i = 0; i < 3; i++) {
        output[i] =
            mat[i][0] * vec[0] + mat[i][1] * vec[1] + mat[i][2] * vec[2];
    }
    return output;
}

mat3 PolyGrav::outer(const vec3 &vec1, const vec3 &vec2) {
    mat3 output;
    for (size_t i = 0; i < 3; i++) {
        for (size_t j = 0; j < 3; j++) {
            output[i][j] = vec1[i] * vec2[j];
        }
    }
    return output;
}

mat3 PolyGrav::transpose(const mat3 &mat) {
    mat3 output;
    for (size_t i = 0; i < 3; i++) {
        for (size_t j = 0; j < 3; j++) {
            output[i][j] = mat[j][i];
        }
    }
    return output;
}


#include "asymmetric_laplace.hpp"
#include "asymmetric_normal.hpp"
#include "mixture.hpp"
#include "normal.hpp"
#include "laplace.hpp"

#include <gtest/gtest.h>

#include <boost/random/mersenne_twister.hpp>

using namespace MultidimensionalArray;
using namespace ProbabilityDistributions;

TEST(MixtureTest, LikelihoodNormal) {
  boost::random::mt19937 rng;
  const unsigned int n_samples = 100;
  Normal<double> dist1(-10, 1);
  Normal<double> dist2(10, 1);
  AsymmetricNormal<double> dist3(0.5, -10, 1);
  AsymmetricNormal<double> dist4(0.5, 10, 1);
  dist3.fix_p(true);
  dist4.fix_p(true);
  auto dist = make_mixture(dist1, dist2);
  auto dist_a = make_mixture(dist3, dist4);
  Array<double> samples;
  dist.sample(samples, n_samples, rng);

  double likelihood1 = dist.log_likelihood(samples);
  double likelihood1_a = dist_a.log_likelihood(samples);
  EXPECT_DOUBLE_EQ(likelihood1, likelihood1_a);

  dist.get_component<0>().set_mu(-0.1);
  dist.get_component<1>().set_mu(0.1);
  dist_a.get_component<0>().set_mu(-0.1);
  dist_a.get_component<1>().set_mu(0.1);

  auto new_dist = make_mixture(Laplace<double>(-0.1, 1),
      Laplace<double>(0.1, 1));

  dist.MLE(samples);
  dist_a.MLE(samples);
  new_dist.MLE(samples);

  double likelihood2 = dist.log_likelihood(samples);
  double likelihood2_a = dist_a.log_likelihood(samples);
  double new_likelihood = new_dist.log_likelihood(samples);

  EXPECT_NEAR(likelihood2, likelihood2_a, 1e-8);
  EXPECT_GE(likelihood2, likelihood1);
  EXPECT_GE(likelihood2, new_likelihood);
}

TEST(MixtureTest, LikelihoodLaplace) {
  boost::random::mt19937 rng;
  const unsigned int n_samples = 100;
  Laplace<double> dist1(-10, 1);
  Laplace<double> dist2(10, 1);
  AsymmetricLaplace<double> dist3(0.5, -10, 1);
  AsymmetricLaplace<double> dist4(0.5, 10, 1);
  dist3.fix_p(true);
  dist4.fix_p(true);
  auto dist = make_mixture(dist1, dist2);
  auto dist_a = make_mixture(dist3, dist4);
  Array<double> samples;
  dist.sample(samples, n_samples, rng);

  double likelihood1 = dist.log_likelihood(samples);
  double likelihood1_a = dist_a.log_likelihood(samples);
  EXPECT_DOUBLE_EQ(likelihood1, likelihood1_a);

  dist.get_component<0>().set_mu(-0.1);
  dist.get_component<1>().set_mu(0.1);
  dist_a.get_component<0>().set_mu(-0.1);
  dist_a.get_component<1>().set_mu(0.1);

  auto new_dist = make_mixture(Normal<double>(-0.1, 1), Normal<double>(0.1, 1));

  dist.MLE(samples);
  dist_a.MLE(samples);
  new_dist.MLE(samples);

  double likelihood2 = dist.log_likelihood(samples);
  double likelihood2_a = dist_a.log_likelihood(samples);
  double new_likelihood = new_dist.log_likelihood(samples);

  EXPECT_NEAR(likelihood2, likelihood2_a, 1e-8);
  EXPECT_GE(likelihood2, likelihood1);
  EXPECT_GE(likelihood2, new_likelihood);
}

TEST(MixtureTest, LikelihoodAsymmetricNormal) {
  boost::random::mt19937 rng;
  const unsigned int n_samples = 100;
  AsymmetricNormal<double> dist1(0.5, -10, 1);
  AsymmetricNormal<double> dist2(0.5, 10, 1);
  auto dist = make_mixture(dist1, dist2);
  dist1.fix_p(true);
  dist2.fix_p(true);
  auto dist_a = make_mixture(dist1, dist2);
  Array<double> samples;
  dist.sample(samples, n_samples, rng);

  double likelihood1 = dist.log_likelihood(samples);
  double likelihood1_a = dist_a.log_likelihood(samples);
  EXPECT_DOUBLE_EQ(likelihood1, likelihood1_a);

  dist.get_component<0>().set_mu(-0.1);
  dist.get_component<1>().set_mu(0.1);
  dist_a.get_component<0>().set_mu(-0.1);
  dist_a.get_component<1>().set_mu(0.1);

  auto new_dist = make_mixture(Laplace<double>(-0.1, 1),
      Laplace<double>(0.1, 1));

  dist.MLE(samples);

  dist_a.MLE(samples);
  new_dist.MLE(samples);

  double likelihood2 = dist.log_likelihood(samples);
  double likelihood2_a = dist_a.log_likelihood(samples);
  double new_likelihood = new_dist.log_likelihood(samples);

  EXPECT_GE(likelihood2, likelihood2_a);
  EXPECT_GE(likelihood2, likelihood1);
  EXPECT_GE(likelihood2, new_likelihood);
}

TEST(MixtureTest, LikelihoodAsymmetricLaplace) {
  boost::random::mt19937 rng;
  const unsigned int n_samples = 100;
  AsymmetricLaplace<double> dist1(0.5, -10, 1);
  AsymmetricLaplace<double> dist2(0.5, 10, 1);
  auto dist = make_mixture(dist1, dist2);
  dist1.fix_p(true);
  dist2.fix_p(true);
  auto dist_a = make_mixture(dist1, dist2);
  Array<double> samples;
  dist.sample(samples, n_samples, rng);

  double likelihood1 = dist.log_likelihood(samples);
  double likelihood1_a = dist_a.log_likelihood(samples);
  EXPECT_DOUBLE_EQ(likelihood1, likelihood1_a);

  dist.get_component<0>().set_mu(-0.1);
  dist.get_component<1>().set_mu(0.1);
  dist_a.get_component<0>().set_mu(-0.1);
  dist_a.get_component<1>().set_mu(0.1);

  auto new_dist = make_mixture(Normal<double>(-0.1, 1), Normal<double>(0.1, 1));

  dist.MLE(samples);

  dist_a.MLE(samples);
  new_dist.MLE(samples);

  double likelihood2 = dist.log_likelihood(samples);
  double likelihood2_a = dist_a.log_likelihood(samples);
  double new_likelihood = new_dist.log_likelihood(samples);

  EXPECT_GE(likelihood2, likelihood2_a);
  EXPECT_GE(likelihood2, likelihood1);
  EXPECT_GE(likelihood2, new_likelihood);
}

TEST(MixtureTest, Samples) {
  boost::random::mt19937 rng;
  const unsigned int n_samples = 100;
  Normal<double> dist1(-10, 1);
  Normal<double> dist2(10, 1);
  auto dist = make_mixture(dist1, dist2);
  Array<double> samples;
  dist.sample(samples, n_samples, rng);
  unsigned int count_0 = 0, count_1 = 0;

  for (size_t i = 0; i < n_samples; i++) {
    if (samples(i,0) < 0) {
      EXPECT_LT(-15, samples(i,0));
      EXPECT_GT(-5, samples(i,0));
      count_0++;
    }
    else {
      EXPECT_LT(5, samples(i,0));
      EXPECT_GT(15, samples(i,0));
      count_1++;
    }
  }

  EXPECT_LT(0, count_0);
  EXPECT_LT(0, count_1);
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_ARITHMETIC_FUNCTIONS_GENERIC_TWO_PROD_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_GENERIC_TWO_PROD_HPP_INCLUDED
#include <boost/simd/arithmetic/functions/two_prod.hpp>
#include <boost/simd/include/functions/minus.hpp>
#include <boost/simd/include/functions/two_split.hpp>
#include <boost/simd/include/functions/multiplies.hpp>
#include <boost/simd/include/functions/is_invalid.hpp>
#include <boost/simd/include/functions/if_zero_else.hpp>
#include <boost/simd/sdk/config.hpp>
#include <boost/fusion/include/std_pair.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::two_prod_
                                    , tag::cpu_
                                    , (A0)
                                    , (generic_< floating_<A0> >)
                                      (generic_< floating_<A0> >)
                                      (generic_< floating_<A0> >)
                                    )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE
    result_type operator()(A0 const& a0,A0 const& a1,A0& a3) const
    {
      result_type a2;
      boost::simd::two_prod(a0, a1, a2, a3);
      return a2;
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::two_prod_, tag::cpu_
                                    , (A0)
                                    , (generic_< floating_<A0> >)
                                      (generic_< floating_<A0> >)
                                    )
  {
    typedef std::pair<A0,A0>                                   result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0,A0 const& a1) const
    {
      result_type res;
      boost::simd::two_prod( a0, a1, res.first, res.second );
      return res;
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::two_prod_, tag::cpu_
                                    , (A0)
                                    , (generic_< floating_<A0> >)
                                      (generic_< floating_<A0> >)
                                      (generic_< floating_<A0> >)
                                      (generic_< floating_<A0> >)
                                    )
  {
    typedef void result_type;
    BOOST_FORCEINLINE
    result_type operator()(A0 const& a,A0 const& b, A0 & r0,A0 & r1) const
    {
      A0 a1, a2, b1, b2;
      r0  = a*b;

      two_split(a, a1, a2);
      two_split(b, b1, b2);

#if defined(BOOST_SIMD_NO_INVALIDS)
      r1 = a2*b2 -(((r0-a1*b1)-a2*b1)-a1*b2);
#else
      r1 = if_zero_else(is_invalid(r0), a2*b2 -(((r0-a1*b1)-a2*b1)-a1*b2));
#endif

    }
  };
} } }

#endif


#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

int main(int, char**)
{
    using namespace mtl;
    dense2D<double>       A(5, 5), H(5, 5);  A= 0.0;

    A[0][1] = 3;  A[1][4] = 7; A[0][0] = 1; A[4][4] = 17;
    A[2][3] = -2; A[2][4] = 5; A[4][0] = 2; A[4][1] = 3;
    A[3][2] = 4;

    H= hessenberg(A);
    std::cout<< "Hessenberg=\n" << H << "\n";
    H= extract_householder_hessenberg(A);
    std::cout<< "extract_householder_hessenberg=\n" << H << "\n";
    H= extract_hessenberg(A);
    std::cout<< "extract_hessenberg=\n" << H << "\n";
    H= householder_hessenberg(A);
    std::cout<< "householder_hessenberg=\n" << H << "\n";
    H= hessenberg_factors(A);
    std::cout<< "hessenberg_factors=\n" << H << "\n";
    // H= hessenberg_q(A);
    // std::cout<< "hessenberg_q=\n" << H << "\n";

   return 0;
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_ARITHMETIC_FUNCTIONS_SIMD_COMMON_AVERAGE_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_SIMD_COMMON_AVERAGE_HPP_INCLUDED
#include <boost/simd/arithmetic/functions/average.hpp>
#include <boost/simd/include/functions/simd/bitwise_and.hpp>
#include <boost/simd/include/functions/simd/bitwise_xor.hpp>
#include <boost/simd/include/functions/simd/plus.hpp>
#include <boost/simd/include/functions/simd/multiplies.hpp>
#include <boost/simd/include/functions/simd/shrai.hpp>
#include <boost/simd/include/constants/half.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::average_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<arithmetic_<A0>,X>))((simd_<arithmetic_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return b_and(a0, a1)+shrai(b_xor(a0, a1),1);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::average_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<floating_<A0>,X>))((simd_<floating_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
       return (a0+a1)*Half<A0>();
    }
  };
} } }

#endif


// From:
// http://stackoverflow.com/questions/6142576/sample-from-multivariate-normal-gaussian-distribution-in-c

#include <iostream>
#include <chrono>
#include <functional>

#include "ThreadVector.hpp"

#include "omp_util.h"

#ifdef USE_BOOST_RANDOM
#include <boost/random.hpp>
#define MERSENNE_TWISTER boost::random::mt19937
#define UNIFORM_REAL_DISTRIBUTION boost::random::uniform_real_distribution<double>
#define GAMMA_DISTRIBUTION boost::random::gamma_distribution<double>
#else
#include <random>
#define MERSENNE_TWISTER std::mt19937
#define UNIFORM_REAL_DISTRIBUTION std::uniform_real_distribution<double>
#define GAMMA_DISTRIBUTION std::gamma_distribution<double>
#endif

#include <Eigen/Dense>

#include "Distribution.h"

using namespace Eigen;

static smurff::thread_vector<MERSENNE_TWISTER> bmrngs;

double smurff::randn0()
{
   return smurff::bmrandn_single_thread();
}

double smurff::randn(double)
{
   return smurff::bmrandn_single_thread();
}

void smurff::bmrandn(double* x, long n)
{
   #pragma omp parallel
   {
      UNIFORM_REAL_DISTRIBUTION unif(-1.0, 1.0);
      auto& bmrng = bmrngs.local();

      #pragma omp for schedule(static)
      for (long i = 0; i < n; i += 2)
      {
         double x1, x2, w;
         do
         {
           x1 = unif(bmrng);
           x2 = unif(bmrng);
           w = x1 * x1 + x2 * x2;
         } while ( w >= 1.0 );

         w = std::sqrt( (-2.0 * std::log( w ) ) / w );
         x[i] = x1 * w;

         if (i + 1 < n)
         {
           x[i+1] = x2 * w;
         }
      }
   }
}

void smurff::bmrandn(Eigen::MatrixXd & X)
{
   long n = X.rows() * (long)X.cols();
   smurff::bmrandn(X.data(), n);
}

double smurff::bmrandn_single_thread()
{
   //TODO: add bmrng as input
   UNIFORM_REAL_DISTRIBUTION unif(-1.0, 1.0);
   auto& bmrng = bmrngs.local();

   double x1, x2, w;
   do
   {
      x1 = unif(bmrng);
      x2 = unif(bmrng);
      w = x1 * x1 + x2 * x2;
   } while ( w >= 1.0 );

   w = std::sqrt( (-2.0 * std::log( w ) ) / w );
   return x1 * w;
}

// to be called within OpenMP parallel loop (also from serial code is fine)
void smurff::bmrandn_single_thread(double* x, long n)
{
   UNIFORM_REAL_DISTRIBUTION unif(-1.0, 1.0);
   auto& bmrng = bmrngs.local();

   for (long i = 0; i < n; i += 2)
   {
      double x1, x2, w;

      do
      {
         x1 = unif(bmrng);
         x2 = unif(bmrng);
         w = x1 * x1 + x2 * x2;
      } while ( w >= 1.0 );

      w = std::sqrt( (-2.0 * std::log( w ) ) / w );
      x[i] = x1 * w;

      if (i + 1 < n)
      {
         x[i+1] = x2 * w;
      }
   }
}

void smurff::bmrandn_single_thread(Eigen::VectorXd & x)
{
   smurff::bmrandn_single_thread(x.data(), x.size());
}

void smurff::bmrandn_single_thread(Eigen::MatrixXd & X)
{
   long n = X.rows() * (long)X.cols();
   smurff::bmrandn_single_thread(X.data(), n);
}

void smurff::init_bmrng()
{
   using namespace std::chrono;
   auto ms = (duration_cast< milliseconds >(system_clock::now().time_since_epoch())).count();
   smurff::init_bmrng(ms);
}

void smurff::init_bmrng(int seed)
{
    std::vector<MERSENNE_TWISTER> v;
    for (int i = 0; i < threads::get_max_threads(); i++)
    {
        v.push_back(MERSENNE_TWISTER(seed + i * 1999));
    }

    bmrngs.init(v);
}

double smurff::rand_unif()
{
   UNIFORM_REAL_DISTRIBUTION unif(0.0, 1.0);
   auto& bmrng = bmrngs.local();
   return unif(bmrng);
}

double smurff::rand_unif(double low, double high)
{
   UNIFORM_REAL_DISTRIBUTION unif(low, high);
   auto& bmrng = bmrngs.local();
   return unif(bmrng);
}

// returns random number according to Gamma distribution
// with the given shape (k) and scale (theta). See wiki.
double smurff::rgamma(double shape, double scale)
{
   GAMMA_DISTRIBUTION gamma(shape, scale);
   return gamma(bmrngs.local());
}

auto smurff::nrandn(int n) -> decltype(Eigen::VectorXd::NullaryExpr(n, std::cref(randn)))
{
   return Eigen::VectorXd::NullaryExpr(n, std::cref(randn));
}

auto smurff::nrandn(int n, int m) -> decltype(Eigen::ArrayXXd::NullaryExpr(n, m, std::cref(randn)))
{
   return Eigen::ArrayXXd::NullaryExpr(n, m, std::cref(randn));
}

Eigen::MatrixXd WishartUnit(int m, int df)
{
   Eigen::MatrixXd c(m,m);
   c.setZero();
   auto& rng = bmrngs.local();

   for ( int i = 0; i < m; i++ )
   {
      GAMMA_DISTRIBUTION gam(0.5*(df - i));
      c(i,i) = std::sqrt(2.0 * gam(rng));
      Eigen::VectorXd r = smurff::nrandn(m-i-1);
      c.block(i,i+1,1,m-i-1) = r.transpose();
   }

   Eigen::MatrixXd ret = c.transpose() * c;

   #ifdef TEST_MVNORMAL
   cout << "WISHART UNIT {\n" << endl;
   cout << "  m:\n" << m << endl;
   cout << "  df:\n" << df << endl;
   cout << "  ret;\n" << ret << endl;
   cout << "  c:\n" << c << endl;
   cout << "}\n" << ret << endl;
   #endif

   return ret;
}

MatrixXd Wishart(const Eigen::MatrixXd &sigma, const int df)
{
   //  Get R, the upper triangular Cholesky factor of SIGMA.
   auto chol = sigma.llt();
   Eigen::MatrixXd r = chol.matrixL();

   //  Get AU, a sample from the unit Wishart distribution.
   Eigen::MatrixXd au = WishartUnit(sigma.cols(), df);

   //  Construct the matrix A = R' * AU * R.
   Eigen::MatrixXd a = r * au * chol.matrixU();

   #ifdef TEST_MVNORMAL
   cout << "WISHART {\n" << endl;
   cout << "  sigma:\n" << sigma << endl;
   cout << "  r:\n" << r << endl;
   cout << "  au:\n" << au << endl;
   cout << "  df:\n" << df << endl;
   cout << "  a:\n" << a << endl;
   cout << "}\n" << endl;
   #endif

  return a;
}

// from julia package Distributions: conjugates/normalwishart.jl
std::pair<Eigen::VectorXd, Eigen::MatrixXd> smurff::NormalWishart(const Eigen::VectorXd & mu, double kappa, const Eigen::MatrixXd & T, double nu)
{
   Eigen::MatrixXd Lam = Wishart(T, nu);
   Eigen::MatrixXd mu_o = smurff::MvNormal_prec(Lam * kappa, mu);

   #ifdef TEST_MVNORMAL
   cout << "NORMAL WISHART {\n" << endl;
   cout << "  mu:\n" << mu << endl;
   cout << "  kappa:\n" << kappa << endl;
   cout << "  T:\n" << T << endl;
   cout << "  nu:\n" << nu << endl;
   cout << "  mu_o\n" << mu_o << endl;
   cout << "  Lam\n" << Lam << endl;
   cout << "}\n" << endl;
   #endif

   return std::make_pair(mu_o , Lam);
}

std::pair<Eigen::VectorXd, Eigen::MatrixXd> smurff::CondNormalWishart(const int N, const Eigen::MatrixXd &NS, const Eigen::VectorXd &NU, const Eigen::VectorXd &mu, const double kappa, const Eigen::MatrixXd &T, const int nu)
{
   int nu_c = nu + N;

   double kappa_c = kappa + N;
   auto mu_c = (kappa * mu + NU) / (kappa + N);
   auto X    = (T + NS + kappa * mu * mu.adjoint() - kappa_c * mu_c * mu_c.adjoint());
   Eigen::MatrixXd T_c = X.inverse();

   return NormalWishart(mu_c, kappa_c, T_c, nu_c);
}

std::pair<Eigen::VectorXd, Eigen::MatrixXd> smurff::CondNormalWishart(const Eigen::MatrixXd &U, const Eigen::VectorXd &mu, const double kappa, const Eigen::MatrixXd &T, const int nu)
{
   auto N = U.cols();
   auto NS = U * U.adjoint();
   auto NU = U.rowwise().sum();
   return CondNormalWishart(N, NS, NU, mu, kappa, T, nu);
}

// Normal(0, Lambda^-1) for nn columns
MatrixXd smurff::MvNormal_prec(const Eigen::MatrixXd & Lambda, int ncols)
{
   int nrows = Lambda.rows(); // Dimensionality (rows)
   LLT<Eigen::MatrixXd> chol(Lambda);

   Eigen::MatrixXd r(nrows, ncols);
   smurff::bmrandn(r);

   return chol.matrixU().solve(r);
}

Eigen::MatrixXd smurff::MvNormal_prec(const Eigen::MatrixXd & Lambda, const Eigen::VectorXd & mean, int nn)
{
   Eigen::MatrixXd r = MvNormal_prec(Lambda, nn);
   return r.colwise() + mean;
}

// Draw nn samples from a size-dimensional normal distribution
// with a specified mean and covariance
Eigen::MatrixXd smurff::MvNormal(const Eigen::MatrixXd covar, const Eigen::VectorXd mean, int nn)
{
   int size = mean.rows(); // Dimensionality (rows)
   Eigen::MatrixXd normTransform(size,size);

   LLT<Eigen::MatrixXd> cholSolver(covar);
   normTransform = cholSolver.matrixL();

   auto normSamples = Eigen::MatrixXd::NullaryExpr(size, nn, std::cref(randn));
   Eigen::MatrixXd samples = (normTransform * normSamples).colwise() + mean;

   return samples;
}


#pragma once

#include <complex>
#include <memory>
#include <utility>

#include <Eigen/SVD>

#include "common.hpp"
#include "piecewise_polynomial.hpp"
#include "irlib/detail/basis_impl.ipp"

namespace irlib {
    /**
     * Abstract class representing an analytical continuation kernel
     */
    template<typename T>
    class kernel {
    public:
        typedef T mp_type;

        virtual ~kernel() {};

        /// return the value of the kernel for given x and y in the [-1,1] interval.
        virtual T operator()(T x, T y) const = 0;

        /// return statistics
        virtual irlib::statistics::statistics_type get_statistics() const = 0;

        /// return lambda
        virtual double Lambda() const = 0;

#ifndef SWIG

        /// return a reference to a copy
        virtual std::shared_ptr<kernel> clone() const = 0;

#endif
    };

#ifdef SWIG
    %template(real_kernel) kernel<mpreal>;
#endif

    /**
     * Fermionic kernel
     */
    template<typename S>
    class fermionic_kernel : public kernel<S> {
    public:
        fermionic_kernel(S Lambda) : Lambda_(Lambda) {}

        virtual ~fermionic_kernel() {};

        S operator()(S x, S y) const {
            const S limit = 100.0;
            if (Lambda_ * y > limit) {
                return std::exp(-0.5 * Lambda_ * x * y - 0.5 * Lambda_ * y);
            } else if (Lambda_ * y < -limit) {
                return std::exp(-0.5 * Lambda_ * x * y + 0.5 * Lambda_ * y);
            } else {
                return std::exp(-0.5 * Lambda_ * x * y) / (2 * std::cosh(0.5 * Lambda_ * y));
            }
        }

        irlib::statistics::statistics_type get_statistics() const {
            return irlib::statistics::FERMIONIC;
        }

        double Lambda() const {
            return Lambda_;
        }

#ifndef SWIG

        std::shared_ptr <kernel<S> > clone() const {
            return std::shared_ptr<kernel<S> >(new fermionic_kernel<S>(Lambda_));
        }

#endif

    private:
        double Lambda_;
    };

    template<>
    class fermionic_kernel<mpreal> : public kernel<mpreal> {
    public:
        fermionic_kernel(double Lambda) : Lambda_(Lambda) {}

        virtual ~fermionic_kernel() {};

        mpreal operator()(mpreal x, mpreal y) const {
            mpreal half_Lambda = mpreal("0.5") * mpreal(Lambda_);

            const double limit = 200.0;
            if (Lambda_ * y > limit) {
                return mpfr::exp(-half_Lambda * x * y - half_Lambda * y);
            } else if (Lambda_ * y < -limit) {
                return mpfr::exp(-half_Lambda * x * y + half_Lambda * y);
            } else {
                return mpfr::exp(-half_Lambda * x * y) / (2 * mpfr::cosh(half_Lambda * y));
            }
        }

        irlib::statistics::statistics_type get_statistics() const {
            return irlib::statistics::FERMIONIC;
        }

        double Lambda() const {
            return Lambda_;
        }

#ifndef SWIG

        std::shared_ptr<kernel<mpreal>> clone() const {
            return std::shared_ptr<kernel<mpreal>>(new fermionic_kernel(Lambda_));
        }

#endif

    private:
        double Lambda_;
    };

    /**
     * Bosonic kernel
     */
    template<typename S>
    class bosonic_kernel : public kernel<S> {
    public:
        bosonic_kernel(double Lambda) : Lambda_(Lambda) {}

        virtual ~bosonic_kernel() {};

        S operator()(S x, S y) const {
            const S limit = 100.0;
            if (std::abs(Lambda_ * y) < 1e-10) {
                return std::exp(-0.5 * Lambda_ * x * y) / Lambda_;
            } else if (Lambda_ * y > limit) {
                return y * std::exp(-0.5 * Lambda_ * x * y - 0.5 * Lambda_ * y);
            } else if (Lambda_ * y < -limit) {
                return -y * std::exp(-0.5 * Lambda_ * x * y + 0.5 * Lambda_ * y);
            } else {
                return y * std::exp(-0.5 * Lambda_ * x * y) / (2 * std::sinh(0.5 * Lambda_ * y));
            }
        }

        irlib::statistics::statistics_type get_statistics() const {
            return irlib::statistics::BOSONIC;
        }

        double Lambda() const {
            return Lambda_;
        }

#ifndef SWIG

        std::shared_ptr <kernel<S>> clone() const {
            return std::shared_ptr<kernel<S>>(new bosonic_kernel<S>(Lambda_));
        }

#endif

    private:
        double Lambda_;
    };

    template<>
    class bosonic_kernel<mpreal> : public kernel<mpreal> {
    public:
        bosonic_kernel(double Lambda) : Lambda_(Lambda) {}

        virtual ~bosonic_kernel() {};

        mpreal operator()(mpreal x, mpreal y) const {
            const double limit = 200.0;
            mpreal half_Lambda = mpreal("0.5") * mpreal(Lambda_);

            if (mpfr::abs(Lambda_ * y) < 1e-30) {
                return mpfr::exp(-half_Lambda * x * y) / Lambda_;
            } else if (Lambda_ * y > limit) {
                return y * mpfr::exp(-half_Lambda * x * y - half_Lambda * y);
            } else if (Lambda_ * y < -limit) {
                return -y * mpfr::exp(-half_Lambda * x * y + half_Lambda * y);
            } else {
                return y * mpfr::exp(-half_Lambda * x * y) / (2 * mpfr::sinh(half_Lambda * y));
            }
        }

        irlib::statistics::statistics_type get_statistics() const {
            return irlib::statistics::BOSONIC;
        }

        double Lambda() const {
            return Lambda_;
        }

#ifndef SWIG

        std::shared_ptr<kernel<mpreal>> clone() const {
            return std::shared_ptr<kernel<mpreal>>(new bosonic_kernel(Lambda_));
        }

#endif

    private:
        double Lambda_;
    };

}


#include <Eigen/Dense>
#include <chrono>
#include <iostream>
#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>
#include <features.hpp>

void cfar1d(cv::Mat fft_data, int window_size, float scale, int guard_cells, int min_range, Eigen::MatrixXd &targets) {
    assert(fft_data.depth() == CV_32F);
    assert(fft_data.channels() == 1);
    auto t1 = std::chrono::high_resolution_clock::now();
    int kernel_size = window_size + guard_cells * 2 + 1;
    cv::Mat kernel = cv::Mat::ones(1, kernel_size, CV_32F) * -1 * scale / window_size;
    kernel.at<float>(0, kernel_size / 2) = 1;
    for (int i = 0; i < guard_cells; i++) {
        kernel.at<float>(0, window_size / 2 + i) = 0;
    }
    for (int i = 0; i < guard_cells; i++) {
        kernel.at<float>(0, kernel_size / 2 + 1 + i) = 0;
    }
    cv::Mat output;
    cv::filter2D(fft_data, output, -1, kernel, cv::Point(-1, -1), 0, cv::BORDER_REFLECT101);
    // Find filter responses > 0
    std::vector<cv::Point2f> t;
    for (int i = 0; i < output.rows; ++i) {
        for (int j = min_range; j < output.cols; j++) {
            if (output.at<float>(i, j) > 0) {
                t.push_back(cv::Point(i, j));
            }
        }
    }
    targets = Eigen::MatrixXd::Ones(3, t.size());
    for (uint i = 0; i < t.size(); ++i) {
        targets(0, i) = t[i].x;
        targets(1, i) = t[i].y;
    }
    auto t2 = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double> e = t2 - t1;
    std::cout << "feature extraction: " << e.count() << std::endl;
}

// Runtime: 0.035s
double cen2018features(cv::Mat fft_data, float zq, int sigma_gauss, int min_range, Eigen::MatrixXd &targets) {
    auto t1 = std::chrono::high_resolution_clock::now();

    std::vector<float> sigma_q(fft_data.rows, 0);
    // Estimate the bias and subtract it from the signal
    cv::Mat q = fft_data.clone();
    for (int i = 0; i < fft_data.rows; ++i) {
        float mean = 0;
        for (int j = 0; j < fft_data.cols; ++j) {
            mean += fft_data.at<float>(i, j);
        }
        mean /= fft_data.cols;
        for (int j = 0; j < fft_data.cols; ++j) {
            q.at<float>(i, j) = fft_data.at<float>(i, j) - mean;
        }
    }

    // Create 1D Gaussian Filter (0.09)
    assert(sigma_gauss % 2 == 1);
    int fsize = sigma_gauss * 3;
    int mu = fsize / 2;
    float sig_sqr = sigma_gauss * sigma_gauss;
    cv::Mat filter = cv::Mat::zeros(1, fsize, CV_32F);
    float s = 0;
    for (int i = 0; i < fsize; ++i) {
        filter.at<float>(0, i) = exp(-0.5 * (i - mu) * (i - mu) / sig_sqr);
        s += filter.at<float>(0, i);
    }
    filter /= s;
    cv::Mat p;
    cv::filter2D(q, p, -1, filter, cv::Point(-1, -1), 0, cv::BORDER_REFLECT101);

    // Estimate variance of noise at each azimuth (0.004)
    for (int i = 0; i < fft_data.rows; ++i) {
        int nonzero = 0;
        for (int j = 0; j < fft_data.cols; ++j) {
            float n = q.at<float>(i, j);
            if (n < 0) {
                sigma_q[i] += 2 * (n * n);
                nonzero++;
            }
        }
        if (nonzero)
            sigma_q[i] = sqrt(sigma_q[i] / nonzero);
        else
            sigma_q[i] = 0.034;
    }

    // Extract peak centers from each azimuth
    std::vector<std::vector<cv::Point2f>> t(fft_data.rows);
#pragma omp parallel for
    for (int i = 0; i < fft_data.rows; ++i) {
        std::vector<int> peak_points;
        float thres = zq * sigma_q[i];
        for (int j = min_range; j < fft_data.cols; ++j) {
            float nqp = exp(-0.5 * pow((q.at<float>(i, j) - p.at<float>(i, j)) / sigma_q[i], 2));
            float npp = exp(-0.5 * pow(p.at<float>(i, j) / sigma_q[i], 2));
            float b = nqp - npp;
            float y = q.at<float>(i, j) * (1 - nqp) + p.at<float>(i, j) * b;
            if (y > thres) {
                peak_points.push_back(j);
            } else if (peak_points.size() > 0) {
                t[i].push_back(cv::Point(i, peak_points[peak_points.size() / 2]));
                peak_points.clear();
            }
        }
        if (peak_points.size() > 0)
            t[i].push_back(cv::Point(i, peak_points[peak_points.size() / 2]));
    }

    int size = 0;
    for (uint i = 0; i < t.size(); ++i) {
        size += t[i].size();
    }
    targets = Eigen::MatrixXd::Ones(3, size);
    int k = 0;
    for (uint i = 0; i < t.size(); ++i) {
        for (uint j = 0; j < t[i].size(); ++j) {
            targets(0, k) = t[i][j].x;
            targets(1, k) = t[i][j].y;
            k++;
        }
    }

    auto t2 = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double> e = t2 - t1;
    return e.count();
}

struct Point {
    float i;
    int a;
    int r;
    Point(float i_, int a_, int r_) {i = i_; a = a_; r = r_;}
};

struct greater_than_pt {
    inline bool operator() (const Point& p1, const Point& p2) {
        return p1.i > p2.i;
    }
};

static void findRangeBoundaries(cv::Mat &s, int a, int r, int &rlow, int &rhigh) {
    rlow = r;
    rhigh = r;
    if (r > 0) {
        for (int i = r - 1; i >= 0; i--) {
            if (s.at<float>(a, i) < 0)
                rlow = i;
            else
                break;
        }
    }
    if (r < s.rows - 1) {
        for (int i = r + 1; i < s.cols; i++) {
            if (s.at<float>(a, i) < 0)
                rhigh = i;
            else
                break;
        }
    }
}

static bool checkAdjacentMarked(cv::Mat &R, int a, int start, int end) {
    int below = a - 1;
    int above = a + 1;
    if (below < 0)
        below = R.rows - 1;
    if (above >= R.rows)
        above = 0;
    for (int r = start; r <= end; r++) {
        if (R.at<float>(below, r) || R.at<float>(above, r))
            return true;
    }
    return false;
}

static void getMaxInRegion(cv::Mat &h, int a, int start, int end, int &max_r) {
    int max = -1000;
    for (int r = start; r <= end; r++) {
        if (h.at<float>(a, r) > max) {
            max = h.at<float>(a, r);
            max_r = r;
        }
    }
}

// Runtime: 0.050s
double cen2019features(cv::Mat fft_data, int max_points, int min_range, Eigen::MatrixXd &targets) {
    auto t1 = std::chrono::high_resolution_clock::now();
    // Calculate gradient along each azimuth using the Prewitt operator
    cv::Mat prewitt = cv::Mat::zeros(1, 3, CV_32F);
    prewitt.at<float>(0, 0) = -1;
    prewitt.at<float>(0, 2) = 1;
    cv::Mat g;
    cv::filter2D(fft_data, g, -1, prewitt, cv::Point(-1, -1), 0, cv::BORDER_REFLECT101);
    g = cv::abs(g);
    double maxg = 1, ming = 1;
    cv::minMaxIdx(g, &ming, &maxg);
    g /= maxg;

    // Subtract the mean from the radar data and scale it by 1 - gradient magnitude
    float mean = cv::mean(fft_data)[0];
    cv::Mat s = fft_data - mean;
    cv::Mat h = s.mul(1 - g);
    float mean_h = cv::mean(h)[0];

    // Get indices in descending order of intensity
    std::vector<Point> vec;
    for (int i = 0; i < fft_data.rows; ++i) {
        for (int j = 0; j < fft_data.cols; ++j) {
            if (h.at<float>(i, j) > mean_h)
                vec.push_back(Point(h.at<float>(i, j), i, j));
        }
    }
    std::sort(vec.begin(), vec.end(), greater_than_pt());

    // Create a matrix, R, of "marked" regions consisting of continuous regions of an azimuth that may contain a target
    int false_count = fft_data.rows * fft_data.cols;
    uint j = 0;
    int l = 0;
    cv::Mat R = cv::Mat::zeros(fft_data.rows, fft_data.cols, CV_32F);
    while (l < max_points && j < vec.size() && false_count > 0) {
        if (!R.at<float>(vec[j].a, vec[j].r)) {
            int rlow = vec[j].r;
            int rhigh = vec[j].r;
            findRangeBoundaries(s, vec[j].a, vec[j].r, rlow, rhigh);
            bool already_marked = false;
            for (int i = rlow; i <= rhigh; i++) {
                if (R.at<float>(vec[j].a, i)) {
                    already_marked = true;
                    continue;
                }
                R.at<float>(vec[j].a, i) = 1;
                false_count--;
            }
            if (!already_marked)
                l++;
        }
        j++;
    }

    std::vector<std::vector<cv::Point2f>> t(fft_data.rows);

#pragma omp parallel for
    for (int i = 0; i < fft_data.rows; i++) {
        // Find the continuous marked regions in each azimuth
        int start = 0;
        int end = 0;
        bool counting = false;
        for (int j = min_range; j < fft_data.cols; j++) {
            if (R.at<float>(i, j)) {
                if (!counting) {
                    start = j;
                    end = j;
                    counting = true;
                } else {
                    end = j;
                }
            } else if (counting) {
                // Check whether adjacent azimuths contain a marked pixel in this range region
                if (checkAdjacentMarked(R, i, start, end)) {
                    int max_r = start;
                    getMaxInRegion(h, i, start, end, max_r);
                    t[i].push_back(cv::Point(i, max_r));
                }
                counting = false;
            }
        }
    }

    int size = 0;
    for (uint i = 0; i < t.size(); ++i) {
        size += t[i].size();
    }
    targets = Eigen::MatrixXd::Ones(3, size);
    int k = 0;
    for (uint i = 0; i < t.size(); ++i) {
        for (uint j = 0; j < t[i].size(); ++j) {
            targets(0, k) = t[i][j].x;
            targets(1, k) = t[i][j].y;
            k++;
        }
    }

    auto t2 = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double> e = t2 - t1;
    return e.count();
}

// Note: the dimensions of polar_points and cart_targets may not align
// Runtime: 100 ms
double cen2019descriptors(std::vector<double> azimuths, cv::Size polar_dims, Eigen::MatrixXd polar_points,
    Eigen::MatrixXd cart_targets, float radar_resolution, float cart_resolution, int cart_pixel_width,
    cv::Mat &descriptors, int navtech_version) {

    auto t1 = std::chrono::high_resolution_clock::now();
    // Create binary grid based on polar feature locations
    cv::Mat polar_binary = cv::Mat::zeros(polar_dims.height, polar_dims.width, CV_32F);
#pragma omp parallel for
    for (uint i = 0; i < polar_points.cols(); ++i) {
        polar_binary.at<float>(polar_points(0, i), polar_points(1, i)) = 1.0;
    }
    // Convert it to cartesian
    cv::Mat cart_binary;
    radar_polar_to_cartesian(azimuths, polar_binary, radar_resolution, cart_resolution, cart_pixel_width, true,
        cart_binary, CV_32F, navtech_version);

    // int M = polar_dims.height;
    int M = 384;
    float azimuth_step = (2 * M_PI) / float(M);
    int N = 128;
    float range_step = (cart_pixel_width / 2.0) / float(N);
    float max_range_sq = pow(N * range_step, 2);

    cv::Mat d1 = cv::Mat::zeros(cart_targets.cols(), M, CV_32F);
    cv::Mat d2 = cv::Mat::zeros(cart_targets.cols(), N, CV_32F);

    std::vector<cv::Point2f> bev_points;
    convert_to_bev(cart_targets, cart_resolution, cart_pixel_width, bev_points);

#pragma omp parallel for collapse(2)
    for (int i = 0; i < cart_binary.rows; ++i) {
        for (int j = 0; j < cart_binary.cols; ++j) {
            if (cart_binary.at<float>(i, j) > 0) {
                for (uint k = 0; k < bev_points.size(); ++k) {
                    float range = pow(i - bev_points[k].y, 2) + pow(j - bev_points[k].x, 2);
                    if (range > max_range_sq)
                        continue;
                    range = sqrt(range);
                    float azimuth = atan2f(bev_points[k].y - i, j - bev_points[k].x);
                    if (azimuth < 0)
                        azimuth += 2 * M_PI;
                    int azimuth_bin = azimuth / azimuth_step;
                    int range_bin = range / range_step;
#pragma omp atomic
                    d1.at<float>(k, azimuth_bin)++;
#pragma omp atomic
                    d2.at<float>(k, range_bin)++;
                }
            }
        }
    }

    // Calculate the FFT for each azimuth, normalize the magnitude
#pragma omp parallel for
    for (uint i = 0; i < cart_targets.cols(); ++i) {
        cv::Mat row = cv::Mat::zeros(1, M, CV_32F);
        for (int j = 0; j < M; ++j) {
            row.at<float>(0, j) = d1.at<float>(i, j);
        }
        cv::Mat planes[] = {cv::Mat_<float>(row), cv::Mat::zeros(row.size(), CV_32F)};
        cv::Mat complexI;
        cv::merge(planes, 2, complexI);         // Add to the expanded another plane with zeros
        cv::dft(complexI, complexI);            // this way the result may fit in the source matrix
        cv::split(complexI, planes);
        cv::magnitude(planes[0], planes[1], planes[0]);
        cv::Mat magI = planes[0];
        cv::normalize(magI, magI, 0, 1, cv::NORM_MINMAX);
        for (int j = 0; j < M; ++j) {
            d1.at<float>(i, j) = magI.at<float>(0, j);
        }
    }

// #pragma omp parallel for
//     // Reorder with the densest column first
//     for (uint i = 0; i < cart_targets.cols(); ++i) {
//         float max = 0;
//         int max_col = 0;
//         for (int j = 0; j < M; ++j) {
//             if (d1.at<float>(i, j) > max) {
//                 max = d1.at<float>(i, j);
//                 max_col = j;
//             }
//         }
//         cv::Mat row = cv::Mat::zeros(1, M, CV_32F);
//         int k = 0;
//         for (int j = max_col; j < M; ++j) {
//             row.at<float>(0, k) = d1.at<float>(i, j);
//             k++;
//         }
//         for (int j = 0; j < max_col; ++j) {
//             row.at<float>(0, k) = d1.at<float>(i, j);
//             k++;
//         }
//         cv::normalize(row, row, 0, 1, cv::NORM_MINMAX);
//         for (int j = 0; j < M; ++j) {
//             d1.at<float>(i, j) = row.at<float>(0, j);
//         }
//     }

    // Normalize the counts for each range bin
#pragma omp parallel for
    for (uint i = 0; i < cart_targets.cols(); ++i) {
        cv::normalize(d2.row(i), d2.row(i), 0, 1, cv::NORM_MINMAX);
    }
    // cv::hconcat(d1, d2, descriptors);
    descriptors = d2;

    auto t2 = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double> e = t2 - t1;
    return e.count();
}


// Copyright 2014 Marco Guazzone (marco.guazzone@gmail.com).

// Use, modification and distribution are subject to the
// Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

// Caution: this file contains Quickbook markup as well as code
// and comments, don't change any of the special comment markups!

//[hyperexponential_more_snip1
#include <boost/math/distributions.hpp>
#include <iostream>
#include <string>

struct ds_info
{
   std::string name;
   double iat_sample_mean;
   double iat_sample_sd;
   boost::math::hyperexponential iat_he;
   double multi_lt_sample_mean;
   double multi_lt_sample_sd;
   boost::math::hyperexponential multi_lt_he;
   double single_lt_sample_mean;
   double single_lt_sample_sd;
   boost::math::hyperexponential single_lt_he;
};

// DS1 dataset
ds_info make_ds1()
{
   ds_info ds;

   ds.name = "DS1";

   // VM interarrival time distribution
   const double iat_fit_probs[] = { 0.34561,0.08648,0.56791 };
   const double iat_fit_rates[] = { 0.0008,0.00005,0.02894 };
   ds.iat_sample_mean = 2202.1;
   ds.iat_sample_sd = 2.2e+4;
   ds.iat_he = boost::math::hyperexponential(iat_fit_probs, iat_fit_rates);

   // Multi-core VM lifetime distribution
   const double multi_lt_fit_probs[] = { 0.24667,0.37948,0.37385 };
   const double multi_lt_fit_rates[] = { 0.00004,0.000002,0.00059 };
   ds.multi_lt_sample_mean = 257173;
   ds.multi_lt_sample_sd = 4.6e+5;
   ds.multi_lt_he = boost::math::hyperexponential(multi_lt_fit_probs, multi_lt_fit_rates);

   // Single-core VM lifetime distribution
   const double single_lt_fit_probs[] = { 0.09325,0.22251,0.68424 };
   const double single_lt_fit_rates[] = { 0.000003,0.00109,0.00109 };
   ds.single_lt_sample_mean = 28754.4;
   ds.single_lt_sample_sd = 1.6e+5;
   ds.single_lt_he = boost::math::hyperexponential(single_lt_fit_probs, single_lt_fit_rates);

   return ds;
}

// DS2 dataset
ds_info make_ds2()
{
   ds_info ds;

   ds.name = "DS2";

   // VM interarrival time distribution
   const double iat_fit_probs[] = { 0.38881,0.18227,0.42892 };
   const double iat_fit_rates[] = { 0.000006,0.05228,0.00081 };
   ds.iat_sample_mean = 41285.7;
   ds.iat_sample_sd = 1.1e+05;
   ds.iat_he = boost::math::hyperexponential(iat_fit_probs, iat_fit_rates);

   // Multi-core VM lifetime distribution
   const double multi_lt_fit_probs[] = { 0.42093,0.43960,0.13947 };
   const double multi_lt_fit_rates[] = { 0.00186,0.00008,0.0000008 };
   ds.multi_lt_sample_mean = 144669.0;
   ds.multi_lt_sample_sd = 7.9e+05;
   ds.multi_lt_he = boost::math::hyperexponential(multi_lt_fit_probs, multi_lt_fit_rates);

   // Single-core VM lifetime distribution
   const double single_lt_fit_probs[] = { 0.44885,0.30675,0.2444 };
   const double single_lt_fit_rates[] = { 0.00143,0.00005,0.0000004 };
   ds.single_lt_sample_mean = 599815.0;
   ds.single_lt_sample_sd = 1.7e+06;
   ds.single_lt_he = boost::math::hyperexponential(single_lt_fit_probs, single_lt_fit_rates);

   return ds;
}

// DS3 dataset
ds_info make_ds3()
{
   ds_info ds;

   ds.name = "DS3";

   // VM interarrival time distribution
   const double iat_fit_probs[] = { 0.39442,0.24644,0.35914 };
   const double iat_fit_rates[] = { 0.00030,0.00003,0.00257 };
   ds.iat_sample_mean = 11238.8;
   ds.iat_sample_sd = 3.0e+04;
   ds.iat_he = boost::math::hyperexponential(iat_fit_probs, iat_fit_rates);

   // Multi-core VM lifetime distribution
   const double multi_lt_fit_probs[] = { 0.37621,0.14838,0.47541 };
   const double multi_lt_fit_rates[] = { 0.00498,0.000005,0.00022 };
   ds.multi_lt_sample_mean = 30739.2;
   ds.multi_lt_sample_sd = 1.6e+05;
   ds.multi_lt_he = boost::math::hyperexponential(multi_lt_fit_probs, multi_lt_fit_rates);

   // Single-core VM lifetime distribution
   const double single_lt_fit_probs[] = { 0.34131,0.12544,0.53325 };
   const double single_lt_fit_rates[] = { 0.000297,0.000003,0.00410 };
   ds.single_lt_sample_mean = 44447.8;
   ds.single_lt_sample_sd = 2.2e+05;
   ds.single_lt_he = boost::math::hyperexponential(single_lt_fit_probs, single_lt_fit_rates);

   return ds;
}

void print_fitted(ds_info const& ds)
{
   const double secs_in_a_hour = 3600;
   const double secs_in_a_month = 30 * 24 * secs_in_a_hour;

   std::cout << "### " << ds.name << std::endl;
   std::cout << "* Fitted Request Interarrival Time" << std::endl;
   std::cout << " - Mean (SD): " << boost::math::mean(ds.iat_he) << " (" << boost::math::standard_deviation(ds.iat_he) << ") seconds." << std::endl;
   std::cout << " - 99th Percentile: " << boost::math::quantile(ds.iat_he, 0.99) << " seconds." << std::endl;
   std::cout << " - Probability that a VM will arrive within 30 minutes: " << boost::math::cdf(ds.iat_he, secs_in_a_hour / 2.0) << std::endl;
   std::cout << " - Probability that a VM will arrive after 1 hour: " << boost::math::cdf(boost::math::complement(ds.iat_he, secs_in_a_hour)) << std::endl;
   std::cout << "* Fitted Multi-core VM Lifetime" << std::endl;
   std::cout << " - Mean (SD): " << boost::math::mean(ds.multi_lt_he) << " (" << boost::math::standard_deviation(ds.multi_lt_he) << ") seconds." << std::endl;
   std::cout << " - 99th Percentile: " << boost::math::quantile(ds.multi_lt_he, 0.99) << " seconds." << std::endl;
   std::cout << " - Probability that a VM will last for less than 1 month: " << boost::math::cdf(ds.multi_lt_he, secs_in_a_month) << std::endl;
   std::cout << " - Probability that a VM will last for more than 3 months: " << boost::math::cdf(boost::math::complement(ds.multi_lt_he, 3.0*secs_in_a_month)) << std::endl;
   std::cout << "* Fitted Single-core VM Lifetime" << std::endl;
   std::cout << " - Mean (SD): " << boost::math::mean(ds.single_lt_he) << " (" << boost::math::standard_deviation(ds.single_lt_he) << ") seconds." << std::endl;
   std::cout << " - 99th Percentile: " << boost::math::quantile(ds.single_lt_he, 0.99) << " seconds." << std::endl;
   std::cout << " - Probability that a VM will last for less than 1 month: " << boost::math::cdf(ds.single_lt_he, secs_in_a_month) << std::endl;
   std::cout << " - Probability that a VM will last for more than 3 months: " << boost::math::cdf(boost::math::complement(ds.single_lt_he, 3.0*secs_in_a_month)) << std::endl;
}

int main()
{
   print_fitted(make_ds1());

   print_fitted(make_ds2());

   print_fitted(make_ds3());
}
//]


//          Copyright Carl Philipp Reh 2009 - 2016.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <fcppt/text.hpp>
#include <fcppt/cast/to_unsigned_fun.hpp>
#include <fcppt/io/cout.hpp>
#include <fcppt/mpl/append.hpp>
#include <fcppt/mpl/ceil_div.hpp>
#include <fcppt/mpl/contains_if.hpp>
#include <fcppt/mpl/implication.hpp>
#include <fcppt/mpl/index_of.hpp>
#include <fcppt/mpl/inner.hpp>
#include <fcppt/mpl/integral_cast.hpp>
#include <fcppt/mpl/max_value.hpp>
#include <fcppt/mpl/partial_sums.hpp>
#include <fcppt/mpl/print.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/mpl/bool.hpp>
#include <boost/mpl/equal.hpp>
#include <boost/mpl/equal_to.hpp>
#include <boost/mpl/integral_c.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/sizeof.hpp>
#include <boost/mpl/vector/vector10.hpp>
#include <boost/mpl/vector/vector10_c.hpp>
#include <ostream>
#include <type_traits>
#include <fcppt/config/external_end.hpp>

namespace
{
namespace append
{
//! [mpl_append]
typedef
boost::mpl::vector2<
	int,
	float
>
vec1;

typedef
boost::mpl::vector2<
	char,
	double
>
vec2;

typedef
fcppt::mpl::append<
	vec1,
	vec2
>::type
vec3;

static_assert(
	boost::mpl::equal<
		vec3,
		boost::mpl::vector4<
			int,
			float,
			char,
			double
		>
	>::value,
	""
);
//! [mpl_append]
}

namespace ceil_div
{
//! [mpl_ceil_div]
typedef
fcppt::mpl::ceil_div<
	unsigned,
	10u,
	3u
>::type
result;

static_assert(
	boost::mpl::equal_to<
		result,
		boost::mpl::integral_c<
			unsigned,
			4u
		>
	>::value,
	""
);
//! [mpl_ceil_div]
}

namespace contains_if
{
//! [mpl_contains_if]
typedef
boost::mpl::vector2<
	float,
	unsigned
>
vec;

typedef
fcppt::mpl::contains_if<
	vec,
	std::is_unsigned<
		boost::mpl::_1
	>
>::type
result;

static_assert(
	boost::mpl::equal_to<
		result,
		boost::mpl::true_
	>::value,
	""
);
//! [mpl_contains_if]
}

namespace implication
{
//! [mpl_implication]
typedef
fcppt::mpl::implication<
	boost::mpl::false_,
	boost::mpl::true_
>::type
result;

static_assert(
	boost::mpl::equal<
		result,
		boost::mpl::true_
	>::value,
	""
);
//! [mpl_implication]
}

namespace index_if
{
//! [mpl_index_of]
typedef
boost::mpl::vector3<
	float,
	int,
	double
>
vec;

typedef
fcppt::mpl::index_of<
	vec,
	int
>::type
result;

static_assert(
	boost::mpl::equal_to<
		result,
		boost::mpl::integral_c<
			unsigned,
			1
		>
	>::value,
	""
);
//! [mpl_index_of]
}

namespace inner
{
//! [mpl_inner]
struct func
{
	typedef int type;
};

typedef
fcppt::mpl::inner<
	func
>::type
result;

static_assert(
	std::is_same<
		result,
		int
	>::value,
	""
);
//! [mpl_inner]
}

namespace integral_cast
{
//! [mpl_integral_cast]
typedef
std::integral_constant<
	int,
	2
> integral;

typedef
fcppt::mpl::integral_cast<
	unsigned,
	fcppt::cast::to_unsigned_fun,
	integral
>::type
result;

static_assert(
	std::is_same<
		result::value_type,
		unsigned
	>::value
	&&
	result::value
	==
	2u,
	""
);
//! [mpl_integral_cast]
}

namespace max_value
{
//! [mpl_max_value]
// Calculate the maximum size of all the types
typedef
boost::mpl::vector3<
	short,
	int,
	long
>
types;

typedef
fcppt::mpl::max_value<
	types,
	boost::mpl::sizeof_<
		boost::mpl::placeholders::_1
	>
>::type
result;

static_assert(
	boost::mpl::equal_to<
		result,
		boost::mpl::sizeof_<
			long
		>
	>::value,
	""
);
//! [mpl_max_value]
}

namespace partial_sums
{
//! [mpl_partial_sums]
typedef
boost::mpl::vector3_c<
	int,
	3,
	4,
	5
>
vec;

typedef
fcppt::mpl::partial_sums<
	vec
>::type
result;

static_assert(
	boost::mpl::equal<
		result,
		boost::mpl::vector4_c<
			int,
			0,
			3,
			7,
			12
		>
	>::value,
	""
);
//! [mpl_partial_sums]
}

namespace print
{
//! [mpl_print}
void
print_vec()
{
	typedef boost::mpl::vector3<
		int,
		float,
		double
	> vec;

	// prints (int, float, double) to cout
	fcppt::mpl::print<
		vec
	>(
		fcppt::io::cout()
	)
	<<
	FCPPT_TEXT('\n');

}
//! [mpl_print}
}

}

int
main()
{
	print::print_vec();
}


#include <alglib/bimap/bimap.h>
#include <string>
#include <iostream>

using namespace std;
using namespace alglib::bimap;

int main() {

  bimap<string, int> bm;

  bm.insert("div", 1);
  bm.insert("dip", 2);

  cout << bm.get_image("div") << endl;
  cout << bm.get_image("dip") << endl;
  cout << bm.get_preimage(1) << endl;
  cout << bm.get_preimage(2) << endl;

  // Iterator tests
  cout << "Domain:  \t";
  for(auto it = bm.domain_begin(); it != bm.domain_end(); ++it)
    cout << *it << "\t";

  cout << endl;

  cout << "Codomain:\t";
  for(auto it = bm.codomain_begin(); it != bm.codomain_end(); ++it)
    cout << *it << "\t";

  cout << endl;

  return 0;
}


//STD Libraries
#include <iostream>
#include <vector>
#include <cmath>
#include <cstdlib>
#include <cstring>

//BOOST
//can replace boost with STD chrono, utility->function, and random
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/function.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/variate_generator.hpp>

using namespace std;

bool DEBUG = 0;

void TimeRoutine(string name,boost::function<int(int,int)>fun,
            int arg1,int arg2, bool doTime);
void SetAB(int, int & , int & );
bool Test(int a, int b, int d);

void args(string progname) {
   cout << progname << " [-l n -r -R n -t -d -a n -b n -h]" << endl;
   cout << endl;
   cout << "\t" << " -l n " << "\t" << " use a preset input" << endl;
   cout << "\t\t" << 0 << "\t  Book's Example " << endl;
   cout << "\t\t" << 1 << "\t  Two factorable numbers " << endl;
   cout << "\t\t" << 2 << "\t  Book's Problem " << endl;
   cout << "\t\t" << 3 << "\t  Medium Sized Numbers " << endl;
   cout << "\t\t" << 4 << "\t  Large Numbers " << endl;
   cout << "\t\t" << 5 << "\t  Very Large Numbers " << endl;
   cout << "\t" << " -r " << "\t" << " Generate random numbers" << endl;
   cout << "\t" << " -R n "<< "\t" << " Generate random numbers 2 to n" << endl;
   cout << "\t" << " -d " << "\t" << " Debug output" << endl;
   cout << "\t" << " -h " << "\t" << " This menu" << endl;
   cout << "\t" << " -t " << "\t" << " Show Timing" << endl;
   cout << "\t" << " -a n " << "\t" << " Set first number to be n" << endl;
   cout << "\t" << " -b n " << "\t" << " Set second number to be n" << endl;
   return;
}

int main(int argc, char * argv[]) {
    int a = 12;
    int b = 15;
    bool doTime = false;
    int arg;

    int i = 1;
    while (i < argc) {
        if (strcmp(argv[i],"-l")==0) {
	   i++;
	   if (i < argc) {
	       arg = -1;
               arg = atoi(argv[i]);
               SetAB(arg, a,b);
	       i++;
	   } else {
	       cout << "-l requires an integer level, using default " << endl;
	   }
	} else if (!strcmp(argv[i],"-a")) {
	   i++;
	   a = atoi(argv[i]);
	   if (a < 2) {
	      a = 12;
	   }
	   i++;
	} else if (!strcmp(argv[i],"-b")) {
	   i++;
	   b = atoi(argv[i]);
	   if (b < 2) {
	      b = 15;
	   }
	   i++;
	} else if (!strcmp(argv[i],"-t")) {
	   doTime = true;
	   i++;
	} else if (!strcmp(argv[i],"-r")) {

	   boost::mt19937 rng;
	   boost::uniform_int<> dist(2,1<<30);
	   boost::variate_generator<boost::mt19937&, boost::uniform_int<> >
	         bigDie(rng,dist);

	   rng.seed(static_cast<unsigned int>(std::time(0)));

	   a = bigDie();
	   b = bigDie();
	   i++;
	} else if (!strcmp(argv[i],"-R")) {
	   int upperLimit;

	   i++;
	   upperLimit = atoi(argv[i]);
	   i++;

	   if (upperLimit < 2) {
	      upperLimit = 1 << 30;
	   }

	   boost::mt19937 rng;
	   boost::uniform_int<> dist(2,upperLimit);
	   boost::variate_generator<boost::mt19937&, boost::uniform_int<> >
	         bigDie(rng,dist);

	   rng.seed(static_cast<unsigned int>(std::time(0)));

	   a = bigDie();
	   b = bigDie();

	} else if (!strcmp(argv[i],"-h")) {
	   args(argv[0]);
	   i++;
	} else if (!strcmp(argv[i],"-d")) {
	   DEBUG = true;
	   i++;
	} else {
	   cout << "unknown argument " << argv[i] << endl;
	   args(argv[0]);
	   i++;
	}
    }

    cout << endl;
    cout << "A = " << a << " and has " << int(log(a)/log(2)) << " bits" << endl;
    cout << "B = " << b << " and has " << int(log(b)/log(2)) << " bits" << endl;
    cout << endl;

    TimeRoutine("Euclid", Euclid_Recursive, a,b,doTime);
    TimeRoutine("Brute ", Brute, a,b,doTime);
    TimeRoutine("School", OldSchool, a,b,doTime);

    return 0;
}

void SetAB(int arg, int & a, int & b) {
       switch(arg) {
          case 0:
             // the book's example
             a = 60;
	     b = 24;
	     break;
	  case 1:
             // 2^2 x 3^3 x 5^5 x 7   and 2^2 x 3^2 x 5^4 (to test factoring)
             a = 45000;
	     b = 787500;
	     break;
	  case 2:
             // an exercise in the book
             a=31415;
	     b= 14142;
	     break;
	  case 3:
             // not to big prime number, look out old school!
             a = 80147*2*3*3;
             b = 21011*2*3*3;
	     break;
	  case 4:
             // two bigger prime numbers
             a=  4095036*2*3*5;
             b = 4095043*2*3*5;
	     break;
	  default:
	     a = 40950391 * 2 * 3;
	     b = 40950401 * 2 * 3;
       }
       return;
}

void TimeRoutine(string name, boost::function < int (int, int)> fun,
                   int arg1,int arg2, bool doTime){

    int gcd;
    boost::posix_time::ptime startTime, endTime;
    boost::posix_time::time_duration totalTime;

    startTime = boost::posix_time::microsec_clock::local_time();
    gcd = fun(arg1, arg2);
    endTime = boost::posix_time::microsec_clock::local_time();
    cout <<"\t" << name << ": " << setw(15) << gcd;
    if (doTime) {
       totalTime = endTime-startTime;
       cout << "\tTime: " << totalTime;
    }
    cout << endl;
    if (!Test(arg1, arg2, gcd)) {
        cout << endl;
        cout << "ERROR ERROR ERROR ERROR ERROR ERROR " << endl;
        cout << endl << endl << endl;
    }

    return;
}