#include <pybind11/pybind11.h>
#include <pybind11/numpy.h>
#include <armadillo>

namespace py = pybind11;

using namespace arma;

typedef py::array_t<double, py::array::f_style | py::array::forcecast> array_tf;
typedef py::array_t<double, py::array::c_style | py::array::forcecast> array_tc;

cube array_to_cube(array_tf m) {

    py::buffer_info _m_buff = m.request();
    int n_rows = _m_buff.shape[0];
    int n_cols = _m_buff.shape[1];
    int n_slices = _m_buff.shape[2];

    cube _m_arma((double *)_m_buff.ptr, n_rows, n_cols, n_slices);

    return _m_arma;
}

mat array_to_mat(array_tf m) {

    py::buffer_info _m_buff = m.request();
    int n_rows = _m_buff.shape[0];
    int n_cols = _m_buff.shape[1];

    mat _m_arma((double *)_m_buff.ptr, n_rows, n_cols);

    return _m_arma;
}

vec array_to_vec(array_tf m) {

    py::buffer_info _m_buff = m.request();
    int n_rows = _m_buff.shape[0];

    vec _m_vec((double *)_m_buff.ptr, n_rows);

    return _m_vec;
}

array_tf cube_to_array(cube m) {

    auto _m_array = array_tf({m.n_rows, m.n_cols, m.n_slices});

    py::buffer_info _m_buff = _m_array.request();
    std::memcpy(_m_buff.ptr, m.memptr(), sizeof(double) * m.n_rows * m.n_cols * m.n_slices);

    return _m_array;
}

array_tf mat_to_array(mat m) {

    auto _m_array = array_tf({m.n_rows, m.n_cols});

    py::buffer_info _m_buff = _m_array.request();
    std::memcpy(_m_buff.ptr, m.memptr(), sizeof(double) * m.n_rows * m.n_cols);

    return _m_array;
}

array_tf vec_to_array(vec m) {

    auto _m_array = array_tf({m.n_rows});

    py::buffer_info _m_buff = _m_array.request();
    std::memcpy(_m_buff.ptr, m.memptr(), sizeof(double) * m.n_rows);

    return _m_array;
}

py::tuple kl_divergence(array_tf _p_K, array_tf _p_kff, array_tf _p_sigma_ctl,
                        array_tf _q_K, array_tf _q_kff, array_tf _q_sigma_ctl,
                        array_tf _mu_x, array_tf _sigma_x,
                        int dm_state, int dm_act, int nb_steps) {

    cube p_K = array_to_cube(_p_K);
    mat p_kff = array_to_mat(_p_kff);
    cube p_sigma_ctl = array_to_cube(_p_sigma_ctl);

    cube q_K = array_to_cube(_q_K);
    mat q_kff = array_to_mat(_q_kff);
    cube q_sigma_ctl = array_to_cube(_q_sigma_ctl);

    mat mu_x  = array_to_mat(_mu_x);
    cube sigma_x = array_to_cube(_sigma_x);

    vec kl(nb_steps);

    for(int i = 0; i < nb_steps; i++) {
        mat q_lambda_ctl = inv_sympd(q_sigma_ctl.slice(i));

        mat diff_K = (q_K.slice(i) - p_K.slice(i)).t() * q_lambda_ctl * (q_K.slice(i) - p_K.slice(i));
        mat diff_crs = (q_K.slice(i) - p_K.slice(i)).t() * q_lambda_ctl * (- q_kff.col(i) + p_kff.col(i));
        mat diff_kff = (- q_kff.col(i) + p_kff.col(i)).t() * q_lambda_ctl * (- q_kff.col(i) + p_kff.col(i));

        kl(i) = as_scalar(0.5 * log( det(q_sigma_ctl.slice(i)) / det(p_sigma_ctl.slice(i)) )
		                  + 0.5 * trace(q_lambda_ctl * p_sigma_ctl.slice(i))
		                  - 0.5 * dm_act
		                  + 0.5 * trace(diff_K * sigma_x.slice(i))
		                  + 0.5 * mu_x.col(i).t() * diff_K * mu_x.col(i)
		                  - mu_x.col(i).t() * diff_crs
		                  + 0.5 * diff_kff);
    }

    array_tf _kl = vec_to_array(kl);

    py::tuple output =  py::make_tuple(_kl);
    return output;
}

double quad_expectation(array_tf _mu, array_tf _sigma_s,
                        array_tf _Q, array_tf _q, double _q0) {

    vec mu  = array_to_vec(_mu);
    mat sigma_s = array_to_mat(_sigma_s);

    mat Q = array_to_mat(_Q);
    vec q = array_to_vec(_q);

	double result = as_scalar(mu.t() * Q * mu) + as_scalar(mu.t() * q) + _q0 + trace(Q * sigma_s);
	return result;
}

py::tuple augment_cost(array_tf _Cxx, array_tf _cx, array_tf _Cuu,
                       array_tf _cu, array_tf _Cxu, array_tf _c0,
                       array_tf _K, array_tf _kff, array_tf _sigma_ctl,
                       array_tf _alpha, int dm_state, int dm_act, int nb_steps) {

    // inputs
    cube Cxx = array_to_cube(_Cxx);
    mat cx = array_to_mat(_cx);
    cube Cuu = array_to_cube(_Cuu);
    mat cu = array_to_mat(_cu);
    cube Cxu = array_to_cube(_Cxu);
    vec c0 = array_to_vec(_c0);

    cube K = array_to_cube(_K);
    mat kff = array_to_mat(_kff);
    cube sigma_ctl = array_to_cube(_sigma_ctl);

    vec alpha = array_to_vec(_alpha);

    // outputs
    cube agCxx(dm_state, dm_state, nb_steps + 1);
    mat agcx(dm_state, nb_steps + 1);
    cube agCuu(dm_act, dm_act, nb_steps + 1);
    mat agcu(dm_act, nb_steps + 1);
    cube agCxu(dm_state, dm_act, nb_steps + 1);
    vec agc0(nb_steps + 1);

    for (int i = 0; i < nb_steps; i++) {
        mat lambda_ctl = inv_sympd(sigma_ctl.slice(i));

        agCxx.slice(i) = Cxx.slice(i) + 0.5 * alpha(i) * K.slice(i).t() * lambda_ctl * K.slice(i);
        agCuu.slice(i) = Cuu.slice(i) + 0.5 * alpha(i) * lambda_ctl;
        agCxu.slice(i) = Cxu.slice(i) - 0.5 * alpha(i) * K.slice(i).t() * lambda_ctl;
        agcx.col(i) = cx.col(i) + alpha(i) * K.slice(i).t() * lambda_ctl * kff.col(i);
        agcu.col(i) = cu.col(i) - alpha(i) * lambda_ctl * kff.col(i);
        agc0(i) = as_scalar(c0(i) + 0.5 * alpha(i) * log( det(2. * datum::pi * sigma_ctl.slice(i)) )
                            + 0.5 * alpha(i) * kff.col(i).t() * lambda_ctl * kff.col(i));
    }

    // last time step
    agCxx.slice(nb_steps) = Cxx.slice(nb_steps);
    agcx.col(nb_steps) = cx.col(nb_steps);
    agCuu.slice(nb_steps) = Cuu.slice(nb_steps);
    agcu.col(nb_steps) = cu.col(nb_steps);
    agCxu.slice(nb_steps) = Cxu.slice(nb_steps);
    agc0(nb_steps) = c0(nb_steps);

    // transform outputs to numpy
    array_tf _agCxx = cube_to_array(agCxx);
    array_tf _agcx = mat_to_array(agcx);
    array_tf _agCuu =  cube_to_array(agCuu);
    array_tf _agcu = mat_to_array(agcu);
    array_tf _agCxu =  cube_to_array(agCxu);
    array_tf _agc0 = vec_to_array(agc0);

    py::tuple output =  py::make_tuple(_agCxx, _agcx, _agCuu, _agcu, _agCxu, _agc0);
    return output;
}

py::tuple forward_pass(array_tf _mu_x0, array_tf _sigma_x0,
                       array_tf _A, array_tf _B, array_tf _c, array_tf _sigma_dyn,
                       array_tf _K, array_tf _kff, array_tf _sigma_ctl,
                       int dm_state, int dm_act, int nb_steps) {

    // inputs
    vec mu_x0 = array_to_vec(_mu_x0);
    mat sigma_x0 = array_to_mat(_sigma_x0);

    cube A = array_to_cube(_A);
    cube B = array_to_cube(_B);
    mat c = array_to_mat(_c);
    cube sigma_dyn = array_to_cube(_sigma_dyn);

    cube K = array_to_cube(_K);
    mat kff = array_to_mat(_kff);
    cube sigma_ctl = array_to_cube(_sigma_ctl);

    // outputs
    mat mu_x(dm_state, nb_steps + 1);
    cube sigma_x(dm_state, dm_state, nb_steps + 1);

    mat mu_u(dm_act, nb_steps);
    cube sigma_u(dm_act, dm_act, nb_steps);

    mat mu_xu(dm_state + dm_act, nb_steps + 1);
    cube sigma_xu(dm_state + dm_act, dm_state + dm_act, nb_steps + 1);

    mu_x.col(0) = mu_x0;
    sigma_x.slice(0) = sigma_x0;

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

        // mu_u = K * mu_x + k
        mu_u.col(i) = K.slice(i) * mu_x.col(i) + kff.col(i);

        // sigma_u = sigma_ctl + K * sigma_x * K_T
        sigma_u.slice(i) = sigma_ctl.slice(i) + K.slice(i) * sigma_x.slice(i) * K.slice(i).t();
        sigma_u.slice(i) = 0.5 * (sigma_u.slice(i) + sigma_u.slice(i).t());

        // sigma_xu =   [[sigma_x,      sigma_x * K_T],
        //               [K*sigma_x,    sigma_u    ]]
        sigma_xu.slice(i) = join_vert(join_horiz(sigma_x.slice(i), sigma_x.slice(i) * K.slice(i).t()),
                                        join_horiz(K.slice(i) * sigma_x.slice(i), sigma_u.slice(i)));
        sigma_xu.slice(i) = 0.5 * (sigma_xu.slice(i) + sigma_xu.slice(i).t());

        // mu_xu =  [[mu_x],
        //           [mu_u]],
        mu_xu.col(i) = join_vert(mu_x.col(i), mu_u.col(i));

        // sigma_x_next = sigma_dyn + [A B] * sigma_xu * [A B]^T
        sigma_x.slice(i+1) = sigma_dyn.slice(i) + join_horiz(A.slice(i), B.slice(i)) * sigma_xu.slice(i) *
                                                              join_vert(A.slice(i).t(), B.slice(i).t());
        sigma_x.slice(i+1) = 0.5 * (sigma_x.slice(i+1) + sigma_x.slice(i+1).t());

        // mu_x_next = [A B] * [s a]^T + c
        mu_x.col(i+1) = join_horiz(A.slice(i), B.slice(i)) * mu_xu.col(i) + c.col(i);

        if(i == nb_steps - 1) {
            mu_xu.col(i+1) = join_vert(mu_x.col(i+1), zeros<vec>(dm_act));
            sigma_xu.slice(i+1).submat(0, 0, dm_state - 1, dm_state - 1) = sigma_x.slice(i+1);
        }
    }

    // transform outputs to numpy
    array_tf _mu_x = mat_to_array(mu_x);
    array_tf _sigma_x = cube_to_array(sigma_x);
    array_tf _mu_u =  mat_to_array(mu_u);
    array_tf _sigma_u = cube_to_array(sigma_u);
    array_tf _mu_xu =  mat_to_array(mu_xu);
    array_tf _sigma_xu = cube_to_array(sigma_xu);

    py::tuple output =  py::make_tuple(_mu_x, _sigma_x, _mu_u, _sigma_u, _mu_xu, _sigma_xu);
    return output;
}

py::tuple backward_pass(array_tf _Cxx, array_tf _cx, array_tf _Cuu,
                        array_tf _cu, array_tf _Cxu, array_tf _c0,
                        array_tf _A, array_tf _B, array_tf _c, array_tf _sigma_dyn,
                        array_tf _alpha, int dm_state, int dm_act, int nb_steps) {

    // inputs
    cube Cxx = array_to_cube(_Cxx);
    mat cx = array_to_mat(_cx);
    cube Cuu = array_to_cube(_Cuu);
    mat cu = array_to_mat(_cu);
    cube Cxu = array_to_cube(_Cxu);
    vec c0 = array_to_vec(_c0);

    cube A = array_to_cube(_A);
    cube B = array_to_cube(_B);
    mat c = array_to_mat(_c);
    cube sigma_dyn = array_to_cube(_sigma_dyn);

    vec alpha = array_to_vec(_alpha);

    // outputs
    cube Q(dm_state + dm_act, dm_state + dm_act, nb_steps);
    cube Qxx(dm_state, dm_state, nb_steps);
    cube Qux(dm_act, dm_state, nb_steps);
    cube Quu(dm_act, dm_act, nb_steps);
    cube Quu_inv(dm_act, dm_act, nb_steps);
    mat qx(dm_state, nb_steps);
    mat qu(dm_act, nb_steps);
    vec q0(nb_steps);

    cube V(dm_state, dm_state, nb_steps + 1);
    mat v(dm_state, nb_steps + 1);
    vec v0(nb_steps + 1);

    cube K(dm_act, dm_state, nb_steps);
    mat kff(dm_act, nb_steps);
    cube sigma_ctl(dm_act, dm_act, nb_steps);
    cube lambda_ctl(dm_act, dm_act, nb_steps);

    int _diverge = 0;

    // last time step
    V.slice(nb_steps) = Cxx.slice(nb_steps);
    v.col(nb_steps) = cx.col(nb_steps);
    v0(nb_steps) = c0(nb_steps);

	for(int i = nb_steps - 1; i>= 0; --i)
	{
        Qxx.slice(i) = - (Cxx.slice(i) + A.slice(i).t() * V.slice(i+1) * A.slice(i)) / alpha(i);
        Quu.slice(i) = - (Cuu.slice(i) + B.slice(i).t() * V.slice(i+1) * B.slice(i)) / alpha(i);
        Qux.slice(i) = - (Cxu.slice(i) + A.slice(i).t() * V.slice(i+1) * B.slice(i)).t() / alpha(i);

        qu.col(i) = - (cu.col(i) + 2.0 * B.slice(i).t() * V.slice(i+1) * c.col(i) + B.slice(i).t() * v.col(i+1)) / alpha(i);
        qx.col(i) = - (cx.col(i) + 2.0 * A.slice(i).t() * V.slice(i+1) * c.col(i) + A.slice(i).t() * v.col(i+1)) / alpha(i);
        q0(i) = - as_scalar(c0(i) + v0(i+1) + c.col(i).t() * V.slice(i+1) * c.col(i)
                            + trace(V.slice(i+1) * sigma_dyn.slice(i)) + v.col(i+1).t() * c.col(i)) / alpha(i);

        if ((Quu.slice(i)).is_sympd()) {
            _diverge = i;
            break;
        }

        Quu_inv.slice(i) = inv(Quu.slice(i));
        K.slice(i) = - Quu_inv.slice(i) * Qux.slice(i);
        kff.col(i) = - 0.5 * Quu_inv.slice(i) * qu.col(i);

        sigma_ctl.slice(i) = - 0.5 * Quu_inv.slice(i);
        sigma_ctl.slice(i) = 0.5 * (sigma_ctl.slice(i).t() + sigma_ctl.slice(i));

        lambda_ctl.slice(i) = - (Quu.slice(i).t() + Quu.slice(i));
        lambda_ctl.slice(i) = 0.5 * (lambda_ctl.slice(i).t() + lambda_ctl.slice(i));

        V.slice(i) = - alpha(i) * (Qxx.slice(i) + Qux.slice(i).t() * K.slice(i));
        V.slice(i) = 0.5 * (V.slice(i) + V.slice(i).t());

        v.col(i) = - alpha(i) * (qx.col(i) + 2. * Qux.slice(i).t() * kff.col(i));
        v0(i) = - alpha(i) * (as_scalar(0.5 * qu.col(i).t() * kff.col(i)) + q0(i)
                              + 0.5 * (dm_act * log (2. * datum::pi) - log(det(- 2. * Quu.slice(i)))));
	}

    // transform outputs to numpy
    array_tf _Qxx = cube_to_array(Qxx);
    array_tf _Qux = cube_to_array(Qux);
    array_tf _Quu = cube_to_array(Quu);

    array_tf _qx = mat_to_array(qx);
    array_tf _qu = mat_to_array(qu);
    array_tf _q0 = mat_to_array(q0);

    array_tf _V = cube_to_array(V);
    array_tf _v = mat_to_array(v);
    array_tf _v0 = vec_to_array(v0);

    array_tf _K = cube_to_array(K);
    array_tf _kff = mat_to_array(kff);
    array_tf _sigma_ctl = cube_to_array(sigma_ctl);

    py::tuple output =  py::make_tuple(_Qxx, _Qux, _Quu, _qx, _qu, _q0,
                                        _V, _v, _v0,
                                        _K, _kff, _sigma_ctl, _diverge);

    return output;
}

PYBIND11_MODULE(core, m)
{
    m.def("kl_divergence", &kl_divergence);
    m.def("quad_expectation", &quad_expectation);
    m.def("augment_cost", &augment_cost);
    m.def("forward_pass", &forward_pass);
    m.def("backward_pass", &backward_pass);
}


/*
! Program for 2D explicit finite element analysis of incompressible Navier-Stokes
!
!
! Author: Dr. Chennakesava Kadapa
! Date  : 17-May-2018
! Place : Swansea, UK
!
!
*/

#include "headersBasic.h"
#include "headersEigen.h"
#include "elementutilitiescfd.h"
#include "SolutionData.h"
#include "BernsteinElem2DINSTria6Node.h"
#include "BernsteinElem2DINSQuad9Node.h"
#include <Eigen/SuperLUSupport>

using namespace std;

int main(int argc, char* argv[])
{
    double tstart, tend;

    int ndim=2, ndof=2, npElem=6;
    double fact, xNode[50], yNode[50];

    vector<vector<int> > ElemDofArray;

    int  nElem, nNode, nDBC, nFBC;
    int  ee, ii, jj, kk, ind, count, row, col;
    int  n1, n2, n3, n4, n5, n6, nsize;
    int  nn, dof;
    int  rr, cc;

    string  infileNodes, infileElems, infileDBCs, infileOutput;
    string  infileFBCs, charTemp, outFileName;

    //Set file names
    //The file names are specified as inputs from the command line
    if(argc == 0)
    {
        cerr << " Error in input data " << endl;
        cerr <<  "Number of input files is not sufficient " << endl;
        cerr <<  "You must enter names of THREE files" << endl;
        cerr <<  "a.) Node file, b.) Element file, and c.) Dirichlet BC file" << endl;
        cerr << "Aborting..." << endl;
    }
    else
    {
       infileNodes = argv[1];
       infileElems = argv[2];
       infileDBCs  = argv[3];

       //if(argc == 5)
         //infileFBCs  = argv[3];

       if(argc == 5)
         infileOutput = argv[4];
    }

    // Read nodal data files
    /////////////////////////////////////

    std::ifstream  infile_nodes(infileNodes);
    std::ifstream  infile_elems(infileElems);
    std::ifstream  infile_DBCs(infileDBCs);
    std::ifstream  infile_FBCs(infileFBCs);

    if(infile_nodes.fail())
    {
       cout << " Could not open the input nodes file " << endl;
       exit(1);
    }

    double  val[50];
    int  val2[50];

    std::string line;

    // read nodal coordinates
    ////////////////////////////////////////////

    cout << " reading nodes " << endl;

    nNode = 0;
    while (std::getline(infile_nodes, line))
      ++nNode;

    vector<vector<double> >  node_coords(nNode, vector<double>(3));

    infile_nodes.clear();
    infile_nodes.seekg(0, infile_nodes.beg);

      ii=0;
      while(infile_nodes >> val[0] >> val[1] >> val[2] )
      {
        //printf("%12.6f \t %12.6f \t %12.6f \n", val[0], val[1], val[2]);

        node_coords[ii][0] = val[1];
        node_coords[ii][1] = val[2];
        node_coords[ii][2] = 0.0;

        ii++;
      }

    // read elements
    ////////////////////////////////////////////
    cout << " reading elements " << endl;

    if(infile_elems.fail())
    {
       cout << " Could not open the input elements file " << endl;
       exit(1);
    }

    nElem = 0;
    while (std::getline(infile_elems, line))
      ++nElem;

    cout << " nElem   " << nElem << endl;

    infile_elems.clear();
    infile_elems.seekg(0, infile_elems.beg);

    vector<vector<int> >  elemNodeConn(nElem, vector<int>(npElem));

    ee=0;
    if(npElem == 6)
    {
      while(infile_elems >> val2[0] >> val2[1] >> val2[2] >> val2[3] >> val2[4] >> val2[5] >> val2[6] >> val2[7] >> val2[8] >> val2[9] )
      {
        //printf("%6d \t %6d \t %6d \t %6d \n", val2[4], val2[5], val2[6], val2[7]);

        for(ii=0; ii<npElem; ii++)
          elemNodeConn[ee][ii] = val2[4+ii]-1;

        ee++;
      }
    }
    else if(npElem == 9)
    {
      while(infile_elems >> val2[0] >> val2[1] >> val2[2] >> val2[3] >> val2[4] >> val2[5] >> val2[6] >> val2[7] >> val2[8] >> val2[9] >> val2[10] >> val2[11] >> val2[12] )
      {
        //printf("%6d \t %6d \t %6d \t %6d \n", val2[4], val2[5], val2[6], val2[7]);

        for(ii=0; ii<npElem; ii++)
          elemNodeConn[ee][ii] = val2[4+ii]-1;

        ee++;
      }
    }
    else
    {
      cerr << " Invalid npElem " << npElem << endl;
      exit(-1);
    }
    //
    // Read Dirichlet BC data
    //
    ////////////////////////////////////////////
    cout << " reading DBCs " << endl;

    if(infile_DBCs.fail())
    {
       cout << " Could not open the input elements file " << endl;
       exit(1);
    }

    vector<vector<double> >  DirichletBCs;
    vector<double>  vecDblTemp(3);

    nDBC = 0;
    while(infile_DBCs >> val[0] >> val[1] >> val[2] )
    {
      vecDblTemp[0] = val[0]-1;
      vecDblTemp[1] = val[1]-1;
      vecDblTemp[2] = val[2];

      //if( val[1] <= ndof )
      //{
        DirichletBCs.push_back(vecDblTemp);
        nDBC++;
      //}
    }

    cout << " nDBC  = " << '\t' << nDBC << endl;

    infile_nodes.close();
    infile_elems.close();
    infile_DBCs.close();

    //
    // Read Output data
    //
    ////////////////////////////////////////////

    vector<int>  OutputData;

    if( !infileOutput.empty() )
    {
      std::ifstream  infile_output(infileOutput);

      cout << " reading output data " << endl;

      if(infile_output.fail())
      {
        cout << " Could not open the input elements file " << endl;
        exit(1);
      }

      ii = 0;
      while (std::getline(infile_output, line))
        ++ii;

      infile_output.clear();
      infile_output.seekg(0, infile_output.beg);

      OutputData.resize(ii);

      ee=0;
      while(infile_output >> val[0] )
      {
        OutputData[ee] = val[0]-1;
        ee++;
      }

      infile_output.close();
    }

    cout << " Input files have been read successfully \n\n " << endl;

      vector<vector<int> >  midNodeData;

      midNodeData.resize(nNode);

      for(ii=0; ii<nNode; ii++)
      {
        midNodeData[ii].resize(3);

        midNodeData[ii][0] = 0;  midNodeData[ii][1] = 0;  midNodeData[ii][2] = 0;
      }

    int nNode_Pres=0;
    vector<int>  pressure_nodes;
    if(npElem == 6)
    {
      for(ee=0; ee<nElem; ee++)
      {
        ii = elemNodeConn[ee][3];
        midNodeData[ii][0] = 1;
        midNodeData[ii][1] = elemNodeConn[ee][0];
        midNodeData[ii][2] = elemNodeConn[ee][1];

        ii = elemNodeConn[ee][4];
        midNodeData[ii][0] = 1;
        midNodeData[ii][1] = elemNodeConn[ee][1];
        midNodeData[ii][2] = elemNodeConn[ee][2];

        ii = elemNodeConn[ee][5];
        midNodeData[ii][0] = 1;
        midNodeData[ii][1] = elemNodeConn[ee][2];
        midNodeData[ii][2] = elemNodeConn[ee][0];

        pressure_nodes.push_back(elemNodeConn[ee][0]);
        pressure_nodes.push_back(elemNodeConn[ee][1]);
        pressure_nodes.push_back(elemNodeConn[ee][2]);
      }
    }
    else if(npElem == 9)
    {
      for(ee=0; ee<nElem; ee++)
      {
        ii = elemNodeConn[ee][4];
        midNodeData[ii][0] = 1;
        midNodeData[ii][1] = elemNodeConn[ee][0];
        midNodeData[ii][2] = elemNodeConn[ee][1];

        ii = elemNodeConn[ee][5];
        midNodeData[ii][0] = 1;
        midNodeData[ii][1] = elemNodeConn[ee][1];
        midNodeData[ii][2] = elemNodeConn[ee][2];

        ii = elemNodeConn[ee][6];
        midNodeData[ii][0] = 1;
        midNodeData[ii][1] = elemNodeConn[ee][2];
        midNodeData[ii][2] = elemNodeConn[ee][3];

        ii = elemNodeConn[ee][7];
        midNodeData[ii][0] = 1;
        midNodeData[ii][1] = elemNodeConn[ee][3];
        midNodeData[ii][2] = elemNodeConn[ee][0];

        pressure_nodes.push_back(elemNodeConn[ee][0]);
        pressure_nodes.push_back(elemNodeConn[ee][1]);
        pressure_nodes.push_back(elemNodeConn[ee][2]);
        pressure_nodes.push_back(elemNodeConn[ee][3]);
      }
    }

    findUnique(pressure_nodes);
    nNode_Pres = pressure_nodes.size();

    vector<int>  pressure_nodes_map(nNode,-1);

    for(ii=0; ii<nNode_Pres; ii++)
    {
      pressure_nodes_map[pressure_nodes[ii]] = ii;
    }

    vector<vector<int> >  ID;
    vector<vector<bool> >  NodeType;
    vector<int>  assyForSoln;

    NodeType.resize(nNode);

    ID.resize(nNode);

    for(ii=0;ii<nNode;ii++)
    {
      NodeType[ii].resize(ndof);
      ID[ii].resize(ndof);

      for(jj=0;jj<ndof;jj++)
      {
        NodeType[ii][jj] = false;
        ID[ii][jj] = -1;
      }
    }

    // fix the pressure at all the mid nodes
    for(ii=0; ii<nNode; ii++)
    {
      if(midNodeData[ii][0])
        NodeType[ii][2] = true;
    }

    if(npElem == 9)
    {
      for(ee=0; ee<nElem; ee++)
      {
        NodeType[elemNodeConn[ee][8]][2] = true;
      }
    }

    // fix the specified Dirichlet BCs
    for(ii=0; ii<nDBC; ii++)
    {
      //cout << ii << '\t' << DirichletBCs[ii][0] << '\t' << DirichletBCs[ii][1] << endl;
      NodeType[DirichletBCs[ii][0]][DirichletBCs[ii][1]] = true;
    }

    //for(ii=0; ii<totalDOF; ii++)
      //cout << ii << '\t' << assyForSoln[ii] << endl;

    int totalDOF = 0;
    for(ii=0;ii<nNode;ii++)
    {
      for(jj=0;jj<ndof;jj++)
      {
        //cout << ii << '\t' << jj << '\t' << NodeType[ii][jj] << endl;
        if(!NodeType[ii][jj])
        {
          ID[ii][jj] = totalDOF++;
          assyForSoln.push_back(ii*ndof+jj);
        }
      }
    }

      cout << " Mesh statistics .....\n" << endl;
      cout << " nElem          = " << '\t' << nElem << endl;
      cout << " nNode          = " << '\t' << nNode  << endl;
      cout << " npElem         = " << '\t' << npElem << endl;
      cout << " ndof           = " << '\t' << ndof << endl;
      cout << " Total DOF      = " << '\t' << totalDOF << endl;

      vector<vector<int> >  LM;

      LM.resize(nElem);

      for(ee=0;ee<nElem;ee++)
      {
        npElem = elemNodeConn[ee].size();

        //printVector(IEN[ee]);

        ind = ndof*npElem;
        LM[ee].resize(ind);

        for(ii=0;ii<npElem;ii++)
        {
          ind = ndof*ii;

          kk = elemNodeConn[ee][ii];

          for(jj=0;jj<ndof;jj++)
          {
            LM[ee][ind+jj] = ID[kk][jj];
          }
        }
      }

      printf("\n element DOF values initialised \n\n");

      /////////////////////////////////////////
      //
      // Eigen based solver
      //
      /////////////////////////////////////////

      cout << " Eigen based solver " << totalDOF << endl;

////////////////////////////////////////////
////////////////////////////////////////////
////////////////////////////////////////////

      double  xx, yy;

      // loop over the nodes and adjust nodal coordinates
      for(nn=0; nn<nNode; nn++)
      {
        if( midNodeData[nn][0] )
        {
          n1 = midNodeData[nn][1];
          n2 = midNodeData[nn][2];

          xx = 0.25*node_coords[n1][0] + 0.25*node_coords[n2][0];
          yy = 0.25*node_coords[n1][1] + 0.25*node_coords[n2][1];

          node_coords[nn][0] = 2.0*(node_coords[nn][0] - xx);
          node_coords[nn][1] = 2.0*(node_coords[nn][1] - yy);
        }
      }

      double  elemData[50];

      //density
      elemData[0] = 1.0;
      //viscosity
      //elemData[1] = 1.0;
      elemData[1] = 0.025;
      //Body force in X-, Y- and Z- direction
      elemData[2] = 0.0;   elemData[3] = 0.0; elemData[4] = 0.0;
      //beta
      elemData[5] = 2.0;

      double  fact1, fact2;

      // create elements and prepare element data
      BernsteinElem2DINSTria6Node   **elems;
      //BernsteinElem2DINSQuad9Node   **elems;

      elems = new BernsteinElem2DINSTria6Node* [nElem];
      //elems = new BernsteinElem2DINSQuad9Node* [nElem];

      for(ee=0;ee<nElem;ee++)
      {
        elems[ee] = new BernsteinElem2DINSTria6Node;
        //elems[ee] = new BernsteinElem2DINSQuad9Node;

        elems[ee]->nodeNums = elemNodeConn[ee];

        //elems[ee]->SolnData = &(SolnData);

        elems[ee]->prepareElemData(node_coords);

        elems[ee]->forAssyVec = LM[ee];
      }

      cout << " elements are created and prepated " << endl;
      cout << " Computing the solution \n" << endl;

      ///////////////////////////////////////////////////////////////
      ///////////////////////////////////////////////////////////////
      ///////////////////////////////////////////////////////////////

     cout << " nNode_Pres = " << nNode_Pres << endl;

      vector<int>  vecTempInt1, vecTempInt2;

    // to compute inf-sup number

    //nNode_Pres = nNode;

    ind = npElem*ndim;
    MatrixXd  Kuu(ind,ind), Kup(ind,9), Kpp(9,9);

    MatrixXd  Kglobal, Mglobal, eigen_vectors;
    VectorXd  eigen_values, eigvec, Flocal, vecTemp;
    MatrixXd  Vmat, Qmat, Bmat;

    Vmat.resize(totalDOF, totalDOF);
    Bmat.resize(totalDOF, nNode_Pres);
    Qmat.resize(totalDOF, totalDOF);

    Kglobal.resize(totalDOF, totalDOF);
    Mglobal.resize(nNode_Pres, nNode_Pres);

    Kglobal.setZero();
    Mglobal.setZero();
    Vmat.setZero();
    Bmat.setZero();
    Qmat.setZero();

    /////////////////////////////////////////
    // compute and assemble matrices
    /////////////////////////////////////////

    for(ee=0; ee<nElem; ee++)  // loop over all the elements
    {
        //cout << "       elem... : " << (ee+1) << endl;

        elems[ee]->toComputeInfSupCondition(node_coords, elemData, Kuu, Kup, Kpp);

        vecTempInt1=elems[ee]->forAssyVec;
        vecTempInt2=elems[ee]->nodeNums;

        n1=vecTempInt1.size();
        n2=3;

        //printVector(vecTempInt1);
        //printVector(vecTempInt2);

        // Kuu
        for(ii=0; ii<n1; ii++)
        {
          rr = vecTempInt1[ii];
          if( rr != -1 )
          {
            for(jj=0; jj<n1; jj++)
            {
              cc = vecTempInt1[jj];
              if( cc != -1 )
              {
                 Kglobal(rr, cc) += Kuu(ii,jj);
              }
            }
          }
        }

        //cout << " aaaaaaaaaaa " << endl;

        // Kup
        for(ii=0; ii<n1; ii++)
        {
          rr = vecTempInt1[ii];
          if( rr != -1 )
          {
            for(jj=0; jj<n2; jj++)
            {
              cc = pressure_nodes_map[vecTempInt2[jj]];
              //cc = vecTempInt2[jj];

              Bmat(rr,cc) += Kup(ii,jj);
            }
          }
        }

        //cout << " aaaaaaaaaaa " << endl;

        // Kpp
        for(ii=0; ii<n2; ii++)
        {
          rr = pressure_nodes_map[vecTempInt2[ii]];
          //rr = vecTempInt2[ii];

          for(jj=0; jj<n2; jj++)
          {
            cc = pressure_nodes_map[vecTempInt2[jj]];
            //cc = vecTempInt2[jj];

            Mglobal(rr, cc) += Kpp(ii,jj);
          }
        }
    }

    cout << "  Solving eigenvalue problem ... " << endl;

    Kglobal = Kglobal.inverse();
    //printMatrix(Vmat);
    //printf("\n\n");
    //printMatrix(globalK);
    //printf("\n\n");
    //printMatrix(globalM);
    //printf("\n\n");
    Kglobal = (Bmat.transpose()*Kglobal)*Bmat;

    GeneralizedSelfAdjointEigenSolver<MatrixXd> es(Kglobal, Mglobal, EigenvaluesOnly);
    eigen_values = es.eigenvalues();
    //eigen_vectors = es.eigenvectors();

    //EigenSolver<MatrixXd>  es(Kglobal, EigenvaluesOnly);
    //cout << "The eigenvalues of A are:" << endl << es.eigenvalues() << endl;
    //eigen_values = es.eigenvalues().col(0);

    cout << "  Eigen analysis successfully completed ... " << endl;

    cout << " The first " << min(20, (int) eigen_values.rows()) << " eigenvalues are ... " << endl;

    for(int ii=0;ii<min(20, (int) eigen_values.rows());ii++)
      printf("\t %5d \t %12.10f \t %12.10f \n", (ii+1), eigen_values(ii), sqrt(abs(eigen_values(ii))));

    if(elems != NULL)
    {
      for(ii=0;ii<nElem;ii++)
        delete elems[ii];

      delete [] elems;
      elems = NULL;
    }

    cout << " Program is successful \n " << endl;

    return 1;
}


#include <gtest/gtest.h>
#include <gmock/gmock.h>
#include <rapidcheck/gtest.h>

#include <crest/geometry/indexed_mesh.hpp>
#include <crest/geometry/refinement.hpp>
#include <crest/basis/lagrange_basis2d.hpp>
#include <crest/basis/homogenized_basis.hpp>
#include <crest/basis/schur_corrector_solver.hpp>
#include <crest/basis/homogenized_basis_dense_fallback.hpp>
#include <crest/basis/amg_corrector_solver.hpp>

#include <crest/util/algorithms.hpp>

#include <util/eigen_matchers.hpp>
#include <util/test_generators.hpp>

#include <Eigen/Sparse>
#include <Eigen/Dense>

using ::testing::ElementsAreArray;
using ::testing::Pointwise;
using ::testing::Eq;
using ::testing::DoubleEq;
using ::testing::Lt;
using ::testing::Each;

using crest::IndexedMesh;
using crest::LagrangeBasis2d;

typedef IndexedMesh<double, int>::Vertex Vertex;
typedef IndexedMesh<double, int>::Element Element;

constexpr static double TOL = 1e-14;

TEST(homogenized_basis_test, correctors_are_in_interpolator_kernel_for_threshold_fine_mesh)
{
    // Recall that by definition, I_H c = 0, for any corrector with weights c.
    const std::vector<Vertex> coarse_vertices {
            Vertex(0.0, 0.0),
            Vertex(1.0, 0.0),
            Vertex(1.0, 1.0),
            Vertex(0.0, 1.0)
    };

    const std::vector<Element> coarse_elements {
            Element({3, 0, 1}),
            Element({1, 2, 3})
    };

    const auto initial_mesh = IndexedMesh<double, int>(coarse_vertices, coarse_elements);

    const auto mesh = crest::threshold(initial_mesh, 0.5, { crest::ReentrantCorner<double, int>(0, 2.35)});
    const auto correctors = crest::SparseLuCorrectorSolver<double>().compute_correctors(mesh, 4);

    ASSERT_THAT(correctors.rows(), Eq(mesh.coarse_mesh().num_vertices()));
    ASSERT_THAT(correctors.cols(), Eq(mesh.fine_mesh().num_vertices()));

    const auto coarse_interior = mesh.coarse_mesh().compute_interior_vertices();
    const auto fine_dof = crest::algo::integer_range<int>(0, mesh.fine_mesh().num_vertices());

    const auto I_H = crest::quasi_interpolator(mesh);
    // We need to test with the coarse interior, because the matrix returned by quasi_interpolator
    // does not impose that I_H x = 0 on the boundary.
    const auto I_H_interior = sparse_submatrix(I_H, coarse_interior, fine_dof);

    // Recall that each row of 'basis' corresponds to weights in the fine space, so by transposing it and
    // left-multiplying by I_H, we effectively compute I_H x_i for each basis function i.
    // Since x_i is in the kernel of I_H, we expect the result to be zero.
    const Eigen::SparseMatrix<double> Z = I_H_interior * correctors.transpose();
    const Eigen::MatrixXd Z_dense = Z;

    EXPECT_TRUE(Z_dense.isZero(TOL));
}

TEST(construct_saddle_point_problem_test, blockwise_correct)
{
    MatrixX<double> A(5, 5);
    A.setZero();
    A(0, 0) = 1.0;
    A(1, 1) = 2.0;
    A(2, 2) = 3.0;
    A(3, 3) = 4.0;
    A(4, 4) = 5.0;

    MatrixX<double> I_H(3, 5);
    I_H.setZero();
    I_H(0, 0) = 6.0;
    I_H(1, 1) = 7.0;
    I_H(2, 2) = 8.0;

    const Eigen::SparseMatrix<double> A_sparse = A.sparseView();
    const Eigen::SparseMatrix<double> I_H_sparse = I_H.sparseView();

    const MatrixX<double> C = crest::detail::construct_saddle_point_problem(A_sparse, I_H_sparse);

    ASSERT_THAT(C.rows(), 8);
    ASSERT_THAT(C.cols(), 8);

    const MatrixX<double> bottomRightCorner = C.bottomRightCorner(3, 3);
    const auto bottomRightCornerElements = std::vector<double>(bottomRightCorner.data(), bottomRightCorner.data() + 9);

    EXPECT_TRUE(C.topLeftCorner(5, 5).isApprox(A));
    EXPECT_TRUE(C.bottomLeftCorner(3, 5).isApprox(I_H));
    EXPECT_TRUE(C.topRightCorner(5, 3).isApprox(I_H.transpose()));
    EXPECT_THAT(bottomRightCornerElements, Each(DoubleEq(0.0)));
}

template <typename CorrectorSolver>
void check_correctors_are_in_interpolator_kernel()
{
    const auto oversampling = *rc::gen::arbitrary<unsigned int>();
    const auto coarse_mesh = *crest::gen::arbitrary_unit_square_mesh(4);

    // Make sure the fine mesh does not coincide with the coarse mesh
    const auto fine_mesh = *rc::gen::suchThat(crest::gen::arbitrary_refinement(coarse_mesh, 1, 4),
                                              [coarse_mesh] (auto mesh)
                                              {
                                                  return mesh.num_elements() != coarse_mesh.num_elements();
                                              });

    const auto biscale = crest::BiscaleMesh<double, int>(coarse_mesh, fine_mesh);

    const auto correctors = CorrectorSolver().compute_correctors(biscale, oversampling);

    ASSERT_THAT(correctors.rows(), Eq(coarse_mesh.num_vertices()));
    ASSERT_THAT(correctors.cols(), Eq(fine_mesh.num_vertices()));

    const auto coarse_interior = coarse_mesh.compute_interior_vertices();
    const auto fine_dof = crest::algo::integer_range<int>(0, fine_mesh.num_vertices());

    const auto I_H = crest::quasi_interpolator(biscale);

    // We need to test with the coarse interior, because the matrix returned by quasi_interpolator
    // does not impose that I_H x = 0 on the boundary.
    const auto I_H_interior = sparse_submatrix(I_H, coarse_interior, fine_dof);

    // Recall that each row of 'basis' corresponds to weights in the fine space, so by transposing it and
    // left-multiplying by I_H, we effectively compute I_H x_i for each basis function i.
    // Since x_i is in the kernel of I_H, we expect the result to be zero.
    const Eigen::SparseMatrix<double> Z = I_H_interior * correctors.transpose();
    const Eigen::MatrixXd Z_dense = Z;

    RC_ASSERT(Z_dense.isZero(TOL));
}

RC_GTEST_PROP(homogenized_basis_test, correctors_are_in_interpolator_kernel_sparselu, ())
{
    check_correctors_are_in_interpolator_kernel<crest::SparseLuCorrectorSolver<double>>();
}

RC_GTEST_PROP(homogenized_basis_test, correctors_are_in_interpolator_kernel_schur, ())
{
    check_correctors_are_in_interpolator_kernel<crest::SchurCorrectorSolver<double>>();
}

RC_GTEST_PROP(homogenized_basis_test, correctors_are_in_interpolator_kernel_sparselu_dense_fallback, ())
{
    typedef crest::DenseFallbackCorrectorSolverWrapper<double, crest::SparseLuCorrectorSolver<double>> Wrapper;
    check_correctors_are_in_interpolator_kernel<Wrapper>();
}

RC_GTEST_PROP(homogenized_basis_test, correctors_are_in_interpolator_kernel_amg, ())
{
    check_correctors_are_in_interpolator_kernel<crest::AmgCorrectorSolver<double>>();
}

template <typename CorrectorSolver>
void check_correctors_are_zero_with_no_refinement()
{
    const auto oversampling = static_cast<unsigned int>(*rc::gen::inRange(0, 6));
    const auto mesh = *crest::gen::arbitrary_unit_square_mesh();
    const auto biscale = crest::BiscaleMesh<double, int>(mesh, mesh);
    const auto correctors = CorrectorSolver().compute_correctors(biscale, oversampling);
    ASSERT_THAT(correctors.rows(), Eq(mesh.num_vertices()));
    ASSERT_THAT(correctors.cols(), Eq(mesh.num_vertices()));

    double max_abs = 0.0;
    for (int o = 0; o < correctors.outerSize(); ++o)
    {
        for (Eigen::SparseMatrix<double>::InnerIterator it(correctors, o); it; ++it)
        {
            max_abs = std::max(max_abs, std::abs(it.value()));
        }
    }

    RC_ASSERT(max_abs < TOL);
}

RC_GTEST_PROP(homogenized_basis_test, correctors_are_zero_with_no_refinement_sparselu, ())
{
    check_correctors_are_zero_with_no_refinement<crest::SparseLuCorrectorSolver<double>>();
}

RC_GTEST_PROP(homogenized_basis_test, correctors_are_zero_with_no_refinement_schur, ())
{
    check_correctors_are_zero_with_no_refinement<crest::SchurCorrectorSolver<double>>();
}

RC_GTEST_PROP(homogenized_basis_test, correctors_are_zero_with_no_refinement_sparselu_dense_fallback, ())
{
    typedef crest::DenseFallbackCorrectorSolverWrapper<double, crest::SparseLuCorrectorSolver<double>> Wrapper;
    check_correctors_are_zero_with_no_refinement<Wrapper>();
}

RC_GTEST_PROP(homogenized_basis_test, correctors_are_zero_with_no_refinement_amg, ())
{
    check_correctors_are_zero_with_no_refinement<crest::AmgCorrectorSolver<double>>();
}

template <typename CorrectorSolver>
void check_corrected_basis_is_orthogonal_to_fine_space()
{
    // Let W be such that W_ij corrected basis function i is defined by
    //
    // lambda_i = sum_j W_ij phi_j
    //
    // where phi_j denotes the fine-scale basis functions. Then the a-orthogonality of the corrected space V_H and
    // the fine-scale finite element space implies that
    //
    // W A N = 0
    //
    // where A is the fine-scale stiffness matrix (interior nodes) and N is a basis for W_H = ker(I_H).
    // In real applications, obtaining a basis for W_H is normally infeasible due to sparsity requirements.
    // However, since we only do small-scale tests here, we have the luxury of just simply building a dense
    // basis through the SVD.
    //
    // Our corrected basis is constructed by a localized approximation, so the above property does not hold exactly.
    // We can, however, choose an oversampling parameter so large that each corrector problem is a global problem.
    // In this case, the above property must still hold.

    // Make sure interior is not empty, because Eigen's SVD fails on empty matrices
    const auto coarse = *rc::gen::suchThat(crest::gen::arbitrary_unit_square_mesh(),
                                           [] (const auto & mesh) {
                                               return !mesh.compute_interior_vertices().empty();
                                           }).as("coarse mesh");
    const auto fine = *crest::gen::arbitrary_refinement(coarse).as("fine mesh");
    const auto biscale = crest::BiscaleMesh<double, int>(coarse, fine);

    const auto fine_interior = fine.compute_interior_vertices();
    const auto coarse_interior = coarse.compute_interior_vertices();
    const auto fine_basis = LagrangeBasis2d<double>(fine);

    const auto oversampling = static_cast<unsigned int>(coarse.num_vertices());
    const auto basis_weights = CorrectorSolver()
            .compute_basis(biscale, oversampling).basis_weights();
    const auto basis_interior_weights = sparse_submatrix(basis_weights, coarse_interior, fine_interior);

    // Construct a basis for W_H, the kernel of I_H.
    const Eigen::MatrixXd I_H = sparse_submatrix(crest::quasi_interpolator(biscale), coarse_interior, fine_interior);
    const Eigen::JacobiSVD<Eigen::MatrixXd> svd(I_H, Eigen::ComputeFullV);
    const auto r = svd.rank();
    const auto n = I_H.cols();
    const Eigen::MatrixXd W_H_kernel_basis = svd.matrixV().rightCols(n - r);

    const Eigen::MatrixXd W = basis_interior_weights;
    const Eigen::MatrixXd A = sparse_submatrix(fine_basis.assemble().stiffness, fine_interior, fine_interior);
    const auto & N = W_H_kernel_basis;

    const Eigen::MatrixXd product = W * A * N;

    RC_LOG() << "W:   " << std::endl << W << std::endl << std::endl;
    RC_LOG() << "A:   " << std::endl << A << std::endl << std::endl;
    RC_LOG() << "N:   " << std::endl << N << std::endl << std::endl;
    RC_LOG() << "WAN: " << std::endl << product << std::endl << std::endl;

    RC_ASSERT(product.isZero(TOL));
}

RC_GTEST_PROP(homogenized_basis_test, corrected_basis_is_orthogonal_to_fine_space_sparselu, ())
{
    check_corrected_basis_is_orthogonal_to_fine_space<crest::SparseLuCorrectorSolver<double>>();
}

RC_GTEST_PROP(homogenized_basis_test, corrected_basis_is_orthogonal_to_fine_space_schur, ())
{
    check_corrected_basis_is_orthogonal_to_fine_space<crest::SchurCorrectorSolver<double>>();
}

RC_GTEST_PROP(homogenized_basis_test, corrected_basis_is_orthogonal_to_fine_space_sparselu_dense_fallback, ())
{
    typedef crest::DenseFallbackCorrectorSolverWrapper<double, crest::SparseLuCorrectorSolver<double>> Wrapper;
    check_corrected_basis_is_orthogonal_to_fine_space<Wrapper>();
}

RC_GTEST_PROP(homogenized_basis_test, corrected_basis_is_orthogonal_to_fine_space_amg, ())
{
    check_corrected_basis_is_orthogonal_to_fine_space<crest::AmgCorrectorSolver<double>>();
}

TEST(standard_coarse_basis_in_fine_space, basic_mesh)
{
    const std::vector<Vertex> coarse_vertices{
            Vertex(0.0, 0.0),
            Vertex(1.0, 0.0),
            Vertex(1.0, 1.0),
            Vertex(0.0, 1.0)
    };

    const std::vector<Element> coarse_elements {
            Element({3, 0, 1}),
            Element({1, 2, 3})
    };

    const std::vector<Vertex> fine_vertices {
            Vertex(0.0, 0.0),
            Vertex(1.0, 0.0),
            Vertex(1.0, 1.0),
            Vertex(0.0, 1.0),
            Vertex(0.5, 0.5)
    };

    const std::vector<Element> fine_elements {
            Element({0, 4, 3}),
            Element({2, 4, 1}),
            Element({1, 4, 0}),
            Element({2, 4, 3})
    };

    const std::vector<int> fine_ancestry { 0, 1, 0, 1 };

    const auto coarse_mesh = IndexedMesh<double, int>(coarse_vertices, coarse_elements);
    const auto fine_mesh = IndexedMesh<double, int>(fine_vertices, fine_elements, fine_ancestry);
    const auto biscale = crest::BiscaleMesh<double, int>(coarse_mesh, fine_mesh);

    const Eigen::Matrix<double, 4, 5> coarse_basis_in_fine =
            crest::detail::standard_coarse_basis_in_fine_space(biscale);

    Eigen::Matrix<double, 4, 5> expected;
    expected <<
             1.0, 0.0, 0.0, 0.0, 0.0,
            0.0, 1.0, 0.0, 0.0, 0.5,
            0.0, 0.0, 1.0, 0.0, 0.0,
            0.0, 0.0, 0.0, 1.0, 0.5;

    ASSERT_TRUE(coarse_basis_in_fine.isApprox(expected));
}

RC_GTEST_PROP(standard_coarse_basis_in_fine_space, quasi_interpolation_recovers_coarse_basis, ())
{
    // Let v_H be the weights of a function in the coarse space. Then v_H is also a function in the fine space,
    // and since I_H is a quasi interpolation, we have that I_H v_h = v_H, where v_h are the fine weights of
    // the coarse function.
    // We can verify that this property holds for all basis functions as represented in the fine space.

    const auto coarse = *crest::gen::arbitrary_unit_square_mesh().as("Coarse mesh");
    const auto fine = *crest::gen::arbitrary_refinement(coarse, 0, 5).as("Fine mesh");
    const auto biscale = crest::BiscaleMesh<double, int>(coarse, fine);

    const auto basis = crest::detail::standard_coarse_basis_in_fine_space(biscale);
    const auto I_H = crest::quasi_interpolator(biscale);

    const Eigen::SparseMatrix<double> interpolated = I_H * basis.transpose();
    const Eigen::MatrixXd interpolated_dense = interpolated;

    RC_LOG() << interpolated_dense << std::endl;

    // We could have had a shaper tolerance here, but it appears Eigen does not do an elementwise comparison
    // for isIdentity (which is very annoying, by the way), as it can spuriously fail even when all non-diagonal
    // elements are smaller than 3e-16.
    RC_ASSERT(interpolated_dense.isIdentity(TOL));
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_DETAIL_CONSTANT_PIO_2LO_HPP_INCLUDED
#define BOOST_SIMD_DETAIL_CONSTANT_PIO_2LO_HPP_INCLUDED

#include <boost/simd/config.hpp>
#include <boost/simd/detail/brigand.hpp>
#include <boost/simd/detail/dispatch.hpp>
#include <boost/simd/detail/constant_traits.hpp>
#include <boost/simd/detail/dispatch/function/make_callable.hpp>
#include <boost/simd/detail/dispatch/hierarchy/functions.hpp>
#include <boost/simd/detail/dispatch/as.hpp>

/*

    @ingroup group-constant

    This constant is such that, for pairs of types (T, Tup)
    (namely (float,  double) and (double, long double)) the sum:

    abs(Tup(Pio_2lo<T>())+Tup(Pio_2<T>())-Pio_2\< Tup \>()) is  less than
    a few Eps<Tup>().

    This is used to improve accurracy when computing sums of the kind
    \f$\pi/2 + x\f$ with x small,  by replacing them by
    Pio_2 + (Pio_2lo + x)

    @par Semantic:

    For type T:

    @code
    T r = Pio_2lo<T>();
    @endcode

    is similar to:

    @code
    if T is double
      r = 6.123233995736766e-17
    else if T is float
      r = -4.3711388e-08
    @endcode

    @return a value of type T

*/

namespace boost { namespace simd
{
  namespace tag
  {
    struct pio_2lo_ : boost::dispatch::constant_value_<pio_2lo_>
    {
      BOOST_DISPATCH_MAKE_CALLABLE(ext,pio_2lo_,boost::dispatch::constant_value_<pio_2lo_>);
      BOOST_SIMD_REGISTER_CONSTANT(0, 0XB33BBD2EUL, 0X3C91A62633145C07ULL);
    };
  }

  namespace ext
  {
    BOOST_DISPATCH_FUNCTION_DECLARATION(tag, pio_2lo_)
  }

  namespace detail
  {
    BOOST_DISPATCH_CALLABLE_DEFINITION(tag::pio_2lo_,pio_2lo);
  }

  template<typename T> BOOST_FORCEINLINE auto Pio_2lo()
  BOOST_NOEXCEPT_DECLTYPE(detail::pio_2lo( boost::dispatch::as_<T>{}))
  {
    return detail::pio_2lo( boost::dispatch::as_<T>{} );
  }
} }

#include <boost/simd/arch/common/scalar/constant/constant_value.hpp>
#include <boost/simd/arch/common/simd/constant/constant_value.hpp>

#endif


#pragma once

#ifndef _MSC_VER
extern "C" {
#include <csim/type.h>
}
#else
#include <csim/type.h>
#endif

#include <complex>
#include <Eigen/Core>
#include <Eigen/Sparse>
typedef std::complex<double> CPPCTYPE;
typedef Eigen::VectorXcd ComplexVector;

// In order to use matrix raw-data without reordering, we use RowMajor as default.
typedef Eigen::Matrix<CPPCTYPE, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> ComplexMatrix;
typedef Eigen::SparseMatrix<CPPCTYPE> SparseComplexMatrix;

#ifdef __GNUC__
#if __GNUC__ >= 8
using namespace std::complex_literals;
#endif
#endif

#ifdef _MSC_VER
inline static FILE* popen(const char* command, const char* mode) { return _popen(command, mode); }
inline static void pclose(FILE* fp) { _pclose(fp);  }
#define strncasecmp _strnicmp
#define strcasecmp _stricmp
#endif

#if defined(__MINGW32__) || defined(_MSC_VER)
#define DllExport __declspec(dllexport)
#else
#define DllExport __attribute__((visibility ("default")))
#endif