//Author: Dr. Shantanu Shahane
#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <time.h>
#include <float.h>
#include <string.h>
#include <iostream>
#include <vector>
#include <Eigen/Dense>
#include <unsupported/Eigen/SparseExtra>
#include <Eigen/SparseLU>
#include <Eigen/OrderingMethods>
#include <Eigen/Core>
#include "_hypre_utilities.h"
#include "HYPRE_krylov.h"
#include "HYPRE.h"
#include "HYPRE_parcsr_ls.h"
#include "general_functions.hpp"
#include "class.hpp"

SEMI_IMPLICIT_SPLIT_SOLVER::SEMI_IMPLICIT_SPLIT_SOLVER(POINTS &points, CLOUD &cloud, PARAMETERS &parameters, vector<bool> &u_dirichlet_flag1, vector<bool> &v_dirichlet_flag1, vector<bool> &p_dirichlet_flag1, int n_outer_iter1, double iterative_tolerance1, int precond_freq_it1)
{
    u_dirichlet_flag = u_dirichlet_flag1, v_dirichlet_flag = v_dirichlet_flag1, p_dirichlet_flag = p_dirichlet_flag1;
    n_outer_iter = n_outer_iter1, iterative_tolerance = iterative_tolerance1, precond_freq_it = precond_freq_it1;
    check_bc(points, parameters);
    clock_t clock_t1 = clock(), clock_t2 = clock();
    solver_p.init(points, cloud, parameters, p_dirichlet_flag, 0.0, 0.0, 1.0, true); //BC for p_prime is identical to BC of p_new
    parameters.factoring_timer = ((double)(clock() - clock_t1)) / CLOCKS_PER_SEC;

    zero_vector = Eigen::VectorXd::Zero(points.nv);
    zero_vector_1 = Eigen::VectorXd::Zero(points.nv + 1);
    p_bc_full_neumann = true;
    for (int iv = 0; iv < points.nv; iv++)
        if (points.boundary_flag[iv] && p_dirichlet_flag[iv])
        { //boundary point found with dirichlet BC
            p_bc_full_neumann = false;
            break;
        }

    if (p_bc_full_neumann)
        p_source = zero_vector_1, p_prime = zero_vector_1; //this is full Neumann for pressure
    else
        p_source = zero_vector, p_prime = zero_vector;
    u_source = zero_vector, v_source = zero_vector;
    u_old_old = zero_vector, v_old_old = zero_vector;
    u_prime = zero_vector, v_prime = zero_vector;
    u_iter_old = zero_vector, v_iter_old = zero_vector;
    normal_mom_x = zero_vector, normal_mom_y = zero_vector;
}

void SEMI_IMPLICIT_SPLIT_SOLVER::check_bc(POINTS &points, PARAMETERS &parameters)
{
    int u_dirichlet_flag_sum = accumulate(u_dirichlet_flag.begin(), u_dirichlet_flag.end(), 0);
    if (u_dirichlet_flag_sum == 0)
    {
        printf("\n\nERROR from SEMI_IMPLICIT_SPLIT_SOLVER::check_bc Setting u_dirichlet_flag to full Neumann BC is not permitted; sum of u_dirichlet_flag: %i\n\n", u_dirichlet_flag_sum);
        throw bad_exception();
    }
    int v_dirichlet_flag_sum = accumulate(v_dirichlet_flag.begin(), v_dirichlet_flag.end(), 0);
    if (v_dirichlet_flag_sum == 0)
    {
        printf("\n\nERROR from FRACTIONASEMI_IMPLICIT_SPLIT_SOLVERL_STEP_1::check_bc Setting v_dirichlet_flag to full Neumann BC is not permitted; sum of v_dirichlet_flag: %i\n\n", v_dirichlet_flag_sum);
        throw bad_exception();
    }
    if (parameters.rho < 0 || parameters.mu < 0)
    {
        printf("\n\nERROR from SEMI_IMPLICIT_SPLIT_SOLVER::check_bc Some parameters are not set; parameters.rho: %g, parameters.mu: %g\n\n", parameters.rho, parameters.mu);
        throw bad_exception();
    }
}

void SEMI_IMPLICIT_SPLIT_SOLVER::calc_vel(POINTS &points, PARAMETERS &parameters, Eigen::VectorXd &u_new, Eigen::VectorXd &v_new, Eigen::VectorXd &p_new, Eigen::VectorXd &u_old, Eigen::VectorXd &v_old, Eigen::VectorXd &p_old, Eigen::VectorXd &body_force_x, Eigen::VectorXd &body_force_y)
{
    if (it == 0)
    { //BDF1: implicit Euler
        u_source = ((parameters.rho / parameters.dt) * u_old) - (points.grad_x_matrix_EIGEN_internal * p_new.head(points.nv)) + body_force_x;
        v_source = ((parameters.rho / parameters.dt) * v_old) - (points.grad_y_matrix_EIGEN_internal * p_new.head(points.nv)) + body_force_y;
    }
    else
    { //BDF2
        u_source = -((bdf2_alpha_2 * parameters.rho / parameters.dt) * u_old) - ((bdf2_alpha_3 * parameters.rho / parameters.dt) * u_old_old) - (points.grad_x_matrix_EIGEN_internal * p_new.head(points.nv)) + body_force_x;
        v_source = -((bdf2_alpha_2 * parameters.rho / parameters.dt) * v_old) - ((bdf2_alpha_3 * parameters.rho / parameters.dt) * v_old_old) - (points.grad_y_matrix_EIGEN_internal * p_new.head(points.nv)) + body_force_y;
    }

    for (int iv = 0; iv < points.nv; iv++)
        if (points.boundary_flag[iv])
        { //retain boundary condition from "_old"
            if (u_dirichlet_flag[iv])
                u_source[iv] = u_old[iv];
            else
                u_source[iv] = 0.0;
            if (v_dirichlet_flag[iv])
                v_source[iv] = v_old[iv];
            else
                v_source[iv] = 0.0;
        }
    u_new = solver_eigen_u.solveWithGuess(u_source, u_new);
    v_new = solver_eigen_v.solveWithGuess(v_source, v_new);
}

void SEMI_IMPLICIT_SPLIT_SOLVER::calc_vel(POINTS &points, PARAMETERS &parameters, Eigen::VectorXd &u_new, Eigen::VectorXd &v_new, Eigen::VectorXd &p_new, Eigen::VectorXd &u_old, Eigen::VectorXd &v_old, Eigen::VectorXd &p_old)
{
    if (it == 0)
    { //BDF1: implicit Euler
        u_source = ((parameters.rho / parameters.dt) * u_old) - (points.grad_x_matrix_EIGEN_internal * p_new.head(points.nv));
        v_source = ((parameters.rho / parameters.dt) * v_old) - (points.grad_y_matrix_EIGEN_internal * p_new.head(points.nv));
    }
    else
    { //BDF2
        u_source = -((bdf2_alpha_2 * parameters.rho / parameters.dt) * u_old) - ((bdf2_alpha_3 * parameters.rho / parameters.dt) * u_old_old) - (points.grad_x_matrix_EIGEN_internal * p_new.head(points.nv));
        v_source = -((bdf2_alpha_2 * parameters.rho / parameters.dt) * v_old) - ((bdf2_alpha_3 * parameters.rho / parameters.dt) * v_old_old) - (points.grad_y_matrix_EIGEN_internal * p_new.head(points.nv));
    }

    for (int iv = 0; iv < points.nv; iv++)
        if (points.boundary_flag[iv])
        { //retain boundary condition from "_old"
            if (u_dirichlet_flag[iv])
                u_source[iv] = u_old[iv];
            else
                u_source[iv] = 0.0;
            if (v_dirichlet_flag[iv])
                v_source[iv] = v_old[iv];
            else
                v_source[iv] = 0.0;
        }
    u_new = solver_eigen_u.solveWithGuess(u_source, u_new);
    v_new = solver_eigen_v.solveWithGuess(v_source, v_new);
}

void SEMI_IMPLICIT_SPLIT_SOLVER::calc_pressure(POINTS &points, CLOUD &cloud, PARAMETERS &parameters, Eigen::VectorXd &u_new, Eigen::VectorXd &v_new, Eigen::VectorXd &p_new, Eigen::VectorXd &u_old, Eigen::VectorXd &v_old, Eigen::VectorXd &p_old)
{
    if (p_bc_full_neumann)
        p_source = zero_vector_1; //this is full Neumann for pressure
    else
        p_source = zero_vector;
    if (it == 0) //BDF1: implicit Euler
        p_source.head(points.nv) = ((points.grad_x_matrix_EIGEN_internal * u_new) + (points.grad_y_matrix_EIGEN_internal * v_new)) * (parameters.rho / parameters.dt);
    else
        p_source.head(points.nv) = ((points.grad_x_matrix_EIGEN_internal * u_new) + (points.grad_y_matrix_EIGEN_internal * v_new)) * (bdf2_alpha_1 * parameters.rho / parameters.dt);
    //BC for p_prime is identical to BC of p_new
    //p_source of p_prime is zero for both dirichlet and neumann for p_new
    if (p_bc_full_neumann) //this is full Neumann for pressure
        solver_p.general_solve(points, parameters, p_prime, zero_vector_1, p_source);
    else
        solver_p.general_solve(points, parameters, p_prime, zero_vector, p_source);
    p_new = p_new + p_prime;

    normal_mom_x = zero_vector, normal_mom_y = zero_vector;
    if (it == 0)
    { //BDF1: implicit Euler
        normal_mom_x = -parameters.rho * (u_new.cwiseProduct(points.grad_x_matrix_EIGEN_boundary * u_new) + v_new.cwiseProduct(points.grad_y_matrix_EIGEN_boundary * u_new)) + parameters.mu * (points.laplacian_matrix_EIGEN_boundary * u_new) - (parameters.rho / parameters.dt) * (u_new - u_old);
        normal_mom_y = -parameters.rho * (u_new.cwiseProduct(points.grad_x_matrix_EIGEN_boundary * v_new) + v_new.cwiseProduct(points.grad_y_matrix_EIGEN_boundary * v_new)) + parameters.mu * (points.laplacian_matrix_EIGEN_boundary * v_new) - (parameters.rho / parameters.dt) * (v_new - v_old);
    }
    else
    {
        normal_mom_x = -parameters.rho * (u_new.cwiseProduct(points.grad_x_matrix_EIGEN_boundary * u_new) + v_new.cwiseProduct(points.grad_y_matrix_EIGEN_boundary * u_new)) + parameters.mu * (points.laplacian_matrix_EIGEN_boundary * u_new) - (parameters.rho / parameters.dt) * ((bdf2_alpha_1 * u_new) + (bdf2_alpha_2 * u_old) + (bdf2_alpha_3 * u_old_old));
        normal_mom_y = -parameters.rho * (u_new.cwiseProduct(points.grad_x_matrix_EIGEN_boundary * v_new) + v_new.cwiseProduct(points.grad_y_matrix_EIGEN_boundary * v_new)) + parameters.mu * (points.laplacian_matrix_EIGEN_boundary * v_new) - (parameters.rho / parameters.dt) * ((bdf2_alpha_1 * v_new) + (bdf2_alpha_2 * v_old) + (bdf2_alpha_3 * v_old_old));
    }

    double rhs, diag_coeff, off_diag_coeff;
    int dim = parameters.dimension, ivnb;

    for (int iv = 0; iv < points.nv; iv++)
    { //set boundary condition
        if (points.boundary_flag[iv])
        {
            if (!p_dirichlet_flag[iv]) //p_new for dirichlet BC is retained when p_prime is added (p_prime is zero at dirichlet BC)
            {                          //p_new for neumann BC has to be set explicitly
                rhs = (normal_mom_x[iv] * points.normal[dim * iv]) + (normal_mom_y[iv] * points.normal[dim * iv + 1]);
                for (int i1 = cloud.nb_points_row[iv]; i1 < cloud.nb_points_row[iv + 1]; i1++)
                {
                    ivnb = cloud.nb_points_col[i1];
                    if (ivnb == iv)
                        diag_coeff = (cloud.grad_x_coeff[i1] * points.normal[dim * iv]) + (cloud.grad_y_coeff[i1] * points.normal[dim * iv + 1]);
                    else
                    {
                        off_diag_coeff = (cloud.grad_x_coeff[i1] * points.normal[dim * iv]) + (cloud.grad_y_coeff[i1] * points.normal[dim * iv + 1]);
                        rhs = rhs - (off_diag_coeff * p_new[ivnb]);
                    }
                }
                p_new[iv] = rhs / diag_coeff;
            }
        }
    }
}

void SEMI_IMPLICIT_SPLIT_SOLVER::calc_vel_corr(POINTS &points, CLOUD &cloud, PARAMETERS &parameters, Eigen::VectorXd &u_new, Eigen::VectorXd &v_new)
{
    if (it == 0)
    { //BDF1: implicit Euler
        u_prime = ((points.grad_x_matrix_EIGEN_internal * p_prime.head(points.nv)) * (parameters.dt / parameters.rho));
        v_prime = ((points.grad_y_matrix_EIGEN_internal * p_prime.head(points.nv)) * (parameters.dt / parameters.rho));
    }
    else
    {
        u_prime = ((points.grad_x_matrix_EIGEN_internal * p_prime.head(points.nv)) * (parameters.dt / (bdf2_alpha_1 * parameters.rho)));
        v_prime = ((points.grad_y_matrix_EIGEN_internal * p_prime.head(points.nv)) * (parameters.dt / (bdf2_alpha_1 * parameters.rho)));
    }

    u_new = u_new - u_prime;
    v_new = v_new - v_prime;
    //internal points are set above; boundary values are obtained implicitly in the solver (for both dirichlet and neumann); u_prime and v_prime are zero at all boundaries
}

void SEMI_IMPLICIT_SPLIT_SOLVER::extras(POINTS &points, PARAMETERS &parameters, Eigen::VectorXd &u_new, Eigen::VectorXd &v_new, Eigen::VectorXd &p_new, Eigen::VectorXd &u_old, Eigen::VectorXd &v_old, Eigen::VectorXd &p_old)
{
    // double total_steady_err, max_err, l1_err;
    // calc_max_l1_error(u_old, u_new, max_err, l1_err);
    // total_steady_err = l1_err / parameters.dt;
    // calc_max_l1_error(v_old, v_new, max_err, l1_err);
    // total_steady_err += l1_err / parameters.dt;
    // parameters.steady_error_log.push_back(total_steady_err);
    u_old_old = u_old, v_old_old = v_old;
    // return total_steady_err;
}

void SEMI_IMPLICIT_SPLIT_SOLVER::modify_vel_matrix(POINTS &points, CLOUD &cloud, PARAMETERS &parameters, Eigen::VectorXd &u_new, Eigen::VectorXd &v_new)
{
    int ivnb, dim = parameters.dimension, index;
    double value, unsteady_factor;
    if (it == 0)
        unsteady_factor = 1.0; //BDF1: implicit Euler
    else
        unsteady_factor = bdf2_alpha_1;
    for (int iv = 0; iv < points.nv; iv++)
        if (!points.boundary_flag[iv])
        { //coefficients of boundary points never updated for velocities
            for (int i1 = cloud.nb_points_row[iv]; i1 < cloud.nb_points_row[iv + 1]; i1++)
            {
                ivnb = cloud.nb_points_col[i1];
                value = -parameters.mu * cloud.laplacian_coeff[i1];                    //diffusion
                value = value + (u_new[iv] * parameters.rho * cloud.grad_x_coeff[i1]); //convection
                value = value + (v_new[iv] * parameters.rho * cloud.grad_y_coeff[i1]); //convection
                if (ivnb == iv)
                    value = value + (unsteady_factor * parameters.rho / parameters.dt); //diagonal term
                index = nb_points_col_matrix_u[i1];
                matrix_u.valuePtr()[index] = value;
                index = nb_points_col_matrix_v[i1];
                matrix_v.valuePtr()[index] = value;
            }
        }
}

void SEMI_IMPLICIT_SPLIT_SOLVER::set_vel_matrix(POINTS &points, CLOUD &cloud, PARAMETERS &parameters, Eigen::VectorXd &u_new, Eigen::VectorXd &v_new)
{
    matrix_u.resize(0, 0), matrix_v.resize(0, 0);
    vector<Eigen::Triplet<double>> triplet;
    int ivnb, dim = parameters.dimension;
    double value, unsteady_factor;
    if (it == 0)
        unsteady_factor = 1.0; //BDF1: implicit Euler
    else
        unsteady_factor = bdf2_alpha_1;
    for (int iv = 0; iv < points.nv; iv++)
    {
        if (points.boundary_flag[iv])
        {
            if (u_dirichlet_flag[iv])
                triplet.push_back(Eigen::Triplet<double>(iv, iv, 1.0));
            else
            {
                for (int i1 = cloud.nb_points_row[iv]; i1 < cloud.nb_points_row[iv + 1]; i1++)
                {
                    ivnb = cloud.nb_points_col[i1];
                    value = points.normal[dim * iv] * cloud.grad_x_coeff[i1] + points.normal[dim * iv + 1] * cloud.grad_y_coeff[i1];
                    triplet.push_back(Eigen::Triplet<double>(iv, ivnb, value));
                }
            }
        }
        else
        {
            for (int i1 = cloud.nb_points_row[iv]; i1 < cloud.nb_points_row[iv + 1]; i1++)
            {
                ivnb = cloud.nb_points_col[i1];
                value = -parameters.mu * cloud.laplacian_coeff[i1];                    //diffusion
                value = value + (u_new[iv] * parameters.rho * cloud.grad_x_coeff[i1]); //convection
                value = value + (v_new[iv] * parameters.rho * cloud.grad_y_coeff[i1]); //convection
                if (ivnb == iv)
                    value = value + (unsteady_factor * parameters.rho / parameters.dt); //diagonal term
                triplet.push_back(Eigen::Triplet<double>(iv, ivnb, value));
            }
        }
    }
    matrix_u.resize(points.nv, points.nv);
    matrix_u.setFromTriplets(triplet.begin(), triplet.end());
    matrix_u.makeCompressed();
    triplet.clear();

    for (int iv = 0; iv < points.nv; iv++)
    {
        if (points.boundary_flag[iv])
        {
            if (v_dirichlet_flag[iv])
                triplet.push_back(Eigen::Triplet<double>(iv, iv, 1.0));
            else
            {
                for (int i1 = cloud.nb_points_row[iv]; i1 < cloud.nb_points_row[iv + 1]; i1++)
                {
                    ivnb = cloud.nb_points_col[i1];
                    value = points.normal[dim * iv] * cloud.grad_x_coeff[i1] + points.normal[dim * iv + 1] * cloud.grad_y_coeff[i1];
                    triplet.push_back(Eigen::Triplet<double>(iv, ivnb, value));
                }
            }
        }
        else
        {
            for (int i1 = cloud.nb_points_row[iv]; i1 < cloud.nb_points_row[iv + 1]; i1++)
            {
                ivnb = cloud.nb_points_col[i1];
                value = -parameters.mu * cloud.laplacian_coeff[i1];                    //diffusion
                value = value + (u_new[iv] * parameters.rho * cloud.grad_x_coeff[i1]); //convection
                value = value + (v_new[iv] * parameters.rho * cloud.grad_y_coeff[i1]); //convection
                if (ivnb == iv)
                    value = value + (parameters.rho / parameters.dt); //diagonal term
                triplet.push_back(Eigen::Triplet<double>(iv, ivnb, value));
            }
        }
    }
    matrix_v.resize(points.nv, points.nv);
    matrix_v.setFromTriplets(triplet.begin(), triplet.end());
    matrix_v.makeCompressed();
    triplet.clear();
}

void SEMI_IMPLICIT_SPLIT_SOLVER::calc_nb_points_col_matrix(POINTS &points, CLOUD &cloud, PARAMETERS &parameters)
{
    int ivnb, index;
    nb_points_col_matrix_u.clear(), nb_points_col_matrix_v.clear();
    for (int i1 = 0; i1 < cloud.nb_points_col.size(); i1++) //initialize to -1
        nb_points_col_matrix_u.push_back(-1), nb_points_col_matrix_v.push_back(-1);
    for (int iv = 0; iv < points.nv; iv++)
        if (!points.boundary_flag[iv])
        { //coefficients of boundary points never updated for velocities; nb_points_col_matrix ahas value of [-1] at bounday points
            for (int i1 = cloud.nb_points_row[iv]; i1 < cloud.nb_points_row[iv + 1]; i1++)
            {
                index = -1;
                ivnb = cloud.nb_points_col[i1];
                for (int i2 = matrix_u.outerIndexPtr()[iv]; i2 < matrix_u.outerIndexPtr()[iv + 1]; i2++)
                    if (matrix_u.innerIndexPtr()[i2] == ivnb)
                    {
                        index = i2;
                        break;
                    }
                if (index < 0)
                {
                    cout << "\n\nError from SEMI_IMPLICIT_SPLIT_SOLVER::calc_nb_points_col_matrix in matrix_u, unable to find ivnb: " << ivnb << " for iv: " << iv << ", points.boundary_flag[iv]: " << points.boundary_flag[iv] << "\n\n";
                    throw bad_exception();
                }
                nb_points_col_matrix_u[i1] = index;
            }
        }
    for (int iv = 0; iv < points.nv; iv++)
        if (!points.boundary_flag[iv])
        { //coefficients of boundary points never updated for velocities; nb_points_col_matrix ahas value of [-1] at bounday points
            for (int i1 = cloud.nb_points_row[iv]; i1 < cloud.nb_points_row[iv + 1]; i1++)
            {
                index = -1;
                ivnb = cloud.nb_points_col[i1];
                for (int i2 = matrix_v.outerIndexPtr()[iv]; i2 < matrix_v.outerIndexPtr()[iv + 1]; i2++)
                    if (matrix_v.innerIndexPtr()[i2] == ivnb)
                    {
                        index = i2;
                        break;
                    }
                if (index < 0)
                {
                    cout << "\n\nError from SEMI_IMPLICIT_SPLIT_SOLVER::calc_nb_points_col_matrix in matrix_v, unable to find ivnb: " << ivnb << " for iv: " << iv << ", points.boundary_flag[iv]: " << points.boundary_flag[iv] << "\n\n";
                    throw bad_exception();
                }
                nb_points_col_matrix_v[i1] = index;
            }
        }
}

void SEMI_IMPLICIT_SPLIT_SOLVER::single_timestep_2d(POINTS &points, CLOUD &cloud, PARAMETERS &parameters, Eigen::VectorXd &u_new, Eigen::VectorXd &v_new, Eigen::VectorXd &p_new, Eigen::VectorXd &u_old, Eigen::VectorXd &v_old, Eigen::VectorXd &p_old, int it1, vector<int> &n_outer_iter_log, vector<double> &iterative_l1_err_log, vector<double> &iterative_max_err_log)
{
    it = it1;
    if (p_source.size() != p_new.size())
        p_new = Eigen::VectorXd::Zero(p_source.size());
    if (p_source.size() != p_old.size())
        p_old = Eigen::VectorXd::Zero(p_source.size());
    double total_steady_err;
    u_new = u_old, v_new = v_old, p_new = p_old; //initialize
    if (it % precond_freq_it == 0 || it == 0 || it == 1)
    {
        set_vel_matrix(points, cloud, parameters, u_new, v_new);
        solver_eigen_u.setTolerance(parameters.solver_tolerance); //default is machine precision (https://eigen.tuxfamily.org/dox/classEigen_1_1IterativeSolverBase.html#ac160a444af8998f93da9aa30e858470d)
        solver_eigen_u.setMaxIterations(parameters.n_iter);       //default is twice number of columns (https://eigen.tuxfamily.org/dox/classEigen_1_1IterativeSolverBase.html#af83de7a7d31d9d4bd1fef6222b07335b)
        solver_eigen_u.preconditioner().setDroptol(parameters.precond_droptol);
        solver_eigen_u.compute(matrix_u);
        solver_eigen_v.setTolerance(parameters.solver_tolerance); //default is machine precision (https://eigen.tuxfamily.org/dox/classEigen_1_1IterativeSolverBase.html#ac160a444af8998f93da9aa30e858470d)
        solver_eigen_v.setMaxIterations(parameters.n_iter);       //default is twice number of columns (https://eigen.tuxfamily.org/dox/classEigen_1_1IterativeSolverBase.html#af83de7a7d31d9d4bd1fef6222b07335b)
        solver_eigen_v.preconditioner().setDroptol(parameters.precond_droptol);
        solver_eigen_v.compute(matrix_v);
        calc_nb_points_col_matrix(points, cloud, parameters);
    }
    for (outer_iter = 0; outer_iter < n_outer_iter; outer_iter++)
    {
        u_iter_old = u_new, v_iter_old = v_new;
        modify_vel_matrix(points, cloud, parameters, u_new, v_new);
        calc_vel(points, parameters, u_new, v_new, p_new, u_old, v_old, p_old);
        calc_pressure(points, cloud, parameters, u_new, v_new, p_new, u_old, v_old, p_old);
        calc_vel_corr(points, cloud, parameters, u_new, v_new);
        iterative_l1_err = (u_new - u_iter_old).lpNorm<1>() / (parameters.dimension * u_new.size());
        iterative_l1_err += ((v_new - v_iter_old).lpNorm<1>() / (parameters.dimension * v_new.size()));
        iterative_max_err = (u_new - u_iter_old).lpNorm<Eigen::Infinity>() / parameters.dimension;
        iterative_max_err += ((v_new - v_iter_old).lpNorm<Eigen::Infinity>() / parameters.dimension);
        if ((outer_iter == n_outer_iter - 1) || (iterative_l1_err <= iterative_tolerance))
        {
            iterative_max_err_log.push_back(iterative_max_err);
            iterative_l1_err_log.push_back(iterative_l1_err);
            n_outer_iter_log.push_back(outer_iter + 1);
            // printf("    SEMI_IMPLICIT_SPLIT_SOLVER::single_timestep_2d outer_iter: %i, iterative l1_err: %g, max_err: %g, tolerance: %g\n", outer_iter, iterative_l1_err, iterative_max_err, iterative_tolerance);
        }
        if (iterative_l1_err <= iterative_tolerance)
            break;
    }
    extras(points, parameters, u_new, v_new, p_new, u_old, v_old, p_old);
    // return total_steady_err;
}

void SEMI_IMPLICIT_SPLIT_SOLVER::single_timestep_2d(POINTS &points, CLOUD &cloud, PARAMETERS &parameters, Eigen::VectorXd &u_new, Eigen::VectorXd &v_new, Eigen::VectorXd &p_new, Eigen::VectorXd &u_old, Eigen::VectorXd &v_old, Eigen::VectorXd &p_old, Eigen::VectorXd &body_force_x, Eigen::VectorXd &body_force_y, int it1, vector<int> &n_outer_iter_log, vector<double> &iterative_l1_err_log, vector<double> &iterative_max_err_log)
{
    it = it1;
    if (p_source.size() != p_new.size())
        p_new = Eigen::VectorXd::Zero(p_source.size());
    if (p_source.size() != p_old.size())
        p_old = Eigen::VectorXd::Zero(p_source.size());
    double total_steady_err;
    u_new = u_old, v_new = v_old, p_new = p_old; //initialize
    if (it % precond_freq_it == 0 || it == 0 || it == 1)
    {
        set_vel_matrix(points, cloud, parameters, u_new, v_new);
        solver_eigen_u.setTolerance(parameters.solver_tolerance); //default is machine precision (https://eigen.tuxfamily.org/dox/classEigen_1_1IterativeSolverBase.html#ac160a444af8998f93da9aa30e858470d)
        solver_eigen_u.setMaxIterations(parameters.n_iter);       //default is twice number of columns (https://eigen.tuxfamily.org/dox/classEigen_1_1IterativeSolverBase.html#af83de7a7d31d9d4bd1fef6222b07335b)
        solver_eigen_u.preconditioner().setDroptol(parameters.precond_droptol);
        solver_eigen_u.compute(matrix_u);
        solver_eigen_v.setTolerance(parameters.solver_tolerance); //default is machine precision (https://eigen.tuxfamily.org/dox/classEigen_1_1IterativeSolverBase.html#ac160a444af8998f93da9aa30e858470d)
        solver_eigen_v.setMaxIterations(parameters.n_iter);       //default is twice number of columns (https://eigen.tuxfamily.org/dox/classEigen_1_1IterativeSolverBase.html#af83de7a7d31d9d4bd1fef6222b07335b)
        solver_eigen_v.preconditioner().setDroptol(parameters.precond_droptol);
        solver_eigen_v.compute(matrix_v);
        calc_nb_points_col_matrix(points, cloud, parameters);
    }
    for (outer_iter = 0; outer_iter < n_outer_iter; outer_iter++)
    {
        u_iter_old = u_new, v_iter_old = v_new;
        modify_vel_matrix(points, cloud, parameters, u_new, v_new);
        calc_vel(points, parameters, u_new, v_new, p_new, u_old, v_old, p_old, body_force_x, body_force_y);
        calc_pressure(points, cloud, parameters, u_new, v_new, p_new, u_old, v_old, p_old);
        calc_vel_corr(points, cloud, parameters, u_new, v_new);
        iterative_l1_err = (u_new - u_iter_old).lpNorm<1>() / (parameters.dimension * u_new.size());
        iterative_l1_err += ((v_new - v_iter_old).lpNorm<1>() / (parameters.dimension * v_new.size()));
        iterative_max_err = (u_new - u_iter_old).lpNorm<Eigen::Infinity>() / parameters.dimension;
        iterative_max_err += ((v_new - v_iter_old).lpNorm<Eigen::Infinity>() / parameters.dimension);
        if ((outer_iter == n_outer_iter - 1) || (iterative_l1_err <= iterative_tolerance))
        {
            iterative_max_err_log.push_back(iterative_max_err);
            iterative_l1_err_log.push_back(iterative_l1_err);
            n_outer_iter_log.push_back(outer_iter + 1);
            // printf("    SEMI_IMPLICIT_SPLIT_SOLVER::single_timestep_2d outer_iter: %i, iterative l1_err: %g, max_err: %g, tolerance: %g\n", outer_iter, iterative_l1_err, iterative_max_err, iterative_tolerance);
        }
        if (iterative_l1_err <= iterative_tolerance)
            break;
    }
    extras(points, parameters, u_new, v_new, p_new, u_old, v_old, p_old);
    // return total_steady_err;
}


#include "gate_matrix.hpp"
// Turn off Eigen warnings
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wsuggest-override"
#pragma GCC diagnostic ignored "-Wdeprecated-copy"
#pragma GCC diagnostic ignored "-Wunused-function"
#include <Eigen/Dense>
#pragma GCC diagnostic pop
#include <cassert>
#include <iostream>
#include <unordered_map>

namespace {
using namespace qcor::utils;
using namespace std::complex_literals;
Eigen::Matrix2cd getGateMat(const qop_t &in_op) {
  static const Eigen::Matrix2cd X_mat = []() {
    Eigen::Matrix2cd result = Eigen::MatrixXcd::Zero(2, 2);
    result << 0.0, 1.0, 1.0, 0.0;
    return result;
  }();
  static const Eigen::Matrix2cd Y_mat = []() {
    Eigen::Matrix2cd result = Eigen::MatrixXcd::Zero(2, 2);
    result << 0.0, -1i, 1i, 0.0;
    return result;
  }();
  static const Eigen::Matrix2cd Z_mat = []() {
    Eigen::Matrix2cd result = Eigen::MatrixXcd::Zero(2, 2);
    result << 1.0, 0.0, 0.0, -1.0;
    return result;
  }();
  static const Eigen::Matrix2cd H_mat = []() {
    Eigen::Matrix2cd result = Eigen::MatrixXcd::Zero(2, 2);
    result << M_SQRT1_2, M_SQRT1_2, M_SQRT1_2, -M_SQRT1_2;
    return result;
  }();

  static const auto rx_mat = [](const std::vector<double> &in_params) {
    assert(in_params.size() == 1);
    auto theta = in_params[0];
    Eigen::Matrix2cd result = Eigen::MatrixXcd::Zero(2, 2);
    result << std::cos(0.5 * theta),
        std::complex<double>(0, -1) * std::sin(0.5 * theta),
        std::complex<double>(0, -1) * std::sin(0.5 * theta),
        std::cos(0.5 * theta);
    return result;
  };

  static const auto ry_mat = [](const std::vector<double> &in_params) {
    assert(in_params.size() == 1);
    auto theta = in_params[0];
    Eigen::Matrix2cd result = Eigen::MatrixXcd::Zero(2, 2);
    result << std::cos(0.5 * theta), -std::sin(0.5 * theta),
        std::sin(0.5 * theta), std::cos(0.5 * theta);
    return result;
  };

  static const auto rz_mat = [](const std::vector<double> &in_params) {
    assert(in_params.size() == 1);
    auto theta = in_params[0];
    Eigen::Matrix2cd result = Eigen::MatrixXcd::Zero(2, 2);
    result << std::exp(std::complex<double>(0, -0.5 * theta)), 0.0, 0.0,
        std::exp(std::complex<double>(0, 0.5 * theta));
    return result;
  };

  static const auto p_mat = [](const std::vector<double> &in_params) {
    assert(in_params.size() == 1);
    auto theta = in_params[0];
    Eigen::Matrix2cd result = Eigen::MatrixXcd::Zero(2, 2);
    result << 1.0, 0.0, 0.0, std::exp(std::complex<double>(0, theta));
    return result;
  };

  static const std::unordered_map<std::string, Eigen::Matrix2cd>
      GateMatrixCache = {{"x", X_mat},           {"y", Y_mat},
                         {"z", Z_mat},           {"h", H_mat},
                         {"t", p_mat({M_PI_4})}, {"tdg", p_mat({-M_PI_4})},
                         {"s", p_mat({M_PI_2})}, {"sdg", p_mat({-M_PI_2})}};
  const auto &gateName = in_op.first;
  const auto &gateParams = in_op.second;
  const auto it = GateMatrixCache.find(gateName);
  if (it != GateMatrixCache.end()) {
    return it->second;
  }
  if (gateName == "rx") {
    return rx_mat(gateParams);
  }
  if (gateName == "ry") {
    return ry_mat(gateParams);
  }
  if (gateName == "rz") {
    return rz_mat(gateParams);
  }
  throw std::runtime_error("Unknown single qubit gate: " + gateName);
  return Eigen::MatrixXcd::Zero(2, 2);
}

// If the matrix is finite: no NaN elements
template <typename Derived>
inline bool isFinite(const Eigen::MatrixBase<Derived> &x) {
  return ((x - x).array() == (x - x).array()).all();
}

// Default tolerace for validation
constexpr double TOLERANCE = 1e-6;

template <typename Derived>
bool allClose(const Eigen::MatrixBase<Derived> &in_mat1,
              const Eigen::MatrixBase<Derived> &in_mat2,
              double in_tol = TOLERANCE) {
  if (!isFinite(in_mat1) || !isFinite(in_mat2)) {
    return false;
  }

  if (in_mat1.rows() == in_mat2.rows() && in_mat1.cols() == in_mat2.cols()) {
    for (int i = 0; i < in_mat1.rows(); ++i) {
      for (int j = 0; j < in_mat1.cols(); ++j) {
        if (std::abs(in_mat1(i, j) - in_mat2(i, j)) > in_tol) {
          return false;
        }
      }
    }

    return true;
  }
  return false;
}

// Use Z-Y decomposition of Nielsen and Chuang (Theorem 4.1).
// An arbitrary one qubit gate matrix can be writen as
// U = [ exp(j*(a-b/2-d/2))*cos(c/2), -exp(j*(a-b/2+d/2))*sin(c/2)
//       exp(j*(a+b/2-d/2))*sin(c/2), exp(j*(a+b/2+d/2))*cos(c/2)]
// where a,b,c,d are real numbers.
// Then U = exp(j*a) Rz(b) Ry(c) Rz(d).
std::tuple<double, double, double, double>
singleQubitGateDecompose(const Eigen::Matrix2cd &matrix) {
  static const Eigen::Matrix2cd ID_MAT = Eigen::Matrix2cd::Identity();
  if (allClose(matrix, ID_MAT)) {
    return std::make_tuple(0.0, 0.0, 0.0, 0.0);
  }
  const auto checkParams = [&matrix](double a, double bHalf, double cHalf,
                                     double dHalf) {
    Eigen::Matrix2cd U;
    U << std::exp(1i * (a - bHalf - dHalf)) * std::cos(cHalf),
        -std::exp(1i * (a - bHalf + dHalf)) * std::sin(cHalf),
        std::exp(1i * (a + bHalf - dHalf)) * std::sin(cHalf),
        std::exp(1i * (a + bHalf + dHalf)) * std::cos(cHalf);

    return allClose(U, matrix);
  };

  double a, bHalf, cHalf, dHalf;
  const double TOLERANCE = 1e-9;
  if (std::abs(matrix(0, 1)) < TOLERANCE) {
    auto two_a = fmod(std::arg(matrix(0, 0) * matrix(1, 1)), 2 * M_PI);
    a = (std::abs(two_a) < TOLERANCE || std::abs(two_a) > 2 * M_PI - TOLERANCE)
            ? 0
            : two_a / 2.0;
    auto dHalf = 0.0;
    auto b = std::arg(matrix(1, 1)) - std::arg(matrix(0, 0));
    std::vector<double> possibleBhalf{fmod(b / 2.0, 2 * M_PI),
                                      fmod(b / 2.0 + M_PI, 2.0 * M_PI)};
    std::vector<double> possibleChalf{0.0, M_PI};
    bool found = false;
    for (size_t i = 0; i < possibleBhalf.size(); ++i) {
      for (size_t j = 0; j < possibleChalf.size(); ++j) {
        bHalf = possibleBhalf[i];
        cHalf = possibleChalf[j];
        if (checkParams(a, bHalf, cHalf, dHalf)) {
          found = true;
          break;
        }
      }
      if (found) {
        break;
      }
    }
    assert(found);
  } else if (std::abs(matrix(0, 0)) < TOLERANCE) {
    auto two_a = fmod(std::arg(-matrix(0, 1) * matrix(1, 0)), 2 * M_PI);
    a = (std::abs(two_a) < TOLERANCE || std::abs(two_a) > 2 * M_PI - TOLERANCE)
            ? 0
            : two_a / 2.0;
    dHalf = 0;
    auto b = std::arg(matrix(1, 0)) - std::arg(matrix(0, 1)) + M_PI;
    std::vector<double> possibleBhalf{fmod(b / 2., 2 * M_PI),
                                      fmod(b / 2. + M_PI, 2 * M_PI)};
    std::vector<double> possibleChalf{M_PI / 2., 3. / 2. * M_PI};
    bool found = false;
    for (size_t i = 0; i < possibleBhalf.size(); ++i) {
      for (size_t j = 0; j < possibleChalf.size(); ++j) {
        bHalf = possibleBhalf[i];
        cHalf = possibleChalf[j];
        if (checkParams(a, bHalf, cHalf, dHalf)) {
          found = true;
          break;
        }
      }
      if (found) {
        break;
      }
    }
    assert(found);
  } else {
    auto two_a = fmod(std::arg(matrix(0, 0) * matrix(1, 1)), 2 * M_PI);
    a = (std::abs(two_a) < TOLERANCE || std::abs(two_a) > 2 * M_PI - TOLERANCE)
            ? 0
            : two_a / 2.0;
    auto two_d = 2. * std::arg(matrix(0, 1)) - 2. * std::arg(matrix(0, 0));
    std::vector<double> possibleDhalf{
        fmod(two_d / 4., 2 * M_PI), fmod(two_d / 4. + M_PI / 2., 2 * M_PI),
        fmod(two_d / 4. + M_PI, 2 * M_PI),
        fmod(two_d / 4. + 3. / 2. * M_PI, 2 * M_PI)};
    auto two_b = 2. * std::arg(matrix(1, 0)) - 2. * std::arg(matrix(0, 0));
    std::vector<double> possibleBhalf{
        fmod(two_b / 4., 2 * M_PI), fmod(two_b / 4. + M_PI / 2., 2 * M_PI),
        fmod(two_b / 4. + M_PI, 2 * M_PI),
        fmod(two_b / 4. + 3. / 2. * M_PI, 2 * M_PI)};
    auto tmp = std::acos(std::abs(matrix(1, 1)));
    std::vector<double> possibleChalf{
        fmod(tmp, 2 * M_PI), fmod(tmp + M_PI, 2 * M_PI),
        fmod(-1. * tmp, 2 * M_PI), fmod(-1. * tmp + M_PI, 2 * M_PI)};
    bool found = false;
    for (size_t i = 0; i < possibleBhalf.size(); ++i) {
      for (size_t j = 0; j < possibleChalf.size(); ++j) {
        for (size_t k = 0; k < possibleDhalf.size(); ++k) {
          bHalf = possibleBhalf[i];
          cHalf = possibleChalf[j];
          dHalf = possibleDhalf[k];
          if (checkParams(a, bHalf, cHalf, dHalf)) {
            found = true;
            break;
          }
        }
        if (found) {
          break;
        }
      }
      if (found) {
        break;
      }
    }
    assert(found);
  }

  // Final check:
  assert(checkParams(a, bHalf, cHalf, dHalf));
  return std::make_tuple(a, bHalf, cHalf, dHalf);
};

std::vector<pauli_decomp_t>
simplifySingleQubitSeq(double zAngleBefore, double yAngle, double zAngleAfter) {
  auto zExpBefore = zAngleBefore / M_PI - 0.5;
  auto middleExp = yAngle / M_PI;
  std::string middlePauli = "rx";
  auto zExpAfter = zAngleAfter / M_PI + 0.5;

  // Helper functions:
  const auto isNearZeroMod = [](double a, double period) -> bool {
    const auto halfPeriod = period / 2;
    const double TOL = 1e-8;
    return std::abs(fmod(a + halfPeriod, period) - halfPeriod) < TOL;
  };

  const auto toQuarterTurns = [](double in_exp) -> int {
    return static_cast<int>(round(2 * in_exp)) % 4;
  };

  const auto isCliffordRotation = [&](double in_exp) -> bool {
    return isNearZeroMod(in_exp, 0.5);
  };

  const auto isQuarterTurn = [&](double in_exp) -> bool {
    return (isCliffordRotation(in_exp) && toQuarterTurns(in_exp) % 2 == 1);
  };

  const auto isHalfTurn = [&](double in_exp) -> bool {
    return (isCliffordRotation(in_exp) && toQuarterTurns(in_exp) == 2);
  };

  const auto isNoTurn = [&](double in_exp) -> bool {
    return (isCliffordRotation(in_exp) && toQuarterTurns(in_exp) == 0);
  };

  // Clean up angles
  if (isCliffordRotation(zExpBefore)) {
    if ((isQuarterTurn(zExpBefore) || isQuarterTurn(zExpAfter)) !=
        (isHalfTurn(middleExp) && isNoTurn(zExpBefore - zExpAfter))) {
      zExpBefore += 0.5;
      zExpAfter -= 0.5;
      middlePauli = "ry";
    }
    if (isHalfTurn(zExpBefore) || isHalfTurn(zExpAfter)) {
      zExpBefore -= 1;
      zExpAfter += 1;
      middleExp = -middleExp;
    }
  }
  if (isNoTurn(middleExp)) {
    zExpBefore += zExpAfter;
    zExpAfter = 0;
  } else if (isHalfTurn(middleExp)) {
    zExpAfter -= zExpBefore;
    zExpBefore = 0;
  }

  std::vector<pauli_decomp_t> composite;
  if (!isNoTurn(zExpBefore)) {
    composite.emplace_back(std::make_pair("rz", zExpBefore * M_PI));
  }
  if (!isNoTurn(middleExp)) {
    composite.emplace_back(std::make_pair(middlePauli, middleExp * M_PI));
  }
  if (!isNoTurn(zExpAfter)) {
    composite.emplace_back(std::make_pair("rz", zExpAfter * M_PI));
  }
  return composite;
}

} // namespace
namespace qcor {
namespace utils {
std::vector<pauli_decomp_t>
decompose_gate_sequence(const std::vector<qop_t> &op_list) {
  Eigen::Matrix2cd totalU = Eigen::MatrixXcd::Identity(2, 2);
  for (const auto &op : op_list) {
    // std::cout << "Gate: " << op.first << ": " << op.second.size() << "\n";
    totalU = getGateMat(op) * totalU;
  }

  // std::cout << "Total U = " << totalU << "\n";
  auto [a, bHalf, cHalf, dHalf] = singleQubitGateDecompose(totalU);

  // Validate U = exp(j*a) Rz(b) Ry(c) Rz(d).
  const auto validate = [](const Eigen::Matrix2cd &in_mat, double a, double b,
                           double c, double d) {
    Eigen::Matrix2cd Rz_b, Ry_c, Rz_d;
    Rz_b << std::exp(-1i * b / 2.0), 0, 0, std::exp(1i * b / 2.0);
    Rz_d << std::exp(-1i * d / 2.0), 0, 0, std::exp(1i * d / 2.0);
    Ry_c << std::cos(c / 2), -std::sin(c / 2), std::sin(c / 2), std::cos(c / 2);
    Eigen::Matrix2cd mat = std::exp(1i * a) * Rz_b * Ry_c * Rz_d;
    return allClose(in_mat, mat);
  };
  // Validate the *raw* decomposition
  assert(validate(totalU, a, 2 * bHalf, 2 * cHalf, 2 * dHalf));
  return simplifySingleQubitSeq(2 * dHalf, 2 * cHalf, 2 * bHalf);
}
} // namespace utils
} // namespace qcor


#include <iostream>
#include <cmath>
#include <vector>
#include <chrono>
#include <memory>
#include <thread>
#include <random>
#include <limits>
#include <boost/range/irange.hpp>
#include <Eigen/Dense>
#include "GPs.h"

// Retrieve aliases from GP namescope
using Matrix = GP::Matrix;
using Vector = GP::Vector;

// Compute pairwise distance between lists of points
void GP::pdist(Matrix & Dv, Matrix & X1, Matrix & X2)
{
  auto n = static_cast<int>(X1.rows());
  auto entryCount = static_cast<int>( (n*(n-1))/2);
  Dv.resize(entryCount, 1);

  // Get thread count
  int threadCount = Eigen::nbThreads( );

  // Get problem dimension d per thread
  auto d = static_cast<int>(n/threadCount);

  std::vector<int> startVals;
  for ( auto i : boost::irange(0,threadCount) )
    startVals.emplace_back(i*d);

  std::vector<int> endVals;
  for ( auto i : boost::irange(1,threadCount) )
    endVals.emplace_back(i*d);
  endVals.emplace_back(n-1);

  // Define lambda function specifying each threads solver task
  auto lambda = [&Dv,&X1,&X2,n](int startInd, int endInd) {
                  for ( auto i : boost::irange(startInd, endInd) )
                    {
                      for ( auto j : boost::irange(i+1,n) )
                        Dv(static_cast<int>(i*n-(i*(i+1))/2+j-i-1), 0) = (static_cast<Vector>(X1.row(i)-X2.row(j))).squaredNorm();
                    }
                };

  // Initialize thread list
  std::vector<std::thread> threadList;

  // Assign tasks to threads
  for ( auto i : boost::irange(0,threadCount) )
    threadList.emplace_back(lambda,startVals[i],endVals[i]);

  // Join threads
  for ( auto & thread : threadList )
    thread.join();

}

// Re-assemble pairwise distances into a dense matrix
void GP::squareForm(Matrix & D, Matrix & Dv, int n, double diagVal)
{
  D.resize(n,n);

  // Get thread count
  int threadCount = Eigen::nbThreads( );

  // Get problem dimension d per thread
  auto d = static_cast<int>(n/threadCount);

  std::vector<int> startVals;
  for ( auto i : boost::irange(0,threadCount) )
    startVals.emplace_back(i*d);

  std::vector<int> endVals;
  for ( auto i : boost::irange(1,threadCount) )
    endVals.emplace_back(i*d);
  endVals.emplace_back(n-1);

  // Define lambda function specifying each threads solver task
  auto lambda = [&D,&Dv,n](int startInd, int endInd) {
                  for ( auto i : boost::irange(startInd,endInd) )
                    {
                      for ( auto j : boost::irange(i+1, n) )
                       D(i,j) = D(j,i) = Dv(static_cast<int>(i*n-(i*(i+1))/2+j-i-1), 0);
                    }
                };

  // Initialize thread list
  std::vector<std::thread> threadList;

  // Assign tasks to threads
  for ( auto i : boost::irange(0,threadCount) )
    threadList.emplace_back(lambda,startVals[i],endVals[i]);

  // Join threads
  for ( auto & thread : threadList )
    thread.join();

  // Add diagonal values to distance matrix
  D.diagonal() = diagVal * Eigen::MatrixXd::Ones(n,1);
}

// Parse kernel parameter vector, separating noise from the kernel hyperparameters
void GP::Kernel::parseParams(const Vector & params, Vector & kernelParams, std::vector<double> & nonKernelParams)
{
  if ( !fixedNoise )
    {
      // Noise = params(0)
      nonKernelParams.emplace_back(params(0));
      if ( !fixedScaling )
        nonKernelParams.emplace_back(params(1));
      else
        nonKernelParams.emplace_back(scalingLevel);
    }
  else
    {
      // Scaling = params(0)
      nonKernelParams.emplace_back(noiseLevel);
      if ( !fixedScaling )
        nonKernelParams.emplace_back(params(0));
      else
        nonKernelParams.emplace_back(scalingLevel);
    }

  // Trim kernel parameter vector
  kernelParams = params.tail(paramCount);

}

// Compute covariance matrix (and gradients) from a vector of squared pairwise distances Dv
void GP::RBF::computeCov(Matrix & K, Matrix & obsX, Vector & params, std::vector<Matrix> & gradList, double jitter, bool evalGrad)
{
  auto n = static_cast<int>(K.rows());

  // Separate noise and scaling parameters from kernel hyperparameters
  Vector kernelParams;
  std::vector<double> noiseAndScaling;
  parseParams(params, kernelParams, noiseAndScaling);
  double noise = noiseAndScaling[0];
  double scaling = noiseAndScaling[1];

  // Compute distance matrix for each call
  Matrix Dv;
  pdist(Dv, obsX, obsX);

  // Evaluate covariance kernel on pairwise distance vector
  Matrix Kv;
  Kv.noalias() = scaling * ( (-0.5 / std::pow(kernelParams(0),2)) * Dv ).array().exp().matrix();

  // Make sure not to scale the jitter and noise terms
  squareForm(K, Kv, n, scaling*1.0 + jitter + noise);

  // Compute gradients w.r.t. kernel hyperparameters
  if ( evalGrad )
    {
      // Prepend gradient list with scaling term
      //int index = 0;
      //if (!fixedScaling)
      //  {
      //    Matrix dK_scaling;
      //    squareForm(dK_scaling, Kv, n, -noise);
      //    gradList[index++] = dK_scaling;
      //  }

      Matrix dK_i;
      Matrix dK_iv;
      dK_iv.noalias() = 1/std::pow(kernelParams(0),2) * ( Dv.array() * Kv.array() ).matrix();
      squareForm(dK_i, dK_iv, n); // Note: diagVal = 0.0
      gradList[0] = dK_i.eval();
      //gradList[index] = dK_i.eval();
    }

};

// Define distance kernel function for RBF
// [ Note: Optimize w.r.t. theta = log(l) for stability  ==>   / l^2  instead of  / l^3 ]
double GP::RBF::evalDistKernel(double d, Vector & params, int n)
{
  switch (n)
    {
    case 0: return std::exp( -d / (2.0*std::pow(params(0),2)));
    case 1: return d / std::pow(params(0),2) * std::exp( -d / (2.0*std::pow(params(0),2)));
    default: std::cout << "\n[*] UNDEFINED DERIVATIVE\n"; return 0.0;
    }
};

// Compute cross covariance between two input vectors using kernel parameters params
void GP::RBF::computeCrossCov(Matrix & K, Matrix & X1, Matrix & X2, Vector & params)
{
  // Get prediction count
  auto m = static_cast<int>(X2.rows());

   // Define lambda function to create unary operator (by clamping kernelParams argument)
  auto lambda = [=,&params](double d)->double { return evalDistKernel(d, params, 0); };
  for ( auto j : boost::irange(0,m) )
    {
      K.col(j) = ((X1.rowwise() - X2.row(j)).rowwise().squaredNorm()).unaryExpr(lambda);
    }

};

// Evaluate NLML for specified kernel hyperparameters p
double GP::GaussianProcess::evalNLML(const Vector & p, Vector & g, bool evalGrad)
{
  time EVAL_start = high_resolution_clock::now();

  // Get matrix input observation count
  auto n = static_cast<int>(obsX.rows());

  // ASSUME OPTIMIZATION OVER LOG VALUES
  auto params = static_cast<Vector>(p);
  params = params.array().exp().matrix();

  // Compute covariance matrix and store Cholesky factor
  Matrix K(n,n);
  time start = high_resolution_clock::now();
  (*kernel).computeCov(K, obsX, params, gradList, jitter, evalGrad);
  time end = high_resolution_clock::now();
  time_computecov += getTime(start, end);

  start = high_resolution_clock::now();
  Eigen::LLT<Matrix> _cholesky(n);
  _cholesky = K.llt();
  end = high_resolution_clock::now();
  time_cholesky_llt += getTime(start, end);

  start = high_resolution_clock::now();
  Matrix _alpha = _cholesky.solve(obsY);
  end = high_resolution_clock::now();
  time_alpha += getTime(start, end);

  // Compute NLML value
  start = high_resolution_clock::now();
  double NLML_value = (obsY.transpose()*_alpha)(0);
  NLML_value += n*std::log(2*PI);
  NLML_value *= 0.5;
  NLML_value += _cholesky.matrixLLT().diagonal().array().log().sum();
  end = high_resolution_clock::now();
  time_NLML += getTime(start, end);

  if ( evalGrad )
    {

      //
      // Precompute the multiplicative term in derivative expressions
      //
      // [ THIS APPEARS TO BE A COMPUTATIONAL BOTTLE-NECK ]
      //

      start = high_resolution_clock::now();

      // Direct Implementation
      //Matrix term(n,n);
      //term.noalias() = _cholesky.solve(Matrix::Identity(n,n));
      //term.noalias() -= _alpha*_alpha.transpose();

      // Using Solve In Place
      //Matrix term = Matrix::Identity(n,n);
      //_cholesky.solveInPlace(term);
      //term.noalias() -= _alpha*_alpha.transpose();

      //
      //  MULTI-THREADED IMPLEMENTATION
      //
      Matrix term = Matrix::Identity(n,n);

      // Get thread count
      int threadCount = Eigen::nbThreads( );

      // Get problem dimension d per thread
      auto d = static_cast<int>(n/threadCount);

      // Construct partitioned list of block terms for solver
      std::vector<Matrix> termList;
      for ( auto i : boost::irange(0,threadCount-1) )
        termList.emplace_back(term.block(0,i*d,n,d));

      // Ensure the final block extends to column n (i.e. account for index roundoff)
      termList.emplace_back(term.block(0, (threadCount-1)*d, n, n - (threadCount-1)*d) );

      // Define lambda function specifying each threads solver task
      auto lambda = [&termList,&_cholesky](int i) { _cholesky.solveInPlace(termList[i]); };

      // Initialize thread list
      std::vector<std::thread> threadList;

      // Assign tasks to threads
      for ( auto i : boost::irange(0,threadCount) )
        threadList.emplace_back(lambda,i);

      // Join threads
      for ( auto & thread : threadList )
        thread.join();

      // Reassemble blocks from solver threads back into original matrix
      for ( auto i : boost::irange(0,threadCount-1) )
        term.block(0,i*d,n,d) = termList[i];
      term.block(0, (threadCount-1)*d, n, n - (threadCount-1)*d) = termList[threadCount-1];

      // Compute final multiplicative term:  K^-1 - alpha*alpha^T
      term.noalias() -= _alpha*_alpha.transpose();

      end = high_resolution_clock::now();
      time_term += getTime(start, end);

      start = high_resolution_clock::now();
      // Compute gradient for noise term if 'fixedNoise=false'
      int index = 0;
      double noise;
      if (!fixedNoise)
        {
          // Specify gradient of white noise kernel  [ dK_i = params(0)*Matrix::Identity(n,n) ]
          //g(index++) = 0.5 * (term * params(0)).trace() ;
          noise = params(0);
          g(index) = term.trace();
          g(index++) *= 0.5 * noise;
          //g(index++) *= 0.5 * params(0);
        }
      else
        noise = noiseLevel;

      if (!fixedScaling)
        {
          if (!fixedNoise)
            {
              double trace = 0.0;
              for ( auto j : boost::irange(0, n) )
                trace -= _alpha(j)*obsY(j);
              trace += n;
              trace *= 0.5;
              // Add  1/2 * trace[(noise * term)]  (i.e. the noise gradient)
              trace -= g(index-1);
              g(index++) = trace;
            }
          else
            {
              double trace = 0.0;
              Matrix dK = K - noise*Matrix::Identity(n,n);
              for ( auto j : boost::irange(0, n) )
                trace += term.row(j)*dK.col(j);
              g(index++) = 0.5*trace;
            }
          //
          //  NOTE: The following implementation does not account for noise term (!)
          //
          //  Ah, it's so close though...
          //
          //  The adjusted covariance matrix is:  K'  =  s * K  +  noise * I
          //
          //  But if we had   K' = s*K   and set   t = log(s) ~ s = exp(t) :
          //
          //  ( so that  dK'/dt  =  d/dt[ s*K ]  =  d/ds[ s*K ] * ds/dt  =  s * K  =  K'  )
          //
          //  then it would give us the reduction:
          //
          //  d/dt -log p(y|X,t)  =  -1/2 * trace[ ( (K'^-1 y)(K'^-1 y)^T - K'^-1 ) dK'/dt  ]
          //
          //   =  -1/2 * trace[ ( K'^-1 y y^T K'^-1 - K'^-1 ) dK'/dt  ]
          //
          //   =  -1/2 * trace[ ( K'^-1 y y^T K'^1 - K'^-1 ) K'  ]
          //
          //   =  -1/2 * trace[  (K'^-1 y) y^T - I  ]
          //
          //   =  -1/2 * trace[  alpha * y^T - I  ]
          //
          //
          //  but...    we do have:
          //
          //   dK'/dt  =  d/dt[s*K]  =  s * K  =  K' - noise * I
          //
          //  so that the (corected) calculation above still yields:
          //
          //   =  -1/2 * trace[  (alpha * y^T - I)  -  noise * term  ]
          //
          //  and the trace of "term" has already been calculated...
          //
          //
          //double trace = 0.0;
          //for ( auto i : boost::irange(0,n) )
          //  trace -= _alpha(i)*obsY(i);
          //trace += n;
          //g(index++) = 0.5 * trace;
        }

      // Specify gradient w.r.t. the kernel scaling parameter
      //if (!fixedScaling)
      //  gradList.insert(gradList.begin(), K - noise*Matrix::Identity(n,n));

      // Compute gradients with respect to kernel hyperparameters
      for (auto dK_i = gradList.begin(); dK_i != gradList.end(); ++dK_i)
        {
          // Compute trace of full matrix
          //g(index++) = 0.5 * (term * (*dK_i) ).trace() ;

          // POSSIBLE MULTI-THREADED IMPLEMENTATION
          // Construct zero initialized vector of partial trace values
          Matrix traceVals = Eigen::MatrixXd::Zero(threadCount,1);

          std::vector<int> startVals;
          for ( auto i : boost::irange(0,threadCount) )
            startVals.emplace_back(i*d);

          std::vector<int> endVals;
          for ( auto i : boost::irange(1,threadCount) )
            endVals.emplace_back(i*d);
          endVals.emplace_back(n);

          // Define lambda function specifying each threads solver task
          auto lambda = [&term,&dK_i,&traceVals](int i, int startInd, int endInd) {
                          for ( auto j : boost::irange(startInd, endInd) )
                            traceVals(i) += term.row(j)*(*dK_i).col(j);
                        };

          // Initialize thread list
          std::vector<std::thread> traceThreadList;

          // Assign tasks to threads
          for ( auto i : boost::irange(0,threadCount) )
            traceThreadList.emplace_back(lambda,i,startVals[i],endVals[i]);

          // Join threads
          for ( auto & thread : traceThreadList )
            thread.join();

          // Compute final trace value for derivative calculation
          g(index++) = 0.5*(traceVals.sum());

        }
      end = high_resolution_clock::now();
      time_grad += getTime(start, end);

      // Update gradient evaluation count
      gradientEvals += 1;

    }

  time EVAL_end = high_resolution_clock::now();
  time_evaluation += getTime(EVAL_start, EVAL_end);
  return NLML_value;

}

// Define simplified interface for evaluating NLML without gradient calculation
double GP::GaussianProcess::evalNLML(const Vector & p)
{
  Vector nullGrad(0);
  return evalNLML(p,nullGrad,false);
}

int GP::GaussianProcess::getAugParamCount(int count)
{
  if (!fixedNoise)
    {
      if (!fixedScaling)
        return count + 2;
      else
        return count + 1;
    }
  else
    {
      if (!fixedScaling)
        return count + 1;
      else
        return count;
    }
}

// Fit model hyperparameters
void GP::GaussianProcess::fitModel()
{

  // Get combined parameter/noise vector size
  paramCount = (*kernel).getParamCount();
  //augParamCount = (fixedNoise) ? static_cast<int>(paramCount) : static_cast<int>(paramCount) + 1 ;
  augParamCount = getAugParamCount(paramCount);

  // Pass noise level to kernel when 'fixedNoise=true'
  if ( fixedNoise )
    (*kernel).setNoise(noiseLevel);
  if ( fixedScaling )
    (*kernel).setScaling(scalingLevel);

  // Declare vector for storing gradient calculations
  Vector g(augParamCount);

  // Initialize gradient list with identity matrices
  //for ( auto i : boost::irange(0,augParamCount) )
  //int gradientCount = paramCount;
  //if (!fixedScaling)
  //  gradientCount += 1;
  //for ( auto i : boost::irange(0,gradientCount) )

  for ( auto i : boost::irange(0,paramCount) )
    {
      // Avoid compiler warning for unused variable
      (void)i;
      gradList.push_back(Matrix::Identity(static_cast<int>(obsY.size()),static_cast<int>(obsY.size())));
    }

  // Convert hyperparameter bounds to log-scale
  Vector lbs, ubs;
  parseBounds(lbs, ubs, augParamCount);

  // Initialize optimal hyperparamter values
  Vector optParams = Eigen::MatrixXd::Zero(augParamCount,1);

  // Define restart count for optimizer
  int restartCount = solverRestarts;

  // Declare variables to store optimization loop results
  double currentVal;
  double optVal = 1e9;
  Vector theta(augParamCount);

  // Define low-precision solver for restart loop
  LBFGSpp::LBFGSParam<double> param;
  param.linesearch  = LBFGSpp::LBFGS_LINESEARCH_BACKTRACKING_STRONG_WOLFE;
  param.m = 10;
  param.epsilon = 1e-6;
  param.max_iterations = 10;
  param.max_linesearch = 5;
  param.delta = 1e-4;

  int niter;

  // Evaluate optimizer with various different initializations
  for ( auto i : boost::irange(0,restartCount) )
    {
      if ( i == 0 )
        {
          // Set initial guess (should make this user specifiable...)
          theta = Eigen::MatrixXd::Zero(augParamCount,1);
        }
      else
        {
          // Sample initial hyperparameter vector
          theta = sampleUnifVector(lbs, ubs);
        }

      // Create solver and function object
      LBFGSpp::LBFGSSolver<double> solver(param);
      niter = solver.minimize(*this, theta, currentVal);

      // Compute current NLML and store parameters if optimal
      if ( currentVal < optVal ) { optVal = currentVal; optParams = theta; }

    }

  // Perform one last optimization starting from best parameters so far
  //if ( restartCount == 0 )
  //  optParams = sampleUnifVector(lbs, ubs);

  LBFGSpp::LBFGSParam<double> finalparam;

  // Line Search Options
  //param.linesearch  = LBFGSpp::LBFGS_LINESEARCH_BACKTRACKING_ARMIJO;
  //param.linesearch  = LBFGSpp::LBFGS_LINESEARCH_BACKTRACKING_WOLFE;
  //param.linesearch  = LBFGSpp::LBFGS_LINESEARCH_BACKTRACKING_STRONG_WOLFE;

  // TRY MODELLING SCIPY fmin_l_bfgs_b PARAMETERS
  finalparam.m = 10;
  finalparam.epsilon = 1e-5;
  finalparam.max_linesearch = 20;
  double eps = 2.220446049250313e-16;
  double factr = solverPrecision;
  finalparam.past = 1;
  //finalparam.ftol = factr*eps;
  finalparam.delta = factr*eps;
  finalparam.max_iterations = 100;

  // Create solver and function object
  LBFGSpp::LBFGSSolver<double> finalsolver(finalparam);
  niter = finalsolver.minimize(*this, optParams, optVal);

  if ( VERBOSE )
    {
      std::cout << "\n[*] Solver Iterations = " << niter <<std::endl;
      std::cout << "\n[*] Function Evaluations = " << gradientEvals <<std::endl;
    }

  // ASSUME OPTIMIZATION OVER LOG VALUES
  optParams = optParams.array().exp().matrix();

  ///* [ This is included in the SciKit Learn model.fit() call as well ]

  // Recompute covariance and Cholesky factor
  auto n = static_cast<int>(obsX.rows());
  Matrix K(n,n);
  (*kernel).computeCov(K, obsX, optParams, gradList, jitter, false);
  cholesky = K.llt();
  alpha.noalias() = cholesky.solve(obsY);

  // Assign tuned parameters to model
  if (!fixedNoise)
    {
      noiseLevel = optParams[0];
      if (!fixedScaling)
          scalingLevel = optParams[1];
    }
  else
    {
      if (!fixedScaling)
        scalingLevel = optParams[0];
    }

  optParams = static_cast<Vector>(optParams.tail(paramCount));
  (*kernel).setParams(optParams);

  // DISPLAY TIMING INFORMATION
  if ( VERBOSE )
    {
      std::cout << "\n Time Diagnostics |\n";
      std::cout << "------------------\n";
      std::cout << "computeCov():\t  " << time_computecov/gradientEvals  << std::endl;
      std::cout << "cholesky.llt():\t  " << time_cholesky_llt/gradientEvals  << std::endl;
      std::cout << "_alpha term:\t  " << time_alpha/gradientEvals  << std::endl;
      std::cout << "NLML:\t  \t  " << time_NLML/gradientEvals  << std::endl;
      std::cout << "Grad term:\t  " << time_term/gradientEvals  << std::endl;
      std::cout << "Gradient:\t  " << time_grad/gradientEvals  << std::endl;
      std::cout << "\nEvaluation:\t  " << time_evaluation/gradientEvals  << std::endl;
    }

};

// Compute predicted values
void GP::GaussianProcess::predict()
{
  // Get matrix input observation count
  auto n = static_cast<int>(obsX.rows());
  auto m = static_cast<int>(predX.rows());

  // Get optimized kernel hyperparameters
  Vector params = (*kernel).getParams();

  // Compute cross covariance for test points
  Matrix kstar_and_v;
  kstar_and_v.resize(n,m);
  (*kernel).computeCrossCov(kstar_and_v, obsX, predX, params);
  kstar_and_v *= scalingLevel;

  // Compute covariance matrix for test points
  Matrix kstarmat;
  kstarmat.resize(m,m);
  (*kernel).computeCrossCov(kstarmat, predX, predX, params);
  kstarmat *= scalingLevel;

  // Set predictive means/variances and compute negative log marginal likelihood
  Matrix cholMat(cholesky.matrixL());
  //predMean.noalias() = kstar_and_v.transpose() * _alpha;
  predMean.noalias() = kstar_and_v.transpose() * alpha;
  cholMat.triangularView<Eigen::Lower>().solveInPlace(kstar_and_v);  // kstar_and_v is now 'v'
  predCov.noalias() = kstarmat - kstar_and_v.transpose() * kstar_and_v;

}

// Draw sample paths from posterior distribution
Matrix GP::GaussianProcess::getSamples(int count)
{
  // Get number of target points
  auto n = static_cast<int>(predX.rows());

  // Construct simple random generator engine from a time-based seed
  unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
  std::default_random_engine generator (seed);
  std::normal_distribution<double> normal (0.0,1.0);

  // Assign i.i.d. random normal values to uVals
  Matrix uVals(n,count);
  for (auto i : boost::irange(0,n))
    {
      for (auto j : boost::irange(0,count))
          uVals(i,j) = normal(generator);
    }

  // Compute Cholesky factor L
  Matrix L = ( predCov + (noiseLevel+jitter)*Matrix::Identity(static_cast<int>(predCov.cols()), static_cast<int>(predCov.cols())) ).llt().matrixL();

  // Draw samples using the formula:  y = m + L*u
  Matrix samples = predMean.replicate(1,count) + L*uVals;

  return samples;
}

// Evaluate NLML [public interface]
double GP::GaussianProcess::computeNLML(const Vector & p)
{
  // Compute log-hyperparameters
  Vector logparams(augParamCount);

  int index = 0;
  logparams(index++) = std::log(noiseLevel);
  logparams(index++) = std::log(scalingLevel);
  for ( auto i : boost::irange(index,augParamCount) )
    logparams(i) = std::log(p(i-index));

  // Evaluate NLML using log-hyperparameters
  return evalNLML(logparams);
}

// Evaluate NLML with default noise level [public interface]
double GP::GaussianProcess::computeNLML()
{
  auto params = (*kernel).getParams();
  return computeNLML(params);
}

// Define function for uniform sampling
Matrix GP::sampleUnif(double a, double b, int N, int dim)
{
  //return (b-a)*(Eigen::MatrixXd::Random(N,1) * 0.5 + 0.5*Eigen::MatrixXd::Ones(N,1)) + a*Eigen::MatrixXd::Ones(N,1);
  return (b-a)*(Eigen::MatrixXd::Random(N,dim) * 0.5 + 0.5*Eigen::MatrixXd::Ones(N,dim) ) + a*Eigen::MatrixXd::Ones(N,dim);
}

// Define function for uniform sampling [Vectors]
Vector GP::sampleUnifVector(Vector lbs, Vector ubs)
{
  auto n = static_cast<int>(lbs.rows());

  Vector sampleVector = ((ubs-lbs).array()*( 0.5*Eigen::MatrixXd::Random(n,1) + 0.5*Eigen::MatrixXd::Ones(n,1) ).array()).matrix() + (lbs.array()*Eigen::MatrixXd::Ones(n,1).array()).matrix();

  return sampleVector;
}

// Define function for sampling from standard normal distribution
Matrix GP::sampleNormal(int N)
{
  // Note: Boost random is currently throwing deprecated header warnings...
  //boost::random::mt19937 rng;
  //boost::random::normal_distribution<> normalDist;

  /*
  // [ NOTE: .noalias() is 100% necessary with "std::default_random_engine" ]
  std::default_random_engine rng;
  std::normal_distribution<double> normalDist(0.0,1.0);
  Matrix sampleVals(N,1);
  for ( auto i : boost::irange(0,N) )
    sampleVals(i) = normalDist(rng);
  */

  //
  //          Crude implementation of Box-Muller Transform
  // ( see https://en.wikipedia.org/wiki/Box%E2%80%93Muller_transform )
  //
  static const double epsilon = std::numeric_limits<double>::min();
  Matrix U1(N,1);
  Matrix U2(N,1);

  // Ensure log operand is not too small
  double scaledLimit = 2.0*epsilon - 1.0;
  do
    {
      U1 = Eigen::MatrixXd::Random(N,1);
      U2 = Eigen::MatrixXd::Random(N,1);
    }
  while ( U1.minCoeff() <= scaledLimit );

  // Rescale and shift Unif(-1,1) values to the interval [0,1]
  U1 =  0.5*(U1 + Eigen::MatrixXd::Ones(N,1));
  U2 =  0.5*(U2 + Eigen::MatrixXd::Ones(N,1));

  // Compute transformed values with .noalias() to ensure evaluation
  Matrix sampleVals;
  sampleVals.noalias() = ((-2.0*(U1.array().log()).matrix()).array().sqrt() * (2*PI*U2).array().cos()).matrix();

  return sampleVals;
}

// Generate equally spaced points on an interval or square region
Matrix GP::linspace(double a, double b, int N, int dim)
{

  Matrix linspaceVals;
  if ( dim == 1 )
    {
      linspaceVals.resize(N,1);
      linspaceVals = Eigen::Array<double, Eigen::Dynamic, 1>::LinSpaced(N, a, b);
    }
  else if ( dim == 2 )
    {
      linspaceVals.resize(N*N,2);
      Matrix linspaceVals1D = Eigen::Array<double, Eigen::Dynamic, 1>::LinSpaced(N, a, b);
      int k = 0;
      for ( auto i : boost::irange(0,N) )
        {
          for ( auto j : boost::irange(0,N) )
            {
              linspaceVals(k,0) = linspaceVals1D(i);
              linspaceVals(k,1) = linspaceVals1D(j);
              k++;
            }
        }
    }
  else
      std::cout << "[*] GP::linspace has not been implemented for dim > 2\n";

  return linspaceVals;
}

// Define function for retrieving time from chrono
float GP::getTime(std::chrono::high_resolution_clock::time_point start, std::chrono::high_resolution_clock::time_point end)
{
  return static_cast<float>(std::chrono::duration_cast<std::chrono::microseconds>( end - start ).count() / 1000000.0);
};

// Define utility function for formatting hyperparameter bounds
void GP::GaussianProcess::parseBounds(Vector & lbs, Vector & ubs, int augParamCount)
{
  lbs.resize(augParamCount);
  ubs.resize(augParamCount);

  double defaultLowerBound = 0.01;
  double defaultUpperBound = 2.0;

  if ( fixedBounds )
    {
      // Check if bounds for noise parameter were provided
      if ( lowerBounds.size() < augParamCount )
        {
          // Set noise bounds to defaults
          lbs(0) = std::log( defaultLowerBound );
          ubs(0) = std::log( defaultUpperBound );

          // Convert specified bounds to log-scale
          for ( auto bi : boost::irange(1,augParamCount) )
            {
              lbs(bi) = std::log(lowerBounds(bi-1));
              ubs(bi) = std::log(upperBounds(bi-1));
            }
        }
      else
        {
          // Convert specified bounds to log-scale
          lbs = (lowerBounds.array().log()).matrix();
          ubs = (upperBounds.array().log()).matrix();
        }
    }
  else
    {
      // Set noise and hyperparameter bounds to defaults
      lbs = ( defaultLowerBound * Eigen::MatrixXd::Ones(augParamCount,1) ).array().log().matrix();
      ubs = ( defaultUpperBound * Eigen::MatrixXd::Ones(augParamCount,1) ).array().log().matrix();
    }

}

/*
//  POSSIBLY UNNEEDED POINTWISE KERNEL DEFINITIONS

// Define kernel function for RBF
double GP::RBF::evalKernel(Matrix & x, Matrix & y, Vector & params, int n)
{
  switch (n)
    {
    case 0: return std::exp( -(x-y).squaredNorm() / (2.0*std::pow(params(0),2)));
    case 1: return (x-y).squaredNorm() / std::pow(params(0),3) * std::exp( -(x-y).squaredNorm() / (2.0*std::pow(params(0),2)));
    default: std::cout << "\n[*] UNDEFINED DERIVATIVE\n"; return 0.0;
    }
};

*/

/*
// POTENTIAL PARALLEL IMPLEMENTATION OF SQUARE FORM; SPEED-UP APPEARS NEGLIGIBLE
// Re-assemble pairwise distances into a dense matrix
void GP::squareFormParallel(Matrix & D, Matrix & Dv, int n, double diagVal)
{

  D.resize(n,n);
  int i;
  int j;
#pragma omp parallel for private(i,j) shared(D,Dv,n)
  for ( i = 0 ; i<n-1; i++ )
    {
      for ( j = i+1 ; j<n ; j++ )
        {
          D(i,j) = D(j,i) = Dv( static_cast<int>(i*n - (i*(i+1))/2 + j - i -1) ,0);
        }
    }
  D.diagonal() = diagVal * Eigen::MatrixXd::Ones(n,1);
}
*/

//
//   SECOND MINIMIZATION IMPLEMENTATION USING CPPOPTLIB
//

// Fit model hyperparameters
//void GP::GaussianProcess::fitModel()

// Initialize gradient vector size
//cppOptLibgrad.resize(augParamCount);

/*
this->setLowerBound(lbs);
this->setUpperBound(ubs);
cppoptlib::LbfgsbSolver<GaussianProcess> solver;

// Specify stopping criteria
cppoptlib::Criteria<double> crit = cppoptlib::Criteria<double>::defaults();
//crit.iterations = 10; //!< Maximum number of iterations
crit.gradNorm = 1e-6;   //!< Minimum norm of gradient vector
//crit.fDelta = 7.5e-5;   //!< Minimum [relative] change in cost function
crit.iterations = solverIterations;
crit.fDelta = solverPrecision;
solver.setStopCriteria(crit);
solver.setHistorySize(10);

solver.minimize(*this, optParams);

*/

// Display final solver criteria values
/*
std::cout << "\nSolver Criteria |";
std::cout << "\n----------------\n" << solver.criteria() << std::endl;
std::cout << "gradEvals =\t" << gradientEvals <<std::endl;
*/

//
//   ORIGINAL MINIMIZATION IMPLEMENTATION USING RASMUSSEN'S CODE
//

//
//    NOTE:
//
//    Remember to include "utils/minimize.h" and derive the
//   'GaussianProcess' class from the 'GradientObj' class:
//
//    i.e.  class GaussianProcess : public minimize::GradientObj
//

/*
void minimize(...)
{

  //
  // Specify the parameters for the minimization algorithm
  //

  //   HIGH ACCURACY SETTINGS   //
  // max of MAX function evaluations per line search
  //int MAX = 30;
  // max number of line searches = length
  //int length = 20;
  // don't reevaluate within INT of the limit of the current bracket
  //double INT = 0.00001;
  // SIG is a constant controlling the Wolfe-Powell conditions
  //double SIG = 0.9;
  // extrapolate maximum EXT times the current step-size
  //double EXT = 5.0;

  //  EFFICIENT SETTINGS  //

  int MAX = 15;
  int length = 10;
  double INT = 0.00001;
  double SIG = 0.9;
  double EXT = 5.0;

  // Define number of exploratory NLML evaluations for specifying
  // a reasonable initial value for the optimization algorithm
  int initParamSearchCount = 30;

  // Define restart count for optimizer
  int restartCount = 0;

  // Convert hyperparameter bounds to log-scale
  Vector lbs, ubs;
  parseBounds(lbs, ubs, augParamCount);

  // Declare variables to store optimization loop results
  double currentVal;
  double optVal = 1e9;
  Vector theta(augParamCount);
  Vector optParams(augParamCount);

  //time start = high_resolution_clock::now();
  // First explore hyperparameter space to get a reasonable initializer for optimization
  for ( auto i : boost::irange(0,initParamSearchCount) )
    {
      if ( i == 0 )
          theta = Eigen::MatrixXd::Zero(augParamCount,1);
      else
          theta = sampleUnifVector(lbs, ubs);

      // Compute current NLML and store parameters if optimal
      currentVal = evalNLML(theta);
      if ( currentVal < optVal )
        {
          optVal = currentVal;
          optParams = theta;
        }
      //std::cout << "Theta Search:  " << theta.transpose() << "  [ NLML = " << currentVal << " ]"<< std::endl;
    }
  //time end = high_resolution_clock::now();
  //time_paramsearch += getTime(start, end);

  // NOTE: THIS NEEDS TO BE RE-WRITTEN TO USE THE PRELIMINARY PARAMETER SEARCH RESULTS
  // Evaluate optimizer with various different initializations
  for ( auto i : boost::irange(0,restartCount) )
    {
      // Avoid compiler warning for unused variable
      //(void)i;

      if ( i == 0 )
        {
          // Set initial guess (should make this user specifiable...)
          theta = Eigen::MatrixXd::Zero(augParamCount,1);
        }
      else
        {
          // Sample initial hyperparameter vector
          theta = sampleUnifVector(lbs, ubs);
        }

      // Optimize hyperparameters
      minimize::cg_minimize(theta, this, g, length, SIG, EXT, INT, MAX);

      // Compute current NLML and store parameters if optimal
      currentVal = evalNLML(theta);
      if ( currentVal < optVal )
        {
          optVal = currentVal;
          optParams = theta;
        }
    }

  // Perform one last optimization starting from best parameters so far
  if ( ( initParamSearchCount == 0 ) && ( restartCount == 0 ) )
    optParams = sampleUnifVector(lbs, ubs);
  //std::cout << "\n[*] FINAL - Initial Values (log):  " << optParams.transpose() << std::endl;
  //std::cout << "[*] FINAL - Initial Values (std):  " << optParams.transpose().array().exp().matrix() << std::endl;
  //start = high_resolution_clock::now();
  minimize::cg_minimize(optParams, this, g, length, SIG, EXT, INT, MAX);
  //end = high_resolution_clock::now();
  //time_minimize += getTime(start, end);

}
*/


/*****************************************************************************
*
* Rokko: Integrated Interface for libraries of eigenvalue decomposition
*
* Copyright (C) 2012-2013 by Tatsuya Sakashita <t-sakashita@issp.u-tokyo.ac.jp>,
*                            Synge Todo <wistaria@comp-phys.org>
*
* Distributed under the Boost Software License, Version 1.0. (See accompanying
* file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
*
*****************************************************************************/

#include <iostream>
#include <fstream>
#include <vector>
#include <boost/tuple/tuple.hpp>

#include <rokko/utility/xyz_hamiltonian.hpp>
#include <rokko/localized_matrix.hpp>
#include <rokko/localized_vector.hpp>

int main(int argc, char *argv[])
{
  if (argc <= 1) {
    std::cerr << "error: " << argv[0] << " xyz.ip" << std::endl;
    exit(1);
  }

  std::ifstream ifs(argv[1]);
  if (!ifs) {
    std::cout << "can't open file" << std::endl;
    exit(2);
  }

  int L, num_bonds;
  std::vector<std::pair<int, int> > lattice;
  std::vector<boost::tuple<double, double, double> > coupling;
  ifs >> L >> num_bonds;
  for (int i=0; i<num_bonds; ++i) {
    int j, k;
    ifs >> j >> k;
    lattice.push_back(std::make_pair(j, k));
  }

  for (int i=0; i<num_bonds; ++i) {
    double jx, jy, jz;
    ifs >> jx >> jy >> jz;
    coupling.push_back(boost::make_tuple(jx, jy, jz));
  }

  std::cout << "L=" << L << " num_bonds=" << num_bonds << std::endl;
  for (int i=0; i<num_bonds; ++i) {
    std::cout << lattice[i].first << " " << lattice[i].second << " " << coupling[i].get<0>() << " " << coupling[i].get<1>() << " " << coupling[i].get<2>() << std::endl;
  }
  int dim = 1 << L;
  int N = dim;
  std::cout << "dim=" << dim << std::endl;

  rokko::localized_matrix<rokko::matrix_col_major> mat1(N, N);
  std::cout << "multiply:" << std::endl;
  for (int i=0; i<N; ++i) {
    std::vector<double> v, w;
    v.assign(N, 0);
    v[i] = 1;
    w.assign(N, 0);
    rokko::xyz_hamiltonian::multiply(L, lattice, coupling, v, w);
    for (int j=0; j<N; ++j) {
      mat1(j,i) = w[j];
      std::cout << w[j] << " ";
    }
    std::cout << std::endl;
  }

  std::cout << "fill_diagonal:" << std::endl;
  rokko::localized_vector diagonal(N);
  std::vector<double> v(N);
  rokko::xyz_hamiltonian::fill_diagonal(L, lattice, coupling, v);
  for (int j=0; j<N; ++j) {
    diagonal(j) = v[j];
    std::cout << v[j] << " ";
  }
  std::cout << std::endl;

  std::cout << "fill_matrix:" << std::endl;
  rokko::localized_matrix<rokko::matrix_col_major> mat2(N, N);
  rokko::xyz_hamiltonian::generate(L, lattice, coupling, mat2);
  for (int i=0; i<N; ++i) {
    for (int j=0; j<N; ++j) {
      std::cout << mat2(i,j) << " ";
    }
    std::cout << std::endl;
  }

  if (mat1 == mat2) {
    std::cout << "OK: matrix by 'multiply' equals to a matrix by 'generate'." << std::endl;
  } else {
    std::cout << "ERROR: matrix by 'multiply' is differnet from a matrix by 'generate'."<< std::endl;
    exit(1);
  }

  if (diagonal == mat2.diagonal()) {
    std::cout << "OK: diagonal by 'fill_diagonal' equals to diagonal elementas of a matrix by 'genertate'."<< std::endl;
  } else {
    std::cout << "ERROR: diagonal by 'fill_diagonal' is differnet from diagonal elementas of a matrix by 'genertate'."<< std::endl;
    exit(1);
  }

}


#include <iostream>
#include <algorithm>
#include <iterator>
#include <cmath>
#include <fstream>
#include <valarray>
#include <sstream>
#include <string>
#include <utility>

using namespace std::string_literals;

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/math/constants/constants.hpp>

#include "vlasovpp/field.h"
#include "vlasovpp/complex_field.h"
#include "vlasovpp/weno.h"
#include "vlasovpp/fft.h"
#include "vlasovpp/array_view.h"
#include "vlasovpp/poisson.h"
#include "vlasovpp/splitting.h"
#include "vlasovpp/lagrange5.h"
#include "vlasovpp/config.h"
#include "vlasovpp/signal_handler.h"
#include "vlasovpp/iteration.h"

namespace math = boost::math::constants;
const std::complex<double> & I = std::complex<double>(0.,1.);

#define SQ(X) ((X)*(X))
#define Xi(i) (i*fh.step.dx+fh.range.x_min)
#define Vk(k) (k*fh.step.dv+fh.range.v_min)

auto
maxwellian ( double rho , double u , double T ) {
  return [=](double x,double v){ return rho/(std::sqrt(2.*math::pi<double>()*T))*std::exp( -0.5*SQ(v-u)/T ); };
}

auto
error_H ( std::size_t Nx , std::size_t Nv , double dt )
{
  const double Tf = 6.5;

  field<double,1> fh(boost::extents[Nv][Nx]);
  complex_field<double,1> hfh(boost::extents[Nv][Nx]);

  const double Kx = 0.5;
  fh.range.v_min = -12.; fh.range.v_max = 12.;
  fh.range.x_min =  0.; fh.range.x_max = 2./Kx*math::pi<double>();
  fh.compute_steps();

  ublas::vector<double> v(Nv,0.);
  std::generate( v.begin() , v.end() , [&,k=0]() mutable {return (k++)*fh.step.dv+fh.range.v_min;} );

  ublas::vector<double> kx(Nx); // beware, Nx need to be odd
  {
    double l = fh.range.len_x();
    for ( auto i=0 ; i<Nx/2 ; ++i ) { kx[i]    = 2.*math::pi<double>()*i/l; }
    for ( int i=-Nx/2 ; i<0 ; ++i ) { kx[Nx+i] = 2.*math::pi<double>()*i/l; }
  }

  const double alpha = 0.2 , ui = 3.4;
  auto tb_M1 = maxwellian( 0.5*alpha , ui , 1. ) , tb_M2 = maxwellian( 0.5*alpha , -ui , 1. );
  for (field<double,2>::size_type k=0 ; k<fh.size(0) ; ++k ) {
    for (field<double,2>::size_type i=0 ; i<fh.size(1) ; ++i ) {
      hfh[k][i] = 0.;
      fh[k][i] = ( tb_M1(Xi(i),Vk(k)) + tb_M2(Xi(i),Vk(k)) )*(1. + 0.01*std::cos(Kx*Xi(i)));
    }
    fft::fft(fh[k].begin(),fh[k].end(),hfh[k].begin());
  }

  std::size_t iter = 0;
  double current_time = 0.;

  ublas::vector<double> uc(Nx,0.);
  ublas::vector<double> E (Nx,0.);

  std::vector<double> H;
  std::vector<double> ee;
  std::vector<double> t;

  const double rho_c = 1.-alpha;
  const double sqrt_rho_c = std::sqrt(rho_c);

  // init E with Poisson solver, init also ee, Emax, H and times
  {
    poisson<double> poisson_solver(Nx,fh.range.len_x());
    ublas::vector<double> rho(Nx,0.); rho = fh.density(); // compute density from init hot data
    for ( auto i=0 ; i<Nx ; ++i ) { rho[i] += (1.-alpha); } // add (1-alpha) for cold particules
    E = poisson_solver(rho);

    double total_energy = energy(fh,E);
    total_energy += 0.; // sum(rho_c*u_c*u_c) = 0 because u_c = 0 at time 0
    H.push_back( total_energy );
  }
  t.push_back(current_time);

  // initialize memory for all temporary variables
  splitting<double,1> Lie( fh , fh.range.len_x() , rho_c );

  ublas::vector<double> uc1(Nx) , uc2(Nx) , uc3(Nx) , uc4(Nx) , ucn(Nx),
                        E1 (Nx) , E2 (Nx) , E3 (Nx) , E4 (Nx) , En (Nx);
  complex_field<double,1> hfh1(boost::extents[Nv][Nx]) , hfh2(boost::extents[Nv][Nx]) ,
                          hfh3(boost::extents[Nv][Nx]) , hfh4(boost::extents[Nv][Nx]) ,
                          hfhn(boost::extents[Nv][Nx]) ;

  const double alpha1=1./(4.-std::cbrt(4.)), alpha2=alpha1, alpha3=1./(1.-SQ(std::cbrt(4.)));

  const double g1 = alpha1, g2 = alpha1+alpha2;
  const double w1 = (g2*(1.-g2))/(g1*(g1-1.)-g2*(g2-1.)) , w2 = 1.-w1 , w3 = w2 , w4 = w1;

  while (  current_time < Tf ) {
  /*std::cout << *std::max_element( hfh[100].begin() , hfh[100].end() , [](auto a,auto b){
    return std::abs(a) < std::abs(b);
  }) << " " << std::endl;*/

///////////////////////////////////////////////////////////////////////////////
// Suzuki /////////////////////////////////////////////////////////////////////

    /*
    Lie.phi_a(0.5*dt,uc,E,hfh);
    Lie.phi_c(0.5*dt,uc,E,hfh);
    Lie.phi_b(dt,uc,E,hfh);
    Lie.phi_c(0.5*dt,uc,E,hfh);
    Lie.phi_a(0.5*dt,uc,E,hfh);
    */

    // Strang alpha1
    Lie.phi_a(alpha1*0.5*dt,uc,E,hfh);
    Lie.phi_c(alpha1*0.5*dt,uc,E,hfh);
    Lie.phi_b(alpha1*dt,uc,E,hfh);
    Lie.phi_c(alpha1*0.5*dt,uc,E,hfh);
    Lie.phi_a(alpha1*0.5*dt,uc,E,hfh);

    // Strang alpha2
    Lie.phi_a(alpha2*0.5*dt,uc,E,hfh);
    Lie.phi_c(alpha2*0.5*dt,uc,E,hfh);
    Lie.phi_b(alpha2*dt,uc,E,hfh);
    Lie.phi_c(alpha2*0.5*dt,uc,E,hfh);
    Lie.phi_a(alpha2*0.5*dt,uc,E,hfh);

    // Strang alpha3
    Lie.phi_a(alpha3*0.5*dt,uc,E,hfh);
    Lie.phi_c(alpha3*0.5*dt,uc,E,hfh);
    Lie.phi_b(alpha3*dt,uc,E,hfh);
    Lie.phi_c(alpha3*0.5*dt,uc,E,hfh);
    Lie.phi_a(alpha3*0.5*dt,uc,E,hfh);

    // Strang alpha2
    Lie.phi_a(alpha2*0.5*dt,uc,E,hfh);
    Lie.phi_c(alpha2*0.5*dt,uc,E,hfh);
    Lie.phi_b(alpha2*dt,uc,E,hfh);
    Lie.phi_c(alpha2*0.5*dt,uc,E,hfh);
    Lie.phi_a(alpha2*0.5*dt,uc,E,hfh);

    // Strang alpha1
    Lie.phi_a(alpha1*0.5*dt,uc,E,hfh);
    Lie.phi_c(alpha1*0.5*dt,uc,E,hfh);
    Lie.phi_b(alpha1*dt,uc,E,hfh);
    Lie.phi_c(alpha1*0.5*dt,uc,E,hfh);
    Lie.phi_a(alpha1*0.5*dt,uc,E,hfh);

///////////////////////////////////////////////////////////////////////////////
// MONITORING /////////////////////////////////////////////////////////////////

    for ( auto k=0 ; k<Nv ; ++k ) {
      fft::ifft(hfh[k].begin(),hfh[k].end(),fh[k].begin());
    }

    double total_energy = kinetic_energy(fh);
    for ( const auto ei : E ) {
      total_energy += ei*ei*fh.step.dx;
    }
    {
      auto rhoh = fh.density();
      fft::spectrum_ hrhoh(Nx); hrhoh.fft(&rhoh[0]);
      fft::spectrum_ hE(Nx); hE.fft(&E[0]);
      fft::spectrum_ hrhoc(Nx);
      hrhoc[0] = I*kx[0]*hE[0] - hrhoh[0] + 1.;
      for ( auto i=1 ; i<Nx ; ++i ) {
        hrhoc[i] = I*kx[i]*hE[i] - hrhoh[i];
      }
      ublas::vector<double> rhoc (Nx,0.); hrhoc.ifft(&rhoc[0]);

      for ( auto i=0 ; i<Nx ; ++i ) {
        total_energy += rhoc[i]*uc[i]*uc[i]*fh.step.dx;
      }
    }
    H.push_back( total_energy );

    double electric_energy = 0.;
    for ( const auto & ei : E ) { electric_energy += ei*ei*fh.step.dx; }
    ee.push_back( std::sqrt(electric_energy) );

    // increment time
    current_time += dt;
    t.push_back(current_time);

    ++iter;
    //if ( current_time+dt > Tf ) { dt = Tf - current_time; }
  } // while (  current_time < Tf ) // end of time loop

  std::ofstream of("H.dat");
  std::transform( H.cbegin() , H.cend() , std::ostream_iterator<std::string>(of,"\n") ,
    [&,count=0] ( const double h ) mutable {
      std::stringstream ss;
      ss << t[count++] << " " << h << " " << std::abs(H[0]-h)/std::abs(H[0]);
      return ss.str();
  });
  of.close();

  of.open("ee.dat");
  std::transform( ee.cbegin() , ee.cend() , std::ostream_iterator<std::string>(of,"\n") ,
    [&,count=0] ( const double e ) mutable {
      std::stringstream ss;
      ss << t[count++] << " " << e;
      return ss.str();
  });
  of.close();

  for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(hfh[k].begin(),hfh[k].end(),fh[k].begin()); }
  fh.write("vp.dat");

  double h_max = std::abs(*std::max_element( H.begin() , H.end() ,
    [&] ( double a , double b ) {
      return std::abs(a-H[0])/std::abs(H[0]) < std::abs(b-H[0])/std::abs(H[0]);
    }
  ));

  double h_last = H.back();
  //return std::make_pair(std::abs(H[0]-h_last)/std::abs(H[0]),std::abs(H[0]-h_max)/std::abs(H[0]));
  double h = std::abs(*std::max_element( H.begin() , H.end() , [&](double a,double b){return ( std::abs((a-H[0])/std::abs(H[0])) < std::abs((b-H[0])/std::abs(H[0])) );} ));
  return std::abs((h-H[0])/std::abs(H[0]));
}

int
main ( int argc , char const * argv[] )
{
  const std::size_t Nx = 75 , Nv = 1024;
  const double dt_max = 1.;//3.*16./Nv;

  for ( auto i=1 ; i<10 ; ++i ) {
    double dt = dt_max/double(i);
    std::cout << dt << " " << std::flush;
    double h = error_H(Nx,Nv,dt);
    std::cout << h << std::endl;
  }
  return 0;
}


#include <pybind11/pybind11.h>
#include <pybind11/stl.h>
#include <pybind11/numpy.h>
#include <vector>
#include<list>

#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/property_map/property_map.hpp>
using namespace boost;
namespace py = pybind11;

// supports DELOCX and DELOCY without copying matrix A
// "Serializes" a map. That means, giving an index for every pixel.
// For example: [0123]
// 				[4567]
#define LOC(y,x,Arows) ((y)*(Arows+1)+(x))
// Deserialising. Returns the x or y coordinate for a given index.
#define DELOCY(i,Arows) ((i)/(Arows+1))
#define DELOCX(i,Arows) ((i)%(Arows+1))

// Typedefs for graph_t, vertex_descriptor, edge_descriptor, Edge,
// and out_edge_iterator.
typedef adjacency_list < listS, vecS, directedS,
    no_property, property < edge_weight_t, double > > graph_t;
typedef graph_traits < graph_t >::vertex_descriptor vertex_descriptor;
typedef graph_traits < graph_t >::edge_descriptor edge_descriptor;
typedef std::pair<int, int> Edge;
typedef graph_traits < graph_t >::out_edge_iterator out_edge_iterator;

/*
  Global state, needs mlock!
*/

struct tState
{
    graph_t g;						                        // the graph
    property_map<graph_t, edge_weight_t>::type weightmap;   // the weights
    std::vector<vertex_descriptor> p;		                // predecessor map
    std::vector<double> d;				                    // distance map
    vertex_descriptor global_s;				      // cache the start of dijkstra
    int Arows;
} ;

// The graph

std::vector<tState> gState;

int py_graphFromBitmap(py::array_t<uint8_t> pyo_image, uint8_t walkable=0)
{
    auto A  = pyo_image.mutable_unchecked<2>();
    tState state;

	// Create a matrix from the bitmap and store the number of rows for LOC/DELOC
    state.Arows = A.shape(0);
    std::cout << "Rows:" << state.Arows << ", Columns: " << A.shape(1) << "\n";

    // prepare the graph and search
    // This is the full graph. All points are vertices
    // hence, some vertices are disconnected and make their
    // own connected component, e.g., black pixels
    graph_t _g(A.shape(0)*A.shape(1));
    state.g = _g;
    // Define macro to check, whether an edge can be added from
    // i,j to i+k, j+l
    auto CHECK_EDGE = [&](int i,int j,int k,int l) { return (
	i+k >= 0 && i+k < A.shape(0) &&
	j+l >= 0 && j+l < A.shape(1) &&
	A(i,j) == walkable &&
	A(i+k,j+l) == walkable );};

	// Iterate through the map and add edges, if possible
    int e=0;
    for (int i=0; i< A.shape(0); i++) {
        for (int j=0; j< A.shape(1); j++) {
            // NOTE!!!
            // left      0 -1
            // right     0  1
            // up       -1  0
            // down      1  0
            if (CHECK_EDGE(i,j,-1,0))
                add_edge(LOC(i,j,state.Arows),LOC(i-1,j,state.Arows),1,state.g);
            if (CHECK_EDGE(i,j,+1,0))
                add_edge(LOC(i,j,state.Arows),LOC(i+1,j,state.Arows),1,state.g);
            if (CHECK_EDGE(i,j,0,-1))
                add_edge(LOC(i,j,state.Arows),LOC(i,j-1,state.Arows),1,state.g);
            if (CHECK_EDGE(i,j,0,1))
                add_edge(LOC(i,j,state.Arows),LOC(i,j+1,state.Arows),1,state.g);
            // DIAGONALS
            if (CHECK_EDGE(i,j,-1,-1))
                add_edge(LOC(i,j,state.Arows),LOC(i-1,j-1,state.Arows),sqrt(2),state.g);
            if (CHECK_EDGE(i,j,-1,1))
                add_edge(LOC(i,j,state.Arows),LOC(i-1,j+1,state.Arows),sqrt(2),state.g);
            if (CHECK_EDGE(i,j,1,-1))
                add_edge(LOC(i,j,state.Arows),LOC(i+1,j-1,state.Arows),sqrt(2),state.g);
            if (CHECK_EDGE(i,j,1,1))
                add_edge(LOC(i,j,state.Arows),LOC(i+1,j+1,state.Arows),sqrt(2),state.g);
        }
    }

    std::cout << "Added " << num_vertices(state.g) << " vertices and "
				  << num_edges(state.g)<<" edges.\n";

	// Store the state
    gState.push_back(state);
    int index = gState.size()-1;
    std::cout << "State is " << index << std::endl;
    return index;

}

// -------------
// pure C++ code
// -------------

std::vector<int> multiply(const std::vector<double>& input)
{
  std::vector<int> output(input.size());

  for ( size_t i = 0 ; i < input.size() ; ++i )
    output[i] = 10*static_cast<int>(input[i]);

  return output;
}

// ----------------
// Python interface
// ----------------

struct predicate_violated {};
/*This function actually performs our homotopy test*/
bool py_empty(py::array_t<uint8_t> pyo_image, py::array_t<double> pyo_points, uint8_t allowed=0)
{
    auto image  = pyo_image.mutable_unchecked<2>();
    auto points = pyo_points.unchecked<2>();

    const int polyCorners = points.shape(0);
    std::vector<double> nodeX;
    nodeX.reserve(polyCorners);
    try {
    for (size_t pixelY=0; pixelY < image.shape(1); pixelY++)
    {
	int i,j;

    auto polyX = [&](int row) {return points(row,0);};
    auto polyY = [&](int row) {return points(row,1);};

	nodeX.clear(); j=polyCorners-1;
	for (i=0; i<polyCorners; i++)
	{
	    if (polyY(i)<(double) pixelY && polyY(j)>=(double) pixelY
		||  polyY(j)<(double) pixelY && polyY(i)>=(double) pixelY)
	    {
		nodeX.push_back( (polyX(i)+(pixelY-polyY(i))/(polyY(j)-polyY(i))*(polyX(j)-polyX(i))));
	   }
	  j=i;
    } // for i ranging polyCorners
    std::sort(nodeX.begin(), nodeX.end());

    const auto IMAGE_LEFT=0;
    const auto IMAGE_RIGHT = image.shape(1);

     for (i=0; i<nodeX.size(); i+=2) {
	if   (nodeX[i  ]>=IMAGE_RIGHT) break;

	if   (nodeX[i+1]> IMAGE_LEFT ) {
	  if (nodeX[i  ]< IMAGE_LEFT ) nodeX[i  ]=IMAGE_LEFT ;
	  if (nodeX[i+1]> IMAGE_RIGHT) nodeX[i+1]=IMAGE_RIGHT;
	  for (auto pixelX=(int)nodeX[i]; pixelX<=(int) (nodeX[i+1]+0.5); pixelX++)
	      if (image(pixelX,pixelY) != allowed)
	         throw(predicate_violated());
	  }
	  }
   } // for y
   }catch(predicate_violated v)
    {
    return false;

   }
   return true;

 }

void py_dijkstra(int handle, py::array_t<uint32_t> pyo_p)
{
    tState *state = &gState[handle];  // @TODO: bound check? else crash
    auto p = pyo_p.unchecked<1>();

    vertex_descriptor s = LOC(p(0),p(1),state->Arows);
    state->global_s = s;

    std::cout << "Search: " << s << "\n";
    std::cout << "Using " << num_vertices(state->g) << " vertices and "
				  << num_edges(state->g) << " edges.\n";
    state->weightmap = get(edge_weight, state->g);	// weightmap
    state->p.resize(num_vertices(state->g));		// predecessor
    state->d.resize(num_vertices(state->g));		// distances

	// Perform dijkstra
    dijkstra_shortest_paths(state->g, s,
		predecessor_map(boost::make_iterator_property_map(state->p.begin(), get(boost::vertex_index, state->g))).
		distance_map(boost::make_iterator_property_map(state->d.begin(), get(boost::vertex_index, state->g))));
    std::cout << "Did it " << std::endl;
}

py::array_t<uint32_t> py_getpath(int handle, py::array_t<uint32_t> pyo_p, double penalty=1.0)
{
    tState *state = &gState[handle];  // @TODO: bound check? else crash
    auto p = pyo_p.unchecked<1>();

    vertex_descriptor e = LOC(p(0),p(1),state->Arows);
    if (state->p[e] == e) { // no path found
	  return (py::array_t<uint32_t>(0));
    }
    // Get the shortest path from s to e

    std::list<vertex_descriptor> shortest_path;
    for(vertex_descriptor v = e;;v=state->p[v] ) {
        shortest_path.push_front(v);
        if ( penalty != 1) {
			edge_descriptor e1; bool found;
			tie(e1, found) = edge(state->p[v], v, state->g);
			if(found) {
//				octave_stdout << e1 << " : " << get(state->weightmap, e1);
				put(state->weightmap, e1, get(state->weightmap, e1)*2);
//				octave_stdout << " -> " << get(state->weightmap, e1) << "\n";
			}
		}
        if(state->p[v] == v)
            break;
    }
// allocate py::array (to pass the result of the C++ function to Python)
  // correctly transfer with a capsule
  uint32_t *result_ptr = new uint32_t[shortest_path.size()*2];
  uint32_t *ptr = result_ptr;
  for(const auto pt:shortest_path) {
        *ptr++ =  DELOCY(pt,state->Arows);
        *ptr++  = DELOCX(pt,state->Arows);
    }

      py::capsule free_when_done(result_ptr, [](void *f) {
            uint32_t *foo = reinterpret_cast<uint32_t *>(f);
            //std::cerr << "Element [0] = " << foo[0] << "\n";
            //std::cerr << "freeing memory @ " << f << "\n";
            delete[] foo;
        });

   return py::array_t<uint32_t>(
            shortest_path.size()*2, result_ptr,free_when_done); // numpy array references this parent
 }

void py_getweights(int handle, py::array_t<double> pyo_image)
{
    auto weights  = pyo_image.mutable_unchecked<2>();
    tState *state = &gState[handle];  // @TODO: bound check? else crash

    for (auto r =0; r < weights.shape(0); r++)
      for (auto c = 0; c < weights.shape(1); c++)
      {
	  vertex_descriptor v = LOC(r,c,state->Arows);
	  out_edge_iterator out_i, out_end;
	  edge_descriptor e;
	  double sum = 0;
	  double N = 0;
	  for (tie(out_i, out_end) = out_edges(v, state->g); out_i != out_end; ++out_i)
	  {
			sum += get(state->weightmap, *out_i);
			N++;
	  }
	  weights(r,c) = (N==0)?0:sum/N;
      }
}
/*This function is for debugging, but it is not needed*/

void py_fill(py::array_t<uint8_t> pyo_image, py::array_t<double> pyo_points)
{
    auto image  = pyo_image.mutable_unchecked<2>();
    auto points = pyo_points.unchecked<2>();

    const int polyCorners = points.shape(0);
    std::vector<double> nodeX;
    nodeX.reserve(polyCorners);

    for (size_t pixelY=0; pixelY < image.shape(1); pixelY++)
    {
//	std::cout << "scanning " << pixelY << std::endl;
    int nodes = 0; int i,j;

    auto polyX = [&](int row) {return points(row,0);};
    auto polyY = [&](int row) {return points(row,1);};

	nodeX.clear(); j=polyCorners-1;
	for (i=0; i<polyCorners; i++)
	{
	    if (polyY(i)<(double) pixelY && polyY(j)>=(double) pixelY
		||  polyY(j)<(double) pixelY && polyY(i)>=(double) pixelY)
	    {
		nodeX.push_back( (polyX(i)+(pixelY-polyY(i))/(polyY(j)-polyY(i))*(polyX(j)-polyX(i))));
	   }
	  j=i;
	  // now we have the X of the scanline
    } // for i ranging polyCorners
    std::sort(nodeX.begin(), nodeX.end());
    //std::cout << "On Scanline " << pixelY << " we have " << std::endl;
  //  Fill the pixels between node pairs.

    const auto IMAGE_LEFT=0;
    const auto IMAGE_RIGHT = image.shape(1);

     for (i=0; i<nodeX.size(); i+=2) {
	if   (nodeX[i  ]>=IMAGE_RIGHT) break;

	if   (nodeX[i+1]> IMAGE_LEFT ) {
	  if (nodeX[i  ]< IMAGE_LEFT ) nodeX[i  ]=IMAGE_LEFT ;
	  if (nodeX[i+1]> IMAGE_RIGHT) nodeX[i+1]=IMAGE_RIGHT;
	  for (auto pixelX=(int)nodeX[i]; pixelX<=(int) (nodeX[i+1]+0.5); pixelX++)
	      image(pixelX,pixelY) = 1;
	  }
	  }

    } // for y

}

/*// wrap C++ function with NumPy array IO
py::array_t<int> py_multiply(py::array_t<double, py::array::c_style | py::array::forcecast> array)
{
  // allocate std::vector (to pass to the C++ function)
  std::vector<double> array_vec(array.size());

  // copy py::array -> std::vector
  std::memcpy(array_vec.data(),array.data(),array.size()*sizeof(double));

  // call pure C++ function
  std::vector<int> result_vec = multiply(array_vec);

  // allocate py::array (to pass the result of the C++ function to Python)
  auto result        = py::array_t<int>(array.size());
  auto result_buffer = result.request();
  int *result_ptr    = (int *) result_buffer.ptr;

  // copy std::vector -> py::array
  std::memcpy(result_ptr,result_vec.data(),result_vec.size()*sizeof(int));

  return result;
  }
  */

// wrap as Python module
PYBIND11_MODULE(cfsrouting,m)
{
  m.doc() = "pybind11 example plugin";

//  m.def("multiply", &py_multiply, "Convert all entries of an 1-D NumPy-array to int and multiply by 10");
//  m.def("test", &py_multiply, "Convert all entries of an 1-D NumPy-array to int and multiply by 10");
  m.def ("fill", &py_fill,py::arg().noconvert(), py::arg().none());
  m.def("empty", &py_empty);
  m.def("graphFromBitmap", &py_graphFromBitmap);
  m.def("dijkstra", &py_dijkstra);
  m.def("getpath", &py_getpath);
  m.def("getweights", &py_getweights, py::arg().noconvert(),py::arg().noconvert());
}

/*template<typename vtype>
py::array wrap(vtype v)
{
   return py::array(v.size(),v.data()); // does a copy
}

PYBIND11_MODULE(spatialfacet,m) {
    py::class_<SpatialFacetMiner>(m, "SpatialFacetMiner")
    .def(py::init<>())
    .def("add_database", &SpatialFacetMiner::add_database)
    .def("query", [](SpatialFacetMiner &m,std::string query_string, int first, int max, int check_at_least){return m.query(query_string,first,max,check_at_least, false);})
    .def("query_with_data", [](SpatialFacetMiner &m,std::string query_string, int first, int max, int check_at_least){return m.query(query_string,first,max,check_at_least, true);})
    .def("getSpyData", [](SpatialFacetMiner &m){
	auto &spy = m.getSpy();
	return py::make_tuple(
	    wrap(spy.coords[0]),
	    wrap(spy.coords[1]),
	    wrap(spy.docids),
	    wrap(spy.weights)
	 );
	 })
   .def("getSpyStringData",[](SpatialFacetMiner &m){
      auto &spy = m.getSpy();
      return py::make_tuple(spy.value1, spy.values);

      })

    .def ("augment",[](SpatialFacetMiner &m, std::string query_string,
		       py::array_t<double, py::array::c_style | py::array::forcecast> documents,
		       int n_terms)
    {
	    std::vector<int> stl_documents;
	    auto r = documents.unchecked<1>();
	    for (py::ssize_t i =0; i < r.shape(0); i++)
	      stl_documents.push_back(r(i));

	    std::vector<string> terms; std::vector<double> weights; std::string query_out;
//    void augment_query_from_documents(std::string query_string, std::vector<int> documents, int n_terms,
//				      std::vector<std::string> &terms, std::vector<double> &weights, std::string &query_out)

	    m.augment_query_from_documents(query_string, stl_documents,n_terms, terms, weights, query_out);
	   return py::make_tuple(terms, weights, query_out);

    });

      ;

}
*/