#include <iostream>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include<vector>
#include <complex.h>
#include <algorithm>
#include "IIR_Butterworth.h"

//#define ARMA_DONT_USE_CXX11
#define ARMA_DONT_USE_CXX11_MUTEX
#include <armadillo>

#define PI 3.141592653589793

//Global variables
double fs = 2;
double u_f1;
double u_f2;
double Wn;
double Bw;

std::vector<std::complex<double>> ptemp;
std::complex<double> complex_real(1.0, 0.0);
std::complex<double> complex_imag(0.0, 1.0);
std::vector<std::complex<double>> p;

int temp_dim_arr_matr;
std::vector<std::vector<std::complex<double>> > a;
std::vector<std::complex<double>> b;
std::vector<std::complex<double>> c;

double d[1];
double t[1];

arma::cx_mat a_arma;
arma::cx_mat b_arma;
arma::cx_mat c_arma;
arma::cx_mat d_arma;

arma::cx_mat t1_arma;
arma::cx_mat t2_arma;
arma::cx_mat ad_arma;
arma::cx_mat bd_arma;
arma::cx_mat cd_arma;
arma::cx_mat dd_arma;

std::vector<double> num_filt;   //Vector where to temporarily save the numerator
std::vector<double> den_filt;   // Vector where to temporarily save the denumerator
std::vector<std::vector<double> > save_filt_coeff;  //Matrix where to save the numerator and denominator. First row is the numerator; second row is the denominator

using namespace IIR_B_F;

//Step 1: get analog, pre - warped frequencies
void IIR_Butterworth::freq_pre_wrapped(int type_filt, double Wnf_1, double Wnf_2)
{

    Bw = 0;

    switch (type_filt)
    {

        //Band-pass
    case 0:

        u_f1 = 2 * fs * tan(PI * Wnf_1 / fs);
        u_f2 = 2 * fs * tan(PI * Wnf_2 / fs);

        break;

        //Band-stop
    case 1:

        u_f1 = 2 * fs * tan(PI * Wnf_1 / fs);
        u_f2 = 2 * fs * tan(PI * Wnf_2 / fs);

        break;

        //High-pass
    case 2:

        u_f1 = 2 * fs * tan(PI * Wnf_1 / fs);

        break;

        //Low-pass
    case 3:

        u_f2 = 2 * fs * tan(PI * Wnf_2 / fs);

        break;

    }
}

//Step 2: convert to low-pass prototype estimate
void IIR_Butterworth::Wn_f1_Wn_f2(int type_filt, double u_f1, double u_f2)
{

    switch (type_filt)
    {

        //Band-pass
    case 0:
        Bw = u_f2 - u_f1;
        Wn = sqrt(u_f1 * u_f2);

        break;

        //Band-stop
    case 1:

        Bw = u_f2 - u_f1;
        Wn = sqrt(u_f1 * u_f2);

        break;

        //Low-pass
    case 2:

        Wn = u_f1;

        break;

        //High-pass
    case 3:

        Wn = u_f2;

        break;

    }
}

//Step 3: Get N - th order Butterworth analog lowpass prototype
void IIR_Butterworth::buttap(int order_filt)
{
    double order_filt_exp = (double)order_filt;
    int temp_length_vec = 0;
    int kkk = 1;
    do {

        temp_length_vec++;
        kkk += 2;

    } while (kkk <= order_filt - 1);

    //Initialize the vector "ptemp"
    for (int i = 0; i < temp_length_vec; i++)
    {

        ptemp.push_back(0);

    }

    int track_cell = 0;
    for (double kk = 0; kk < (double)order_filt - 1; kk += 2)
    {

        ptemp.at(track_cell) = exp(complex_imag * ((PI * (kk + 1)) / (2 * order_filt_exp) + PI / 2));

        track_cell++;

    }

    //Initialize the vector "p"
    for (int i = 0; i < order_filt; i++)

    {

        p.push_back(0);

    }

    double temp_rem = remainder(order_filt, 2);

    if (temp_rem != 0)
    {

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

            if (kk < order_filt - 1)
            {

                p.at(kk) = complex_imag;

            }

            else
            {

                p.at(kk) = -complex_real;

            }

        }

    }

    else
    {

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

            p.at(kk) = complex_imag;

        }

    }

    if (order_filt > 1)
    {
        track_cell = 0;
        for (int kk = 0; kk < temp_length_vec * 2; kk += 2)
        {

            p.at(kk) = ptemp.at(track_cell);
            p.at(kk + 1) = conj(ptemp.at(track_cell));

            track_cell++;

        }
    }

}

//Step 4: Transform to state-space
//Intermidiate step: calculate the coefficients of the polynomial (based on Matlab code)
std::vector<std::complex<double>> IIR_Butterworth::poly(std::vector<std::complex<double>> temp_array_poly, int col_poly)
{
    std::vector<std::complex<double>> coeff_pol_f(col_poly + 1);
    coeff_pol_f.at(0) = 1;

    for (int ll = 0; ll < col_poly; ll++)
    {

        int yy = 0;

        do
        {

            coeff_pol_f.at(ll + 1 - yy) = coeff_pol_f.at(ll + 1 - yy) - temp_array_poly.at(ll) * coeff_pol_f.at(ll - yy);
            yy++;

        } while (yy <= ll);

    }

    return coeff_pol_f;

}

//Calculate the factorial of the given number
int IIR_Butterworth::factorial(int i)
{

    if (i <= 1)
    {

        return 1;

    }

    return i * factorial(i - 1);

}

//Method to calculate all the possible combinations. Code taken and slightly modified from:
//http://rosettacode.org/wiki/Combinations#C.2B.2B
std::vector<std::vector<int> > IIR_Butterworth::combination_method(int N, int K, int comb_n)
{

    int rows_f = 0;

    std::vector<std::vector<int> > matrix_comb_f;

        std::vector<int> temp_v;

        for (int ff = 0; ff < K; ff++)
        {

            temp_v.push_back(0);

        }

        for (int hh = 0; hh < comb_n; hh++)
        {

            matrix_comb_f.push_back(temp_v);

        }

    std::string bitmask(K, 1); // K leading 1's
    bitmask.resize(N, 0); // N-K trailing 0's

    int col_f;
    do {

        col_f = 0;

        for (int i = 0; i < N; ++i) // [0..N-1] integers
        {

            if (bitmask[i])

            {

                matrix_comb_f[rows_f][col_f] = i;

                col_f++;

            }
        }

        rows_f++;

    } while (std::prev_permutation(bitmask.begin(), bitmask.end()));

    return matrix_comb_f;

}

//Intermidiate step: calculate the coefficients of the polynomial (Bernard Brooks' paper (2016))
std::vector<std::complex<double>> IIR_Butterworth::char_poly(arma::cx_mat temp_matr_poly, int row_col)
{

    std::vector<std::complex<double>> coeff_pol_ff(row_col + 1);
    arma::cx_mat temp_val = arma::zeros <arma::cx_mat>(1,1);
    double num_det;
    arma::cx_mat temp_matr = arma::zeros <arma::cx_mat>(row_col, row_col);

    std::vector<std::vector<int> > matrix_comb;

    for (int kk = 0; kk < row_col + 1; kk++)

    {

        if (kk == 0)
        {

            coeff_pol_ff[row_col - kk] = pow(-1, row_col) * arma::det(temp_matr_poly);

        }

        else
        {

            temp_val(0, 0) = 0;

            num_det = (double)factorial(row_col) / (double)(factorial(row_col - kk) * (double)factorial(kk));  //Calculate the number of combinations

        //Catching the situation where the denominator is zero. Assign the default value of 10^10 to the denominator
            if (isnan(num_det))
            {

                for (int ff = 0; ff < row_col + 1; ff++)
                {

                    coeff_pol_ff[ff] = pow(10, 10);

                }

                break;

            }

            if (num_det < 1)    //Handling the situation where numerical overflow generates a negative or zero value for the num_det because of the instability of the filter
            {

                for (int ff = 0; ff < row_col + 1; ff++)
                {

                    coeff_pol_ff[ff] = pow(10, 10);

                }

                break;

            }

            std::vector<int> temp_v;

            for (int ff = 0; ff < num_det; ff++)
            {

                temp_v.push_back(0);

            }

            for (int hh = 0; hh < num_det; hh++)
            {

                matrix_comb.push_back(temp_v);

            }

            if (num_det - (int)num_det == 0)
            {
                // Generate the combinations
                matrix_comb = combination_method(row_col, kk, (int)num_det);

                for (int mm = 0; mm < num_det; mm++)

                {

                    temp_matr = temp_matr_poly;

                    for (int pp = 0; pp < row_col; pp++)
                    {

                        temp_matr(matrix_comb[mm][0], pp) = 0;
                        temp_matr(pp, matrix_comb[mm][0]) = 0;
                        temp_matr(matrix_comb[mm][0], matrix_comb[mm][0]) = -1;

                    }

                    for (int nn = 1; nn < kk; nn++)
                    {

                        for (int pp = 0; pp < row_col; pp++)
                        {

                            temp_matr(matrix_comb[mm][nn], pp) = 0;
                            temp_matr(pp, matrix_comb[mm][nn]) = 0;
                            temp_matr(matrix_comb[mm][nn], matrix_comb[mm][nn]) = -1;

                        }

                    }

                    temp_val(0, 0) += det(temp_matr);

                }

                coeff_pol_ff[row_col - kk] = pow(-1, row_col) * temp_val(0, 0);

            }

            else
            {

                for (int ff = 0; ff < row_col + 1; ff++)
                {

                    coeff_pol_ff[ff] = pow(10, 10);

                }

                break;

            }

        }

    }

    return coeff_pol_ff;

}

//Step 4: Transform to state-space
void IIR_Butterworth::zp2ss(int order_filt)
{

    //Order the pairs of complex conjugate. The pairs with the smallest real part come first. Within pairs, the ones with the negative imaginary part comes first
    std::complex<double> temp_max;

    //Using the selection sort algorithm to order based on the real part
    double order_filt_d = (double)order_filt;
    double temp_rem;
    if (remainder(order_filt, 2) == 0)
    {

        temp_rem = order_filt_d;

    }

    else
    {

        temp_rem = order_filt_d - 1;

    }

    int min_real;
    for (int kk = 0; kk < temp_rem - 1; kk++)
    {
        min_real = kk;

        for (int jj = kk + 1; jj < temp_rem; jj++)
        {
            if (real(p[jj]) < real(p[min_real]))
            {

                min_real = jj;

                temp_max = p[kk];
                p[kk] = p[min_real];
                p[min_real] = temp_max;

            }
        }
    }

    //Using the selection sort algorithm to order the values based on the imaginary part
    for (int kk = 0; kk < temp_rem - 1; kk += 2)
    {
        min_real = kk;

        if (imag(p[kk]) > imag(p[kk + 1]))
        {

            min_real = kk + 1;

            temp_max = p[kk];
            p[kk] = p[min_real];
            p[min_real] = temp_max;

        }
    }

    // Initialize state - space matrices for running series
    d[0] = 1;

    int track_index = 1;

    // Take care of any left over unmatched pole pairs.
        // H(s) = 1 / (s ^ 2 + den(2)s + den(3))
    std::vector<std::complex<double>> temp_poly_p;

    double wn = 1;

    if (order_filt > 1)
    {

        double b1[2] = { 1,0 };
        double c1[2] = { 0,1 };
        double d1 = 0;

        temp_rem = remainder(order_filt, 2);
        int order_filt_temp;
        int dim_matr;
        std::vector<std::vector<std::complex<double>> > temp_matrix_a;    //Temporary matrix where to save the coefficients at each interaction
        int coeff_numb = 3;

        if (temp_rem == 0)
        {

            order_filt_temp = order_filt;
            dim_matr = order_filt_temp / 2;

                std::vector<std::complex<double>> temp_v;

                for (int ll = 0; ll < coeff_numb; ll++)
                {

                    temp_v.push_back(0);

                }

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

                    temp_matrix_a.push_back(temp_v);

                }

        }

        else
        {

            order_filt_temp = order_filt - 1;
            dim_matr = order_filt_temp / 2;

                std::vector<std::complex<double>> temp_v;

                for (int ll = 0; ll < coeff_numb; ll++)
                {

                    temp_v.push_back(0);

                }

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

                    temp_matrix_a.push_back(temp_v);

                }

        }

        int track_cycles = 0;
        int temp_val_pos_check;
        int dim_poly_p = 2;

        for (int i = 0; i < dim_poly_p; i++)
        {

            temp_poly_p.push_back(0);

        }

        temp_dim_arr_matr = dim_poly_p + (order_filt % 2);

        while (track_index < order_filt_temp)
        {

            for (int rr = track_index - 1; rr < track_index + 1; rr++)
            {

                temp_poly_p[rr - track_index + 1] = p[rr];

            }

            std::vector<std::complex<double>> coeff_pol(dim_poly_p + 1);

            coeff_pol = poly(temp_poly_p, dim_poly_p);

            for (int qq = 0; qq < coeff_numb; qq++)
            {

                temp_matrix_a[track_cycles][qq] = -real(coeff_pol[qq]);

            }

            //Update the state-space arrays/matrix
            track_cycles += 1;

                std::vector<std::complex<double>> temp_v;

                for (int ll = 0; ll < temp_dim_arr_matr; ll++)
                {

                    temp_v.push_back(0);

                }

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

                    a.push_back(temp_v);

                }

            int track_index_coeff = 0;

            if (remainder(order_filt, 2) == 0)
            {
                ///////////////////////////////////////////////////////////////////////////////////////////////
                //Even number of poles
                for (int kk = 0; kk < temp_dim_arr_matr; kk++)
                {

                    for (int gg = 0; gg < temp_dim_arr_matr; gg++)

                    {

                        temp_val_pos_check = kk - gg;

                        switch (temp_val_pos_check)
                        {
                        case 1:

                            a[kk][gg] = 1;

                            break;

                        case 0:

                            if ((remainder(kk + 1, 2) != 0) || (kk == 0))
                            {

                                a[kk][gg] = temp_matrix_a[track_index_coeff][1];

                            }

                            else
                            {

                                a[kk][gg] = 0;

                            }

                            break;

                        case -1:

                            if ((remainder(kk + 1, 2) != 0) || (kk == 0))
                            {

                                a[kk][gg] = temp_matrix_a[track_index_coeff][2];
                                track_index_coeff++;

                            }

                            else
                            {

                                a[kk][gg] = 0;

                            }
                            break;

                        default:

                            a[kk][gg] = 0;

                            break;

                        }

                    }

                }
                ///////////////////////////////////////////////////////////////////////////////////////////////
            }

            else
            {
                ///////////////////////////////////////////////////////////////////////////////////////////////
                //Odd number of poles
                for (int kk = 0; kk < temp_dim_arr_matr; kk++)
                {

                    for (int gg = 0; gg < temp_dim_arr_matr; gg++)

                    {

                        temp_val_pos_check = kk - gg;

                        switch (temp_val_pos_check)
                        {
                        case 1:

                            a[kk][gg] = 1;

                            break;

                        case 0:

                            if (kk == 0)
                            {

                                a[kk][gg] = -1;

                            }

                            else
                            {

                                if ((remainder(kk + 1, 2) == 0))
                                {

                                    a[kk][gg] = temp_matrix_a[track_index_coeff][1];

                                }

                                else
                                {

                                    a[kk][gg] = 0;

                                }
                            }

                            break;

                        case -1:

                            if ((remainder(kk + 1, 2) == 0))
                            {

                                a[kk][gg] = temp_matrix_a[track_index_coeff][2];
                                track_index_coeff++;

                            }

                            else
                            {

                                a[kk][gg] = 0;

                            }
                            break;

                        default:

                            a[kk][gg] = 0;

                            break;

                        }

                    }

                }

            }
             ///////////////////////////////////////////////////////////////////////////////////////////////

             //Initialize the vectors "b" and "c"
            for (int i = 0; i < temp_dim_arr_matr; i++)
            {

                b.push_back(0);
                c.push_back(0);

            }

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

                if (kk == 0)
                {

                    b[kk] = 1;

                }

                else
                {

                    b[kk] = 0;

                }

            }

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

                if (kk == temp_dim_arr_matr - 1)
                {

                    c[kk] = 1;

                }

                else
                {

                    c[kk] = 0;

                }

            }

            track_index += 2;

            if (track_index < order_filt_temp)
            {

                //Clean up the matrix "a" and the arrays "b" and "c", so they can be re-initialized
                a.erase(a.begin(), a.begin() + temp_dim_arr_matr);
                b.erase(b.begin(), b.begin() + temp_dim_arr_matr);
                c.erase(c.begin(), c.begin() + temp_dim_arr_matr);

            }

            dim_matr += 2;
            temp_dim_arr_matr += 2;

        }

    }

    else
    {

        std::vector<std::complex<double>> temp_v;
        temp_v.push_back(0);
        a.push_back(temp_v);
        a[0][0] = p[0];

        b.push_back(1);
        c.push_back(1);

    }

    d[0] = 0;

}

//Extract the coefficients of the band pass filter
std::vector<std::vector<double> > IIR_Butterworth::lp2bp(double W_f1, double W_f2, int order_filt)
{

    //Check that the low cut-off frequency is higher than the low cut-off frequency
    if (W_f2 <= W_f1)
    {

        throw new std::exception("The low cut-off frequency needs to be higher than the low cut-off frequency");

    }

    //Check that the normalized frequencies are within the correct range of values
    if ((W_f1 <= 0) || (W_f1 >= 1) || (W_f2 <= 0) || (W_f2 >= 1))
    {

        throw new std::exception("Cut-off frequencies must be in the (0,1) range");

    }

    //Check that the order of the filter is > 0
    if (order_filt <= 0)
    {

        throw new std::exception("The order of the filter must be > 0");

    }

    //Clean up the global variables for a new analysis
    if (save_filt_coeff.size() > 0)
    {

        save_filt_coeff.erase(save_filt_coeff.begin(), save_filt_coeff.begin() + save_filt_coeff.size());
        ptemp.erase(ptemp.begin(), ptemp.begin() + ptemp.size());
        p.erase(p.begin(), p.begin() + p.size());

        a.erase(a.begin(), a.begin() + a.size());
        b.erase(b.begin(), b.begin() + b.size());
        c.erase(c.begin(), c.begin() + c.size());

        num_filt.erase(num_filt.begin(), num_filt.begin() + num_filt.size());
        den_filt.erase(den_filt.begin(), den_filt.begin() + den_filt.size());

    }

        std::vector<double> temp_v;

        for (int ff = 0; ff < 2*order_filt + 1; ff++)
        {

            temp_v.push_back(0);

        }

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

            save_filt_coeff.push_back(temp_v);

        }

    int type_filt = 0;

    //Step 1: get analog, pre - warped frequencies
    freq_pre_wrapped(type_filt, W_f1, W_f2);

    //Step 2: convert to low-pass prototype estimate
    Wn_f1_Wn_f2(type_filt, u_f1, u_f2);

    //Step 3: Get N - th order Butterworth analog lowpass prototype
    buttap(order_filt);

    //Step 4: Transform to state-space
    zp2ss(order_filt);

    if (order_filt > 1)
    {

        temp_dim_arr_matr -= 2;

    }

    else
    {

        temp_dim_arr_matr = order_filt;

    }

    //Copy the values of the matrix/arrays "arma" matrix/array in order to compute the pseudo-inverse of the matrix and other matrix operations
    a_arma = arma::zeros <arma::cx_mat>(2 * temp_dim_arr_matr, 2 * temp_dim_arr_matr);
    arma::cx_mat a_arma_p_eye = arma::zeros <arma::cx_mat>(temp_dim_arr_matr, temp_dim_arr_matr);
    a_arma_p_eye = a_arma_p_eye.eye();
    arma::cx_mat a_arma_n_eye = arma::zeros <arma::cx_mat>(temp_dim_arr_matr, temp_dim_arr_matr);
    a_arma_n_eye = -a_arma_p_eye.eye();
    b_arma = arma::zeros <arma::cx_mat>(2 * temp_dim_arr_matr, 1);
    c_arma = arma::zeros <arma::cx_mat>(1, 2 * temp_dim_arr_matr);
    d_arma = arma::zeros <arma::cx_mat>(1, 1);

    double q = Wn / Bw;
    for (int kk = 0; kk < 2 * temp_dim_arr_matr; kk++)
    {
        if (kk < temp_dim_arr_matr)
        {

            b_arma(kk, 0) = b[kk] * Wn / q;
            c_arma(0, kk) = c[kk];

        }

        for (int ll = 0; ll < 2 * temp_dim_arr_matr; ll++)
        {

            if (kk < temp_dim_arr_matr)
            {

                if (ll < temp_dim_arr_matr)

                {

                    a_arma(kk, ll) = Wn * a[kk][ll] / q;

                }

                else
                {

                    a_arma(kk, ll) = Wn * a_arma_p_eye(kk, ll - temp_dim_arr_matr);

                }
            }

            else
            {
                if (ll < temp_dim_arr_matr)
                {

                    a_arma(kk, ll) = Wn * a_arma_n_eye(kk - temp_dim_arr_matr, ll);

                }
            }

        }

    }

    d_arma = d_arma;

    //Step 5: Use Bilinear transformation to find discrete equivalent
    bilinear(a_arma, b_arma, c_arma, d_arma, fs, type_filt);

    //Step 6: Transform to zero-pole-gain and polynomial forms
    zero_pole_gain(ad_arma, type_filt, order_filt, Wn, Bw);

    return save_filt_coeff;

}

//Extract the coefficients of the band stop filter
std::vector<std::vector<double> > IIR_Butterworth::lp2bs(double W_f1, double W_f2, int order_filt)
{

    //Check that the low cut-off frequency is higher than the low cut-off frequency
    if (W_f2 <= W_f1)
    {

        throw new std::exception("The low cut-off frequency needs to be higher than the low cut-off frequency");

    }

    //Check that the normalized frequencies are within the correct range of values
    if ((W_f1 <= 0) || (W_f1 >= 1) || (W_f2 <= 0) || (W_f2 >= 1))
    {

        throw new std::exception("Cut-off frequencies must be in the (0,1) range");

    }

    //Check that the order of the filter is > 0
    if (order_filt <= 0)
    {

        throw new std::exception("The order of the filter must be > 0");

    }

    //Clean up the global variables for a new analysis
    if (save_filt_coeff.size() > 0)
    {

        save_filt_coeff.erase(save_filt_coeff.begin(), save_filt_coeff.begin() + save_filt_coeff.size());
        ptemp.erase(ptemp.begin(), ptemp.begin() + ptemp.size());
        p.erase(p.begin(), p.begin() + p.size());

        a.erase(a.begin(), a.begin() + a.size());
        b.erase(b.begin(), b.begin() + b.size());
        c.erase(c.begin(), c.begin() + c.size());

        num_filt.erase(num_filt.begin(), num_filt.begin() + num_filt.size());
        den_filt.erase(den_filt.begin(), den_filt.begin() + den_filt.size());

    }

        std::vector<double> temp_v;

        for (int ff = 0; ff < 2 * order_filt + 1; ff++)
        {

            temp_v.push_back(0);

        }

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

            save_filt_coeff.push_back(temp_v);

        }

    int type_filt = 1;

    //Step 1: get analog, pre - warped frequencies
    freq_pre_wrapped(type_filt, W_f1, W_f2);

    //Step 2: convert to low-pass prototype estimate
    Wn_f1_Wn_f2(type_filt, u_f1, u_f2);

    //Step 3: Get N - th order Butterworth analog lowpass prototype
    buttap(order_filt);

    //Step 4: Transform to state-space
    zp2ss(order_filt);

    if (order_filt > 1)
    {

        temp_dim_arr_matr -= 2;

    }

    else
    {

        temp_dim_arr_matr = order_filt;

    }

    //Copy the values of the matrix/arrays "arma" matrix/array in order to compute the pseudo-inverse of the matrix and other matrix operations
    a_arma = arma::zeros <arma::cx_mat>(2 * temp_dim_arr_matr, 2 * temp_dim_arr_matr);
    arma::cx_mat a_arma_p_eye = arma::zeros <arma::cx_mat>(temp_dim_arr_matr, temp_dim_arr_matr);
    a_arma_p_eye = a_arma_p_eye.eye();
    arma::cx_mat a_arma_n_eye = arma::zeros <arma::cx_mat>(temp_dim_arr_matr, temp_dim_arr_matr);
    a_arma_n_eye = -a_arma_p_eye.eye();
    b_arma = arma::zeros <arma::cx_mat>(2 * temp_dim_arr_matr, 1);
    c_arma = arma::zeros <arma::cx_mat>(1, 2 * temp_dim_arr_matr);
    d_arma = arma::zeros <arma::cx_mat>(1, 1);

    arma::cx_mat a_arma_pinv = arma::zeros <arma::cx_mat>(temp_dim_arr_matr, temp_dim_arr_matr);
    arma::cx_mat b_arma_temp = arma::zeros <arma::cx_mat>(temp_dim_arr_matr, 1);
    arma::cx_mat c_arma_temp = arma::zeros <arma::cx_mat>(1, temp_dim_arr_matr);

    arma::cx_mat a_b_arma_temp = arma::zeros <arma::cx_mat>(temp_dim_arr_matr, 1);
    arma::cx_mat c_a_arma_temp = arma::zeros <arma::cx_mat>(1, temp_dim_arr_matr);

    for (int kk = 0; kk < temp_dim_arr_matr; kk++)
    {
        b_arma_temp(kk, 0) = b[kk];
        c_arma_temp(0, kk) = c[kk];

        for (int ll = 0; ll < temp_dim_arr_matr; ll++)
        {

            a_arma_pinv(kk, ll) = a[kk][ll];

        }

    }

    double q = Wn / Bw;

    try
    {

        a_arma_pinv = (Wn / q) * arma::pinv(a_arma_pinv);

    }

    catch (std::runtime_error)
    {

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

            for (int hh = 0; hh < 2 * order_filt + 1; hh++)
            {

                save_filt_coeff[kk][hh] = pow(10, 10);

            }

        }

        return save_filt_coeff;

    }

    a_b_arma_temp = (Wn) * (a_arma_pinv * b_arma_temp) / q;
    c_a_arma_temp = c_arma_temp * a_arma_pinv;

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

            b_arma(kk, 0) = -a_b_arma_temp(kk, 0);
            c_arma(0, kk) = c_a_arma_temp(0, kk);

        }

        for (int ll = 0; ll < 2 * temp_dim_arr_matr; ll++)
        {

            if (kk < temp_dim_arr_matr)
            {

                if (ll < temp_dim_arr_matr)

                {

                    a_arma(kk, ll) = a_arma_pinv(kk, ll);

                }

                else
                {

                    a_arma(kk, ll) = Wn * a_arma_p_eye(kk, ll - temp_dim_arr_matr);

                }
            }

            else
            {
                if (ll < temp_dim_arr_matr)
                {

                    a_arma(kk, ll) = Wn * a_arma_n_eye(kk - temp_dim_arr_matr, ll);

                }
            }

        }

    }

    d_arma = d[0] + c_a_arma_temp * b_arma_temp;

    //Step 5: Use Bilinear transformation to find discrete equivalent
    bilinear(a_arma, b_arma, c_arma, d_arma, fs, type_filt);

    //Step 6: Transform to zero-pole-gain and polynomial forms
    zero_pole_gain(ad_arma, type_filt, order_filt, Wn, Bw);

    return save_filt_coeff;

}

//Extract the coefficients of the high pass filter
std::vector<std::vector<double> > IIR_Butterworth::lp2hp(double W_f2, int order_filt)
{

    //Check that the normalized frequencies are within the correct range of values
    if ((W_f2 <= 0) || (W_f2 >= 1))
    {

        throw new std::exception("Cut-off frequencies must be in the (0,1) range");

    }

    //Check that the order of the filter is > 0
    if (order_filt <= 0)
    {

        throw new std::exception("The order of the filter must be > 0");

    }

    //Clean up the global variables for a new analysis
    if (save_filt_coeff.size() > 0)
    {

        save_filt_coeff.erase(save_filt_coeff.begin(), save_filt_coeff.begin() + save_filt_coeff.size());
        ptemp.erase(ptemp.begin(), ptemp.begin() + ptemp.size());
        p.erase(p.begin(), p.begin() + p.size());

        a.erase(a.begin(), a.begin() + a.size());
        b.erase(b.begin(), b.begin() + b.size());
        c.erase(c.begin(), c.begin() + c.size());

        num_filt.erase(num_filt.begin(), num_filt.begin() + num_filt.size());
        den_filt.erase(den_filt.begin(), den_filt.begin() + den_filt.size());

    }

    std::vector<double> temp_v;

    for (int ff = 0; ff < 2 * order_filt; ff++)
    {

        temp_v.push_back(0);

    }

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

        save_filt_coeff.push_back(temp_v);

    }

    int type_filt = 2;

    //Step 1: get analog, pre - warped frequencies
    freq_pre_wrapped(type_filt, W_f2, 0);

    //Step 2: convert to low-pass prototype estimate
    Wn_f1_Wn_f2(type_filt, u_f1, u_f2);

    //Step 3: Get N - th order Butterworth analog lowpass prototype
    buttap(order_filt);

    //Step 4: Transform to state-space
    zp2ss(order_filt);

    if (order_filt > 1)
    {

        temp_dim_arr_matr -= 2;

    }

    else
    {

        temp_dim_arr_matr = order_filt;

    }

    //Copy the values of the matrix/arrays "arma" matrix/array in order to compute the pseudo-inverse of the matrix and other matrix operations
    a_arma = arma::zeros <arma::cx_mat>(temp_dim_arr_matr, temp_dim_arr_matr);
    b_arma = arma::zeros <arma::cx_mat>(temp_dim_arr_matr, 1);
    c_arma = arma::zeros <arma::cx_mat>(1, temp_dim_arr_matr);
    d_arma = arma::zeros <arma::cx_mat>(1, 1);

    for (int kk = 0; kk < temp_dim_arr_matr; kk++)
    {
        b_arma(kk, 0) = b[kk];
        c_arma(0, kk) = c[kk];

        for (int ll = 0; ll < temp_dim_arr_matr; ll++)
        {

            a_arma(kk, ll) = a[kk][ll];

        }

    }

    try
    {

        d_arma = d_arma - (c_arma * arma::pinv(a_arma)) * b_arma;

    }

    catch (std::runtime_error)
    {

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

            for (int hh = 0; hh < order_filt + 1; hh++)
            {

                save_filt_coeff[kk][hh] = pow(10, 10);

            }

        }

        return save_filt_coeff;

    }

    c_arma = c_arma * arma::pinv(a_arma);
    b_arma = -Wn * arma::pinv(a_arma) * b_arma;
    a_arma = Wn * arma::pinv(a_arma);

    //Step 5: Use Bilinear transformation to find discrete equivalent
    bilinear(a_arma, b_arma, c_arma, d_arma, fs, type_filt);

    //Step 6: Transform to zero-pole-gain and polynomial forms
    zero_pole_gain(ad_arma, type_filt, order_filt, Wn, Bw);

    return save_filt_coeff;

}

//Extract the coefficients of the low pass filter
std::vector<std::vector<double> > IIR_Butterworth::lp2lp(double W_f1, int order_filt)
{
    //Check that the normalized frequencies are within the correct range of values
    if ((W_f1 <= 0) || (W_f1 >= 1))
    {

        throw new std::exception("Cut-off frequencies must be in the (0,1) range");

    }

    //Check that the order of the filter is > 0
    if (order_filt <= 0)
    {

        throw new std::exception("The order of the filter must be > 0");

    }

    //Clean up the global variables for a new analysis
    if (save_filt_coeff.size() > 0)
    {

        save_filt_coeff.erase(save_filt_coeff.begin(), save_filt_coeff.begin() + save_filt_coeff.size());
        ptemp.erase(ptemp.begin(), ptemp.begin() + ptemp.size());
        p.erase(p.begin(), p.begin() + p.size());

        a.erase(a.begin(), a.begin() + a.size());
        b.erase(b.begin(), b.begin() + b.size());
        c.erase(c.begin(), c.begin() + c.size());

        num_filt.erase(num_filt.begin(), num_filt.begin() + num_filt.size());
        den_filt.erase(den_filt.begin(), den_filt.begin() + den_filt.size());

    }

        std::vector<double> temp_v;

        for (int ff = 0; ff < 2 * order_filt; ff++)
        {

            temp_v.push_back(0);

        }

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

            save_filt_coeff.push_back(temp_v);

        }

    int type_filt = 3;

    //Step 1: get analog, pre - warped frequencies
    freq_pre_wrapped(type_filt, 0, W_f1);

    //Step 2: convert to low-pass prototype estimate
    Wn_f1_Wn_f2(type_filt, u_f1, u_f2);

    //Step 3: Get N - th order Butterworth analog lowpass prototype
    buttap(order_filt);

    //Step 4: Transform to state-space
    zp2ss(order_filt);

    if (order_filt > 1)
    {

        temp_dim_arr_matr -= 2;

    }

    else
    {

        temp_dim_arr_matr = order_filt;

    }

    //Copy the values of the matrix/arrays "arma" matrix/array in order to compute the pseudo-inverse of the matrix and other matrix operations
    a_arma = arma::zeros <arma::cx_mat>(temp_dim_arr_matr, temp_dim_arr_matr);
    b_arma = arma::zeros <arma::cx_mat>(temp_dim_arr_matr, 1);
    c_arma = arma::zeros <arma::cx_mat>(1, temp_dim_arr_matr);
    d_arma = arma::zeros <arma::cx_mat>(1, 1);

    for (int kk = 0; kk < temp_dim_arr_matr; kk++)
    {
        b_arma(kk, 0) = b[kk];
        c_arma(0, kk) = c[kk];

        for (int ll = 0; ll < temp_dim_arr_matr; ll++)
        {

            a_arma(kk, ll) = a[kk][ll];

        }

    }

    d_arma = d_arma;
    c_arma = c_arma;
    b_arma = Wn * b_arma;
    a_arma = Wn * a_arma;

    //Step 5: Use Bilinear transformation to find discrete equivalent
    bilinear(a_arma, b_arma, c_arma, d_arma, fs, type_filt);

    //Step 6: Transform to zero-pole-gain and polynomial forms
    zero_pole_gain(ad_arma, type_filt, order_filt, Wn, Bw);

    return save_filt_coeff;

}

//Step 5: Use Bilinear transformation to find discrete equivalent
void IIR_Butterworth::bilinear(arma::cx_mat a_arma_f, arma::cx_mat b_arma_f, arma::cx_mat c_arma_f, arma::cx_mat d_arma_f, double fs_f, int type_filt_f)
{

    double t_arma;
    double r_arma;

    if (type_filt_f > 1)
    {
        t1_arma = arma::zeros <arma::cx_mat>(temp_dim_arr_matr, temp_dim_arr_matr);
        t2_arma = arma::zeros <arma::cx_mat>(temp_dim_arr_matr, temp_dim_arr_matr);
        ad_arma = arma::zeros <arma::cx_mat>(temp_dim_arr_matr, temp_dim_arr_matr);
        bd_arma = arma::zeros <arma::cx_mat>(temp_dim_arr_matr, 1);
        cd_arma = arma::zeros <arma::cx_mat>(1, temp_dim_arr_matr);
        dd_arma = arma::zeros <arma::cx_mat>(1, 1);
    }

    else
    {

        t1_arma = arma::zeros <arma::cx_mat>(2 * temp_dim_arr_matr, 2 * temp_dim_arr_matr);
        t2_arma = arma::zeros <arma::cx_mat>(2 * temp_dim_arr_matr, 2 * temp_dim_arr_matr);
        ad_arma = arma::zeros <arma::cx_mat>(2 * temp_dim_arr_matr, 2 * temp_dim_arr_matr);
        bd_arma = arma::zeros <arma::cx_mat>(2 * temp_dim_arr_matr, 1);
        cd_arma = arma::zeros <arma::cx_mat>(1, 2 * temp_dim_arr_matr);
        dd_arma = arma::zeros <arma::cx_mat>(1, 1);

    }

    try
    {
        t_arma = (1 / fs_f);
        r_arma = sqrt(t_arma);
        t1_arma = t1_arma.eye() + a_arma_f * t_arma * 0.5; //t1_arma.eye()
        t2_arma = t2_arma.eye() - a_arma_f * t_arma * 0.5;
        ad_arma = t1_arma * arma::pinv(t2_arma);
        bd_arma = (t_arma / r_arma) * arma::solve(t2_arma, b_arma_f);
        cd_arma = (r_arma * c_arma_f) * arma::pinv(t2_arma);
        dd_arma = (c_arma_f * arma::pinv(t2_arma)) * b_arma_f * (t_arma / 2) + d_arma_f;
    }

    catch (std::runtime_error)
    {

    }
}

//Step 6: Transform to zero-pole-gain and polynomial forms
void IIR_Butterworth::zero_pole_gain(arma::cx_mat a_arma_f, int type_filt_f, int order_filt_f, double Wn_f_f, double Bw_f)
{

    int dim_array;

    if (type_filt_f > 1)
    {

        //Initialize the vectors "num_filt" and "den_filt"
        for (int i = 0; i < order_filt_f + 1; i++)
        {

            num_filt.push_back(0);
            den_filt.push_back(0);

        }

        dim_array = temp_dim_arr_matr;

    }

    else
    {

        //Initialize the vectors "num_filt" and "den_filt"
        for (int i = 0; i < 2 * order_filt_f + 1; i++)
        {

            num_filt.push_back(0);
            den_filt.push_back(0);

        }

        dim_array = 2 * temp_dim_arr_matr;

    }

    //Extract the coefficients of the denumerator
    std::vector<std::complex<double>> coeff_pol(temp_dim_arr_matr + 1);

    if (type_filt_f > 1)

    {

        coeff_pol = char_poly(a_arma_f, temp_dim_arr_matr);

    }

    else
    {

        coeff_pol = char_poly(a_arma_f, 2 * temp_dim_arr_matr);

    }

    for (int qq = 0; qq < dim_array + 1; qq++)
    {

        den_filt[qq] = real(coeff_pol[qq]);
        save_filt_coeff[1][qq] = den_filt[qq];

    }

    //Extract the coefficients of the denominator
    double w;
    Wn = 2 * std::atan2(Wn, 4);
    std::vector<std::complex<double>> r;

    switch (type_filt_f)
    {

    case 0: // band-pass

        for (int i = 0; i <= dim_array; i++)
        {

            r.push_back(0);

        }

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

            if (kk < temp_dim_arr_matr)
            {

                r[kk] = 1;

            }

            else
            {

                r[kk] = -1;

            }

        }

        w = Wn;

        break;

    case 1: // band-stop

        for (int i = 0; i <= dim_array; i++)
        {

            r.push_back(0);

        }

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

            r[kk] = exp(complex_imag * Wn * pow(-1, kk));

        }
        w = 0;

        break;

    case 2: //high-pass

        for (int i = 0; i <= dim_array; i++)
        {

            r.push_back(0);

        }

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

            r[kk] = 1;

        }

        w = PI;
        break;

    case 3: // low-pass

        for (int i = 0; i <= dim_array; i++)
        {

            r.push_back(0);

        }

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

            r[kk] = -1;

        }
        w = 0;
        break;

    default:
        for (int i = 0; i <= dim_array; i++)
        {

            r.push_back(0);

        }

    }

    std::vector<std::complex<double>> coeff_pol_num(dim_array + 1);

    coeff_pol_num = poly(r, dim_array);

    std::vector<std::complex<double>> kern(dim_array + 1);

    for (int kk = 0; kk < dim_array + 1; kk++)
    {

        kern[kk] = exp(-complex_imag * w * double(kk));

    }

    std::complex<double> temp_sum_I;
    std::complex<double> temp_sum_II;

     for (int kk = 0; kk < dim_array + 1; kk++)
    {

        temp_sum_I = 0.0;
        temp_sum_II = 0.0;

        for (int hh = 0; hh < dim_array + 1; hh++)
        {

            temp_sum_I += kern[hh] * den_filt[hh];
            temp_sum_II += kern[hh] * coeff_pol_num[hh];

        }

        num_filt[kk] = real(coeff_pol_num[kk] * temp_sum_I / temp_sum_II);
        save_filt_coeff[0][kk] = num_filt[kk];

    }

}
    //Check the stability of the filter
    bool IIR_Butterworth::check_stability_iir(std::vector<std::vector<double> > coeff_filt)
    {
        bool stability_flag = true;

        //Calculate the roots
        arma::mat roots_den_matrix = arma::zeros(coeff_filt[1].size() - 1, coeff_filt[1].size() - 1);
        for (int kk = 0; kk < coeff_filt[1].size() - 2; kk++)
        {

            roots_den_matrix(kk + 1, kk) = 1;

        }

         for (int kk = 0; kk < coeff_filt[1].size() - 1; kk++)
        {

            roots_den_matrix(0, kk) = -coeff_filt[1][kk + 1];

        }

        std::vector<double> magnitude_roots_den (coeff_filt[1].size() - 1);
        arma::cx_vec roots_den;
        arma::cx_mat eigvec;

        arma::eig_gen(roots_den,eigvec, roots_den_matrix);

        for (int kk = 0; kk < coeff_filt[1].size() - 1; kk++)
        {

            magnitude_roots_den[kk] = abs(roots_den[kk]);

            if (magnitude_roots_den[kk] >= 1)
            {

                stability_flag = false;
                break;
            }

        }

        return stability_flag;

    }

//Filter the data by using the Direct-Form II Transpose, as explained in the Matlab documentation
std::vector<double> IIR_Butterworth::Filter_Data(std::vector<std::vector<double> > coeff_filt, std::vector<double> pre_filt_signal)
{

        std::vector<double> filt_signal(pre_filt_signal.size(), 0.0);

        std::vector<std::vector<double>> w_val;
        std::vector<double> temp_v;

        for (int ff = 0; ff < pre_filt_signal.size(); ff++)
        {

                temp_v.push_back(0);

        }

        for (int hh = 0; hh < coeff_filt[0].size(); hh++)
        {

                w_val.push_back(temp_v);

        }

        //Convolution product to filter the data
        for (int kk = 0; kk < pre_filt_signal.size(); kk++)
        {

                if (kk == 0)
                {

                        filt_signal[kk] = pre_filt_signal[kk] * coeff_filt[0][0];

                        for (int ww = 1; ww < coeff_filt[0].size(); ww++)
                        {

                                w_val[ww - 1][kk] = pre_filt_signal[kk] * coeff_filt[0][ww] - filt_signal[kk] * coeff_filt[1][ww];

        }

                }

                else
                {

                        filt_signal[kk] = pre_filt_signal[kk] * coeff_filt[0][0] + w_val[0][kk - 1];

                                for (int ww = 1; ww < coeff_filt[0].size(); ww++)
                                {

                                        w_val[ww - 1][kk] = pre_filt_signal[kk] * coeff_filt[0][ww] + w_val[ww][kk - 1] - filt_signal[kk] * coeff_filt[1][ww];

                                        if (ww == coeff_filt[0].size() - 1)
                                        {

                                                 w_val[ww - 1][kk] = pre_filt_signal[kk] * coeff_filt[0][ww] - filt_signal[kk] * coeff_filt[1][ww];

                                        }

                                }

                }

        }

        return filt_signal;

}


#ifndef MLT_UTILS_OPTIMIZERS_STOCHASTIC_GRADIENT_DESCENT_TRAINER_HPP
#define MLT_UTILS_OPTIMIZERS_STOCHASTIC_GRADIENT_DESCENT_TRAINER_HPP

#include <algorithm>
#include <random>
#include <tuple>
#include <vector>

#include <Eigen/Core>

#include "../../defs.hpp"
#include "../eigen.hpp"
#include "gradient_descent_updates.hpp"

namespace mlt {
namespace utils {
namespace optimizers {
	using namespace eigen;

    // Implementation of Stochastic Gradient Descent
    // Parameters:
    // - size_t batch_size: size of each batch when using mini-batches (set to 0 if using full batch gradient descent)
    // - size_t epochs: number of epochs to run the descent steps through the training data
    // - double learning_rate: initial learning rate
    // - double learning_rate_decay: learning_rate is multiplied by this after each epoch
    // - UpdateMethod update_method: object implementing the update strategy to use
	template <class UpdateMethod = RMSPropGradientDescentUpdate>
    class StochasticGradientDescent {
    public:
		StochasticGradientDescent(size_t batch_size = 1, size_t epochs = 1000, double learning_rate = 0.001, double learning_rate_decay = 0.99,
			const UpdateMethod& update_method = UpdateMethod()) : _batch_size(batch_size), _epochs(epochs), _learning_rate(learning_rate),
			_current_learning_rate(learning_rate), _learning_rate_decay(learning_rate_decay), _update_method(update_method) {}

        template <class Model, class Target>
		auto operator()(const Model& model, Features input, Target target, MatrixXdRef init, bool cold_start) {
			assert(input.cols() == target.cols());

            if (cold_start) {
                _current_learning_rate = _learning_rate;
				_update_method.restart();
            }

            auto iters_per_epoch = _batch_size > 0 && _batch_size <= input.cols() ? input.cols() / _batch_size : 1;

			MatrixXd params = init;

			random_device rd;
            default_random_engine generator(rd());

            for (auto epoch = 0; epoch < _epochs; epoch++) {
                for (auto iter = 0; iter < iters_per_epoch; iter++) {
					if (_batch_size > 0) {
						auto subset = tied_random_cols_subset(input, target, _batch_size, generator);
						auto input_batch = get<0>(subset);
						auto target_batch = get<1>(subset);

						params += _update_method.step(_current_learning_rate, model.gradient(params, input_batch, target_batch));
					} else {
						params += _update_method.step(_current_learning_rate, model.gradient(params, input, target));
					}
                }

#ifdef MLT_VERBOSE
				if (epoch % 10 == 0) {
					auto loss = model.loss(params, input, target);
					MLT_LOG_LINE("[SGD]: Epoch " << epoch + 1 << "/" << _epochs << " Loss " << loss);
				}
#endif

				_current_learning_rate *= _learning_rate_decay;
            }

			return params;
        }

	protected:
		size_t _batch_size;
		size_t _epochs;
		double _learning_rate;
		double _current_learning_rate;
		double _learning_rate_decay;
		UpdateMethod _update_method;
    };
}
}
}
#endif


// Copyright Nick Thompson, 2017
// 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)

#include <iostream>
#include <cmath>
#include <boost/math/quadrature/tanh_sinh.hpp>
#include <boost/math/quadrature/sinh_sinh.hpp>
#include <boost/math/quadrature/exp_sinh.hpp>

using boost::math::quadrature::tanh_sinh;
using boost::math::quadrature::sinh_sinh;
using boost::math::quadrature::exp_sinh;
using boost::math::constants::pi;
using boost::math::constants::half_pi;
using boost::math::constants::half;
using boost::math::constants::third;
using boost::math::constants::root_pi;
using std::log;
using std::cos;
using std::cosh;
using std::exp;
using std::sqrt;

int main()
{
    std::cout << std::setprecision(std::numeric_limits<double>::digits10);
    double tol = sqrt(std::numeric_limits<double>::epsilon());
    // For an integral over a finite domain, use tanh_sinh:
    tanh_sinh<double> tanh_integrator(tol, 10);
    auto f1 = [](double x) { return log(x)*log(1-x); };
    double Q = tanh_integrator.integrate(f1, (double) 0, (double) 1);
    double Q_expected = 2 - pi<double>()*pi<double>()*half<double>()*third<double>();

    std::cout << "tanh_sinh quadrature of log(x)log(1-x) gives " << Q << std::endl;
    std::cout << "The exact integral is                        " << Q_expected << std::endl;

    // For an integral over the entire real line, use sinh-sinh quadrature:
    sinh_sinh<double> sinh_integrator(tol, 10);
    auto f2 = [](double t) { return cos(t)/cosh(t);};
    Q = sinh_integrator.integrate(f2);
    Q_expected = pi<double>()/cosh(half_pi<double>());
    std::cout << "sinh_sinh quadrature of cos(x)/cosh(x) gives " << Q << std::endl;
    std::cout << "The exact integral is                        " << Q_expected << std::endl;

    // For half-infinite intervals, use exp-sinh.
    // Endpoint singularities are handled well:
    exp_sinh<double> exp_integrator(tol, 10);
    auto f3 = [](double t) { return exp(-t)/sqrt(t); };
    Q = exp_integrator.integrate(f3, 0, std::numeric_limits<double>::infinity());
    Q_expected = root_pi<double>();
    std::cout << "exp_sinh quadrature of exp(-t)/sqrt(t) gives " << Q << std::endl;
    std::cout << "The exact integral is                        " << Q_expected << std::endl;

}


#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/boost/graph/graph_traits_Delaunay_triangulation_2.h>
#include <CGAL/Constrained_Delaunay_triangulation_2.h>

#include <boost/chrono.hpp>
#include <boost/graph/iteration_macros.hpp>
#include <boost/pending/disjoint_sets.hpp>

#include "GraphDefs.h"
#include "Timer.h"
#include "GraphUtil.h"
#include "GraphParse.h"
#include "GraphGen.h"

#include <fstream>
#include <string>

#define SHOW_DEBUG false

void printGraph(const char* title, VertexVector* vertices, EdgeVector* edges, bool printVertices = false) {
	std::cout << std::endl << "=== " << title << std::endl;

	if (printVertices) {
		// Iterate through the vertices and print them out
		for (int i = 0; i < vertices->size(); i++) {
			CGALPoint* v = (*vertices)[i];
			std::cout << "index: " << i << " (" << v->x() << ", " << v->y() << ")" << std::endl;
		}
	}

	// Iterate through the edges and print them out
	std::cout << std::endl << "Edges: " << std::endl;
	for (int i = 0; i < edges->size(); i++) {
		SimpleEdge* e = (*edges)[i];
		CGALPoint* src = (*vertices)[e->u];
		CGALPoint* tar = (*vertices)[e->v];
		//std::cout << edgeWeightMap->at(e).weight << " (" << (*g)[src].pt << ") (" << (*g)[tar].pt << ")" << std::endl;
		std::cout << e->weight << " (" << (*src) << ") (" << (*tar) << ")" << std::endl;
	}

	std::cout << std::endl;
}

void printGDFGraph(const char* fileName, VertexVector* vertices, EdgeVector* edges) {
	std::ofstream myfile;
	myfile.open(fileName, std::ios::out | std::ios::in);

	myfile << "nodedef> name VARCHAR,label VARCHAR,width DOUBLE,height DOUBLE,x DOUBLE,y DOUBLE,color VARCHAR" << std::endl;

	// Iterate through the vertices and print them out
	boost::unordered_map<CGALPoint*, VertexIndex> vertexHandles;
	for (int i = 0; i < vertices->size(); i++) {
		CGALPoint* v = (*vertices)[i];
		myfile << i << ",,10.0,10.0," << (*v).x() << "," << (*v).y() << ",'153,153,153'" << std::endl;
		vertexHandles.emplace(v, i);
	}

	myfile << "edgedef> node1,node2,weight DOUBLE,directed BOOLEAN,color VARCHAR" << std::endl;

	// Iterate through the edges and print them out
	for (int i = 0; i < edges->size(); i++) {
		SimpleEdge* e = (*edges)[i];
		CGALPoint* src = (*vertices)[e->u];
		CGALPoint* tar = (*vertices)[e->v];
		VertexIndex srcInd = vertexHandles[src];
		VertexIndex tarInd = vertexHandles[tar];
		//EdgeWeight weight = sqrt(CGAL::squared_distance(*src, *tar));
		EdgeWeight weight = 1.0;
		myfile << srcInd << "," << tarInd << "," << weight << ",false,'128,128,128'" << std::endl;
	}

	myfile.close();
}

CDT* computeCdt(VertexVector* vertices, EdgeVector* edges, boost::unordered_map<TriVertexHandle, VertexIndex>** handlesToIndex) {
	CDT* cdt = new CDT();

	(*handlesToIndex) = new boost::unordered_map<TriVertexHandle, VertexIndex>();
	boost::unordered_map<VertexIndex, TriVertexHandle> vertexHandles;
	for (int i = 0; i < vertices->size(); i++) {
		CGALPoint* pt = (*vertices)[i];
		TriVertexHandle vHandle = cdt->insert(*pt);
		vertexHandles.emplace(i, vHandle);
		(*handlesToIndex)->emplace(vHandle, i);
	}

	// Insert constraint edges
	for (int i = 0; i < edges->size(); i++) {
		SimpleEdge* edge = (*edges)[i];
		VertexIndex u = edge->u;
		VertexIndex v = edge->v;
		TriVertexHandle uH = vertexHandles[u];
		TriVertexHandle vH = vertexHandles[v];
		cdt->insert_constraint(uH, vH);
	}

	assert(cdt->is_valid());

	return cdt;
}

// Given 3 colinear points, if the 2 furthest points are constrained, how is this delaunay triangulated?
// It seems the outcome is implementation dependent. CGAL will split the constraint into 2 edges.
// Other implementations might allow for overlapping, collinear edges. Either way, the current plan
// is to assume that an input forest, F, with collinear edges will be replaced by smaller constraint edges.
EdgeVector* newConstraintSetFromCdt(CDT* cdt, VertexVector* originalVertices) {
	EdgeVector* newEdgeVector = new EdgeVector();

	boost::unordered_map<CGALPoint, VertexIndex> vertexIndex;
	for (int i = 0; i < originalVertices->size(); i++) {
		vertexIndex[*(*originalVertices)[i]] = i;
	}

	// Add edges to graph
	for (CDT::Edge_iterator eit = cdt->edges_begin(); eit != cdt->edges_end(); ++eit) {
		CDT::Edge cgal_e = *eit;

		if (cdt->is_constrained(cgal_e)) {
			// Assumes point coord are unique
			CGALSegment segement = cdt->segment(cgal_e);
			CGALPoint cgal_u = segement.point(0);
			CGALPoint cgal_v = segement.point(1);
			VertexIndex u = vertexIndex[cgal_u];
			VertexIndex v = vertexIndex[cgal_v];

			SimpleEdge* edge = new SimpleEdge(u, v, 0);
			newEdgeVector->push_back(edge);
		}
	}
	return newEdgeVector;
}

TriVertexHandle OppositeOfEdge(TriVertexHandle ev0, TriVertexHandle ev1, TriFaceHandle f) {
	TriVertexHandle v0 = f->vertex(0);
	if (v0 != ev0 && v0 != ev1) {
		return v0;
	}

	TriVertexHandle v1 = f->vertex(1);
	if (v1 != ev0 && v1 != ev1) {
		return v1;
	}

	TriVertexHandle v2 = f->vertex(2);
	if (v2 != ev0 && v2 != ev1) {
		return v2;
	}

	assert(false); // Unreachable
	return NULL;
}

bool IsInsideCircle(CGALPoint* p, CGALPoint* q, CGALPoint* t) {
	if (SHOW_DEBUG) {
		std::cout << "(" << (*p) << ") (" << (*q) << ") (" << (*t) << ")" << std::endl;
	}
	CGALCircle c(*p, *q);
	CGAL::Bounded_side side = c.bounded_side(*t);

	// v is outside or on the circumcircle of f
	return side == CGAL::Bounded_side::ON_BOUNDED_SIDE
		|| side == CGAL::Bounded_side::ON_BOUNDARY;
}

void computeNonLocallyGabriel(
	VertexVector* vertices,
	EdgeVector* edges,
	EdgeVector** NewEdges,
	EdgeVector** S_Edges) {

	boost::chrono::high_resolution_clock::time_point start;
	boost::chrono::high_resolution_clock::time_point end;
	boost::chrono::milliseconds duration(0);
	boost::chrono::milliseconds total(0);

	// Compute CDT(F)
	start = boost::chrono::high_resolution_clock::now();
	boost::unordered_map<TriVertexHandle, VertexIndex>* handlesToIndex;
	CDT* cdt = computeCdt(vertices, edges, &handlesToIndex);
	end = boost::chrono::high_resolution_clock::now();
	duration = (boost::chrono::duration_cast<boost::chrono::milliseconds>(end - start));
	total += duration;
	printDuration("CDT(F)", duration);

	// Replace F with NewF
	start = boost::chrono::high_resolution_clock::now();
	(*NewEdges) = newConstraintSetFromCdt(cdt, vertices);
	end = boost::chrono::high_resolution_clock::now();
	duration = (boost::chrono::duration_cast<boost::chrono::milliseconds>(end - start));
	total += duration;
	printDuration("F -> NewF", duration);

	// Compute Non-Locally Gabriel edges
	start = boost::chrono::high_resolution_clock::now();
	boost::unordered_set<TriEdge>* S = new boost::unordered_set<TriEdge>();
	TriVertexHandle infiniteVertex = cdt->infinite_vertex();
	int edgeCount = 0;
	for (FiniteEdgeIter iter = cdt->finite_edges_begin(); iter != cdt->finite_edges_end(); ++iter) {
		edgeCount++;

		// typedef std::pair<Face_handle, int> Edge;
		TriEdge e = *iter;
		int eIndex = e.second;

		// Edge shared by faces f0 and f1
		TriFaceHandle f0 = e.first;
		TriFaceHandle f1 = e.first->neighbor(eIndex);

		// Vertex opposite of edge e in f0, f1
		TriVertexHandle opp0 = f0->vertex(eIndex);

		// Vertex of edge e
		TriVertexHandle e0 = f0->vertex(f0->cw(eIndex));
		TriVertexHandle e1 = f0->vertex(f0->ccw(eIndex));

		TriVertexHandle opp1 = OppositeOfEdge(e0, e1, f1);

		if (SHOW_DEBUG) {
			// Vertex endpoint v0, v1 of edge e for f0 (doesn't seem to be possible to identify the same edge for f1 in a similar way)
			std::cout << eIndex << std::endl;
			std::cout << "(" << *e0 << ") (" << *e1 << ")" << std::endl;
			std::cout << "(" << *opp0 << ")" << std::endl;
		}

		if (e0 != infiniteVertex && e1 != infiniteVertex) {
			CGALPoint p = e0->point();
			CGALPoint q = e1->point();

			bool addToConstraint = false;

			if (opp0 != infiniteVertex) {
				CGALPoint t = opp0->point();
				if (IsInsideCircle(&p, &q, &t)) {
					addToConstraint = true;
				}
			}

			if (opp1 != infiniteVertex) {
				CGALPoint t = opp1->point();
				if (IsInsideCircle(&p, &q, &t)) {
					addToConstraint = true;
				}
			}

			if (addToConstraint) {
				S->emplace(e);
			}
		}
	}

	(*S_Edges) = new EdgeVector();
	for (boost::unordered_set<TriEdge>::iterator iter = S->begin(); iter != S->end(); ++iter) {
		TriEdge e = *iter;
		int eIndex = e.second;
		TriFaceHandle f0 = e.first;
		TriVertexHandle e0 = f0->vertex(f0->cw(eIndex));
		TriVertexHandle e1 = f0->vertex(f0->ccw(eIndex));

		VertexIndex u = (*handlesToIndex)[e0];
		VertexIndex v = (*handlesToIndex)[e1];

		(*S_Edges)->push_back(new SimpleEdge(u, v, 0));
	}

	end = boost::chrono::high_resolution_clock::now();
	duration = (boost::chrono::duration_cast<boost::chrono::milliseconds>(end - start));
	total += duration;
	printDuration("computeNonLocallyGabriel duration", duration);

	printDuration("Total", total);

	delete S;
	delete cdt;
	delete handlesToIndex;
}

void computeCgg(
	VertexVector* vertices,
	EdgeVector* edges,
	EdgeVector** NewEdges,
	EdgeVector** gabriel) {

	boost::unordered_set<SimpleEdge>* constraintEdgesSet = createSimpleEdgeSet(edges);

	boost::unordered_map<TriVertexHandle, VertexIndex>* handlesToIndex;
	CDT* cdt = computeCdt(vertices, edges, &handlesToIndex);
	(*NewEdges) = newConstraintSetFromCdt(cdt, vertices);

	boost::unordered_set<TriEdge>* S = new boost::unordered_set<TriEdge>();

	TriVertexHandle infiniteVertex = cdt->infinite_vertex();
	int edgeCount = 0;

	boost::chrono::high_resolution_clock::time_point startTotal = boost::chrono::high_resolution_clock::now();

	for (FiniteEdgeIter iter = cdt->finite_edges_begin(); iter != cdt->finite_edges_end(); ++iter) {
		edgeCount++;

		// typedef std::pair<Face_handle, int> Edge;
		TriEdge e = *iter;
		int eIndex = e.second;

		// Edge shared by faces f0 and f1
		TriFaceHandle f0 = e.first;
		TriFaceHandle f1 = e.first->neighbor(eIndex);

		// Vertex opposite of edge e in f0
		TriVertexHandle opp0 = f0->vertex(eIndex);

		// Vertex of edge e
		TriVertexHandle e0 = f0->vertex(f0->cw(eIndex));
		TriVertexHandle e1 = f0->vertex(f0->ccw(eIndex));

		TriVertexHandle opp1 = OppositeOfEdge(e0, e1, f1);

		if (SHOW_DEBUG) {
			// Vertex endpoint v0, v1 of edge e for f0 (doesn't seem to be possible to identify the same edge for f1 in a similar way)
			std::cout << eIndex << std::endl;
			std::cout << "(" << *e0 << ") (" << *e1 << ")" << std::endl;
			std::cout << "(" << *opp0 << ")" << std::endl;
		}

		if (e0 != infiniteVertex && e1 != infiniteVertex) {
			CGALPoint p = e0->point();
			CGALPoint q = e1->point();
			VertexIndex u = (*handlesToIndex)[e0];
			VertexIndex v = (*handlesToIndex)[e1];

			bool addToS = true;

			if (opp0 != infiniteVertex) {
				CGALPoint t = opp0->point();
				// Is not locally gabriel
				// Is not a constraint edge
				if (IsInsideCircle(&p, &q, &t)
					&& (constraintEdgesSet->count(SimpleEdge(u, v, 0)) < 1)) {
					addToS = false;
				}
			}

			if (opp1 != infiniteVertex) {
				CGALPoint t = opp1->point();
				// Is not locally gabriel
				// Is not a constraint edge
				if (IsInsideCircle(&p, &q, &t)
					&& (constraintEdgesSet->count(SimpleEdge(u, v, 0)) < 1)) {
					addToS = false;
				}
			}

			if (addToS) {
				S->emplace(e);
			}
		}
	}

	(*gabriel) = new EdgeVector();
	for (boost::unordered_set<TriEdge>::iterator iter = S->begin(); iter != S->end(); ++iter) {
		TriEdge e = *iter;
		int eIndex = e.second;
		TriFaceHandle f0 = e.first;
		TriVertexHandle e0 = f0->vertex(f0->cw(eIndex));
		TriVertexHandle e1 = f0->vertex(f0->ccw(eIndex));

		VertexIndex u = (*handlesToIndex)[e0];
		VertexIndex v = (*handlesToIndex)[e1];

		(*gabriel)->push_back(new SimpleEdge(u, v, 0));
	}

	boost::chrono::high_resolution_clock::time_point endTotal = boost::chrono::high_resolution_clock::now();
	boost::chrono::milliseconds total = (boost::chrono::duration_cast<boost::chrono::milliseconds>(endTotal - startTotal));
	printDuration("computeCgg duration", total);

	delete S;
	delete cdt;
	delete handlesToIndex;
	delete constraintEdgesSet;
}

EdgeVector* intersectInputSetWithConstraintSet(EdgeVector* inputSet, EdgeVector* constraintSet) {
	boost::unordered_set<SimpleEdge>* NewEdges_Hashset = new boost::unordered_set<SimpleEdge>();
	for (int i = 0; i < inputSet->size(); i++) {
		SimpleEdge* e = (*inputSet)[i];
		NewEdges_Hashset->emplace((*e));
	}

	EdgeVector* intersect = new EdgeVector();
	for (int i = 0; i < constraintSet->size(); i++) {
		SimpleEdge* se = (*constraintSet)[i];
		if (NewEdges_Hashset->count(*se) > 0) {
			intersect->push_back(new SimpleEdge(se->u, se->v, se->weight));
		}
	}

	return intersect;
}

bool containsEdge(boost::unordered_set<SimpleEdge>* edgeSet, SimpleEdge* edge) {
	SimpleEdge se(edge->u, edge->v, 0);
	return edgeSet->count(se) > 0;
}

// True if A a subgraph of B
bool isSubgraph(VertexVector* vertices, EdgeVector* a, EdgeVector* b) {
	boost::unordered_set<SimpleEdge>* bEdgeSet = createSimpleEdgeSet(b);

	// Iterate through the edges
	for (int i = 0; i < a->size(); i++) {
		SimpleEdge* edge = (*a)[i];
		if (!containsEdge(bEdgeSet, edge)) {
			CGALPoint* u = (*vertices)[edge->u];
			CGALPoint* v = (*vertices)[edge->v];
			//EdgeWeight weight = CGAL::squared_distance(*u, *v);
			CGAL::Lazy_exact_nt<CGAL::Gmpq> exactWeight = CGAL::squared_distance(*u, *v);
			EdgeWeight weight = CGAL::to_double(exactWeight);
			std::cout << "b contains edge not in a: " << weight << " (" << *u << ") (" << *v << ")" << std::endl;
			return false;
		}
	}

	return true;
}

EdgeVector* convertCdtToGraph(VertexVector* vertices, CDT* cdt) {
	EdgeVector* edgeVec = new EdgeVector();
	// edgeVec->reserve(cdt->number_of_faces() * 3); // Upper bound on number of edges (typically too much)

	// Map CGALPoint -> VertexIndex
	boost::unordered_map<CGALPoint, VertexIndex> vertexIndex;
	for (int i = 0; i < vertices->size(); i++) {
		vertexIndex[*(*vertices)[i]] = i;
	}

	// Add edges to graph
	for (CDT::Edge_iterator eit = cdt->edges_begin(); eit != cdt->edges_end(); ++eit) {
		CDT::Edge cgal_e = *eit;
		CGALSegment segement = cdt->segment(cgal_e);
		CGALPoint cgal_u = segement.point(0);
		CGALPoint cgal_v = segement.point(1);
		VertexIndex u = vertexIndex[cgal_u];
		VertexIndex v = vertexIndex[cgal_v];

		SimpleEdge* edge = new SimpleEdge(u, v, 0);
		edgeVec->push_back(edge);
	}

	return edgeVec;
}

bool isCdtSubgraph(VertexVector* vertices, EdgeVector* edgesF, EdgeVector* edgesS) {
	boost::unordered_map<TriVertexHandle, VertexIndex>* handlesToIndex;
	CDT* cdtS = computeCdt(vertices, edgesS, &handlesToIndex); // CDT of mimimum edge constraint
	EdgeVector* ev_cdtS = convertCdtToGraph(vertices, cdtS);
	bool res = isSubgraph(vertices, edgesF, ev_cdtS);

	delete ev_cdtS;
	delete cdtS;
	delete handlesToIndex;

	return res;
}

bool isCggSubgraph(VertexVector* vertices, EdgeVector* edgesF, EdgeVector* edgesS) {
	EdgeVector* newEdgesS;
	EdgeVector* gabriel_S;
	computeCgg(vertices, edgesS, &newEdgesS, &gabriel_S);
	bool res = isSubgraph(vertices, edgesF, gabriel_S);

	delete gabriel_S;
	delete newEdgesS;

	return res;
}

int main(int argc, char* argv[]) {
	const char* vertFile = (argc > 2) ? argv[1] : NULL;
	const char* edgeFile = (argc > 2) ? argv[2] : NULL;

	VertexVector* vertices = NULL;
	EdgeVector* edges = NULL;

	if (vertFile == NULL || edgeFile == NULL) {
		// Random graph
		//createRandomCirclePlaneForest(1000, 1000, 100, &vertices, &edges);
		//createRandomMediumLengthPlaneForest(1000, 1000, 100, &vertices, &edges);
		createRandomNearTriangulation(1000, 1000, 100, &vertices, &edges);
	}
	else {
		// Load graph from file
		// e.g. D:\g\data\HI.nodes D:\g\data\HI.edges
		parseGraph(vertFile, edgeFile, &vertices, &edges);
	}

	if (SHOW_DEBUG) {
		printGraph("Input", vertices, edges, true);
	}

	// Compute CGG(V, E)
	EdgeVector* NewEdges;
	EdgeVector* cggS;
	computeNonLocallyGabriel(vertices, edges, &NewEdges, &cggS);
	EdgeVector* S = intersectInputSetWithConstraintSet(NewEdges, cggS);
	//std::cout << "Edges in E: " << NewEdges->size() << " Edges in S: " << S->size() << " Ratio: " << (double)((double)S->size() / (double)NewEdges->size()) << std::endl;
	std::cout << S->size() << std::endl;
	std::cout << (double)((double)S->size() / (double)NewEdges->size()) << std::endl;
	std::cout << std::endl;

	//printGDFGraph("D:\\g\\results\\graph examples\\hi_gg_S.gdf", vertices, S);

	// Other possible validatation
	// CGG ⊆ CDT
	// S ⊆ CDT

	// S ⊆ E
	if (!isSubgraph(vertices, S, NewEdges)) {
		std::cout << "Error: isSubgraph is false" << std::endl;
	}

	// F ⊆ CDT(V, S) (Note: CGG is contained in CDT)
	if (!isCdtSubgraph(vertices, NewEdges, S)) {
		std::cout << "Error: isCdtSubgraph is false" << std::endl;
	}

	// F ⊆ CGG(V, S), CGG is computed by running a CDT on S, followed by
	// a locally gabriel check on all edge from the CDT and eliminating edges
	// that are non-locally gabriel AND not a constraint edge
	if (!isCggSubgraph(vertices, NewEdges, S)) {
		std::cout << "Error: isCggSubgraph is false" << std::endl;
	}

	deleteEdgeVector(S);
	deleteEdgeVector(cggS);
	deleteEdgeVector(NewEdges);

	deleteEdgeVector(edges);
	deleteVerticesVector(vertices);

	return 0;
}


// Copyright(c) 2019-present, Alexander Silva Barbosa & bflib contributors.
// Distributed under the MIT License (http://opensource.org/licenses/MIT)

/**
 * @author Alexander Silva Barbosa <alexander.ti.ufv@gmail.com>
 * @date 2019
 * Particle Filter (Monte Carlo)
 */

#pragma once

#include <Eigen/Dense>
#include <random>
#include <chrono>
#include <thread>
#include <iostream>
#include <vector>

using namespace Eigen;
using namespace std;

template <typename dataType, int states, int inputs, int outputs, int outputSize>
class PF
{
    private:
        typedef Matrix<dataType, states, 1> MatNx1;
        typedef Matrix<dataType, inputs, 1> MatMx1;
        typedef Matrix<dataType, outputs, 1> MatPx1;
        typedef Matrix<dataType, states, states> MatNxN;

    public:
        typedef MatNx1 State;
        typedef MatMx1 Input;
        typedef MatPx1 Output;
        typedef Input Control;
        typedef Output Sensor;
        typedef MatPx1 SensorStd;
        typedef MatNx1 ResampleStd;

        double eps;

    private:
        typedef void (*ModelFunction)(State &x, Input &u, double dt);
        typedef void (*SensorFunction)(std::vector<Output> &y, State &x, double dt);

        std::default_random_engine gen;
        std::normal_distribution<double> distr{0.0, 1.0};
        std::chrono::time_point<std::chrono::high_resolution_clock> start;

        int N, NMax;
        std::vector<State> W;
        std::vector< std::vector<Output> > Y;
        std::vector<double> w;
        double wMax;

        State minState, maxState, meanState;

        State x;
        ResampleStd Q;
        SensorStd R;

        MatNxN resampleMatrix;
        MatNx1 randX;

        ModelFunction modelFn;
        SensorFunction sensorFn;

        void init()
        {
            modelFn = NULL;
            sensorFn = NULL;

            resampleMatrix = Q.asDiagonal();

            N = NMax;
            W.resize(N);
            Y.resize(N);
            w.resize(N);
            meanState = (maxState - minState) / 2;
            initW();

            eps = 1e-10;

            start = std::chrono::high_resolution_clock::now();
        }
    public:

        PF(int N, State minState, State maxState) : NMax(N), minState(minState), maxState(maxState)
        {
            Q.setIdentity();
            R.setIdentity();
            x.setZero();
            init();
        }

        PF(int N, State X, State minState, State maxState) : x(X), NMax(N), minState(minState), maxState(maxState)
        {
            Q.setIdentity();
            R.setIdentity();
            init();
        }

        PF(int N, State minState, State maxState, ResampleStd Q, SensorStd R) : Q(Q), R(R), NMax(N), minState(minState), maxState(maxState)
        {
            x.setZero();
            init();
        }

        PF(int N, State X, State minState, State maxState, ResampleStd Q, SensorStd R) : x(X), Q(Q), R(R), NMax(N), minState(minState), maxState(maxState)
        {
            init();
        }

        virtual ~PF()
        {

        }

        void seed()
        {
            unsigned int s = std::chrono::system_clock::now().time_since_epoch().count();
            seed(s);
        }

        void seed(unsigned int s)
        {
            gen = std::default_random_engine(s);
            srand (s);
            std::srand(s);
            initW();
        }

        State state()
        {
            MatNx1 x;
            x.setZero();
            return x;
        }

        Input input()
        {
            MatMx1 u;
            u.setZero();
            return u;
        }

        Output output()
        {
            MatPx1 y;
            y.setZero();
            return y;
        }

        SensorStd createR()
        {
            SensorStd R;
            R.setZero();
            return R;
        }

        void setR(SensorStd R)
        {
            this->R = R;
        }

        ResampleStd createQ()
        {
            ResampleStd Q;
            Q.setZero();
            return Q;
        }

        void setQ(ResampleStd Q)
        {
            this->Q = Q;
            resampleMatrix = Q.asDiagonal();
        }

        double time()
        {
            auto end = std::chrono::high_resolution_clock::now();
            std::chrono::duration<double> diff = end - start;
            start = std::chrono::high_resolution_clock::now();
            return diff.count();
        }

        double delay(double s)
        {
            double ellapsed = time();
            double remain = s - ellapsed;
            if(remain < 0)
                return ellapsed;
            std::this_thread::sleep_for(std::chrono::nanoseconds((long long)(remain * 1e9)));
            ellapsed += time();
            return ellapsed;
        }

        std::vector<State>& particles()
        {
            return W;
        }

        void setModel(ModelFunction fn)
        {
            modelFn = fn;
        }

        void setSensor(SensorFunction fn)
        {
            sensorFn = fn;
        }

        virtual void model(State &x, Input &u, double dt)
        {

        }

        virtual void sensor(std::vector<Output> &z, State &x, double dt)
        {

        }

        void simulate(State &x, std::vector<Output> &y, Input &u, double dt)
        {
            doModel(x, u, dt);

            y.resize(outputSize);
            doSensor(y, x, dt);
        }

        void run(State &xK, std::vector<Output> &y, Input &u, double dt)
        {
            y.resize(outputSize);

            applyControl(u, dt);
            sense(dt);
            compare(y);
            resample();
            measure();

            xK = x;
        }

	void VaryN(float error, int Nmin, int Ni, float error_lim, int Nmax)
	{
	    int Np=0;
	    if(error>error_lim)
	    {
	    	float l;

            	l = round(error/error_lim);
	    	int k = (int)l;
                Np = Nmin + Ni*k;
            	if(Np > Nmax)
		{
			this->N = Nmax;
		}
		else
		{
			this->N = Np;
		}
            }
	    else
	    {
                Np = Nmin;
		this->N = Np;
            }
	  cout << this-> N << endl;
	}

    private:
        void initW()
        {
            for(int n = 0; n < N; n++)
            {
                W[n] = State::Random();
                for (size_t i = 0; i < states; i++)
                {
                    W[n](i) = meanState(i) + W[n](i) * meanState(i);
                }
            }
        }

        void applyControl( Input &u, double dt)
        {
            for(int i = 0; i < N; i++)
            {
                doModel(W[i], u, dt);
            }
        }

        void sense(double dt)
        {
            for(int i = 0; i < N; i++)
            {
                Y[i].resize(outputSize);
                doSensor(Y[i], W[i], dt);
            }
        }

        void compare(std::vector<Output> &y)
        {
            double sum = 0;
            double lh = 0;
            for(int i = 0; i < N; i++)
            {
                w[i] = 1.0;
                for(int j = 0; j < outputSize; j++)
                {
                    lh = likelihood(y[j], Y[i][j]);
                    w[i] *= lh;
                }
                sum += w[i];
            }

            wMax = 0;
            for(int i = 0; i < N; i++)
            {
                w[i] /= sum;
                if(w[i] > wMax)
                    wMax = w[i];
            }
        }

        double likelihood(Output y, Output z)
        {
            double lh = 0;
            double df, sq, ex, pi2, m;

            pi2 = sqrt(2 * 3.14159265358979);

            for (size_t i = 0; i < outputs; i++)
            {
                if(R(i) == 0)
                    continue;

                m = 1.0 / ( R(i) * pi2 );

                df = (z(i) - y(i)) / R(i);

                sq = df*df;
                ex = exp( -0.5 * sq );

                lh += m * ex;
            }

            lh += eps;
        }

        void resample()
        {
            int index = rand() % N;
            double F;
            std::vector<State> nW(N);

            for(int i = 0; i < N; i++)
            {
                F = 10 * wMax * ( ( rand() % 100 ) / 100.0);
                while(F > w[index])
                {
                    F -= w[index];
                    index = ( index + 1 ) % N;
                }
                randn(randX);
                nW[i] = W[index] + resampleMatrix * randX;
                for (size_t j = 0; j < outputs; j++)
                {
                    if(nW[i](j) < minState(j))
                        nW[i](j) = maxState(j) - ( minState(j) - nW[i](j) );
                    else if(nW[i](j) > maxState(j))
                        nW[i](j) = minState(j) + ( nW[i](j) - maxState(j) );
                }
            }

            W = nW;
        }

        void measure()
        {
            x.setZero();
            for(int i = 0; i < N; i++)
            {
                x += W[i];
            }
            x /= N;
        }

        void doModel(State &x, Input &u, double dt)
        {
            if(modelFn != NULL)
                modelFn(x, u, dt);
            else
                model(x, u, dt);
        }

        void doSensor(std::vector<Output> &z, State &x, double dt)
        {
            if(sensorFn != NULL)
                sensorFn(z, x, dt);
            else
                sensor(z, x, dt);
        }

        template<class T>
        void randn(T &mat)
        {
            for (size_t i = 0; i < mat.rows(); i++)
            {
                for (size_t j = 0; j < mat.cols(); j++)
                {
                    mat(i, j) = distr(gen);
                }
            }
        }

};


#include <iostream>
#include <Eigen/Lgsm>

int main(int argc, char* argv [])
{
  Eigen::Vector3d pos(1, 0, 0);
  Eigen::Displacementd H_1_0 = Eigen::Displacementd(pos);
  Eigen::Displacementd H_2_1 = Eigen::Displacementd(pos);

  Eigen::Vector3d axis(0, 0, 1);

  Eigen::Twistd tw(Eigen::AngularVelocityd(axis), pos.cross(axis));

  tw *=  1 * M_PI / 2.0;

  // See Murray Lie Sastry page 50.
  Eigen::Displacementd H_2_0 = tw.exp()  * H_1_0 * H_2_1;
  //std::cout << "twist : " << tw.transpose() << std::endl;
  std::cout << "H_1_0 : " << H_1_0 << std::endl;
  std::cout << "eTw " << tw.exp() << std::endl;
  //std::cout << "H_2_1 : " << H_2_1 << std::endl;
  //std::cout << "Brockett factor : " << tw.exp()  * H_1_0 << std::endl;
  std::cout << "H_2_0 " << H_2_0 << std::endl;
  //std::cout <<  tw.exp()  * H_1_0 * H_2_1 << std::endl;
  std::cout <<  tw.exp()  * H_1_0 << std::endl;

  return 0;
}


#include <iostream>
#include <chrono>
#include <sstream>
#include <fstream>
#include <Eigen/Dense>
#include "Timer.h"
#include "Projection.h"
#include "Volumetric_helper.h"
#include "KinectFrame.h"
#include "KinectSpecs.h"
#include "ICP.h"

typedef double Decimal;

void compute_vertices(std::vector<Eigen::Matrix<Decimal, 4, 1>>& vertices, const KinectFrame& frame, Decimal fovy, Decimal aspect_ratio, Decimal near_plane, Decimal far_plane)
{
	Eigen::Matrix<Decimal, 3, 1> vert_uv, vert_u1v, vert_uv1;

	for (int x = 0; x < frame.depth_width() - 1; ++x)
	{
		for (int y = 0; y < frame.depth_height() - 1; ++y)
		{
			const Decimal depth = frame.depth[y * frame.depth_width() + x];

			if (depth > 0.01)
			{
				vert_uv = window_coord_to_3d(Eigen::Matrix<Decimal, 2, 1>(x, y), depth, fovy, aspect_ratio, near_plane, far_plane, frame.depth_width(), frame.depth_height());

				int i = y * frame.depth_width() + x;

#if 1
				vertices[i] = vert_uv.homogeneous();
#else

				if (!vert_uv.isZero(0.0001))
				{
					vertices.push_back(vert_uv.homogeneous());
				}
#endif
			}

		}
	}
}

// Usage: ./Icp.exe ../../data/frame_0.knt ../../data/frame_1.knt 0 10 3
int main(int argc, char **argv)
{

	if (argc < 3)
	{
		std::cerr
			<< "Missing parameters. Abort."
			<< std::endl
			<< "Usage: ./Icp.exe <knt_frame> <knt_frame> <filter_width> <max_distance_threshold> "
			<< "Usage: ./Icp.exe ../../data/frame_1.knt ../../data/frame_1.knt 10 3"
			<< std::endl;
		return EXIT_FAILURE;
	}

	Timer timer;
	const std::string input_filename[] = { argv[1], argv[2] };

	const int filter_width = (argc > 2) ? atoi(argv[3]) : 10;
	const Decimal max_distance = (Decimal)((argc > 3) ? atof(argv[4]) : 100);

	try
	{
		timer.start();
		KinectFrame frame[2] =
		{
			KinectFrame(input_filename[0]),
			KinectFrame(input_filename[1])
		};
		timer.print_interval("Importing frames          : ");

		if (frame[0].depth_width() != frame[1].depth_width() ||
			frame[0].depth_height() != frame[1].depth_height())
		{
			std::cout
				<< "Frame size doens not match: " << std::endl
				<< frame[0].depth_width() << ", " << frame[0].depth_height() << std::endl
				<< frame[1].depth_width() << ", " << frame[1].depth_height() << std::endl
				<< std::endl;
			return EXIT_FAILURE;
		}

		const uint16_t width = frame[0].depth_width();
		const uint16_t height = frame[0].depth_height();
		const uint32_t pixel_count = width * height;

		Decimal fovy = KINECT_V2_FOVY;
		Decimal aspect_ratio = KINECT_V2_DEPTH_ASPECT_RATIO;
		Decimal near_plane = KINECT_V2_DEPTH_MIN;
		Decimal far_plane = KINECT_V2_DEPTH_MAX;

		if (width != 512)	// it is not kinect version 2. It is version 1
		{
			fovy = KINECT_V1_FOVY;
			aspect_ratio = KINECT_V1_ASPECT_RATIO;
			near_plane = KINECT_V1_DEPTH_MIN;
			far_plane = KINECT_V1_DEPTH_MAX;
		}

		std::vector<Eigen::Matrix<Decimal, 4, 1>> vertices[2] =
		{
			std::vector<Eigen::Matrix<Decimal, 4, 1>>(pixel_count, Eigen::Matrix<Decimal, 4, 1>(0, 0, 0, 0)),
			std::vector<Eigen::Matrix<Decimal, 4, 1>>(pixel_count, Eigen::Matrix<Decimal, 4, 1>(0, 0, 0, 0))
		};

		for (int i = 0; i < 2; ++i)
		{
			timer.start();
			compute_vertices(vertices[i], frame[i], fovy, aspect_ratio, near_plane, far_plane);
			timer.print_interval("Depth map back projection : ");
		}

		ICP<Decimal> icp;
		icp.setInputCloud(vertices[0]);
		icp.setTargetCloud(vertices[1]);

		Eigen::Matrix<Decimal, 4, 4> identity = Eigen::Matrix<Decimal, 4, 4>::Identity();
		Eigen::Matrix<Decimal, 4, 4> rigidTransform = Eigen::Matrix<Decimal, 4, 4>::Identity();
		Eigen::Matrix<Decimal, 3, 3> R;
		Eigen::Matrix<Decimal, 3, 1> t;

		for (int i = 0; i < 1; i++)
		{

			timer.start();
			bool icp_success = icp.align_iteration(vertices[0], vertices[1],
				filter_width, max_distance, width, height,
				R, t);

			timer.print_interval("Computing icp             : ");

			if (icp_success)
			{
				std::cout << "Icp Success" << std::endl;
			}
			else
			{
				std::cout << "Icp Failed" << std::endl;
			}

			std::cout
				<< std::fixed << std::endl
				<< R << std::endl << std::endl
				<< t.transpose() << std::endl << std::endl;
		}

	}
	catch (const std::exception& ex)
	{
		std::cerr << "Error: " << ex.what() << std::endl;
	}

	return 0;
}


#include <string>
#include <iostream>
#include <fstream>
#include <CGAL/Aff_transformation_3.h>
#include <CGAL/Cartesian.h>
#include <CGAL/IO/STL_reader.h>
#include <CGAL/Surface_mesh.h>
#include <CGAL/Polygon_mesh_processing/intersection.h>
#include <CGAL/Polygon_mesh_processing/transform.h>
#include <ctime>
#include <Eigen/Dense>

typedef CGAL::Simple_cartesian<double> K;
typedef CGAL::Surface_mesh<K::Point_3> Mesh;

namespace PMP = CGAL::Polygon_mesh_processing;

int main(int argc, char *argv[])
{
  const char* filename1 = "../meshes/BIG_SCREW.off";
  std::ifstream input1(filename1);
  Mesh mesh1;
  if (!input1 || !(input1 >> mesh1) || !CGAL::is_triangle_mesh(mesh1))
  {
    std::cerr << "Not a valid input file." << std::endl;
    return 1;
  }

  const char* filename2 = "../meshes/fingertip_8faces.off";
  std::ifstream input2(filename2);
  Mesh mesh2;
  if (!input2 || !(input2 >> mesh2) || !CGAL::is_triangle_mesh(mesh2))
  {
    std::cerr << "Not a valid input file." << std::endl;
    return 1;
  }

  Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
  for (int i = 0; i < 10; ++i) {
    double begin = std::clock();
    Eigen::Vector3d p;
    p << 0, 2*i, 0;
    K::Aff_transformation_3 transform_cgal(
        R(0,0), R(0,1), R(0,2), p(0),
        R(1,0), R(1,1), R(1,2), p(1),
        R(2,0), R(2,1), R(2,2), p(2), 1);
    // K::Aff_transformation_3 transform_cgal(CGAL::TRANSLATION, K::Vector_3(0,i*2,0));
    PMP::transform (transform_cgal, mesh1);
    bool intersecting = PMP::do_intersect (mesh1, mesh2);
    std::cout << "Computation Time: " << (std::clock() - begin) / CLOCKS_PER_SEC << " sec" << std::endl;
    std::cout << "intersecting: " << intersecting << std::endl;
  }
}


#include <opencv2/opencv.hpp>
#include <sophus/se3.hpp>
#include <boost/format.hpp>
#include <pangolin/pangolin.h>

using namespace std;

typedef vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;

// Camera intrinsics
double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;
// baseline
double baseline = 0.573;
// paths
string left_file = "../../images/left.png";
string disparity_file = "../../images/disparity.png";
boost::format fmt_others("../../images/%06d.png");    // other files

// useful typedefs
typedef Eigen::Matrix<double, 6, 6> Matrix6d;
typedef Eigen::Matrix<double, 2, 6> Matrix26d;
typedef Eigen::Matrix<double, 6, 1> Vector6d;

/// class for accumulator jacobians in parallel
class JacobianAccumulator {
public:
    JacobianAccumulator(
        const cv::Mat &img1_,
        const cv::Mat &img2_,
        const VecVector2d &px_ref_,
        const vector<double> depth_ref_,    //FIXME: Use &?//FIXME: Use &?
        Sophus::SE3d &T21_) :
        img1(img1_), img2(img2_), px_ref(px_ref_), depth_ref(depth_ref_), T21(T21_) {
        projection = VecVector2d(px_ref.size(), Eigen::Vector2d(0, 0));
    }

    /// accumulate jacobians in a range
    void accumulate_jacobian(const cv::Range &range);

    /// get hessian matrix
    Matrix6d hessian() const { return H; }

    /// get bias
    Vector6d bias() const { return b; }

    /// get total cost
    double cost_func() const { return cost; }

    /// get projected points
    VecVector2d projected_points() const { return projection; }

    /// reset h, b, cost to zero
    void reset() {
        H = Matrix6d::Zero();
        b = Vector6d::Zero();
        cost = 0;
    }

private:
    const cv::Mat &img1;
    const cv::Mat &img2;
    const VecVector2d &px_ref;
    const vector<double> depth_ref;  //FIXME: Use &?
    Sophus::SE3d &T21;
    VecVector2d projection; // projected points

    std::mutex hessian_mutex;
    Matrix6d H = Matrix6d::Zero();
    Vector6d b = Vector6d::Zero();
    double cost = 0;
};

/**
 * pose estimation using direct method
 * @param img1
 * @param img2
 * @param px_ref
 * @param depth_ref
 * @param T21
 */
void DirectPoseEstimationMultiLayer(
    const cv::Mat &img1,
    const cv::Mat &img2,
    const VecVector2d &px_ref,
    const vector<double> depth_ref,
    Sophus::SE3d &T21
);

/**
 * pose estimation using direct method
 * @param img1
 * @param img2
 * @param px_ref
 * @param depth_ref
 * @param T21
 */
void DirectPoseEstimationSingleLayer(
    const cv::Mat &img1,
    const cv::Mat &img2,
    const VecVector2d &px_ref,
    const vector<double> depth_ref,
    Sophus::SE3d &T21
);

// bilinear interpolation
inline float GetPixelValue(const cv::Mat &img, float x, float y) {
    // boundary check
    if (x < 0) x = 0;
    if (y < 0) y = 0;
    if (x >= img.cols) x = img.cols - 1;
    if (y >= img.rows) y = img.rows - 1;
    uchar *data = &img.data[int(y) * img.step + int(x)];
    float xx = x - floor(x);
    float yy = y - floor(y);
    return float(
        (1 - xx) * (1 - yy) * data[0] +
        xx * (1 - yy) * data[1] +
        (1 - xx) * yy * data[img.step] +
        xx * yy * data[img.step + 1]
    );
}

int main(int argc, char **argv) {

    cv::Mat left_img = cv::imread(left_file, 0);
    cv::Mat disparity_img = cv::imread(disparity_file, 0);

    // let's randomly pick pixels in the first image and generate some 3d points in the first image's frame
    cv::RNG rng;
    int nPoints = 2000;
    int boarder = 20;
    VecVector2d pixels_ref;
    vector<double> depth_ref;

    // generate pixels in ref and load depth data
    for (int i = 0; i < nPoints; i++) {
        int x = rng.uniform(boarder, left_img.cols - boarder);  // don't pick pixels close to boarder
        int y = rng.uniform(boarder, left_img.rows - boarder);  // don't pick pixels close to boarder
        int disparity = disparity_img.at<uchar>(y, x);
        double depth = fx * baseline / disparity; // you know this is disparity to depth
        depth_ref.push_back(depth);
        pixels_ref.push_back(Eigen::Vector2d(x, y));
    }

    // estimates 01~05.png's pose using this information
    Sophus::SE3d T_cur_ref;

    for (int i = 1; i < 6; i++) {  // 1~10
        cv::Mat img = cv::imread((fmt_others % i).str(), 0);
        // try single layer by uncomment this line
        // DirectPoseEstimationSingleLayer(left_img, img, pixels_ref, depth_ref, T_cur_ref);
        DirectPoseEstimationMultiLayer(left_img, img, pixels_ref, depth_ref, T_cur_ref);
    }
    return 0;
}

void DirectPoseEstimationSingleLayer(
    const cv::Mat &img1,
    const cv::Mat &img2,
    const VecVector2d &px_ref,
    const vector<double> depth_ref,  //FIXME: Use &?
    Sophus::SE3d &T21) {

    const int iterations = 10;
    double cost = 0, lastCost = 0;
    auto t1 = chrono::steady_clock::now();
    JacobianAccumulator jaco_accu(img1, img2, px_ref, depth_ref, T21);

    for (int iter = 0; iter < iterations; iter++) {
        jaco_accu.reset();
        cv::parallel_for_(cv::Range(0, px_ref.size()),
                          std::bind(&JacobianAccumulator::accumulate_jacobian, &jaco_accu, std::placeholders::_1));
        Matrix6d H = jaco_accu.hessian();
        Vector6d b = jaco_accu.bias();

        // solve update and put it into estimation
        Vector6d update = H.ldlt().solve(b);;
        T21 = Sophus::SE3d::exp(update) * T21;
        cost = jaco_accu.cost_func();

        if (std::isnan(update[0])) {
            // sometimes occurred when we have a black or white patch and H is irreversible
            cout << "update is nan" << endl;
            break;
        }
        if (iter > 0 && cost > lastCost) {
            cout << "cost increased: " << cost << ", " << lastCost << endl;
            break;
        }
        if (update.norm() < 1e-3) {
            // converge
            break;
        }

        lastCost = cost;
        cout << "iteration: " << iter << ", cost: " << cost << endl;
    }

    cout << "T21 = \n" << T21.matrix() << endl;
    auto t2 = chrono::steady_clock::now();
    auto time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
    cout << "direct method for single layer: " << time_used.count() << endl;

    // plot the projected pixels here
    cv::Mat img2_show;
    cv::cvtColor(img2, img2_show, CV_GRAY2BGR);
    VecVector2d projection = jaco_accu.projected_points();
    for (size_t i = 0; i < px_ref.size(); ++i) {
        auto p_ref = px_ref[i];
        auto p_cur = projection[i];
        if (p_cur[0] > 0 && p_cur[1] > 0) {
            cv::circle(img2_show, cv::Point2f(p_cur[0], p_cur[1]), 2, cv::Scalar(0, 250, 0), 2);
            cv::line(img2_show, cv::Point2f(p_ref[0], p_ref[1]), cv::Point2f(p_cur[0], p_cur[1]),
                     cv::Scalar(0, 250, 0));
        }
    }
    cv::imshow("current", img2_show);
    cv::waitKey();
}

void JacobianAccumulator::accumulate_jacobian(const cv::Range &range) {

    // parameters
    const int half_patch_size = 1;
    int cnt_good = 0;
    Matrix6d hessian = Matrix6d::Zero();
    Vector6d bias = Vector6d::Zero();
    double cost_tmp = 0;

    for (size_t i = range.start; i < range.end; i++) {

        // compute the projection in the second image
        Eigen::Vector3d point_ref =
            depth_ref[i] * Eigen::Vector3d((px_ref[i][0] - cx) / fx, (px_ref[i][1] - cy) / fy, 1);
        Eigen::Vector3d point_cur = T21 * point_ref;
        if (point_cur[2] < 0)   // depth invalid
            continue;

        float u = fx * point_cur[0] / point_cur[2] + cx, v = fy * point_cur[1] / point_cur[2] + cy;
        if (u < half_patch_size || u > img2.cols - half_patch_size || v < half_patch_size ||
            v > img2.rows - half_patch_size)
            continue;

        projection[i] = Eigen::Vector2d(u, v);
        double X = point_cur[0], Y = point_cur[1], Z = point_cur[2],
            Z2 = Z * Z, Z_inv = 1.0 / Z, Z2_inv = Z_inv * Z_inv;
        cnt_good++;

        // and compute error and jacobian
        for (int x = -half_patch_size; x <= half_patch_size; x++)
            for (int y = -half_patch_size; y <= half_patch_size; y++) {

                double error = GetPixelValue(img1, px_ref[i][0] + x, px_ref[i][1] + y) -
                               GetPixelValue(img2, u + x, v + y);
                Matrix26d J_pixel_xi;
                Eigen::Vector2d J_img_pixel;

                J_pixel_xi(0, 0) = fx * Z_inv;
                J_pixel_xi(0, 1) = 0;
                J_pixel_xi(0, 2) = -fx * X * Z2_inv;
                J_pixel_xi(0, 3) = -fx * X * Y * Z2_inv;
                J_pixel_xi(0, 4) = fx + fx * X * X * Z2_inv;
                J_pixel_xi(0, 5) = -fx * Y * Z_inv;

                J_pixel_xi(1, 0) = 0;
                J_pixel_xi(1, 1) = fy * Z_inv;
                J_pixel_xi(1, 2) = -fy * Y * Z2_inv;
                J_pixel_xi(1, 3) = -fy - fy * Y * Y * Z2_inv;
                J_pixel_xi(1, 4) = fy * X * Y * Z2_inv;
                J_pixel_xi(1, 5) = fy * X * Z_inv;

                J_img_pixel = Eigen::Vector2d(
                    0.5 * (GetPixelValue(img2, u + 1 + x, v + y) - GetPixelValue(img2, u - 1 + x, v + y)),
                    0.5 * (GetPixelValue(img2, u + x, v + 1 + y) - GetPixelValue(img2, u + x, v - 1 + y))
                );

                // total jacobian
                Vector6d J = -1.0 * (J_img_pixel.transpose() * J_pixel_xi).transpose();

                hessian += J * J.transpose();
                bias += -error * J;
                cost_tmp += error * error;
            }
    }

    if (cnt_good) {
        // set hessian, bias and cost
        unique_lock<mutex> lck(hessian_mutex);
        H += hessian;
        b += bias;
        cost += cost_tmp / cnt_good;
    }
}

void DirectPoseEstimationMultiLayer(
    const cv::Mat &img1,
    const cv::Mat &img2,
    const VecVector2d &px_ref,
    const vector<double> depth_ref,  //FIXME: Use &?
    Sophus::SE3d &T21) {

    // parameters
    int pyramids = 4;
    double pyramid_scale = 0.5;
    double scales[] = {1.0, 0.5, 0.25, 0.125};

    // create pyramids
    vector<cv::Mat> pyr1, pyr2; // image pyramids
    for (int i = 0; i < pyramids; i++) {
        if (i == 0) {
            pyr1.push_back(img1);
            pyr2.push_back(img2);
        } else {
            cv::Mat img1_pyr, img2_pyr;
            cv::resize(pyr1[i - 1], img1_pyr,
                       cv::Size(pyr1[i - 1].cols * pyramid_scale, pyr1[i - 1].rows * pyramid_scale));
            cv::resize(pyr2[i - 1], img2_pyr,
                       cv::Size(pyr2[i - 1].cols * pyramid_scale, pyr2[i - 1].rows * pyramid_scale));
            pyr1.push_back(img1_pyr);
            pyr2.push_back(img2_pyr);
        }
    }

    double fxG = fx, fyG = fy, cxG = cx, cyG = cy;  // backup the old values
    for (int level = pyramids - 1; level >= 0; level--) {
        VecVector2d px_ref_pyr; // set the keypoints in this pyramid level
        for (auto &px: px_ref) {
            px_ref_pyr.push_back(scales[level] * px);
        }

        // scale fx, fy, cx, cy in different pyramid levels
        fx = fxG * scales[level];
        fy = fyG * scales[level];
        cx = cxG * scales[level];
        cy = cyG * scales[level];
        DirectPoseEstimationSingleLayer(pyr1[level], pyr2[level], px_ref_pyr, depth_ref, T21);
    }

}


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/

#pragma once

#include "FluidEigenMappings.hpp"
#include "../../data/FluidIndex.hpp"
#include "../../data/TensorTypes.hpp"
#include <Eigen/Core>
#include <cassert>
#include <cmath>

namespace fluid {
namespace algorithm {

class WeightedStats
{
public:
  Eigen::ArrayXd process(Eigen::Ref<Eigen::ArrayXd> input,
                         Eigen::Ref<Eigen::ArrayXd> weights, double low,
                         double mid, double high)
  {
    using namespace Eigen;
    using namespace std;
    index   length = input.size();
    ArrayXd out = ArrayXd::Zero(7);
    double  mean = (weights * input).sum();
    double  stdev = sqrt((weights * (input - mean).square()).sum());
    double  skewness =
        (weights * ((input - mean) / (stdev == 0 ? 1 : stdev)).cube()).sum();
    double kurtosis =
        (weights * ((input - mean) / (stdev == 0 ? 1 : stdev)).pow(4)).sum();
    ArrayXd sorted = input;
    ArrayXidx perm = ArrayXidx::LinSpaced(length, 0, length - 1);
    std::sort(perm.data(), perm.data() + length,
              [&](index i, index j) { return input(i) < input(j); });
    index  level = 0;
    double lowVal{input(perm(0))};
    double midVal{input(perm(lrint(length - 1) / 2))};
    double hiVal{input(perm(lrint(length - 1)))};
    double acc = weights(perm(0)), prevAcc = 0;
    for (index i = 1; i < length; i++)
    {
      acc += weights(perm(i));
      if (level == 0 && acc >= low)
      {
        lowVal = abs(prevAcc - low) <= abs(acc - low) ? input(perm(i - 1))
                                                      : input(perm(i));
        level = 1;
      }
      if (level == 1 && acc >= mid)
      {
        midVal = abs(prevAcc - mid) < abs(acc - mid) ? input(perm(i - 1))
                                                     : input(perm(i));
        level = 2;
      }
      if (level == 2 && acc >= high)
      {
        hiVal = abs(prevAcc - high) < abs(acc - high) ? input(perm(i - 1))
                                                      : input(perm(i));
        break;
      }
      prevAcc = acc;
    }
    out << mean, stdev, skewness, kurtosis, lowVal, midVal, hiVal;
    return out;
  }
};
} // namespace algorithm
} // namespace fluid


#include <geom/aabb.h>
#include <geom/matrix.h>
#include <geom/primitives.h>
#include <geom/properties.h>

#include <Eigen/Dense>

double SignedTriangleVolume6(double3 a, double3 b, double3 c) {
    double z = a.z;
    z += b.z;
    z += c.z;
    double s;
    s = (a.y + b.y) * (a.x - b.x);
    s += (b.y + c.y) * (b.x - c.x);
    s += (c.y + a.y) * (c.x - a.x);
    return s * z;
}

double SignedRingVolume6(cspan<double3> ring) {
    double z = 0;
    for (auto e : ring) z += e.z;
    double s = 0;
    for (auto e : Edges(ring)) s += (e.a.y + e.b.y) * (e.a.x - e.b.x);
    return s * z;
}

double SignedVolume(const mesh3& mesh) {
    double v = 0;
    for (const triangle3& f : mesh) v += SignedTriangleVolume6(f.a, f.b, f.c);
    return v / 6.0;
}

double SignedVolume(const xmesh3& mesh) {
    double v = 0;
    for (face f : mesh)
        for (auto ring : f) v += SignedRingVolume6(ring);
    return v / 6.0;
}

// TODO test with volume of cube (randomly rotated)

// Center of mass of a valid polyhedron
double3 CenterOfMass(const mesh3& mesh) {
    double3 P = {0, 0, 0};
    double V = 0;
    for (const triangle3& f : mesh) {
        double v = dot(f.a, cross(f.b, f.c));
        P += (f.a + f.b + f.c) * v;
        V += v;
    }
    return P / (V * 4);
}

double2 centroid(cspan<double2> poly) {
    double2 P = {0, 0};
    double2 V = 0;
    for (auto i : range(poly.size())) {
        double2 a = poly[i];
        double2 b = poly[(i + 1) % poly.size()];
        double v = a.x * b.y - a.y * b.x;
        P += (a + b) * v;
        V += v;
    }
    return P / (V * 3);
}

double2 CenterOfMass(const polygon2& poly) {
    double2 P = {0, 0};
    double2 V = 0;
    for (auto [a, b] : Edges(poly)) {
        double v = a.x * b.y - a.y * b.x;
        P += (a + b) * v;
        V += v;
    }
    return P / (V * 3);
}

// Moment of inertia of a valid polyhedron with Center of Mass = 0 and Density = 1
double33 moment_of_inertia(const mesh3& mesh) {
    constexpr double a = 1 / 60., b = 1 / 120.;
    const double33 canonical = {{a, b, b}, {b, a, b}, {b, b, a}};
    auto C = double33::make(0);  // covariance
    for (const auto& f : mesh) {
        double33 A;
        // TODO setting rows or columns?
        A.a = f[0];
        A.b = f[1];
        A.c = f[2];
        C += transpose(A) * canonical * A * det(A);
    }
    return double33::make(trace(C)) - C;  // C -> I
}

bool is_aabb(const mesh3& mesh) {
    aabb3 box(mesh);

    // All vertices must be made from extreme coordinates
    for (auto f : mesh)
        for (double3 v : f)
            if (v.x != box.min[0] && v.x != box.max[0] && v.y != box.min[1] && v.y != box.max[1] && v.z != box.min[2] &&
                v.z != box.max[2])
                return false;

    // Every face must have one coordinate constant
    for (auto f : mesh) {
        bool xx = f.a.x == f.b.x && f.b.x == f.c.x;
        bool yy = f.a.y == f.b.y && f.b.y == f.c.y;
        bool zz = f.a.z == f.b.z && f.b.z == f.c.z;
        if (!xx && !yy && !zz) return false;
    }
    return true;
}

double3 eigen_vector(cspan<double3> points) {
    // compute mean
    double3 m = {0, 0, 0};
    for (auto p : points) m += p;
    m /= points.size();

    // compute matrix
    double3 ss = {0, 0, 0};
    double xy = 0, xz = 0, yz = 0;
    for (auto p : points) {
        p -= m;
        ss += p * p;
        xy += p.x * p.y;
        xz += p.x * p.z;
        yz += p.y * p.z;
    }

    Eigen::MatrixXd a = Eigen::MatrixXd::Zero(3, 3);
    a(0, 0) = ss.x;
    a(1, 1) = ss.y;
    a(2, 2) = ss.z;
    a(0, 1) = a(1, 0) = xy;
    a(0, 2) = a(2, 0) = xz;
    a(1, 2) = a(2, 1) = yz;

    Eigen::EigenSolver<Eigen::MatrixXd> es(a, true);
    auto values = es.eigenvalues();
    int i = 0;
    if (values(1).real() < values(i).real()) i = 1;
    if (values(2).real() < values(i).real()) i = 2;

    auto vectors = es.eigenvectors();
    return {vectors(0, i).real(), vectors(1, i).real(), vectors(2, i).real()};
}