#include <boost/lexical_cast.hpp>
#include <boost/filesystem.hpp>
#include <boost/numeric/ublas/lu.hpp>

#include "flame/constants.h"
#include "flame/moment.h"

#define sqr(x)  ((x)*(x))
#define cube(x) ((x)*(x)*(x))

#ifdef DEFPATH
    #define defpath DEFPATH
#else
    #define defpath "."
#endif

std::map<std::string,boost::shared_ptr<Config> > CurveMap;

// http://www.crystalclearsoftware.com/cgi-bin/boost_wiki/wiki.pl?LU_Matrix_Inversion
// by LU-decomposition.
void inverse(MomentElementBase::value_t& out, const MomentElementBase::value_t& in)
{
    using boost::numeric::ublas::permutation_matrix;
    using boost::numeric::ublas::lu_factorize;
    using boost::numeric::ublas::lu_substitute;
    using boost::numeric::ublas::identity_matrix;

    MomentElementBase::value_t scratch(in); // copy
    permutation_matrix<size_t> pm(scratch.size1());
    if(lu_factorize(scratch, pm)!=0)
        throw std::runtime_error("Failed to invert matrix");
    out.assign(identity_matrix<double>(scratch.size1()));
    //out = identity_matrix<double>(scratch.size1());
    lu_substitute(scratch, pm, out);
}

void RotMat(const double dx, const double dy,
            const double theta_x, const double theta_y, const double theta_z,
            typename MomentElementBase::value_t &R)
{
    typedef typename MomentElementBase::state_t state_t;

    MomentState::matrix_t T = boost::numeric::ublas::identity_matrix<double>(state_t::maxsize);

    R = boost::numeric::ublas::identity_matrix<double>(state_t::maxsize);

    // Left-handed coordinate system => theta_y -> -theta_y.

    double m11 =  cos(-theta_y)*cos(theta_z),
           m12 =  sin(theta_x)*sin(-theta_y)*cos(theta_z) + cos(theta_x)*sin(theta_z),
           m13 = -cos(theta_x)*sin(-theta_y)*cos(theta_z) + sin(theta_x)*sin(theta_z),

           m21 = -cos(-theta_y)*sin(theta_z),
           m22 = -sin(theta_x)*sin(-theta_y)*sin(theta_z) + cos(theta_x)*cos(theta_z),
           m23 =  cos(theta_x)*sin(-theta_y)*sin(theta_z) + sin(theta_x)*cos(theta_z),

           m31 =  sin(-theta_y),
           m32 = -sin(theta_x)*cos(-theta_y),
           m33 =  cos(theta_x)*cos(-theta_y);

    R(0, 0) = m11, R(0, 2) = m12, R(0, 4) = m13;
    R(2, 0) = m21, R(2, 2) = m22, R(2, 4) = m23;
    R(4, 0) = m31, R(4, 2) = m32, R(4, 4) = m33;

    R(1, 1) = m11, R(1, 3) = m12, R(1, 5) = m13;
    R(3, 1) = m21, R(3, 3) = m22, R(3, 5) = m23;
    R(5, 1) = m31, R(5, 3) = m32, R(5, 5) = m33;

    T(0, 6) = -dx, T(2, 6) = -dy;

    R = prod(R, T);
}

void GetQuadMatrix(const double L, const double K, const unsigned ind, typename MomentElementBase::value_t &M)
{
    // 2D quadrupole transport matrix.
    double sqrtK,
           psi,
           cs,
           sn;

    if (K > 0e0) {
        // Focusing.
        sqrtK = sqrt(K);
        psi = sqrtK*L;
        cs = ::cos(psi);
        sn = ::sin(psi);

        M(ind, ind) = M(ind+1, ind+1) = cs;
        if (sqrtK != 0e0)
            M(ind, ind+1) = sn/sqrtK;
        else
            M(ind, ind+1) = L;
        if (sqrtK != 0e0)
            M(ind+1, ind) = -sqrtK*sn;
        else
            M(ind+1, ind) = 0e0;
    } else {
        // Defocusing.
        sqrtK = sqrt(-K);
        psi = sqrtK*L;
        cs = ::cosh(psi);
        sn = ::sinh(psi);

        M(ind, ind) = M(ind+1, ind+1) = cs;
        if (sqrtK != 0e0)
            M(ind, ind+1) = sn/sqrtK;
        else
            M(ind, ind+1) = L;
        if (sqrtK != 0e0)
            M(ind+1, ind) = sqrtK*sn;
        else
            M(ind+1, ind) = 0e0;
    }
}

void GetSextMatrix(const double L, const double K3, double Dx, double Dy,
                   const double D2x, const double D2y, const double D2xy, const bool thinlens, const bool dstkick, typename MomentElementBase::value_t &M)
{
    typedef typename MomentElementBase::state_t state_t;
    // 2D sextupole transport matrix.
    double sqrtK, psi, cs, sn, ch, sh,
           dr = sqrt(sqr(Dx)+sqr(Dx));

    if (thinlens) {

        //thin-lens (drift-kick-drift) model

        MomentState::matrix_t T = boost::numeric::ublas::identity_matrix<double>(state_t::maxsize);
        MomentState::matrix_t P = boost::numeric::ublas::identity_matrix<double>(state_t::maxsize);
        MomentState::matrix_t scratch;

        T(state_t::PS_X, state_t::PS_PX) = L/2e0;
        T(state_t::PS_Y, state_t::PS_PY) = L/2e0;

        P(state_t::PS_PX, state_t::PS_X) = -K3*L*Dx;
        P(state_t::PS_PX, state_t::PS_Y) =  K3*L*Dy;

        P(state_t::PS_PY, state_t::PS_X) = K3*L*Dy;
        P(state_t::PS_PY, state_t::PS_Y) = K3*L*Dx;

        scratch = prod(P,T);
        M = prod(T,scratch);

    } else {

        //thick-lens model

        sqrtK = sqrt(fabs(K3)*dr);
        psi = sqrtK*L;

        // Focusing or Defocusing switch
        Dx *= copysign(1e0,K3);
        Dy *= copysign(1e0,K3);

        cs = ::cos(psi);
        sn = ::sin(psi);
        ch = ::cosh(psi);
        sh = ::sinh(psi);

        if (sqrtK != 0e0) {
            M(state_t::PS_X, state_t::PS_X) = M(state_t::PS_PX, state_t::PS_PX) = ((dr+Dx)*cs+(dr-Dx)*ch)/(2e0*dr);
            M(state_t::PS_X, state_t::PS_PX) = ((dr+Dx)*sn+(dr-Dx)*sh)/(2e0*sqrtK*dr);
            M(state_t::PS_X, state_t::PS_Y)  = M(state_t::PS_PX, state_t::PS_PY) =
            M(state_t::PS_Y, state_t::PS_X)  = M(state_t::PS_PY, state_t::PS_PX) = Dy*(-cs+ch)/(2e0*dr);
            M(state_t::PS_X, state_t::PS_PY) = Dy*(-sn+sh)/(2e0*sqrtK*dr);

            M(state_t::PS_PX, state_t::PS_X) = sqrtK*(-(dr+Dx)*sn+(dr-Dx)*sh)/(2e0*dr);
            M(state_t::PS_PX, state_t::PS_Y) = M(state_t::PS_PY, state_t::PS_X) =  Dy*sqrtK*(sn+sh)/(2e0*dr);

            M(state_t::PS_Y, state_t::PS_PX) = Dy*(-sn+sh)/(2e0*sqrtK*dr);
            M(state_t::PS_Y, state_t::PS_Y) = M(state_t::PS_PY, state_t::PS_PY) = ((dr-Dx)*cs+(dr+Dx)*ch)/(2e0*dr);
            M(state_t::PS_Y, state_t::PS_PY) = ((dr-Dx)*sn+(dr+Dx)*sh)/(2e0*sqrtK*dr);

            M(state_t::PS_PY, state_t::PS_Y) = sqrtK*(-(dr-Dx)*sn+(dr+Dx)*sh)/(2e0*dr);

        } else {
            M(state_t::PS_X, state_t::PS_PX) = L;
            M(state_t::PS_Y, state_t::PS_PY) = L;
        }

    } // option
    if (dstkick){
        M(state_t::PS_PX, 6) = -K3*L*(D2x-D2y);
        M(state_t::PS_PY, 6) =  2e0*K3*L*D2xy;
    }
}

void GetEdgeMatrix(const double rho, const double phi, typename MomentElementBase::value_t &M)
{
    typedef typename MomentElementBase::state_t state_t;

    M = boost::numeric::ublas::identity_matrix<double>(state_t::maxsize);

    M(state_t::PS_PX, state_t::PS_X) =  tan(phi)/rho;
    M(state_t::PS_PY, state_t::PS_Y) = -tan(phi)/rho;
}

void GetEEdgeMatrix(const double fringe_x, const double fringe_y, const double kappa, typename MomentElementBase::value_t &M)
{
    // Edge focusing for electrostatic dipole.
    typedef typename MomentElementBase::state_t state_t;

    M = boost::numeric::ublas::identity_matrix<double>(state_t::maxsize);

    M(state_t::PS_PX, state_t::PS_X)  = fringe_x;
    M(state_t::PS_PX, state_t::PS_PX) = sqrt(1e0+kappa);
    M(state_t::PS_PY, state_t::PS_Y)  = fringe_y;
    M(state_t::PS_PS, state_t::PS_PS) = 1e0+kappa;
}

void GetSBendMatrix(const double L, const double phi, const double phi1, const double phi2, const double K,
                    const double IonEs, const double ref_gamma, const double qmrel,
                    const double dip_beta, const double dip_gamma, const double d, const double dip_IonK, typename MomentElementBase::value_t &M)
{
    typedef typename MomentElementBase::state_t state_t;

    MomentState::matrix_t edge1, edge2, R;

    double  rho = L/phi,
            Kx  = K + 1e0/sqr(rho),
            Ky  = -K,
            dx  = 0e0,
            sx  = 0e0;

    // Horizontal plane.
    GetQuadMatrix(L, Kx, (unsigned)state_t::PS_X, M);
    // Vertical plane.
    GetQuadMatrix(L, Ky, (unsigned)state_t::PS_Y, M);

    // Include dispersion.
    if (Kx == 0e0) {
        dx = sqr(L)/(2e0*rho);
        sx = L/rho;
    } else if (Kx > 0e0) {
        dx = (1e0-cos(sqrt(Kx)*L))/(rho*Kx);
        sx = sin(sqrt(Kx)*L)/(rho*sqrt(Kx));
    } else {
        dx = (1e0-cosh(sqrt(-Kx)*L))/(rho*Kx);
        sx = sin(sqrt(-Kx)*L)/(rho*sqrt(-Kx));
    }

    M(state_t::PS_X,  state_t::PS_PS) = dx/(sqr(dip_beta)*dip_gamma*IonEs/MeVtoeV);
    M(state_t::PS_PX, state_t::PS_PS) = sx/(sqr(dip_beta)*dip_gamma*IonEs/MeVtoeV);

    M(state_t::PS_S,  state_t::PS_X)  = sx*dip_IonK;
    M(state_t::PS_S,  state_t::PS_PX) = dx*dip_IonK;
    // Low beta approximation.
    M(state_t::PS_S,  state_t::PS_PS) =
            ((L/rho-sx)/(Kx*rho)-L/sqr(ref_gamma))*dip_IonK
            /(sqr(dip_beta)*dip_gamma*IonEs/MeVtoeV);

    // Add dipole terms.
    M(state_t::PS_S,  6) = ((L/rho-sx)/(Kx*rho)*d-L/sqr(ref_gamma)*(d+qmrel))*dip_IonK;
    M(state_t::PS_X,  6) = dx*d;
    M(state_t::PS_PX, 6) = sx*d;

    // Edge focusing.
    GetEdgeMatrix(rho, phi1, edge1);
    GetEdgeMatrix(rho, phi2, edge2);

    M = prod(M, edge1);
    M = prod(edge2, M);

    // Longitudinal plane.
    // For total path length.
    //        M(state_t::PS_S,  state_t::PS_S) = L;
}

void GetSolMatrix(const double L, const double K, typename MomentElementBase::value_t &M)
{
    typedef typename MomentElementBase::state_t state_t;

    double C = ::cos(K*L),
           S = ::sin(K*L);

    M(state_t::PS_X, state_t::PS_X)
            = M(state_t::PS_PX, state_t::PS_PX)
            = M(state_t::PS_Y, state_t::PS_Y)
            = M(state_t::PS_PY, state_t::PS_PY)
            = sqr(C);

    if (K != 0e0)
        M(state_t::PS_X, state_t::PS_PX) = S*C/K;
    else
        M(state_t::PS_X, state_t::PS_PX) = L;
    M(state_t::PS_X, state_t::PS_Y) = S*C;
    if (K != 0e0)
        M(state_t::PS_X, state_t::PS_PY) = sqr(S)/K;
    else
        M(state_t::PS_X, state_t::PS_PY) = 0e0;

    M(state_t::PS_PX, state_t::PS_X)  = -K*S*C;
    M(state_t::PS_PX, state_t::PS_Y)  = -K*sqr(S);
    M(state_t::PS_PX, state_t::PS_PY) = S*C;

    M(state_t::PS_Y, state_t::PS_X)   = -S*C;
    if (K != 0e0)
        M(state_t::PS_Y, state_t::PS_PX) = -sqr(S)/K;
    else
        M(state_t::PS_Y, state_t::PS_PX) = 0e0;
    if (K != 0e0)
        M(state_t::PS_Y, state_t::PS_PY) = S*C/K;
    else
        M(state_t::PS_Y, state_t::PS_PY) = L;

    M(state_t::PS_PY, state_t::PS_X)  = K*sqr(S);
    M(state_t::PS_PY, state_t::PS_PX) = -S*C;
    M(state_t::PS_PY, state_t::PS_Y)  = -K*S*C;

    // Longitudinal plane.
    // For total path length.
//        M(state_t::PS_S, state_t::PS_S) = L;
}

void GetEBendMatrix(const double L, const double phi, const double fringe_x, const double fringe_y, const double kappa,
                    const double Kx, const double Ky, const double IonEs, const double real_gamma, const double eta0, const double h,
                    const double delta_K, const double delta_KZ, const double SampleIonK, typename MomentElementBase::value_t &M)
{
    typedef typename MomentElementBase::state_t state_t;

    MomentState::matrix_t edge;

    double  rho = L/phi,
            scl = (real_gamma - 1e0)*IonEs/MeVtoeV,
            dx  = 0e0,
            sx  = 0e0;

    // Horizontal plane.
    GetQuadMatrix(L, Kx, (unsigned)state_t::PS_X, M);
    // Vertical plane.
    GetQuadMatrix(L, Ky, (unsigned)state_t::PS_Y, M);

    // Include dispersion.
    if (Kx == 0e0) {
        dx = 0e0;
        sx = 0e0;
    } else if (Kx > 0e0) {
        dx = (1e0-cos(sqrt(Kx)*L))/(rho*Kx);
        sx = sin(sqrt(Kx)*L)/sqrt(Kx);
    } else {
        dx = (1e0-cosh(sqrt(-Kx)*L))/(rho*Kx);
        sx = sin(sqrt(Kx)*L)/sqrt(Kx);
    }

    double Nk   = (sqr(1e0+2e0*eta0)+h)/(2e0*(1e0+eta0)*(1e0+2e0*eta0)),
           Nt   = 1e0 + h/sqr(1e0+2e0*eta0),
           CorT = -real_gamma/(1e0+real_gamma),
           tx   = sx/rho*Nt*CorT,
           txp  = dx*Nt*CorT,
           tzp  = (-L/(2e0*(1e0+eta0)*(1e0+2e0*eta0))+((L-sx)/Kx/sqr(rho))*Nk*Nt)*CorT;

    M(state_t::PS_X,  state_t::PS_PS) = dx*Nk/scl;
    M(state_t::PS_PX, state_t::PS_PS) = sx/rho*Nk/scl;

    M(state_t::PS_S,  state_t::PS_X)  = -tx*SampleIonK;
    M(state_t::PS_S,  state_t::PS_PX) = -txp*SampleIonK;
    // Low beta approximation.
    M(state_t::PS_S,  state_t::PS_PS) = -tzp*SampleIonK/scl;

    // Add dipole terms.
    double Nkz = (1e0+2e0*eta0-h)/(2e0*(1e0+eta0)*(1e0+2e0*eta0));

    M(state_t::PS_X,  6) = dx*(Nk*delta_K+Nkz*delta_KZ);
    M(state_t::PS_PX, 6) = sx/rho*(Nk*delta_K+Nkz*delta_KZ);

    // Edge focusing.
    GetEEdgeMatrix(fringe_x, fringe_y ,kappa, edge);

    M = prod(M, edge);
    M = prod(edge, M);

    // Longitudinal plane.
    // For total path length.
    //        M(state_t::PS_S,  state_t::PS_S) = L;
}

void GetCurveData(const Config &c, const unsigned ncurve, std::vector<double> &Scales,
                  std::vector<std::vector<double> > &Curves)
{
    boost::shared_ptr<Config> conf;

    std::string filename;
    std::vector<double> range;
    bool checker = c.tryGet<std::string>("CurveFile", filename),
         rngchecker = c.tryGet<std::vector<double> >("use_range", range);

    if (checker){
        std::string CurveFile =  c.get<std::string>("Eng_Data_Dir", defpath);
        CurveFile += "/" + filename;
        std::string key(SB()<<CurveFile<<"|"<<boost::filesystem::last_write_time(CurveFile));
        if ( CurveMap.find(key) == CurveMap.end() ) {
            // not found in CurveMap
            try {
                try {
                    GLPSParser P;
                    conf.reset(P.parse_file(CurveFile.c_str(), false));
                }catch(std::exception& e){
                    throw std::runtime_error(SB()<<"Parse error: "<<e.what()<<"\n");
                }

            }catch(std::exception& e){
                throw std::runtime_error(SB()<<"Error: "<<e.what()<<"\n");
            }
            CurveMap.insert(std::make_pair(key, conf));
        } else {
            // found in CurveMap
            conf=CurveMap[key];
        }
    }

    size_t prev_size = 0;
    for (unsigned n=0; n<ncurve; n++) {
        std::string num(boost::lexical_cast<std::string>(n));
        std::vector<double> cv;
        bool cvchecker;
        if (checker){
            cvchecker = conf->tryGet<std::vector<double> >("curve"+num, cv);
        } else {
            cvchecker = c.tryGet<std::vector<double> >("curve"+num, cv);
        }
        if (!cvchecker)throw std::runtime_error(SB()<<"'curve" << num << "' is missing in lattice file.\n");

        if (rngchecker){
            unsigned start, end;
            if (range.size() != 2)
                throw std::runtime_error(SB()<<"Size of 'use_range' must be 2.\n");
            start = static_cast<unsigned>(range[0]);
            end   = static_cast<unsigned>(range[1]);

            if (start > cv.size() or end > cv.size())
                throw std::runtime_error(SB()<<"'use_range' is out of curve size.\n");

            std::vector<double> part(cv.begin()+start, cv.begin()+end);
            Curves.push_back(part);
        } else {
            Curves.push_back(cv);
        }

        if (n != 0 and prev_size != cv.size())
            throw std::runtime_error(SB()<<"Size of 'curve" << n << "' (" << cv.size() <<") and 'curve" << n-1 <<
                                           "' (" << prev_size <<") are inconsistent.  All curves must have the same size.\n");
        prev_size = cv.size();

        Scales.push_back(c.get<double>("scl_fac"+num, 0.0));
    }
}


#include <pa/stages/color_generator.hpp>

#define _USE_MATH_DEFINES

#include <algorithm>
#include <cmath>
#include <cstddef>

#include <boost/algorithm/clamp.hpp>
#include <tbb/tbb.h>

#include <pa/math/convert/coordinates.hpp>

namespace pa
{
void    color_generator::generate(integral_curves* integral_curves, mode mode, scalar constant_parameter)
{
  auto& vertices = integral_curves->vertices;
  auto& colors   = integral_curves->colors  ;
  auto& indices  = integral_curves->indices ;

  colors.resize(vertices.size());

  tbb::parallel_for(std::size_t(0), indices.size(), std::size_t(1), [&] (const std::size_t i)
  {
    auto& index   = indices[i];
    auto  tangent = (vertices[index + 1] - vertices[index]).normalized();

    if      (mode == mode::hsl_constant_s) colors[index] = map_hsl(tangent, true , constant_parameter);
    else if (mode == mode::hsl_constant_l) colors[index] = map_hsl(tangent, false, constant_parameter);
    else if (mode == mode::hsv_constant_s) colors[index] = map_hsv(tangent, true , constant_parameter);
    else if (mode == mode::hsv_constant_v) colors[index] = map_hsv(tangent, false, constant_parameter);
    else if (mode == mode::rgb           ) colors[index] = map_rgb(tangent);

    // Last vertex also gets a color.
    if (i != indices.size() - 1 && indices[i + 1] != index + 1)
      colors[index + 1] = colors[index];
  });
}

vector4 color_generator::correct_range  (vector4 spherical )
{
  if (spherical[1] <  scalar(0))          spherical[1] += scalar(M_PI);
  if (spherical[1] >= scalar(M_PI))       spherical[1] -= scalar(M_PI);
  spherical[1] = scalar(M_PI) - spherical[1];

  if (spherical[2] <  scalar(0))          spherical[2]  = std::abs(spherical[2]);
  if (spherical[2] >= scalar(M_PI / 2.0)) spherical[2]  = scalar(M_PI) - spherical[2];

  spherical[1] = scalar(spherical[1] / M_PI);
  spherical[2] = scalar(spherical[2] / (M_PI / 2.0f));

  return spherical;
}
vector4 color_generator::hue_to_rgba    (const scalar   hue)
{
  return vector4
  (
    static_cast<scalar>(boost::algorithm::clamp(std::abs(6.0f * hue - 3.0f) - 1.0f, 0.0f, 1.0f)),
    static_cast<scalar>(boost::algorithm::clamp(2.0f - std::abs(6.0f * hue - 2.0f), 0.0f, 1.0f)),
    static_cast<scalar>(boost::algorithm::clamp(2.0f - std::abs(6.0f * hue - 4.0f), 0.0f, 1.0f)),
    1.0f
  );
}
vector4 color_generator::hsl_to_rgba    (const vector4& hsl)
{
  auto color      = hue_to_rgba(hsl[0]);
  auto precompute = (1.0F - std::abs(2.0F * hsl[2] - 1.0F)) * hsl[1];
  color           = (color.array() - 0.5F) * precompute + hsl[2];
  color[3]        = 1.0f;
  return color;
}
vector4 color_generator::hsv_to_rgba    (const vector4& hsv)
{
  auto color = hue_to_rgba(hsv[0]);
  color      = ((color.array() - 1.0f) * hsv[1] + 1.0f) * hsv[2];
  color[3]   = 1.0f;
  return color;
}

vector4 color_generator::map_hsl        (const vector4& tangent, const bool constant_saturation = true, const scalar constant_parameter = scalar(0.5))
{
  auto spherical = correct_range(to_spherical(tangent));
  return constant_saturation
    ? hsl_to_rgba(vector4(spherical[1], constant_parameter, spherical[2]      , 0.0f))
    : hsl_to_rgba(vector4(spherical[1], spherical[2]      , constant_parameter, 0.0f));
}
vector4 color_generator::map_hsv        (const vector4& tangent, const bool constant_saturation = true, const scalar constant_parameter = scalar(0.5))
{
  auto spherical = correct_range(to_spherical(tangent));
  return constant_saturation
    ? hsv_to_rgba(vector4(spherical[1], constant_parameter, spherical[2]      , 0.0f))
    : hsv_to_rgba(vector4(spherical[1], spherical[2]      , constant_parameter, 0.0f));
}
vector4 color_generator::map_rgb        (const vector4& tangent)
{
  return vector4(std::abs(tangent[0]), std::abs(tangent[2]), std::abs(tangent[1]), 1.0f);
}
}


#ifndef TRIUMF_BNMR_SLR_GAUSS_DIST_EXP_HPP
#define TRIUMF_BNMR_SLR_GAUSS_DIST_EXP_HPP

#include <boost/math/constants/constants.hpp>
#include <cmath>
#include <triumf/bnmr/slr/common.hpp>
#include <triumf/math/faddeeva.hpp>
#include <iostream>

// TRIUMF: Canada's particle accelerator centre
namespace triumf {

// β-detected nuclear magnetic resonance (β-NMR)
namespace bnmr {

// spin-lattice relaxation (SLR)
namespace slr {

/// pulsed Gaussian distribution of exponentials integral
/// (from 0 to time_p <= time)
template <typename T = double>
T pulsed_gauss_dist_exp_integral(T time, T time_p, T nuclear_lifetime,
                                 T slr_rate, T sigma) {
  // imaginary number i
  std::complex<T> i(0.0, -1.0);
  // common exponential factor
  T expo =
      std::exp(-slr_rate / nuclear_lifetime / sigma / sigma -
               1.0 / 2.0 / sigma / sigma / nuclear_lifetime / nuclear_lifetime -
               slr_rate * slr_rate / 2.0 / sigma / sigma);
  std::cout << "expo = " << expo << "\n";
  // argument for the first error function
  std::complex<T> erf_arg1 =
      (((boost::math::constants::root_two<T>() * i * time -
         boost::math::constants::root_two<T>() * i * time_p) *
            sigma * sigma -
        boost::math::constants::root_two<T>() * i * slr_rate) *
           nuclear_lifetime -
       boost::math::constants::root_two<T>() * i) /
      (2.0 * sigma * nuclear_lifetime);
  std::cout << "erf_arg1 = " << erf_arg1 << "\n";
  // argument for the second error function
  std::complex<T> erf_arg2 =
      ((i * time * sigma * sigma - i * slr_rate) * nuclear_lifetime - i) /
      (boost::math::constants::root_two<T>() * sigma * nuclear_lifetime);
  std::cout << "erf_arg2 = " << erf_arg2 << "\n";
  // evaluate the result
  std::complex<T> result = boost::math::constants::root_pi<T>() * i * expo *
                           (triumf::math::faddeeva::erf(erf_arg1) -
                            triumf::math::faddeeva::erf(erf_arg2)) /
                           (boost::math::constants::root_two<T>() * sigma);
  std::cout << "result = " << result << "\n";
  // return only the real part
  return result.real();
}

/// pulsed Gaussian distribution of exponentials
template <typename T = double>
T pulsed_gauss_dist_exp(T time, T nuclear_lifetime, T pulse_length, T asymmetry,
                        T slr_rate, T sigma) {
  if (time == 0.0) {
    return asymmetry;
  } else if (time > 0.0 and time <= pulse_length) {
    return asymmetry *
           pulsed_gauss_dist_exp_integral(time, time, nuclear_lifetime,
                                          slr_rate, sigma) /
           normalization(time, nuclear_lifetime);
  } else if (time > pulse_length) {
    return (asymmetry *
            pulsed_gauss_dist_exp_integral(pulse_length, pulse_length,
                                           nuclear_lifetime, slr_rate, sigma) /
            normalization(pulse_length, nuclear_lifetime)) /
           std::exp(-(time - pulse_length) / nuclear_lifetime);
  } else {
    return 0.0;
  }
}

/// pulsed Gaussian distribution of exponentials (ROOT)
template <typename T = double>
T pulsed_gauss_dist_exp(const T *x, const T *par) {
  return pulsed_gauss_dist_exp<T>(*x, par[0], par[1], par[2], par[3], par[4]);
}

} // namespace slr

} // namespace bnmr

} // namespace triumf

#endif // TRIUMF_BNMR_SLR_GAUSS_DIST_EXP_HPP


#include <Eigen/Dense>
#include <iostream>

#include <ancse/config.hpp>
#include <ancse/polynomial_basis.hpp>
#include <ancse/dg_handler.hpp>
#include <ancse/dg_limiting.hpp>
#include <ancse/cfl_condition.hpp>
#include <ancse/dg_rate_of_change.hpp>
#include <ancse/snapshot_writer.hpp>
#include <ancse/time_loop.hpp>

static int n_vars = 3;

template<class F>
Eigen::MatrixXd ic(const F &f,
                   const Grid &grid,
                   const PolynomialBasis &poly_basis,
                   const DGHandler &dg_handler)
{
    int n_coeff = 1 + poly_basis.get_degree();
    Eigen::MatrixXd u0 = Eigen::MatrixXd::Zero (n_vars*n_coeff,
                                                grid.n_cells);

    auto [quad_points, quad_weights] = dg_handler.get_quadrature();
    int n_quad = static_cast<int>(quad_points.size());

    /// eval basis and its derivate for all quadrature points
    Eigen::MatrixXd basis(n_coeff, n_quad);
    for (int k = 0; k < n_quad; k++) {
        basis.col(k) = poly_basis(quad_points(k));
    }

    // L2-projection
    for (int j = 0; j < grid.n_cells; ++j)
    {
        for (int k = 0; k < quad_points.size(); k++)
        {
            auto fVal = f(cell_point(grid, j, quad_points(k)));
            for (int i = 0; i < n_vars; i++) {
                u0.col(j).segment(i*n_coeff, n_coeff)
                        += quad_weights(k)*fVal(i)*basis.col(k);
            }
        }
    }

    return u0*grid.dx;
}

TimeLoop make_dg(const nlohmann::json &config,
                 const Grid &grid,
                 const PolynomialBasis &poly_basis,
                 const DGHandler &dg_handler,
                 std::shared_ptr<Model> &model)
{
    double t_end = config["t_end"];
    double cfl_number = config["cfl_number"];

    auto n_ghost = grid.n_ghost;
    auto n_cells = grid.n_cells;

    auto n_vars = model->get_nvars();
    int n_coeff = 1 + poly_basis.get_degree();

    auto simulation_time = std::make_shared<SimulationTime>(t_end);
    auto boundary_condition
            = make_boundary_condition(n_ghost,
                                      config["boundary_condition"]);
    auto dg_rate_of_change
            = make_dg_rate_of_change(config, grid, model,
                                     poly_basis, dg_handler,
                                     simulation_time);
    auto dg_limiting
            = make_dg_limiting(config, grid, dg_handler);
    auto time_integrator = make_runge_kutta(config,
                                            dg_rate_of_change,
                                            boundary_condition,
                                            dg_limiting,
                                            n_vars*n_coeff, n_cells);
    auto cfl_condition
            = make_cfl_condition(grid, model, dg_handler, cfl_number);
    auto snapshot_writer = std::make_shared<JSONSnapshotWriter <DG>>
            (grid, model, dg_handler, simulation_time,
             std::string(config["output_dir"]),
             std::string(config["output_file"]));

    return TimeLoop(simulation_time, time_integrator,
                    cfl_condition, snapshot_writer);
}

void sod_shock_tube_test(const nlohmann::json &config)
{
    int deg = int(config["degree"]);

    double gamma = 7./5.;
    std::shared_ptr<Model> model = std::make_shared<Euler>();
    auto model_euler = dynamic_cast<Euler*>(model.get());
    model_euler->set_gamma(gamma);

    auto fn = [gamma](double x) {
        Eigen::VectorXd u(n_vars);
        if (x <= 0.5) { // left state
            u(0) = 1;
            u(1) = 0;
            u(2) = 1/(gamma-1);
        } else {        // right state
            u(0) = 0.125;
            u(1) = 0;
            u(2) = 0.1/(gamma-1);
        }
        return u;
    };

    int n_ghost = config["n_ghost"];
    int n_cells = int(config["n_interior_cells"]) + n_ghost * 2;

    auto grid = Grid({0.0, 1.0}, n_cells, n_ghost);
    auto poly_basis = PolynomialBasis(deg, 1./sqrt(grid.dx));
    auto dg_handler = DGHandler(model, poly_basis);
    auto u0 = ic(fn, grid, poly_basis, dg_handler);

    auto dg = make_dg(config, grid, poly_basis, dg_handler, model);
    dg(u0);
}

int main(int argc, char* const argv[])
{
    nlohmann::json config;
    std::string fileName;
    if (argc == 2) {
        fileName = argv[1];
    } else {
        fileName = "../config.json";
    }
    config = get_config (fileName);

    std::string ic_key = config["initial_conditions"];
    if (ic_key == "sod_shock_tube") {
        sod_shock_tube_test(config);
    }

    return 0;
}


/*
 * The MIT License (MIT)
 *
 * Copyright (c) 2017 Hidekazu Ikeno
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */

#include <iomanip>
#include <iostream>

#include "constants.hpp"
#include "legendre.hpp"
#include "tanh_sinh_rule.hpp"

#include <mxpfit/aak_reduction.hpp>
#include <mxpfit/balanced_truncation.hpp>
#include <mxpfit/exponential_sum.hpp>

#include <boost/math/special_functions/bessel.hpp>

template <typename T>
constexpr T pi()
{
    return T(
        3.1415926535897932384626433832795028841971693993751058209749445923078164062862089986280348253421170679);
}
///
/// ### BesselKernel
///
/// Compute the multi-exponential function that approximates the spherical
/// Bessel function of the first kind such that
///
/// \f[
///   \|j_{n}(x)-sum_{m=1}^{M} w_{m} e^{-a_{m} x} \| < \epsilon
/// \f]
///
/// for integer `n` on real axis `(x)`, where \f$ \epsilon \f$ is an arbitrary
/// small positive number.
///
/// The multi-exponential function is obtained by discretize the integral
/// representation of spherical Bessel function
///
/// \f[
///   j_{n}(x)=\int_{-1}^{1} e^{ixt} P_n(t) dt
/// \f]
///
/// The integrand in above expression is highly oscillating when `x` becomes
/// large, thus the direct application of some quadrature rule is inefficient.
/// To avoid this problem, we extend the integral on the complex plane and
/// select the contour as the steepest descent paths, as follows.
///
/// \f{eqnarray*}{
///   j_{n}(x) &=& I_1 + I_2 + (-1)^{n}(I_1^{\ast} + I_{2}^{\ast}), \\
///   I_{1} &=& \frac{(-i)^{n+1}}{2} \int_{0}^{R} e^{-(y-i)x} P_{n}(1+iy) dy, \\
///   I_{2} &=& \frac{(-i)^{n}}{2} \int_{-1}^{0} e^{-(R-ix)y} P_{n}(y+iR) dy, \\
/// \f}
///
/// The integrals \f$ I_1 \f$ and \f$ I_2 \f$ are discretized using appropriate
/// quadrature rule, then the balanced truncation method is applied to reduce
/// the number of terms.
///
template <typename T>
class BesselKernel
{
public:
    typename VecY, using Index = Eigen::Index;
    using Real    = T;
    using Complex = std::complex<Real>;

    using ExponentialSumType = mxpfit::ExponentialSum<Complex, Complex>;
    using ExponentsArray     = typename ExponentialSumType::ExponentsArray;
    using WeightsArray       = typename ExponentialSumType::WeightsArray;

    static ExponentialSumType compute(Index order, Real threshold);
};

template <typename T>
typename BesselKernel<T>::ExponentialSumType
BesselKernel<T>::compute(Index n, Real threshold)
{
    using Eigen::numext::conj;
    using Eigen::numext::exp;
    using Eigen::numext::log;
    using Eigen::numext::cos;
    using Eigen::numext::acos;
    using Eigen::numext::sin;
    using Eigen::numext::sqrt;
    // ----- Constants
    constexpr const auto zero = Real();
    constexpr const auto one  = Real(1);
    constexpr const auto half = Real(0.5);
    constexpr const auto eps  = std::numeric_limits<Real>::epsilon();
    // (-i)^n
    constexpr const Complex pow_i[4] = {Complex(one, zero), Complex(zero, -one),
                                        Complex(-one, zero),
                                        Complex(zero, one)};

    //----- adjustable parameters
    const auto R = Real(3) / std::max(Index(1), n);

    const auto n_quad1 = Index(500);
    const auto tiny1   = sqrt(eps) * threshold / n_quad1;

    const auto n_quad2 = Index(150 + 2 * n);
    const auto tiny2   = eps / n_quad2;
    //---------------------------

    //
    // Find R_shift such that j_n(R_shift) < eps
    //
    Real R_shift = Real();
    if (n > Index())
    {
        Real log_dfact = Real(); // log[2^n * n!]
        for (Index k = 1; k <= n; ++k)
        {
            log_dfact += log(Real(2 * k));
        }
        const auto thresh = std::min(threshold, sqrt(eps));
        R_shift           = exp((log_dfact + log(thresh)) / Real(n));
        if (R_shift < Real(0.1))
        {
            R_shift = Real();
        }
    }

    ExponentialSumType es_merged(n_quad1 + n_quad2);

    //
    // Discretization of integral I_2 on the path `z = x + iR` for `0<=x<=1`
    //
    quad::DESinhTanhRule<T> rule2(n_quad2, tiny2);

    // std::cout << "*** quadrature rule\n";
    // Real xsum = Real();
    // for (Index k = 0; k < n_quad2; ++k)
    // {
    //     xsum += rule2.adjacentDifference(k);
    //     std::cout << rule2.x(k) << '\t' << rule2.adjacentDifference(k) <<
    //     '\t'
    //               << xsum << '\t' << std::abs(rule2.x(k) - xsum) << '\n';
    // }

    for (Index k = 0; k < n_quad2; ++k)
    {
        const auto xk = half * rule2.distanceFromLower(k); // node
        const auto uk = half * rule2.distanceFromUpper(k); // 1 - x
        const auto wk = half * rule2.w(k);                 // weight

        const auto ak = Complex(R, -xk);
        const auto pk = wk * exp(ak * R_shift) *
                        cos(Real(n) * acos(Complex(xk, R))) /
                        sqrt(Complex(one + xk, R) * Complex(uk, -R));

        es_merged.exponent(k) = ak;
        es_merged.weight(k)   = pk;
    }

    //
    // Discretization of integral I_1 on the path `1 + i y`  for
    // `0 <= y <= R`
    //
    quad::DESinhTanhRule<T> rule1(n_quad1, tiny1);

    const auto R_half = R / 2;
    Index ipos        = n_quad2;
    for (Index k = n_quad1; k > 0; --k)
    {
        const auto yk = R_half * rule1.distanceFromLower(k - 1); // node
        const auto wk = R_half * rule1.w(k - 1);                 // weight
        const auto ak = Complex(yk, -one);
        const auto pk = Complex(zero, -wk) * exp(ak * R_shift) *
                        cos(Real(n) * acos(Complex(one, yk))) /
                        (sqrt(yk * Complex(yk, Real(-2))));

        es_merged.exponent(ipos) = ak;
        es_merged.weight(ipos)   = pk;
        ++ipos;
    }

    // Remove terms with same exponents
    es_merged.uniqueExponents(Real());

    //
    // Truncation for I_1 and I_2 separately
    //
    // mxpfit::BalancedTruncation<Complex> trunc1;
    // es1 = trunc1.compute(es1, threshold * eps);

    // mxpfit::BalancedTruncation<Complex> trunc2;
    // es2 = trunc2.compute(es2, threshold);

    //
    // Truncation for I_1 and I_2 simultaneously
    //
    mxpfit::BalancedTruncation<Complex> trunc1;
    es_merged = trunc1.compute(es_merged, threshold * eps * eps);

    // mxpfit::AAKReduction<Complex> reduction;
    // reduction.setThreshold(threshold);
    // reduction.compute(es_merged);

    ExponentialSumType es_result(2 * es_merged.size());
    const auto pre1 = pow_i[n % 4] / pi<Real>(); // (-i)^n / pi
    const auto pre2 = (n & 1) ? -pre1 : pre1;

    for (Index i = 0; i < es_merged.size(); ++i)
    {
        const auto ai                 = es_merged.exponent(i);
        es_result.exponent(2 * i + 0) = ai;
        es_result.exponent(2 * i + 1) = conj(ai);

        const auto wi               = es_merged.weight(i) * exp(-ai * R_shift);
        es_result.weight(2 * i + 0) = pre1 * wi;
        es_result.weight(2 * i + 1) = pre2 * conj(wi);
    }

    return es_result;
}

// template <typename T>
// typename BesselKernel<T>::ExponentialSumType
// BesselKernel<T>::compute(Index n, Real threshold)
// {
//     using Eigen::numext::conj;
//     using Eigen::numext::exp;
//     using Eigen::numext::log;
//     using Eigen::numext::cos;
//     using Eigen::numext::acos;
//     using Eigen::numext::sin;
//     using Eigen::numext::sqrt;
//     // ----- Constants
//     constexpr const auto zero = Real();
//     constexpr const auto one  = Real(1);
//     constexpr const auto half = Real(0.5);
//     constexpr const auto eps  = std::numeric_limits<Real>::epsilon();
//     // (-i)^n
//     constexpr const Complex pow_i[4] = {Complex(one, zero), Complex(zero,
//     -one),
//                                         Complex(-one, zero),
//                                         Complex(zero, one)};

//     //----- adjustable parameters
//     const auto R       = Real(3) / std::max(Index(1), n);
//     const auto n_quad1 = Index(500);
//     // const auto tiny1   = eps * eps;
//     const auto tiny1 = eps * sqrt(eps) / n_quad1;

//     const auto n_quad2 = Index(150 + 2 * n);
//     const auto tiny2   = eps / n_quad2;
//     //---------------------------

//     //
//     // Find R_shift such that j_n(R_shift) < eps
//     //
//     Real R_shift = Real();
//     if (n > Index())
//     {
//         Real log_dfact = Real(); // log[2^n * n!]
//         for (Index k = 1; k <= n; ++k)
//         {
//             log_dfact += log(Real(2 * k));
//         }
//         const auto thresh = std::min(threshold, sqrt(eps));
//         R_shift           = exp((log_dfact + log(thresh)) / Real(n));
//         if (R_shift < Real(0.1))
//         {
//             R_shift = Real();
//         }
//     }

//     //
//     // Discretization of integral I_1 on the path `-1 + i y`  for
//     // `0 <= y <= R`
//     //
//     quad::DESinhTanhRule<T> rule1(n_quad1, tiny1);
//     ExponentialSumType es1(n_quad1);

//     const auto R_half = R / 2;
//     for (Index k = 0; k < n_quad1; ++k)
//     {
//         const auto yk = R_half * rule1.distanceFromLower(k); // node
//         const auto wk = R_half * rule1.w(k);                 // weight
//         const auto ak = Complex(yk, one);
//         const auto pk = Complex(zero, wk) * exp(ak * R_shift) *
//                         cos(Real(n) * acos(Complex(-one, yk))) /
//                         (sqrt(yk * Complex(yk, Real(2))));

//         es1.exponent(k) = ak;
//         es1.weight(k)   = pk;
//     }

//     //
//     // Discretization of integral I_2 on the path `z = x + iR` for `-1<=x<=0`
//     //
//     quad::DESinhTanhRule<T> rule2(n_quad2, tiny2);
//     ExponentialSumType es2(n_quad2);

//     for (Index k = 0; k < n_quad2; ++k)
//     {
//         const auto xk = -half * rule2.distanceFromUpper(k); // node
//         const auto lk = half * rule2.distanceFromLower(k);  // x + 1
//         const auto wk = half * rule2.w(k);                  // weight

//         const auto ak = Complex(R, -xk);
//         const auto pk = wk * exp(ak * R_shift) *
//                         cos(Real(n) * acos(Complex(xk, R))) /
//                         sqrt(Complex(lk, R) * Complex(one - xk, -R));

//         es2.exponent(k) = ak;
//         es2.weight(k)   = pk;
//     }
//     //
//     // Truncation for I_1 and I_2 separately
//     //
//     // mxpfit::BalancedTruncation<Complex> trunc1;
//     // es1 = trunc1.compute(es1, threshold * eps);

//     // mxpfit::BalancedTruncation<Complex> trunc2;
//     // es2 = trunc2.compute(es2, threshold);

//     //
//     // Merge two sums
//     //
//     ExponentialSumType es_merged(es1.size() + es2.size());
//     es_merged.exponents().head(es1.size()) = es1.exponents();
//     es_merged.exponents().tail(es2.size()) = es2.exponents();
//     es_merged.weights().head(es1.size())   = es1.weights();
//     es_merged.weights().tail(es2.size())   = es2.weights();

//     //
//     // Truncation for I_1 and I_2 simultaneously
//     //
//     // mxpfit::BalancedTruncation<Complex> trunc1;
//     // es_merged = trunc1.compute(es_merged, threshold * eps * eps);

//     ExponentialSumType es_result(2 * es_merged.size());
//     const auto pre1 = pow_i[n % 4] / pi<Real>(); // (-i)^n / pi
//     const auto pre2 = (n & 1) ? -pre1 : pre1;

//     for (Index i = 0; i < es_merged.size(); ++i)
//     {
//         const auto ai                 = es_merged.exponent(i);
//         es_result.exponent(2 * i + 0) = ai;
//         es_result.exponent(2 * i + 1) = conj(ai);

//         const auto wi               = es_merged.weight(i) * exp(-ai *
//         R_shift); es_result.weight(2 * i + 0) = pre1 * wi; es_result.weight(2
//         * i + 1) = pre2 * conj(wi);
//     }

//     return es_result;
// }

//==============================================================================
// Main
//==============================================================================

using Index              = Eigen::Index;
using Real               = double;
using Complex            = std::complex<Real>;
using RealArray          = Eigen::Array<Real, Eigen::Dynamic, 1>;
using ComplexArray       = Eigen::Array<Complex, Eigen::Dynamic, 1>;
using ExponentialSumType = BesselKernel<Real>::ExponentialSumType;

void bessel_j_kernel_error(int l, const RealArray& x,
                           const ExponentialSumType& ret)
{
    RealArray exact(x.size());
    RealArray approx(x.size());

    for (Index i = 0; i < x.size(); ++i)
    {
        exact(i)  = boost::math::cyl_bessel_j(l, x(i));
        approx(i) = std::real(ret(x(i)));
    }

    RealArray abserr(Eigen::abs(exact - approx));

    for (Index i = 0; i < x.size(); ++i)
    {
        std::cout << std::setw(24) << x(i) << ' '      // point
                  << std::setw(24) << exact(i) << ' '  // exact value
                  << std::setw(24) << approx(i) << ' ' // approximation
                  << std::setw(24) << abserr(i) << '\n';
    }

    Index imax;
    abserr.maxCoeff(&imax);

    std::cout << "\n  abs. error in interval [" << x(0) << ","
              << x(x.size() - 1) << "]\n"
              << "    maximum : " << abserr(imax) << '\n'
              << "    averaged: " << abserr.sum() / x.size() << std::endl;
}

int main()
{
    std::cout.precision(15);
    std::cout.setf(std::ios::scientific);

    const Real threshold = 1.0e-14;
    const Real eps       = Eigen::NumTraits<Real>::epsilon();
    const Index lmax     = 4;
    const Index N        = 2000; // # of sampling points

    std::cout
        << "# Approximation of spherical Bessel function by exponential sum\n";

    RealArray x = Eigen::exp(RealArray::LinSpaced(N, -20.0, 20.0));
    ExponentialSumType ret;
    for (Index l = 0; l <= lmax; ++l)
    {
        std::cout << "\n# --- order " << l;
        ret                      = BesselKernel<Real>::compute(l, threshold);
        const auto thresh_weight = std::max(eps, threshold) / Real(ret.size());
        ret                      = mxpfit::removeIf(
            ret, [=](const Complex& /*exponent*/, const Complex& wi) {
                return std::abs(std::real(wi)) < thresh_weight &&
                       std::abs(std::imag(wi)) < thresh_weight;
            });
        std::cout << " (" << ret.size() << " terms approximation)\n"
                  << ret << '\n';

        // std::cout << " (" << ret.size() << " terms approximation)\n";
        // std::cout
        //     << "# real(exponent), imag(exponent), real(weight),
        //     imag(weight)\n";
        // for (Index i = 0; i < ret.size(); ++i)
        // {
        //     std::cout << std::setw(24) << std::real(ret.exponent(i)) << '\t'
        //               << std::setw(24) << std::imag(ret.exponent(i)) << '\t'
        //               << std::setw(24) << std::real(ret.weight(i)) << '\t'
        //               << std::setw(24) << std::imag(ret.weight(i)) << '\n';
        // }
        // std::cout << '\n' << std::endl;

        std::cout << "\n# errors in small x\n";
        bessel_j_kernel_error(l, x, ret);
    }

    return 0;
}


/*
   For more information, please see: http://software.sci.utah.edu

   The MIT License

   Copyright (c) 2020 Scientific Computing and Imaging Institute,
   University of Utah.

   Permission is hereby granted, free of charge, to any person obtaining a
   copy of this software and associated documentation files (the "Software"),
   to deal in the Software without restriction, including without limitation
   the rights to use, copy, modify, merge, publish, distribute, sublicense,
   and/or sell copies of the Software, and to permit persons to whom the
   Software is furnished to do so, subject to the following conditions:

   The above copyright notice and this permission notice shall be included
   in all copies or substantial portions of the Software.

   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
   OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
   THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
   FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
   DEALINGS IN THE SOFTWARE.

	 Author: 							Jaume Coll-Font, Yesim Serinagaoglu & Alireza Ghodrati
	 Last Modification:		September 6 2017
*/

#include <Core/Datatypes/DenseMatrix.h>
#include <Core/Datatypes/DenseColumnMatrix.h>
#include <Core/Datatypes/SparseRowMatrix.h>
#include <Core/Logging/LoggerInterface.h>

// Tikhonov inverse libraries
#include <Core/Algorithms/Legacy/Inverse/TikhonovAlgoAbstractBase.h>
#include <Core/Algorithms/Legacy/Inverse/SolveInverseProblemWithTSVD_impl.h>

// EIGEN LIBRARY
#include <Eigen/Eigen>
#include <Eigen/SVD>

using namespace SCIRun;
using namespace SCIRun::Core::Datatypes;
// using namespace SCIRun::Modules::Inverse;
// using namespace SCIRun::Dataflow::Networks;
using namespace SCIRun::Core::Logging;
using namespace SCIRun::Core::Algorithms;
using namespace SCIRun::Core::Algorithms::Inverse;

///////////////////////////////////////////////////////////////////
/////// prealocate Matrices for inverse compuation
///     This function precalcualtes the SVD of the forward matrix and prepares singular vectors and values for posterior computations
///////////////////////////////////////////////////////////////////
void SolveInverseProblemWithTSVD_impl::preAlocateInverseMatrices(const SCIRun::Core::Datatypes::DenseMatrix& forwardMatrix_, const SCIRun::Core::Datatypes::DenseMatrix& measuredData_ , const SCIRun::Core::Datatypes::DenseMatrix& sourceWeighting_, const SCIRun::Core::Datatypes::DenseMatrix& sensorWeighting_, const SCIRun::Core::Datatypes::DenseMatrix& matrixU_, const SCIRun::Core::Datatypes::DenseMatrix& singularValues_, const SCIRun::Core::Datatypes::DenseMatrix& matrixV_)
{

		// alocate U and V matrices
			svd_MatrixU = matrixU_;
			svd_MatrixV = matrixV_;

		// alocate singular values
			if (singularValues_.ncols() == 1 ){
				svd_SingularValues = singularValues_;
			}
			else{
				svd_SingularValues = singularValues_.diagonal();
			}

		// Compute the projection of data y on the left singular vectors
			Uy = svd_MatrixU.transpose() * (measuredData_);

		// determine rank
	        rank = svd_SingularValues.nrows();

}

void SolveInverseProblemWithTSVD_impl::preAlocateInverseMatrices(const SCIRun::Core::Datatypes::DenseMatrix& forwardMatrix_, const SCIRun::Core::Datatypes::DenseMatrix& measuredData_ , const SCIRun::Core::Datatypes::DenseMatrix& sourceWeighting_, const SCIRun::Core::Datatypes::DenseMatrix& sensorWeighting_)
{

	    // Compute the SVD of the forward matrix
	        Eigen::JacobiSVD<SCIRun::Core::Datatypes::DenseMatrix::EigenBase> SVDdecomposition( forwardMatrix_, Eigen::ComputeFullU | Eigen::ComputeFullV);

		// alocate the left and right singular vectors and the singular values
			svd_MatrixU = SVDdecomposition.matrixU();
			svd_MatrixV = SVDdecomposition.matrixV();
			svd_SingularValues = SVDdecomposition.singularValues();

	    // determine rank
	        rank = SVDdecomposition.nonzeroSingularValues();

	    // Compute the projection of data y on the left singular vectors
	        Uy = svd_MatrixU.transpose() * (measuredData_);
}

//////////////////////////////////////////////////////////////////////
// THIS FUNCTION returns regularized solution by tikhonov method
//////////////////////////////////////////////////////////////////////
SCIRun::Core::Datatypes::DenseMatrix SolveInverseProblemWithTSVD_impl::computeInverseSolution( double lambda, bool inverseCalculation ) const
{

    // prealocate matrices
        const int N = svd_MatrixV.cols();
        const int M = svd_MatrixU.rows();
        const int numTimeSamples = Uy.ncols();
        DenseMatrix solution(DenseMatrix::Zero(N,numTimeSamples));
        DenseMatrix tempInverse(DenseMatrix::Zero(N,M));

		const int truncationPoint = Min( int(lambda), rank, int(9999999999999) );

    // Compute inverse SolveInverseProblemWithTSVD
        for (int rr=0; rr < truncationPoint ; rr++)
        {
            // evaluate filter factor
                double singVal = svd_SingularValues[rr];
                auto filterFactor_i =  1 / ( singVal );

            // update solution
                solution += filterFactor_i * svd_MatrixV.col(rr) * Uy.row(rr);

            // update inverse operator
                if (inverseCalculation)
                    tempInverse += filterFactor_i * ( svd_MatrixV.col(rr) *  svd_MatrixU.col(rr).transpose() );
        }

    // output solutions
    //   if (inverseCalculation)
    //       inverseMatrix_.reset( new SCIRun::Core::Datatypes::DenseMatrix(tempInverse) );

        return solution;
}

//////////////////////////////////////////////////////////////////////
// THIS FUNCTION returns a string of lambdas from which the L-curve is computed
//////////////////////////////////////////////////////////////////////
std::vector<double> SolveInverseProblemWithTSVD_impl::computeLambdaArray( double lambdaMin, double lambdaMax, int nLambda ) const
{
	std::vector<double> lambdaArray(nLambda,0.0);
	const double lam_step = 1;

	lambdaArray[0] = lambdaMin;
	for (int j = 1; j < nLambda; j++)
	{
		lambdaArray[j] = lambdaArray[j-1]  + lam_step;
	}
	return lambdaArray;
}


// Distributed under the MIT License.
// See LICENSE.txt for details.

/// \file
/// Declares function find_root_of_function

#pragma once

#include <functional>
#include <limits>

#include <boost/math/tools/toms748_solve.hpp>

/*! \ingroup Functors
 *  \brief Finds the root of the function f with the TOMS_748 method.
 *
 *  \requires Function f be callable
 */
template <typename Function>
double find_root_of_function(Function f, const double lower_bound,
                             const double upper_bound,
                             const double absolute_tolerance,
                             const double relative_tolerance,
                             const size_t max_iterations = 100) {
  boost::uintmax_t max_iter = max_iterations;

  // This solver requires tol to be passed as a termination condition. This
  // termination condition is equivalent to the convergence criteria used by the
  // GSL
  auto tol = [absolute_tolerance, relative_tolerance](double lhs, double rhs) {
    return (fabs(lhs - rhs) <=
            absolute_tolerance +
                relative_tolerance * fmin(fabs(lhs), fabs(rhs)));
  };
  // Lower and upper bound are shifted by absolute tolerance so that the root
  // find does not fail if upper or lower bound are equal to the root within
  // tolerance
  auto result = boost::math::tools::toms748_solve(
      f, lower_bound - absolute_tolerance, upper_bound + absolute_tolerance,
      tol, max_iter);
  return result.first + 0.5 * (result.second - result.first);
}


//==================================================================================================
/*!
  @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_FUNCTION_ASIND_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_ASIND_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-trigonometric
    Function object implementing asind capabilities

    inverse sine in degree.

    @par Semantic:

    For every parameter of floating type

    @code
    auto r = asind(x);
    @endcode

    Returns the arc @c r in the interval
    \f$[-90, 90[\f$ such that <tt>sin(r) == x</tt>.
    If @c x is outside \f$[-1, 1[\f$ the result is Nan.

  **/
  Value asind(Value const & v0);
} }
#endif

#include <boost/simd/function/scalar/asind.hpp>
#include <boost/simd/function/simd/asind.hpp>

#endif


#ifndef GAUSSIAN_PROCESS_CLASSIFICATION_HPP
#define GAISSIAN_PROCESS_CLASSIFICATION_HPP

#include "gplib/kernel.hpp"
#include <Eigen/Cholesky>
#include <vector>

namespace librav{
    const int64_t kDimension = 2;

    class GaussianProcessClassification{
        public:
            ~GaussianProcessClassification() {};
            // The keyword explicit avoid the implicit type conversion constructor
            explicit GaussianProcessClassification(const Kernel::Ptr& kernel, double noise, const std::shared_ptr<std::vector<Eigen::VectorXd>> points, const Eigen::VectorXd& targets, size_t max_points = 100);

            // Evaluate mean and variance at a point
            void Evaluate(const Eigen::VectorXd& x, double& mean, double& variance) const;
            void EvaluateTrainingPoint(size_t ii, double& mean, double& variance) const;

            // Add new point(s). Return whether or not points were added (points will
            // only be added until 'max_points' is reached).
            bool Add(const Eigen::VectorXd& x, double target);
            bool Add(const std::vector<Eigen::VectorXd>& points, const Eigen::VectorXd& targets);

            // Update the training targets in the direction of the gradient of the
            // mean squared error at the given points. Returns the mean squared error.
            // If 'finalize' is set, computes regressed targets - only set to false if
            // you are doing repeated updates, and be sure to set true on final update.
            double UpdateTargets(const std::vector<Eigen::VectorXd>& points,
                                 const std::vector<double>& targets,
                                 double step_size, bool finalize = true);

            std::pair<bool,double> Predict(const Eigen::VectorXd& x, Eigen::VectorXd* pi_, Eigen::VectorXd *W_sr_, Eigen::MatrixXd *LLT_);

            // Immutable accessors
            const Eigen::MatrixXd& ImmutableCovariance() const { return covariance_; };
            const Eigen::MatrixXd& ImmutableCovarianceGradient() const {return covariance_gradient_;};
            const Eigen::VectorXd& ImmutableRegressedTargets() const { return regressed_; };
            const Eigen::VectorXd& ImmutableTargerts() const { return targets_; };
            const std::shared_ptr<const std::vector<Eigen::VectorXd>> ImmutablePoints() const { return points_; };
            const Eigen::LLT<Eigen::MatrixXd>& ImmutableCholesky() const { return llt_; };
            size_t Dimension() const { return dimension_; };

        private:
            // Compute the covariance and cross covariance against the trainning points.
            void Covariance();
            void CrossCovariance(const Eigen::VectorXd& x, Eigen::VectorXd& cross) const;
            void CovarianceGradient();
            Eigen::MatrixXd Covariance(const Eigen::VectorXd& target);

            // Kernel.
            const Kernel::Ptr kernel_;

            // Noise variance
            double noise_;

            // Trainning points, targets, and regressed targets (inv(cov) * targets).
            const std::shared_ptr<std::vector<Eigen::VectorXd>> points_;
            const std::shared_ptr<std::vector<int>> targets_y_;
            size_t dimension_;
            Eigen::VectorXd targets_;
            Eigen::VectorXd regressed_;

            // Maximum number of points.
            const size_t max_points_;

            // Covariance matrix, with Cholesky decomposition.
            Eigen::MatrixXd covariance_;
            Eigen::MatrixXd covariance_gradient_;
            Eigen::LLT<Eigen::MatrixXd> llt_;
    };
}

#endif /* GAUSSIAN_PROCESS_CLASSIFICATION_HPP */


// Copyright (c) 2021 by Ignacio Alzugaray <alzugaray dot ign at gmail dot com>
// ETH Zurich, Vision for Robotics Lab.
#pragma once

#include <Eigen/Eigen>

namespace haste {

template<typename Scalar_>
struct PinholeRadTanCamera {
 public:
  using Scalar = Scalar_;
  using Vector2 = Eigen::Matrix<Scalar, 2, 1>;
  using Matrix22 = Eigen::Matrix<Scalar, 2, 2>;

  inline auto distortPointNormalized(Vector2 &p, Matrix22 *jacobian) const -> void;
  inline auto undistortPointNormalized(const Vector2 &p_distorted) const -> Vector2;
  inline auto undistortPoint(const Vector2 &p_distorted) const -> Vector2;

  struct UndistortionMap {
   public:
    UndistortionMap(const PinholeRadTanCamera &camera);
    inline auto operator()(const int &x, const int &y) const -> std::pair<Scalar, Scalar>;
   private:
    Eigen::Array<Vector2, -1, -1> data_;
  };

  inline auto createUndistortionMap() const -> UndistortionMap;

  size_t width, height;     ///< Sensor size.
  Scalar fx, fy, cx, cy;    ///< Intrinsics.
  Scalar k1, k2, p1, p2, k3;/// << Rad-tan distortion.

 protected:
  static constexpr auto kNumItersUndistortion_ = 50;  ///< Max number of iterations until convergence.
  static constexpr auto kMaxErrorUndistortion_ = 1e-3;///< Max error in iterative distortion-undistortion.

};

}// namespace haste

#include "camera_impl.hpp"


//Copyright (c) 2018, Changjie
//All rights reserved.
//
//Redistribution and use in source and binary forms, with or without
//modification, are permitted provided that the following conditions are met:
//
//* Redistributions of source code must retain the above copyright notice, this
//  list of conditions and the following disclaimer.
//
//* Redistributions in binary form must reproduce the above copyright notice,
//  this list of conditions and the following disclaimer in the documentation
//  and/or other materials provided with the distribution.
//
//THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
//AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
//IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
//DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
//FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
//DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
//SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
//CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
//OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
//OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//

#ifndef __BFGS_HPP__
#define __BFGS_HPP__

#include <functional>
#include <memory>
#include <iostream>
#include <Eigen/Core>
#include <Eigen/QR>
#include <Eigen/Dense>

using namespace Eigen;

using Eigen::MatrixXd;
using Eigen::VectorXd;

// ref: http://en.cppreference.com/w/cpp/utility/functional/function

// input is the position(parameter)
// Add const to avoid error of invalid initialization of non-const reference
typedef std::function<double(VectorXd const &)> Cost_Fun;
// param1: returned gradient
// param2: input position
typedef std::function<void(VectorXd &, VectorXd &)> Diff_Fun;
// param1: returned hessian matrix
// param2: input position
typedef std::function<void(MatrixXd &, VectorXd &)> Hess_Fun;

struct NonConstraintObj
{
	VectorXd m_x;
	size_t m_n;

	NonConstraintObj()
	{
		m_n = 1;
		m_x.resize(m_n);
		m_x.fill(0.);
	}
	virtual ~NonConstraintObj() = default;
	virtual double cost(VectorXd const &x) = 0;
	virtual void grad(VectorXd &grad, VectorXd &x) = 0;
};

struct BFGS
{
	size_t MAX_STEP;
	size_t stop_step;
	size_t step;
	double rho;
	double sigma;
	double epsilon;

	bool improved;
	bool initialized;

	Cost_Fun cost_fun;
	Diff_Fun grad_fun;

	std::shared_ptr<NonConstraintObj> funPtr;
	BFGS()
	{
		MAX_STEP = 80;
		stop_step = 5;
		step = 0;
		rho = 0.55;
		sigma = 0.4;
		epsilon = 1e-5;
		improved = false;
		initialized = false;
		funPtr = nullptr;
	}
	template <class T>
	void init(T &obj)
	{
		// store a call to a member function and object
		using std::placeholders::_1;
		using std::placeholders::_2;
		cost_fun = std::bind(&T::cost, obj, _1);
		grad_fun = std::bind(&T::grad, obj, _1, _2);
		initialized = true;
	}
	double solve_(Cost_Fun fun, Diff_Fun dfun, VectorXd &input);
	double solve_(Cost_Fun fun, Diff_Fun dfun, Hess_Fun hfun, VectorXd &input);
	double solve(VectorXd &input)
	{
		if (initialized)
		{
			return solve_(cost_fun, grad_fun, input);
		}
		else
		{
			printf("BFGS is not initialized! Return zeros.\n");
			return 1e9;
		}
	}
};

struct BFGS_V2
{
	//double *Bk;

	BFGS_V2()
	{
		//Bk = nullptr;
		//cudaMalloc((void **)&Bk, sizeof(double)*3);
	}
	template <class T>
	static double bfgs(std::shared_ptr<T> ptr, VectorXd &x0)
	{
		size_t maxk = 500, stop_step = 5;
		double rho = 0.55, sigma = 0.4, epsilon = 1e-5;

		size_t k = 0, n = x0.size();
		MatrixXd Bk = MatrixXd::Identity(n, n);

		VectorXd gk, dk, sk, yk, x;
		size_t m = 0, mk = 0, bad = 0;
		double fit = 0., tmp = 0.;
		double best = 1e10;
		while (k < maxk && bad < stop_step)
		{
			fit = ptr->cost(x0);
			if (fit < best - epsilon)
			{
				best = fit;
				bad = 0;
			}
			else
			{
				bad++;
			}
			ptr->grad(gk, x0);
			//std::cout<<"gk = "<<gk<<std::endl;
			if (gk.norm() < epsilon)
			{
				break;
			}
			dk = Bk.householderQr().solve(-gk); // llt ?
			m = 0, mk = 0;

			tmp = gk.transpose() * dk;
			while (m < 100)
			{
				if (ptr->cost(x0 + pow(rho, m) * dk) < (fit + sigma * pow(rho, m) * tmp))
				{
					mk = m;
					break;
				}
				m++;
			}
			x = x0 + pow(rho, mk) * dk;
			sk = pow(rho, mk) * dk;
			ptr->grad(yk, x);
			yk = yk - gk;
			if (yk.transpose() * sk > 0)
			{
				Bk = Bk - (Bk * sk * sk.transpose() * Bk) / (sk.transpose() * Bk * sk) + (yk * yk.transpose()) / (yk.transpose() * sk);
			}
			x0 = x;
			k++;
		}
		x0 = x;
		#ifdef DEBUG
		std::cout << "BFGS stops at step " << k << std::endl;
		#endif
		return fit;
	}
};

#endif


#ifndef RNN_TYPE_HPP
#define RNN_TYPE_HPP

#include <Eigen/Core>

namespace rnn::generic {
using real = double;

template<std::size_t row_dim_t_, std::size_t col_dim_t_>
using rnn_matrix = Eigen::Matrix<real, row_dim_t_, col_dim_t_>;

template<std::size_t dim_t_>
using rnn_square_matrix = rnn_matrix<dim_t_, dim_t_>;

template<std::size_t dim_t_>
using rnn_vector = Eigen::Vector<real, dim_t_>;

#ifdef USE_FLOAT
typedef float real;
typedef Eigen::MatrixXf MatD;
typedef Eigen::VectorXf VecD;
#else
typedef Eigen::MatrixXd MatD;
typedef Eigen::VectorXd VecD;
#endif

typedef Eigen::MatrixXi MatI;
typedef Eigen::VectorXi VecI;
#define REAL_MAX std::numeric_limits<real>::max()

} // namespace rnn

#endif


#ifndef fast_gauss_noise_h
#define fast_gauss_noise_h

#include <cstdint>
#include <cstddef>
#include <list>
#include <iostream>
#include <cstdlib>
#include <ctime>
#include <cmath>
#include <climits>
#include <cstring>
#include <tuple>
#include <typeinfo>
#include <gmp.h>
#include <mpfr.h>
#include "fastrandombytes.h"

#ifdef BOOST_RAPHSON
#include <boost/math/tools/roots.hpp>
#endif

//#define OUTPUT_BARRIERS
//#define OUTPUT_LUT_FLAGS
// Use it to test rare values do not happen too often
//#define UNITTEST_ONEMILLION

namespace nfl {

template<typename T0, typename T1>
constexpr inline auto tstbit(T0 x, T1 n) -> decltype((x << (63 - n)) >> 63 ) { return ((x << (63 - n)) >> 63 ); }

template<class in_class, class out_class>
struct output {
  out_class val;
  bool flag;
  std::list<in_class*> l_b_ptr;
};

template<class in_class, class out_class, unsigned _lu_depth>
class FastGaussianNoise {
    typedef output<in_class, out_class> output_t;
  private:
    unsigned int _bit_precision;
    unsigned int _word_precision;
    unsigned int _number_of_barriers;
    unsigned int _lu_size;
    unsigned int _flag_ctr1;
    unsigned int _flag_ctr2;
    double _sigma;
    mpfr_t _const_sigma;
    mpfr_t _center;
    int rounded_center;
    unsigned int _security;
    unsigned int _samples;
    double _tail_bound;
    bool _verbose;

    in_class **barriers;
    output_t *lu_table;
    output_t **lu_table2;

    void check_template_params();
    void init();
    void precomputeBarrierValues();
    void buildLookupTables();
    void nn_gaussian_law(mpfr_t rop, const mpfr_t x_fr);
    int cmp(in_class *op1, in_class *op2);

  public:
    static const unsigned int default_k;
    FastGaussianNoise(double sigma, unsigned int security, unsigned int samples, double center_d = 0, bool verbose = false);
    FastGaussianNoise(double sigma, unsigned int security, unsigned int samples, mpfr_t center, bool verbose = false);
    ~FastGaussianNoise();
    void getNoise(out_class * const rand_data2out, uint64_t rlen);
};

/* NOTATIONS (from paper Sampling from discrete Gaussians
 * for lattice-based cryptography on a constrainded device
 * Nagarjun C. Dwarakanath, Steven Galbraith
 * Journal : AAECC 2014 (25)
 *
 * m : lattice dimension, we set it to 1 in here (better results for same security) !!!!
 *
 * security : output distribution should be within 2^{-security}
 * statistical distance from the Gaussian distribution
 *
 * sigma : parameter of the Gaussian distribution (close
 * to the standard deviation but not equal)
 * SUM = \sum_{k=-\inf}^{\inf} exp(-(k-c)^2/(2*sigma^2))
 *
 * D_{sigma} : distribution such that for x\in \mathcal{Z}
 * Pr(x) = ro_{sigma}(x) = 1/SUM exp(-(x-c)^2/(2*sigma^2))
 *
 * Warning : can be replaced in some works by s = sqrt(2*pi)*sigma
 * if Pr(x) = 1/SUM exp(-pi*(x-c)^2/s^2)
 *
 * tail_bound : tail bound we only output points with
 * norm(point-c) < tail_bound*sigma
 *
 * bit_precision : bit precision when computing the probabilities
 *
 * delta_tailbound : statistical distance introduced
 * by the tail bound
 *
 * delta_epsi : statistical distance introduced by
 * probability approximation
 */

// HELPER FUNCTIONS

/* /!\ Warning only for x64 */
extern __inline__ uint64_t rdtsc(void)
{
  uint64_t a, d;
  __asm__ volatile ("rdtsc" : "=a" (a), "=d" (d));
  return (d<<32) | a;
}

// Function to be used in a Newton-Raphson solver
struct funct
{
	funct(double const& target) : k(target){}
	std::tuple<double, double> operator()(double const& x)
	{
		return std::make_tuple(x*x - 2*log(x) - 1 - 2*k*log(2), 2*x-2/x);
	}
	private:
	double k;
};

inline
double newton_raphson(double k, double max_guess, int digits)
{
  unsigned max_counter = 1U<<15;
  std::tuple<double, double> values;
  double delta;
  double guess = max_guess;
  for (unsigned counter = 0 ; counter < max_counter; counter++)
  {
    values = funct(k)(guess);
    delta = std::get<0>(values)/std::get<1>(values);
    guess -= delta;
    if ( fabs(delta)/fabs(guess) < pow(10.0,-digits) ) break;
  }
  // In case there is a flat zone in the function
  while(0.95*guess*0.95*guess - 2*log(0.95*guess) - 1 - 2*k*log(2)>=0) guess*=0.95;
  // Test result
  if(guess*guess - 2*log(guess) - 1 - 2*k*log(2)<0)
  {
    std::cout << "FastGaussianNoise: WARNING Newton-Raphson failed the generator is NOT secure" << std::endl;
  }
  return guess;
}

// Constructors

template<class in_class, class out_class, unsigned _lu_depth>
FastGaussianNoise<in_class, out_class, _lu_depth>::FastGaussianNoise( double sigma,
		unsigned int security, unsigned int samples, double center_d /*=0*/, bool verbose /*=true*/):
	_sigma(sigma),
	_security(security),
  _samples(samples),
  _verbose(verbose)
{
  // Check template parameters
  check_template_params();

  //Center cannot be initialized before the constructor
  mpfr_init_set_d(_center, center_d, MPFR_RNDN);
  rounded_center = round(center_d);

  //Initialization functions
	init();
	precomputeBarrierValues();
	buildLookupTables();
}

template<class in_class, class out_class, unsigned _lu_depth>
FastGaussianNoise<in_class, out_class, _lu_depth>::FastGaussianNoise( double sigma,
		unsigned int security, unsigned int samples, mpfr_t center /*=0*/, bool verbose /*=true*/):
	_sigma(sigma),
	_security(security),
  _samples(samples),
  _verbose(verbose)
{
  // Check template parameters
  check_template_params();

  //Center cannot be initialized before the constructor
  mpfr_init_set(_center, center, MPFR_RNDN);
  rounded_center = mpfr_get_d(_center, MPFR_RNDN);

  //Initialization functions
	init();
	precomputeBarrierValues();
	buildLookupTables();
}

// Check template parameters
template<class in_class, class out_class, unsigned _lu_depth>
void FastGaussianNoise<in_class, out_class, _lu_depth>::check_template_params()
{
  // Lookup tables can only have depth 1 or 2
  if (_lu_depth != 1 && _lu_depth != 2)
  {
    std::cout << "FastGaussianNoise: CRITICAL _lu_depth must be 1 or 2" << std::endl;
    exit(1);
  }

  // Lookup tables can only have uint8_t or uint16_t indexes
  if (typeid(in_class) == typeid(uint8_t))
    _lu_size = 1<<8;
  else if(typeid(in_class) == typeid(uint16_t))
    _lu_size = 1<<16;
  else
  {
    std::cout << "FastGaussianNoise: CRITICAL in_class must be uint8_t or uint16_t" << std::endl;
    exit(2);
  }
}

// Compute some values (precision, number of barriers and outputs)
template<class in_class, class out_class, unsigned _lu_depth>
void FastGaussianNoise<in_class, out_class, _lu_depth>::init()
{
	double epsi, k;

	/* Lemma 1:
	 * tail_bound >= sqrt(1 + 2*log(tail_bound) + 2*k*log(2))
	 * IMPLIES delta_tailbound < 2**(-k)
   *
   * -epsi <= -k - log2(2 * tail_bound * sigma))
	 * IMPLIES 2 * tail_bound * sigma * 2**(-epsi) <= 2**(-k)
	 *
   * Lemma 2 (and Lemma 1):
	 * 2 * tail_bound * sigma * 2**(-epsi) <= 2**(-k)
	 * IMPLIES  delta_epsilon < 2**(-k)
   */

	// THUS setting
	k = _security + 1 + ceil(log(_samples)/log(2));
	// IMPLIES (delta_tailbound + delta_epsilon)*_samples < 2**(-_security)
	// We can thus generate vectors of _samples samples securely

  // To compute the tail bound we use Newton-Raphson with three digits of precision
	// WE HAVE tail_bound >= sqrt(1 + 2*log(tail_bound) + 2*k*log(2))
  // IS EQUIV TO tail_bound**2 -2*log(tail_bound) - 1- 2*k*log(2) >= 0
	int digits = 3;
#ifdef BOOST_RAPHSON
  double max_guess = 1+2*k*log(2);
  double min_guess = sqrt(1 + 2*k*log(2)), guess = min_guess;
	_tail_bound = boost::math::tools::newton_raphson_iterate(funct(k),guess, min_guess, max_guess, digits);
#else
  double min_guess = sqrt(1 + 2*k*log(2));
  _tail_bound = newton_raphson(k, min_guess, digits);
#endif
	// We now can compute the precision needed
	epsi = k + log2(2 * _tail_bound * _sigma);
	_bit_precision = ceil(epsi);
	_word_precision = ceil(_bit_precision/(8.0*sizeof(in_class)));
  // For the same cost we get a simpler situation and more precision
	_bit_precision = _word_precision*8*sizeof(in_class);
  // And the number of probabilities that must be computed
  // Only half of them are computed (others are symmetric)
  // but all values are stored to speedup noise generation
  _number_of_barriers = 1+2*ceil(_tail_bound*_sigma);
  if ( ((uint64_t)_number_of_barriers >> (sizeof(out_class) * 8 - 1)) != 0)
    std::cout << "FastGaussianNoise: WARNING out_class too small to contain some of the (signed) results" << std::endl;
  if ( ((uint64_t)_number_of_barriers >> (sizeof(int) * 8 - 1)) != 0)
    std::cout << "FastGaussianNoise: WARNING outputs are above 2**31, unexpected results" << std::endl;

  // Finally we precompute 1/(2*sigma**2) to accelerate things
	mpfr_inits2(_bit_precision, _const_sigma, nullptr);
  mpfr_set_d(_const_sigma, _sigma, MPFR_RNDN);
	mpfr_sqr(_const_sigma, _const_sigma, MPFR_RNDN);
	mpfr_mul_ui(_const_sigma, _const_sigma, 2, MPFR_RNDN);
	mpfr_ui_div(_const_sigma, 1, _const_sigma, MPFR_RNDN);

  // Give some feedback
  if (_verbose) std::cout << "FastGaussianNoise: " << _number_of_barriers <<
    " barriers with " << _bit_precision <<
    " bits of precision will be computed" << std::endl;
}

template<class in_class, class out_class, unsigned _lu_depth>
void FastGaussianNoise<in_class, out_class, _lu_depth>::precomputeBarrierValues()
{
  // Declare and init mpfr vars
  mpfr_t sum, tmp, tmp2;
  mpfr_t *mp_barriers;
  mpfr_inits2(_bit_precision, sum, tmp, tmp2, nullptr);

  // This var is used to export mpfr values
  mpz_t int_value;
  mpz_init2(int_value, _bit_precision);

  // Init SUM = \sum_{k=-ceil(tail_bound*sigma)}^{ceil(tail_bound*sigma)} exp(-(k+round(c)-c)^2/(2*sigma^2))
  // and compute on the loop with the barriers
  mpfr_set_ui(sum, 0, MPFR_RNDN);

  // Allocate memory for the barrier pointers
  barriers = (in_class **) malloc(_number_of_barriers*sizeof(in_class *));
  mp_barriers = (mpfr_t *) malloc(_number_of_barriers*sizeof(mpfr_t));

  // Now loop over the barriers
  for (int i = 0; i < (int)_number_of_barriers; i++)
  {
    // Init mpfr var
    mpfr_init2(mp_barriers[i], _bit_precision);

    // Compute the barrier value (without normalization)
    mpfr_set_si(tmp2, rounded_center+i-((int)_number_of_barriers-1)/2, MPFR_RNDN);
    nn_gaussian_law(tmp, tmp2);
    //mpfr_out_str(stdout, 10, 0, tmp2, MPFR_RNDN);
    //std::cout << std::endl;
    if (i==0) mpfr_set(mp_barriers[0], tmp, MPFR_RNDN);
    else
    {
      mpfr_add(mp_barriers[i], mp_barriers[i-1], tmp, MPFR_RNDN);
    }

    // Add the probability to the sum
    mpfr_add(sum, sum, tmp, MPFR_RNDN);
  }

  // Invert the sum and scale it
  mpfr_ui_div(sum, 1, sum, MPFR_RNDN);
  mpfr_set_ui(tmp, 2, MPFR_RNDN);
  mpfr_pow_ui(tmp, tmp, _bit_precision, MPFR_RNDN);
  mpfr_sub_ui(tmp, tmp, 1, MPFR_RNDN);
  mpfr_mul(sum, sum, tmp, MPFR_RNDN);

  // Now that we got the inverted sum normalize and export
  for (unsigned i = 0; i < _number_of_barriers; i++)
  {
    // Allocate space
    barriers[i] = (in_class *) calloc(_word_precision,sizeof(in_class));

    mpfr_mul(mp_barriers[i], mp_barriers[i], sum, MPFR_RNDN);
    mpfr_get_z(int_value, mp_barriers[i], MPFR_RNDN);
    mpz_export((void *) (barriers[i] + ((int)_word_precision -
           (int)ceil( (float)mpz_sizeinbase(int_value, 256)/sizeof(in_class) ))), nullptr, 1, sizeof(in_class), 0, 0, int_value);
#ifdef OUTPUT_BARRIERS
    mpz_out_str(stdout, 10, int_value);
    std::cout << " = Barriers[" << i << "] = " << std::endl;
    if (sizeof(in_class) == 1) for (unsigned j = 0 ; j < _word_precision; j++)
      printf("%.2x", barriers[i][j]);
    if (sizeof(in_class) == 2) for (unsigned j = 0 ; j < _word_precision; j++)
      printf("%.4x", barriers[i][j]);
    std::cout <<  std::endl;
#endif
    mpfr_clear(mp_barriers[i]);
  }
	mpfr_clears(sum, tmp, tmp2, nullptr);
  mpz_clear(int_value);
	mpfr_free_cache();
  free(mp_barriers);
}

//Build lookup tables used during noise generation
template<class in_class, class out_class, unsigned _lu_depth>
void FastGaussianNoise<in_class, out_class, _lu_depth>::buildLookupTables()
{
	unsigned lu_index1 = 0, lu_index2 = 0;
  _flag_ctr1 = _flag_ctr2 = 0;

  // Allocate space for the lookup tables
  lu_table = new output_t[_lu_size]();
  if(_lu_depth == 2)
    lu_table2 = (output_t **) calloc(_lu_size,sizeof(output_t *));

	// We start building the first dimension of the lookup table
  // corresponding to the first in_class word of the barriers
	for (int64_t val = -((int)_number_of_barriers-1)/2 + rounded_center, b_index = 0; val <= ((int)_number_of_barriers-1)/2 + rounded_center && lu_index1 < _lu_size;)
  {

		while (lu_index1 < barriers[b_index][0] && lu_index1 < _lu_size)
    {
      lu_table[lu_index1].val = val;
			lu_index1++;
		}

		// Flag the entry
		lu_table[lu_index1].val = val;
		lu_table[lu_index1].flag = true;
    _flag_ctr1++;
		// If _lu_depth == 1 we have to list the barriers here
    if (_lu_depth == 1)
    {
#ifdef OUTPUT_LUT_FLAGS
      std::cout << "FastGaussianNoise: flagged lu_table["
       << lu_index1 << "] for barriers " << val;
#endif

      // Prepare the first element of the chained list of barrier
			lu_table[lu_index1].l_b_ptr.push_back(barriers[b_index++]);
      val++;
			// If more that one barrier is present, we add them to the chained list
			while ( (b_index<_number_of_barriers) && (lu_index1 == barriers[b_index][0]))
      {
			  lu_table[lu_index1].l_b_ptr.push_back(barriers[b_index]);
#ifdef OUTPUT_LUT_FLAGS
      std::cout << "FastGaussianNoise: flagged lu_table[" << lu_index1 << "] for barriers " << val;
#endif
				b_index++;
				val++;
			} // while
    } // if

    if (_lu_depth == 2)
    {
      // When we meet a barrier in an entry of the lu_table,
      // we build another lu_table inside that entry
		  // corresponding to the next in_class word of the barriers
		  lu_index2 = 0;
      lu_table2[lu_index1] = new output_t[_lu_size]();
		  while (lu_index2 < _lu_size)
      {
			  if(lu_index1 < barriers[b_index][0] || lu_index2 < barriers[b_index][1])
        {
          lu_table2[lu_index1][lu_index2].val = val;
        }
        else
        {
		      // If we are on a barrier
		      if (lu_index1 == barriers[b_index][0] && lu_index2 == barriers[b_index][1])
          {
			      // Flag the entry
			      lu_table2[lu_index1][lu_index2].val = val;
			      lu_table2[lu_index1][lu_index2].flag = true;
#ifdef OUTPUT_LUT_FLAGS
            std::cout << "FastGaussianNoise: flagged lu_table2[" << lu_index1 << "][" << lu_index2 << "] for barriers " << val;
#endif
            _flag_ctr2++;
			      // And prepare the first element of the chained list of barrier
			      lu_table2[lu_index1][lu_index2].l_b_ptr.push_back(barriers[b_index++]);
            val++;
			      // If more that one barrier is present, we add them to the chained list
			      while ( (b_index<_number_of_barriers) &&
                (lu_index1 == barriers[b_index][0]) &&
                (lu_index2 == barriers[b_index][1]) )
            {
				      lu_table2[lu_index1][lu_index2].l_b_ptr.push_back(barriers[b_index]);
#ifdef OUTPUT_LUT_FLAGS
            std::cout << " " << val;
#endif
				      b_index++;
				      val++;
			      } // while
#ifdef OUTPUT_LUT_FLAGS
            std::cout << std::endl;
#endif
          } // if
        } // else
        lu_index2++;
      } // while
		} // if

		lu_index1++;
	}
  // Give some feedback
  if (_verbose) std::cout << "FastGaussianNoise: Lookup tables built" << std::endl;
}

template<class in_class, class out_class, unsigned _lu_depth>
void FastGaussianNoise<in_class, out_class, _lu_depth>::getNoise(out_class* const rand_outdata, uint64_t rlen)
{
	uint64_t computed_outputs, innoise_bytesize, innoise_words, used_words;
	int64_t output;
  bool flagged;
	in_class *noise, *noise_init_ptr, input1, input2;
  float innoise_multiplier;

  // Expected number of input bytes per output byte. Lowering this
  // (e.g. by a factor .5) works but could lead to segfaults.
  if (_lu_depth == 1)
  {
    innoise_multiplier = 1.05 * ((float)(_lu_size - _flag_ctr1)/(float)_lu_size) + _word_precision * ((float)_flag_ctr1/_lu_size);
  }
  else // _lu_depth == 2
  {
    innoise_multiplier = 1.05 * ((float)(_lu_size - _flag_ctr1)/(float)_lu_size) + 2.0 * ((float)_flag_ctr1/(float)_lu_size) + _word_precision * ((float)_flag_ctr2/((float)_lu_size*_lu_size));
  }
  innoise_words = rlen * innoise_multiplier;
  innoise_bytesize = sizeof(in_class) * innoise_words;
	noise = noise_init_ptr = new in_class[innoise_words];
  used_words = 0;
  if (_verbose) std::cout << "FastGaussianNoise: Using " << " " <<innoise_multiplier*sizeof(in_class)*8/std::min(log2(_number_of_barriers),(double)sizeof(out_class)*8) << " input bits per output bit" << std::endl;

  // Count time for uniform noise generation
	uint64_t start = rdtsc();
	fastrandombytes((uint8_t*)noise, innoise_bytesize);
	uint64_t stop = rdtsc();

  // Give some feedback
	if (_verbose) printf("FastGaussianNoise: Uniform noise  cycles = %.2e bits = %.2e cycles/bit = %.2e\n", (float) stop - start, (float) innoise_bytesize * 8, (float)(stop-start)/(innoise_bytesize*8));

  // Loop until all the outputs have been generated
  computed_outputs = 0;
	while (computed_outputs < rlen )
  {
		input1 = *noise;
		flagged = lu_table[input1].flag;

    // If flagged we have to look at the next in_class word
    if (flagged)
    {
		  if (_lu_depth == 1)
      {
				output = lu_table[input1].val;
				for(in_class* b_ptr : lu_table[input1].l_b_ptr)
        {
					// If the barrier value is greater than the noise
					if(cmp(b_ptr, noise)==1) break;
					output++;
				}
        // We shift the noise pointer of word_precision minus 1
        // As there another byte shift later
				noise += _word_precision - 1;
				used_words += _word_precision - 1;

      }
      else // _lu_depth == 2
      {
			  input2 = *(noise+1);
			  flagged = lu_table2[input1][input2].flag;
        // If flagged again we compare using full precision the random value
        // with all barriers in the linked list of the lookup table
			  if (flagged)
        {
			    output = lu_table2[input1][input2].val;
				  for(in_class* b_ptr : lu_table2[input1][input2].l_b_ptr)
          {
					  // If the barrier value is greater than the noise
					  if(cmp(b_ptr, noise)==1) break;
					  output++;
				  }
          // We shift the noise pointer of word_precision minus 2
          // As there are two other one byte shifts later
				  noise += _word_precision - 2;
				  used_words += _word_precision - 2;
			  } // if
        else
        {
			    output = lu_table2[input1][input2].val;
        }
        noise++;
        used_words++;
      } // else
    }
    else
    {
		  output = lu_table[input1].val;
    }
		noise++;
    used_words++;
		// Add the obtained result to the list of outputs
		rand_outdata[computed_outputs++] = (out_class) output;

#ifdef UNITTEST_ONEMILLION
    if ( (output > _sigma * 6) || (output < -_sigma*6) )
    {
      std::cout << output << "FastGaussianNoise: Unit test failed, this should happen once in a million. Uniform input leading to this is  ";
      for (unsigned i = 0; i < _word_precision ; i++)
        printf("%.2x", *(noise-_word_precision+i));
      std::cout << std::endl;
    }
#endif

#if 1
    // If too much noise has been used regenerate it
    if ((used_words+_word_precision) >= innoise_words)
    {
      noise = noise_init_ptr;
      used_words = 0;
      if (_verbose) std::cout << "FastGaussianNoise: All the input bits have been used, regenerating them ..." << std::endl;

	    fastrandombytes((uint8_t*)noise, innoise_bytesize);
    }
#endif
	}
	delete[] noise_init_ptr;
}

/* Compare two arrays word by word.
 * return 1 if op1 > op2, 0 if equals and -1 if op1 < op2 */
template<class in_class, class out_class, unsigned _lu_depth>
inline int FastGaussianNoise<in_class, out_class, _lu_depth>::cmp(in_class *op1, in_class *op2)
{

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

		if (op1[i] > op2[i]) return 1;

		else if (op1[i] < op2[i]) return -1;
	}
	return 0;
}

// Compute exp(-(x-center)^2/(2*sigma^2)) this is not normalized ! (hence the nn)
template<class in_class, class out_class, unsigned _lu_depth>
void  inline FastGaussianNoise<in_class, out_class, _lu_depth>::nn_gaussian_law(mpfr_t rop, const mpfr_t x)
{
	mpfr_sub(rop, x, _center, MPFR_RNDN);
	mpfr_sqr(rop, rop, MPFR_RNDN);
	mpfr_neg(rop, rop, MPFR_RNDN);
	mpfr_mul(rop, rop, _const_sigma, MPFR_RNDN);
	mpfr_exp(rop, rop, MPFR_RNDN);
}

template<class in_class, class out_class, unsigned _lu_depth>
FastGaussianNoise<in_class, out_class, _lu_depth>::~FastGaussianNoise()
{
  // Freed allocated memory for the barriers
  for (unsigned ctr = 0; ctr < _number_of_barriers; ctr++)
  {
    if (barriers[ctr] != nullptr) free(barriers[ctr]);
    barriers[ctr] = nullptr;
  }
  if (barriers != nullptr) free(barriers);
  barriers=nullptr;

  // Free other variables
  mpfr_clear(_const_sigma);
  mpfr_clear(_center);
  delete[](lu_table);
  if(_lu_depth == 2)
  {
    for (unsigned ctr = 0 ; ctr < _lu_size; ctr++)
    {
      if (lu_table2[ctr]!=nullptr) delete[](lu_table2[ctr]);
    }
    free(lu_table2);
  }
}

}  // namespace nfl

#endif


#include <iostream>
#include <fstream>
#include <algorithm>
#include <complex>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/algorithm/minmax.hpp>

#include "../include/TriMesh.h"
#include "../include/utils.h"
#include "../include/lagrange.h"
#include "../include/geometry.h"
#include "../include/ConstructCurveMesh.h"
#include "../include/GetQuadraturePointsWeight2D.h"
#include "../include/solver.h"
#include "../include/euler.h"
#include "../include/Param.h"
#include "../include/Collective.h"
#include "../include/InvertMatrix.h"

using namespace std;
using namespace utils;
using namespace lagrange;

int main(int argc, char *argv[])
{
    namespace ublas = boost::numeric::ublas;

    Param param;
    // Set up the param struct
    param = ReadParamIn(string(argv[1]));
    TriMesh mesh(param.mesh_file);
    // Testing Calculate Residaul
    int p = param.order;
    int q = param.order_geo;
    int Np = int((p + 1) * (p + 2) / 2);
    TriMesh curved_mesh = mesh;
    string boundary_name="bottom";
    ConstructCurveMesh(mesh, curved_mesh, geometry::BumpFunction, boundary_name, q);

    ublas::vector<double> States (curved_mesh.num_element * Np * 4, 0.0);
    // Construct free stream state
    ublas::vector<double> u_free = euler::CalcFreeStreamState_2DEuler(param);
    for (int ielem = 0; ielem < curved_mesh.num_element; ielem++)
    {
        for (int ip = 0; ip < Np; ip++)
        {
            States(ielem * Np * 4 + ip * 4 + 0) = u_free(0);
            States(ielem * Np * 4 + ip * 4 + 1) = u_free(1);
            States(ielem * Np * 4 + ip * 4 + 2) = u_free(2);
            States(ielem * Np * 4 + ip * 4 + 3) = u_free(3);
        }
    }

    ResData resdata, resdata_postproc;
    solver::CalcResData(curved_mesh, p, resdata);
    if (p == 0)
    {
        solver::CalcResData(curved_mesh, 1, resdata_postproc);
    }else
    {
        solver::CalcResData(curved_mesh, p, resdata_postproc);
    }

    ublas::vector<double> dt(curved_mesh.E.size());
    // ublas::vector<double> Residual = solver::CalcResidual(curved_mesh, param, resdata, States, dt, p);
    // cout << setprecision(20) << ublas::norm_2(Residual) << endl;
    ublas::vector<ublas::matrix<double> > M = lagrange::ConstructMassMatrix(p, curved_mesh, resdata);
    ublas::vector<ublas::matrix<double> > invM = lagrange::CalcInvMassMatrix(M);
    int MAXITER = param.MAXITER;
    int converged = 0;
    ublas::vector<double> States_new (curved_mesh.num_element * Np * 4, 0.0);
    ofstream file_residual;
    file_residual.open("residual.log");
    for (int niter = 0; niter < MAXITER; niter++)
    {
        // cout << niter << endl;
        double norm_residual = 0.0;
        States_new = solver::TimeMarching_TVDRK3(curved_mesh, param, resdata, States, invM, p, converged, norm_residual);
	    if (niter % param.dnOutput == 0)
	    {
            std::cout << "NITER: " << niter << "\t" << "Residual Norm_Inf: ";
            cout.setf(ios::scientific, ios::floatfield);
            std::cout << setprecision(10) << norm_residual << std::endl;
        }
        file_residual << niter << "\t" << setprecision(20) << norm_residual << std::endl;
        States = States_new;
        if (converged)
           break;
    }
    file_residual.close();

    // PostProcessing the States, the output will be the global coordinate of the lagrange nodes in each element and the states on the nodes
    int Np_solution;
    if (p == 0)
        Np_solution = 3;
    else
        Np_solution = Np;
    ublas::vector<ublas::matrix<double> > Nodes(curved_mesh.E.size(), ublas::matrix<double>(Np_solution, 2, 0.0));
    ublas::vector<ublas::matrix<double> > State_on_Nodes(curved_mesh.E.size(), ublas::matrix<double>(Np_solution, 4, 0.0));
    solver::PostProc(curved_mesh, States, p, Nodes, State_on_Nodes);
    // Write the Nodes Coordinates and States into the file
    ofstream file_nodes, file_states, file_info;
    file_nodes.open("nodes.dat");
    file_states.open("states.dat");
    file_info.open("info.dat");
    for (int ielem = 0; ielem < curved_mesh.E.size(); ielem++)
    {
        for (int ip = 0; ip < Np_solution; ip++)
        {
            file_nodes  << Nodes(ielem)(ip, 0) << ' ' << Nodes(ielem)(ip, 1) << std::endl;
            file_states << State_on_Nodes(ielem)(ip, 0) << ' ' << State_on_Nodes(ielem)(ip, 1) << ' ' <<
                            State_on_Nodes(ielem)(ip, 2) << ' ' << State_on_Nodes(ielem)(ip, 3) << std::endl;
        }
    }
    file_info << curved_mesh.E.size() << ' ' << p << std::endl;
    file_nodes.close();
    file_states.close();
    file_info.close();

    return 0;
}


#include <ctime>
#include <string>
#include <algorithm>
#include <iostream>
#include <random>
#include <math.h>
#include <vtkNew.h>
#include <vtkPolyDataReader.h>
#include <vtkPolyDataWriter.h>
#include <vtkCellLocator.h>
#include <vtkDelaunay3D.h>
#include <vtkDataSetSurfaceFilter.h>
#include <vtkPolyData.h>
#include <vtkDoubleArray.h>
#include <vtkPoints.h>
#include <vtkIdFilter.h>
#include <vtkPointData.h>
#include <vtkCellArray.h>
#include <vtkIdList.h>
#include <vtkLinearSubdivisionFilter.h>
#include <vtkGenericCell.h>
#include <vtkVertexGlyphFilter.h>
#include <vtkDataObject.h>
#include <Eigen/Dense>
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/Triangulation_vertex_base_with_info_2.h>
#include "SHTools.h"

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef CGAL::Triangulation_vertex_base_with_info_2<unsigned,K> Vb;
typedef CGAL::Triangulation_data_structure_2<Vb> Tds;
typedef CGAL::Delaunay_triangulation_2<K,Tds> Delaunay;
typedef Delaunay::Face_circulator Face_circulator;
typedef Delaunay::Face_handle Face_handle;
typedef Delaunay::Point Point;
typedef Eigen::Vector3d Vector3d;
typedef Eigen::VectorXd VectorXd;
typedef Eigen::Matrix3d Matrix3d;
typedef Eigen::Matrix3Xd Matrix3Xd;
typedef Eigen::Map<Matrix3Xd> Map3Xd;

int main(){

    // Read the polydata
    vtkNew<vtkPolyDataReader> reader;
    reader->SetFileName("T7.vtk");
    reader->Update();
    auto pointCloud = reader->GetOutput();
    int N = pointCloud->GetNumberOfPoints();

    // A lambda function to write a Eigen::Matrix3Xd to VTK file
    auto writeMatToVTK =
	[](Eigen::Matrix3Xd M, std::string file, bool Proj = false){
	    vtkNew<vtkPoints> pts;
	    vtkNew<vtkCellArray> verts;
	    for( auto b = 0; b < M.cols(); ++b ){
		double x, y, z;
		x = M(0,b);
		y = M(1,b);
		z = Proj? 0.0 : M(2,b);
		pts->InsertNextPoint(x, y, z);
		verts->InsertNextCell( 1 );
		verts->InsertCellPoint( b );
	    }
	    vtkNew<vtkPolyData> poly;
	    poly->SetPoints( pts );
	    poly->SetVerts( verts );
	    vtkNew<vtkPolyDataWriter> wr;
	    wr->SetInputData( poly );
	    wr->SetFileName( file.c_str() );
	    wr->Write();
	};

    // Test function: 2*Y_1,-1 + Y_10 + 3*Y_11, so l=1, m=-1,0,1
    int lmax = std::floor( sqrt(N) - 1 );
    Eigen::VectorXd plms( (lmax + 1)*(lmax + 2)/2 );
    auto Plm = [&plms](const size_t l, const size_t m){
	return plms( l*(l+1)/2 + m );
    };

    // Map the point coordinates into a matrix
    Map3Xd pointsOrig((double_t*)pointCloud->GetPoints()
	    ->GetData()->GetVoidPointer(0),3,N);
    Matrix3Xd points(3,N);
    points = pointsOrig;

    //**********************************************************************//
    // Create the Gauss-Lengendre grid
    int nlat, nlong;
    Eigen::VectorXd latglq(lmax + 1);
    Eigen::VectorXd longlq(2*lmax + 1);
    glqgridcoord_wrapper_(latglq.data(), longlq.data(), &lmax, &nlat, &nlong);
    auto numQ = nlat*nlong;

    Eigen::VectorXd gridglq((lmax + 1)*(2*lmax + 1));
    Eigen::VectorXd plx( (lmax + 1)*(lmax + 1)*(lmax + 2)/2 );
    Eigen::VectorXd w( lmax + 1 ), zero( lmax + 1 );
    Eigen::VectorXd cilm( 2*(lmax + 1)*(lmax + 1) );
    Eigen::VectorXd pspectrum( lmax + 1 );

    // Pre-compute all the matrices needed for expansion
    shglq_wrapper_(&lmax, zero.data(), w.data(), plx.data() );

    //Now we need to first identify which triangles each of the quadrature
    //points belongs to. For this, we need the coordinates of the points
    Eigen::Matrix3Xd Q0(3,numQ);// Original quadrature points
    Eigen::Matrix3Xd Qr(3,numQ);// Rotated quadrature points
    Eigen::Matrix3Xd Qsp(3,numQ);// StereoProjection of quadrature points

    size_t index = 0;
    for( auto ip = 0; ip < nlong; ++ip){
	auto phi = longlq(ip)*M_PI/180.0;
	auto sin_p = std::sin(phi);
	auto cos_p = std::cos(phi);
	for( auto it = 0; it < nlat; ++it){
	    auto theta = (90.0 - latglq(it))*M_PI/180.0;
	    auto sin_t = std::sin(theta);
	    Q0.col(index++) << sin_t*cos_p, sin_t*sin_p, std::cos(theta);
	}
    }

    // Write Quadrature points to VTK file
    //writeMatToVTK( Q0, "QuadPointsOrig.vtk" );

    //**********************************************************************//

    VectorXd finput(N);
    // A function to interpolate data to the quadrature points
    auto interpolate = [&points, &Q0, &gridglq, &finput](const size_t i,
	    const size_t j, const size_t k, const size_t q){
	Eigen::Vector3d v0 = points.col(i);
	Eigen::Vector3d v1 = points.col(j);
	Eigen::Vector3d v2 = points.col(k);
	Eigen::Vector3d qp = Q0.col(q);
	auto A0 = ((v1-qp).cross((v2-qp))).norm();
	auto A1 = ((v2-qp).cross((v0-qp))).norm();
	auto A2 = ((v0-qp).cross((v1-qp))).norm();
	auto A = A0 + A1 + A2;
	gridglq(q) = (A0/A)*finput(i) + (A1/A)*finput(j) + (A2/A)*finput(k);
    };

    clock_t t = clock();
    for( auto z = 0; z < 1000; ++z){

	// Project points to unit sphere
	points.colwise().normalize();

	// Reset the center of the sphere to origin by translating
	Vector3d center = points.rowwise().mean();
	points = points.colwise() - center;

	// Calculate the function to be expanded using Spherical Harmonics
	for(auto i = 0; i < N; ++i){
	    Eigen::Vector3d q;
	    q =  points.col(i);
	    double_t x = q(0);
	    double_t y = q(1);
	    double_t z = q(2);
	    plmon_wrapper_(plms.data(), &lmax, &z);
	    auto phi = std::atan2(y,x);
	    finput(i) = 2*Plm(1,1)*std::sin(phi) + Plm(1,0) +
		3*Plm(1,1)*std::cos(phi);
	}

	// Rotate all points so that the 0th point is along z-axis
	Vector3d c = points.col(0);
	double_t cos_t = c(2);
	double_t sin_t = std::sqrt( 1 - cos_t*cos_t );
	Vector3d axis;
	axis << c(1), -c(0), 0.;
	axis.normalize();
	Matrix3d rotMat, axis_cross, outer;
	axis_cross << 0. , -axis(2), axis(1),
		   axis(2), 0., -axis(0),
		   -axis(1), axis(0), 0.;

	outer.noalias() = axis*axis.transpose();

	rotMat = cos_t*Matrix3d::Identity() + sin_t*axis_cross + (1-cos_t)*outer;
	Matrix3Xd rPts(3,N);
	rPts = rotMat*points; // The points on a sphere rotated

	// Write the rotated points to VTK
	//writeMatToVTK( rPts, "RotBasePoints.vtk" );

	// Calculate the stereographic projections
	Vector3d p0;
	Map3Xd l0( &(rPts(0,1)), 3, N-1 );
	Matrix3Xd l(3,N-1), proj(3,N-1);
	p0 << 0,0,-1; // Point on the plane of projection
	c = rPts.col(0); // The point from which we are projecting
	l = (l0.colwise() - c).colwise().normalized(); // dirns of projections
	for( auto j=0; j < N-1; ++j ){
	    proj.col(j) = ((p0(2) - l0(2,j))/l(2,j))*l.col(j) + l0.col(j);
	}

	// Write the rotated points to VTK
	//writeMatToVTK( proj, "ProjBasePoints.vtk" );

	// Insert the projected points in a CGAL vertex_with_info vector
	std::vector< std::pair< Point, unsigned> > verts;
	for( auto j=0; j < N-1; ++j ){
	    verts.push_back(std::make_pair(Point(proj(0,j),proj(1,j)),j+1));
	}

	// Triangulate
	Delaunay dt( verts.begin(), verts.end() );

	/*
	// Write the triangulation to file
	vtkNew<vtkPolyData> sphere;
	vtkNew<vtkCellArray> sphereTri;
	vtkNew<vtkPoints> spherePts;
	for(auto zz = 0; zz < N; ++zz){
	Vector3d pp = points.col(zz);
	spherePts->InsertNextPoint( &pp(0) );
	}
	for( auto fc = dt.all_faces_begin(); fc != dt.all_faces_end(); ++fc ){
	sphereTri->InsertNextCell(3);
	for( auto zz = 2; zz >= 0; --zz ){
	auto vid = dt.is_infinite( fc->vertex( zz ) )? 0 :
	fc->vertex(zz)->info();
	sphereTri->InsertCellPoint(vid);
	}
	}
	sphere->SetPoints( spherePts );
	sphere->SetPolys( sphereTri );
	vtkNew<vtkPolyDataWriter> wr;
	wr->SetInputData( sphere );
	wr->SetFileName( "UnitSphere.vtk" );
	wr->Write();
	*/

	// Rotate and project the quadrature points
	Qr = rotMat*Q0; // Rotate the quadrature points

	// Write the rotated points to VTK
	//writeMatToVTK( Qr, "RotQuadPoints.vtk" );

	Eigen::Matrix3Xd lQ(3,numQ);
	lQ = (Qr.colwise() - c).colwise().normalized();//projn dirn unit vectors
	for( auto j=0; j < numQ; ++j ){
	    Qsp.col(j) = ((p0(2) - Qr(2,j))/lQ(2,j))*lQ.col(j) + Qr.col(j);
	}

	// Write Stereographic projections of quadrature points to VTK file
	//writeMatToVTK( Qsp, "ProjQuadPoints.vtk" );

	// Locate the quadrature points using stereographic triangulation
	for( auto j=0; j < numQ; ++j ){
	    auto query = Point( Qsp(0,j), Qsp(1,j) );
	    Delaunay::Locate_type lt;
	    int li;
	    Face_handle face = dt.locate( query, lt, li );
	    switch(lt){
		case Delaunay::FACE:
		    {
			auto id0 = face->vertex(0)->info();
			auto id1 = face->vertex(1)->info();
			auto id2 = face->vertex(2)->info();
			interpolate( id0, id1, id2, j );
			break;
		    }
		case Delaunay::EDGE:
		    {
			auto id1 = face->vertex( (li + 1)%3 )->info();
			auto id2 = face->vertex( (li + 2)%3 )->info();
			Eigen::Vector3d v1, v2, qp;
			v1 = points.col(id1);
			v2 = points.col(id2);
			qp = Q0.col(j);
			double_t ratio = (qp - v1).norm()/(qp - v2).norm();
			gridglq(j) = (finput(id1) +
				ratio*finput(id2))/(1 + ratio);
			break;
		    }
		case Delaunay::VERTEX:
		    gridglq(j) = finput( face->vertex( li )->info() );
		    break;
		case Delaunay::OUTSIDE_CONVEX_HULL:
		    {
			Eigen::Vector3d v0, v1, v2;
			auto id0 = dt.is_infinite(face->vertex(0))?
			    0 : face->vertex(0)->info();
			auto id1 = dt.is_infinite(face->vertex(1))?
			    0 : face->vertex(1)->info();
			auto id2 = dt.is_infinite(face->vertex(2))?
			    0 : face->vertex(2)->info();
			interpolate( id0, id1, id2, j );
			break;
		    }
		default:
		    std::cout<< "Quadrature point " << j << " not found!"
			<< std::endl;
	    }
	}

	// Expand using Gauss-Legendre Quadrature and get the power spectrum
	shexpandglq_wrapper_( cilm.data(), &lmax, gridglq.data(), w.data(),
		plx.data());
	shpowerspectrum_wrapper_( cilm.data(), &lmax, pspectrum.data() );
    }
    t = clock() - t;
    std::cout<< "Time for 1000 steps GLQ = " << ((float)t)/CLOCKS_PER_SEC
	<< std::endl;

    // Cross-check
    Eigen::VectorXd gridglq_out( (lmax + 1)*(2*lmax + 1) );
    makegridglq_wrapper_(gridglq_out.data(), cilm.data(), &lmax, plx.data());
    std::cout<< "Error norm of GLQ = " << (gridglq - gridglq_out).norm()
	<< std::endl;

    // Print the coefficients
    std::cout<< "l\tm\tAlm_GLQ" << std::endl;
    for( int l = 0; l <= lmax; ++l ){
	for( int m = -l; m <=l; ++m ){
	    int i = m < 0? 1 : 0;
	    int n = m < 0? -m: m;
	    std::cout<< l << "\t" << m << "\t"
		<< cilm( i + 2*(l + n*(lmax + 1)) )<< "\t"
		<< std::endl;
	}
    }
    return 0;
}