#include <frovedis.hpp>
#include <frovedis/matrix/blas_wrapper.hpp>

#define BOOST_TEST_MODULE FrovedisTest
#include <boost/test/unit_test.hpp>
#include "../../rmse.hpp"

using namespace frovedis;
using namespace std;

BOOST_AUTO_TEST_CASE( frovedis_test )
{
    int argc = 1;
    char** argv = NULL;
    use_frovedis use(argc, argv);

    // creating a colmajor matrix local from file
    colmajor_matrix_local<float> cm1 (
    make_rowmajor_matrix_local_load<float>("./sample_3x4"));
    auto cm2 = cm1;

    // slicing row and col
    auto row1 = make_row_vector<float> (cm1,0); // (4x1)
    auto col1 = make_col_vector<float> (cm1,0); // (3x1)

    // col1-of-cm1 = cm2*row1-of-cm1
    gemv<float>(cm2,row1,col1); // (3x4) * (4x1) => (OK)

    bool isError = false;
    // row1-of-cm1 = cm2*col1-of-cm1
    try {
      gemv<float>(cm2,col1,row1); // (3x4) * (3x1) => (Error)
    }
    catch (exception& e) {
      isError = true;
    }
    BOOST_CHECK (isError);

    // row1-of-cm1 = trans(cm2)*col1-of-cm1
    gemv<float>(cm2,col1,row1,'T'); // (4x3) * (3x1) => (OK)

    double tol = 0.01;
    colmajor_matrix_local<float> ref (
    make_rowmajor_matrix_local_load<float> ("./ref_3x4"));
    BOOST_CHECK (calc_rms_err<float> (cm1.val, ref.val) < tol);
}


/*
Test inner shortened RS code over binary extension field
*/

#include <iostream>
#include <fstream>
#include <vector>
#include <boost/program_options.hpp>
#include "../include/DFT.hpp"
#include "../include/helpers.hpp"
#include "../include/GF2M.hpp"
//#include "../include/encodedecode.hpp"
#include "../include/ReedSolomon.hpp"
#include <string>
#include <fstream>
#include <streambuf>
#include <random>
#include <chrono>

using namespace std;

// define static variables

int main(int ac, char* av[])
{

// GF(2^6) with primitive polynomail 91
const unsigned m = 6;
const unsigned prim_poly = 91;
typedef GF2M<m,prim_poly> gf;

// randomness
random_device r;
default_random_engine gen(r());
uniform_int_distribution<int> randbit(0, 1);
uniform_int_distribution<int> randint(0, 63);

// parameters
const unsigned N = 20;
const unsigned N_u = 63; // the underlying length of the shortened inner code
const unsigned K = 18;
const unsigned Qi = 9;
const unsigned Pi = 7; //P*Q = 2^6 - 1

///// GF2M definitions

gf fa = gf(2,0); // 64 + 2 = 1000010 = x + x^5

cout << "primitive polynomial: " << bitset<32>(prim_poly) << endl;
cout << "                      " <<  bitset<32>( (1 << m) ) << endl;
cout << "order of a: " << fa.order() << endl;

DFT_FFT<gf> dftgf(N_u,fa,Pi,Qi); // Fourier transform for the outer code
//DFT_PRIM<gf> dftgf(N_u,fa);

//RScode<gf,DFT_PRIM<gf>> innercode(N,K,fa,dftgf,N_u);
RScode<gf, DFT_FFT<gf> > innercode(N,K,fa,dftgf,N_u);

///// test the inner code

// generate information

vector<gf> infvec(innercode.k); // information vector;
for(gf& el: infvec)
	el = gf(randbit(gen),0);

// encode

vector<gf> c; // outer codeword
innercode.RS_shortened_encode(infvec,c);

// disturb

uniform_int_distribution<int> randcoef(0, c.size()-1);

// insert one random error
// c[randcoef(gen)] = gf(randbit(gen),0);

// insert one random error
c[randcoef(gen)] = gf();
c[randcoef(gen)] = gf();

// decoding

auto t1 = chrono::high_resolution_clock::now();
vector<gf> infvecrec;
pair<unsigned,unsigned> erctroc = innercode.RS_shortened_decode(infvecrec,c);
auto t2 = chrono::high_resolution_clock::now();
cout << "decoding took "
     << std::chrono::duration_cast<std::chrono::milliseconds>(t2-t1).count()
     << " milliseconds\n";

cout << "outer code: " << erctroc.first << " errasures, " << erctroc.second << " errors corrected"<< endl;

if( infvec ==  infvecrec)
	cout << "I did correct all errors!" << endl;
else
	cout << "I couldn't correct all errors!" << endl;

}


#include <blitz/array.h>

using namespace blitz;

int main()
{
    Array<int,2> A(6,6), B(3,3);

    // Set the upper left quadrant of A to 5
    A(Range(0,2), Range(0,2)) = 5;

    // Set the upper right quadrant of A to an identity matrix
    B = 1, 0, 0,
        0, 1, 0,
        0, 0, 1;
    A(Range(0,2), Range(3,5)) = B;

    // Set the fourth row to 1
    A(3, Range::all()) = 1;

    // Set the last two rows to 0
    A(Range(4, toEnd), Range::all()) = 0;

    // Set the bottom right element to 8
    A(5,5) = 8;

    cout << "A = " << A << endl;

    return 0;
}


#include <iostream>
#include <algorithm>
#include <iterator>
#include <cmath>
#include <fstream>
#include <valarray>
#include <sstream>
#include <complex>
#include <tuple>

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

#include "miMaS/field.h"
#include "miMaS/complex_field.h"
#include "miMaS/weno.h"
#include "miMaS/fft.h"
#include "miMaS/array_view.h"
#include "miMaS/poisson.h"
#include "miMaS/splitting.h"
#include "miMaS/lagrange5.h"

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

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

#define ping(X) std::cerr << __LINE__ << " " << #X << ":" << X << std::endl
int debug = 0;

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

int main(int,char**)
{
  std::size_t Nx = 135, Nv = 256;

  // $(u_c,E,\hat{f}_h)$ and $f_h$
  ublas::vector<double> uc(Nx,0.);
  ublas::vector<double> E (Nx,0.);
  field<double,1> fh(boost::extents[Nv][Nx]);
  complex_field<double,1> hfh(boost::extents[Nv][Nx]);

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

  // compute dx, dv
  fh.step.dv = (fh.range.v_max-fh.range.v_min)/Nv;
  fh.step.dx = (fh.range.x_max-fh.range.x_min)/Nx;

  double dt = 0.05;//1.*fh.step.dv;
  double Tf = 200.;

  // velocity and frequency
  ublas::vector<double> v (Nv,0.); for ( std::size_t k=0 ; k<Nv ; ++k ) { v[k] = Vk(k); }
  const double l = fh.range.x_max-fh.range.x_min;
  ublas::vector<double> kx(Nx);
  for ( auto i=0 ; i<Nx/2 ; ++i ) { kx[i]    = 2.*math::pi<double>()*i/l; }
  for ( int i=-Nx/2 ; i<0 ; ++i ) { kx[Nx+i] = 2.*math::pi<double>()*i/l; }

  // initial condition
  double ui=2., alpha=0.2;
  auto tb_M1 = maxwellian(0.5*alpha,ui,1.) , tb_M2 = maxwellian(0.5*alpha,-ui,1.);
  for (field<double,2>::size_type k=0 ; k<fh.size(0) ; ++k ) {
    for (field<double,2>::size_type i=0 ; i<fh.size(1) ; ++i ) {
      //fh[k][i] = ( 0.5*alpha/std::sqrt(2.*math::pi<double>())*std::exp(-0.5*SQ(Vk(k)-ui)) + 0.5*alpha/std::sqrt(2.*math::pi<double>())*std::exp(-0.5*SQ(Vk(k)+ui)) )*(1.+0.04*std::cos(0.3*Xi(i)));
      //fh[k][i] = ( 0.5*alpha/std::sqrt(2.*math::pi<double>())*std::exp(-0.5*SQ(Vk(k)-ui)) + 0.5*alpha/std::sqrt(2.*math::pi<double>())*std::exp(-0.5*SQ(Vk(k)+ui)) )*(1.+0.04*std::cos(Kx*Xi(i)));

      fh[k][i] = ( tb_M1(Xi(i),Vk(k)) + tb_M2(Xi(i),Vk(k)) )*(1. + 0.01*std::cos(Kx*Xi(i)));
    }
    fft::fft(&(fh[k][0]),&(fh[k][Nx-1])+1,&(hfh[k][0]));
  }
  fh.write("vphl/split/init.dat");

  std::cout << "Nx: " << Nx << "\n";
  std::cout << "Nv: " << Nv << "\n";
  std::cout << "v_min: " << fh.range.v_min << "\n";
  std::cout << "v_max: " << fh.range.v_max << "\n";
  std::cout << "x_min: " << fh.range.x_min << "\n";
  std::cout << "x_max: " << fh.range.x_max << "\n";
  std::cout << "dt: " << dt << "\n";
  std::cout << "dx: " << fh.step.dx << "\n";
  std::cout << "dv: " << fh.step.dv << "\n";
  std::cout << "Tf: " << Tf << "\n";
  std::cout << "f_0: " << "\"tb\"" << "\n";
  std::cout << std::endl;

  const double rho_c = 1.-alpha;
  // init E (electric field) with Poisson
  {
    poisson<double> poisson_solver(Nx,l);
    ublas::vector<double> rho(Nx,0.);
    rho = fh.density(); // compute density from init data
    for ( auto i=0 ; i<Nx ; ++i ) { rho[i] += rho_c; } // add (1-alpha) for cold particules
    E = poisson_solver(rho);
  }

  // monitoring data
  std::vector<double> ee;
  std::vector<double> Emax;
  std::vector<double> times;

  times.push_back(0);
  {
    double electric_energy = 0.;
    for ( const auto & ei : E ) { electric_energy += ei*ei*fh.step.dx; }
    ee.push_back( std::sqrt(electric_energy) );
  }
  Emax.push_back( std::abs(*std::max_element( E.begin() , E.end() , [](double a,double b){return (std::abs(a) < std::abs(b));} )) );

  //U_type<double,1> U({uc,E,hfh};
  splitting<double,1> Lie( fh , l , rho_c );

  std::size_t i_t = 0;
  double current_time = 0.;
  while ( current_time < Tf ) {
    std::cout << " [" << std::setw(5) << i_t << "] " << i_t*dt << "\r" << std::flush;

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

    Emax.push_back( std::abs(*std::max_element( E.begin() , E.end() , [](double a,double b){return (std::abs(a) < std::abs(b));} )) );

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

    current_time += dt;
    ++i_t;
    times.push_back( current_time );
  } // while current_time < Tf
  std::cout<<" ["<<std::setw(5)<<i_t<<"] "<<i_t*dt<<std::endl;

  std::ofstream of;
  std::size_t count = 0;
  auto dt_y = [&,count=0](auto const& y) mutable { std::stringstream ss; ss<<times[count++]<<" "<<y; return ss.str(); };

  of.open("vphl/split/ee.dat");
  std::transform( ee.begin() , ee.end() , std::ostream_iterator<std::string>(of,"\n") , dt_y );
  of.close();

  of.open("vphl/split/Emax.dat");
  std::transform( Emax.begin() , Emax.end() , std::ostream_iterator<std::string>(of,"\n") , dt_y );
  of.close();

  for ( auto k=0 ; k<hfh.shape()[0] ; ++k ) { fft::ifft(&(hfh[k][0]),&(hfh[k][Nx-1])+1,&(fh[k][0])); }
  fh.write("vphl/split/vp.dat");

  return 0;
}


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

#include <cgi/logger/logger.hpp>
#include <cgi/mosaic/Mosaic.hpp>
#include <cgi/mosaic/MosaicUtility.hpp>
#include <boost/filesystem/convenience.hpp>

static const int nBands = 8;

namespace Eigen {
  typedef Eigen::Matrix<float, nBands, nBands> Matrix8f;
  typedef Eigen::Matrix<float, nBands, 1> Vector8f;
}

Eigen::Vector8f v2v(const std::vector<unsigned short>& sVec)
{
  Eigen::Vector8f eVec;
  for(unsigned int i=0; i<sVec.size(); ++i)
    eVec(i,0) = sVec[i];
  return eVec;
}

std::vector<unsigned short> v2v(const Eigen::VectorXf& eVec)
{
  std::vector<unsigned short> sVec(eVec.rows());
  for(unsigned int i=0; i<sVec.size(); ++i)
    sVec[0] = eVec(i,0);
  return sVec;
}

cgi::Tile<unsigned char> threshold(const cgi::Tile<float>& inTile, const Eigen::VectorXf& thresh)
{
  const cgi::core::Size2d<int>& size(inTile.getSize());
  const int height = size.getHeight();
  const int width = size.getWidth();
  const int depth = inTile.getBandCount();

  cgi::Tile<unsigned char> outTile(size, depth, 0);

  for(int r=0; r<height; ++r)
    {
      for(int c=0; c<width; ++c)
	{
	  for(int b=0; b<depth; ++b)
	    {
	      if(inTile[b](r,c) > thresh(b,0))
		outTile[b](r,c) = 255;
	    }
	}
    }

  return outTile;
}

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

int main(int argc, char** argv)
{
  if(argc != 4)
    {
      std::cerr << "USAGE: " << argv[0] << " <input filename> <output filename> <number of output levels>" << std::endl;
      return 1;
    }

  cgi::log::MetaLogStream::instance().setTargets(cgi::log::STDOUT);
  cgi::log::MetaLogStream::instance().setPriorityThreshold(cgi::log::Priority::DEBUG);
  cgi::log::MetaLogStream& log (cgi::log::MetaLogStream::instance());
  ////////////////////////////////////////////////////////
  ////////////////////////////////////////////////////////

  const std::string outputFile(argv[2]);
  CGI_THROW_IF(true == boost::filesystem::exists(outputFile), std::runtime_error);

  const int l = boost::lexical_cast<int>(argv[3]);

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

  cgi::Mosaic m;
  CGI_THROW_IF(cgi::NoError != m.open(argv[1]), std::runtime_error);

  CGI_THROW_IF(nBands != m.getBandCount(), std::runtime_error);

  m.setTileSize(cgi::core::Size2d< int >(1000,1000));

  const cgi::core::Size2d< int > &size = m.getTileSize();
  const int numRows = size.getHeight();
  const int numCols = size.getWidth();

  //const int everyNthTile = m.getColumnCount()/2 - 1;
  const int everyNthTile = 100;

  // initialization
  Eigen::Vector8f mean = Eigen::Vector8f::Zero();
  int validPixelCount = 0;

  log << cgi::log::Priority::INFO << "eigen_dmp" << "Calculating mean for..." << cgi::log::flush;

  // Calculate the mean vector across all tiles
  for(int i=0; i<m.getTileCount(); ++i)
    {
      if(0 != (i % everyNthTile))
	continue;

      log << cgi::log::Priority::INFO << "eigen_dmp" << "Tile " << i << " of " << m.getTileCount() << cgi::log::flush;

      const cgi::Tile<unsigned short> t(m.getTile<unsigned short>(i));
      const boost::numeric::ublas::matrix< bool > mask (t.getValidMask(cgi::valid_mask::ALL));

      for(int r=0; r<numRows; ++r)
	{
	  for(int c=0; c<numCols; ++c)
	    {
	      if(mask(r,c))
		{
		  const Eigen::Vector8f eVec(v2v(t(r,c)));
		  mean += eVec;
		  ++validPixelCount;
		}
	    }
	}

    }

  mean /= validPixelCount;
  std::cout << "mean = " << std::endl << mean << std::endl;

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

  //
  // Calculate the covariance matrix
  //

  Eigen::Matrix8f cov = Eigen::Matrix8f::Zero();

  log << cgi::log::Priority::INFO << "eigen_dmp" << "Calculating covariance for..." << cgi::log::flush;

  for(int i=0; i<m.getTileCount(); ++i)
    {
      if(0 != (i % everyNthTile))
	continue;

      log << cgi::log::Priority::INFO << "eigen_dmp" << "Tile " << i << " of " << m.getTileCount() << cgi::log::flush;

      const cgi::Tile<unsigned short> t(m.getTile<unsigned short>(i));
      const boost::numeric::ublas::matrix< bool > mask (t.getValidMask(cgi::valid_mask::ALL));

      for(int r=0; r<numRows; ++r)
	{
	  for(int c=0; c<numCols; ++c)
	    {
	      if(mask(r,c))
		{

		  const Eigen::Vector8f eVec(v2v(t(r,c)));
		  cov += (eVec - mean) * (eVec - mean).transpose();
		}
	    }
	}
    }

  cov /= validPixelCount;
  //cov -= mean * mean.transpose();
  std::cout << "cov = " << std::endl << cov << std::endl;

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

  Eigen::SelfAdjointEigenSolver<Eigen::Matrix8f> eigensolver(cov);

  if (eigensolver.info() != Eigen::Success)
    abort();

  std::cout << "The eigenvalues of A are:" << std::endl << eigensolver.eigenvalues() << std::endl;
  std::cout << "Here's a matrix whose columns are eigenvectors of A " << std::endl
	    << "corresponding to these eigenvalues:" << std::endl
	    << eigensolver.eigenvectors() << std::endl;

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

  Eigen::Vector8f evals = eigensolver.eigenvalues();
  evals /= evals(evals.rows()-1, 0);
  std::cout << "Cummulative energy content for each eigenvector: " << std::endl
	    << evals << std::endl;

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

  //const Eigen::Vector8f z = eigensolver.eigenvectors().col(nBands-1);
  Eigen::MatrixXf zz(nBands,l);
  zz = eigensolver.eigenvectors().block(0,nBands-l, nBands,l);

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

  Eigen::VectorXf meanOfEigenImage = zz.transpose() * mean;
  std::cout << "meanOfEigneImage = " << std::endl
	    << meanOfEigenImage << std::endl;

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

  const cgi::Tile<unsigned short> t(m.getTile<unsigned short>(m.getTileIndex1D(m.getRowCount()/2, m.getColumnCount()/2)));
  cgi::Tile<float> tt(t.getSize(), l);

  for(int r=0; r<numRows; ++r)
    {
      for(int c=0; c<numCols; ++c)
        {
	  const Eigen::Vector8f eVec(v2v(t(r,c)));

	  //const float pp = z.transpose() * (eVec - mean);
	  const Eigen::VectorXf ppv (zz.transpose() * (eVec - mean));

	  for(int b=0; b < ppv.rows(); ++b)
	    tt[b](r,c) = ppv(b,0);
        }
    }

  cgi::MosaicUtility::writeImage<unsigned short>(t, "dmp.tif");
  cgi::MosaicUtility::writeImage<float>(tt, "out.tif");

  cgi::MosaicUtility::writeImage<unsigned char>(threshold(tt, 3*meanOfEigenImage), "thresh.tif");

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

  return 0;

  /*
  Eigen::Matrix8f A;

  //srand(time(NULL));

  const int n = nBands;
  for(int i=0; i<n; ++i)
    for(int j=i; j<n; ++j)
      A(i,j) = A(j,i) = (-1 + (2 * (rand()%2))) * (rand() / static_cast<float>(RAND_MAX));

  cout << "Here is the matrix A:\n" << A << endl;
  Eigen::SelfAdjointEigenSolver<Eigen::Matrix8f> eigensolver(A);
  if (eigensolver.info() != Eigen::Success) abort();
  cout << "The eigenvalues of A are:\n" << eigensolver.eigenvalues() << endl;
  cout << "Here's a matrix whose columns are eigenvectors of A \n"
       << "corresponding to these eigenvalues:\n"
       << eigensolver.eigenvectors() << endl;

  */
}


//#define BOOST_MATH_ASSERT_UNDEFINED_POLICY false
//#define BOOST_MATH_OVERFLOW_ERROR_POLICY errno_on_error

#include <boost/math/distributions/bernoulli.hpp>
#include <boost/math/distributions/beta.hpp>
#include <boost/math/distributions/binomial.hpp>
#include <boost/math/distributions/cauchy.hpp>
#include <boost/math/distributions/chi_squared.hpp>
#include <boost/math/distributions/exponential.hpp>
#include <boost/math/distributions/extreme_value.hpp>
#include <boost/math/distributions/fisher_f.hpp>
#include <boost/math/distributions/gamma.hpp>
#include <boost/math/distributions/geometric.hpp>
#include <boost/math/distributions/laplace.hpp>
#include <boost/math/distributions/lognormal.hpp>
#include <boost/math/distributions/negative_binomial.hpp>
#include <boost/math/distributions/non_central_chi_squared.hpp>
#include <boost/math/distributions/normal.hpp>
#include <boost/math/distributions/poisson.hpp>
#include <boost/math/distributions/students_t.hpp>
#include <boost/math/distributions/triangular.hpp>
#include <boost/math/distributions/uniform.hpp>
#include <boost/math/distributions/weibull.hpp>

#include <boost/log/trivial.hpp>
#include <boost/log/attributes/scoped_attribute.hpp>

#include "GenericDistribution.h"

namespace Distribution
{
namespace Detail
{
    namespace bm = boost::math;

    struct pdf_visitor : boost::static_visitor<>
    {
        pdf_visitor(QVector<double> const& x, QVector<double>& y) :
            _x(x), _y(y) {}

        // Uniform Distributions
        void operator()(UniformInt const& d) const {
            auto m = bm::uniform_distribution<>(d.a(), d.b());
            for(const auto &x : _x) _y.push_back(bm::pdf(m, x));
        }
        void operator()(UniformReal const& d) const {
            auto m = bm::uniform_distribution<>(d.a(), d.b());
            for(const auto &x : _x) _y.push_back(bm::pdf(m, x));
        }
        void operator()(Binomial const& d) const {
            auto m = bm::binomial_distribution<>(d.t(), d.p());
            for(const auto &x : _x) _y.push_back(bm::pdf(m, x));
        }
        void operator()(Geometric const& d) const {
            auto m = bm::geometric_distribution<>(d.p());
            for(const auto &x : _x) _y.push_back(bm::pdf(m, x));
        }
        void operator()(NegativeBinomial const& d) const {
            auto m = bm::negative_binomial_distribution<>(d.k(), d.p());
            for(const auto &x : _x) _y.push_back(bm::pdf(m, x));
        }
        // Poisson Distributions
        void operator()(Poisson const& d) const {
            auto m = bm::poisson_distribution<>(d.mean());
            for(const auto &x : _x) _y.push_back(bm::pdf(m, x));
        }
        void operator()(Exponential const& d) const {
            auto m = bm::exponential_distribution<>(d.lambda());
            for(const auto &x : _x) _y.push_back(bm::pdf(m, x));
        }
        void operator()(Gamma const& d) const {
            auto m = bm::gamma_distribution<>(d.alpha(), d.beta());
            for(const auto &x : _x) _y.push_back(bm::pdf(m, x));
        }
        void operator()(Weibull const& d) const {
            auto m = bm::weibull_distribution<>(d.a(), d.b());
            for(const auto &x : _x) _y.push_back(bm::pdf(m, x));
        }
        void operator()(ExtremeValue const& d) const {
            auto m = bm::extreme_value_distribution<>(d.a(), d.b());
            for(const auto &x : _x) _y.push_back(bm::pdf(m, x));
        }
        void operator()(Beta const& d) const {
            auto m = bm::beta_distribution<>(d.alpha(), d.beta());
            for(const auto &x : _x) _y.push_back(bm::pdf(m, x));
        }
        void operator()(Laplace const& d) const {
            auto m = bm::laplace_distribution<>(d.mean(), d.beta());
            for(const auto &x : _x) _y.push_back(bm::pdf(m, x));
        }
        // Normal Distributions
        void operator()(Normal const& d) const {
            auto m = bm::normal_distribution<>(d.mean(), d.sigma());
            for(const auto &x : _x) _y.push_back(bm::pdf(m, x));
        }
        void operator()(Lognormal const& d) const {
            auto m = bm::lognormal_distribution<>(d.m(), d.s());
            for(const auto &x : _x) _y.push_back(bm::pdf(m, x));
        }
        void operator()(ChiSquared const& d) const {
            auto m = bm::chi_squared_distribution<>(d.n());
            for(const auto &x : _x) _y.push_back(bm::pdf(m, x));
        }
        void operator()(NCChiSquared const& d) const {
            auto m = bm::non_central_chi_squared_distribution<>(d.k(), d.lambda());
            for(const auto &x : _x) _y.push_back(bm::pdf(m, x));
        }
        void operator()(Cauchy const& d) const {
            auto m = bm::cauchy_distribution<>(d.median(), d.sigma());
            for(const auto &x : _x) _y.push_back(bm::pdf(m, x));
        }
        void operator()(FisherF const& d) const {
            auto m = bm::fisher_f_distribution<>(d.m(), d.n());
            for(const auto &x : _x) _y.push_back(bm::pdf(m, x));
        }
        void operator()(StudentT const& d) const {
            auto m = bm::students_t_distribution<>(d.n());
            for(const auto &x : _x) _y.push_back(bm::pdf(m, x));
        }
        // Sampling Distributions
        void operator()(Discrete const& d) const {
            _y = QVector<double>::fromStdVector(d.probabilities());
        }
        void operator()(PiecewiseConstant const& d) const {
            _y = QVector<double>::fromStdVector(d.densities());
        }
        void operator()(PiecewiseLinear const& d) const {
            _y = QVector<double>::fromStdVector(d.densities());
        }
        // Miscellaneous Distributions
        void operator()(Triangle const& d) const {
            auto m = bm::triangular_distribution<>(d.a(), d.b(), d.c());
            for(const auto &x : _x) _y.push_back(bm::pdf(m, x));
        }
        // Placeholder
        void operator()(Constant const& d) const {
            for(const auto &x : _x) _y.push_back(d.a());
        }

    private:
        const QVector<double> &_x;
        QVector<double> &_y;
    };

} // namespace Distribution
} // namespace Detail

void GenericDistribution::pdf(QVector<double> const& x, QVector<double> &y) const
{
    BOOST_LOG_SCOPED_THREAD_TAG("Tag", "Distribution");

    y.reserve(x.size());
    try {
        boost::apply_visitor(Distribution::Detail::pdf_visitor(x, y), *this);
    } catch (const std::exception &e) {
        BOOST_LOG_TRIVIAL(warning) << e.what();
        return;
    }
}


#ifndef MATH_HPP
#define MATH_HPP

#include <armadillo>

void build_psi_tht_phi_TM(const double &psi, const double &tht, const double &phi, arma::mat &AMAT);
arma::mat skew_sym(arma::vec3 const &vec);
void Matrix2Quaternion(arma::mat33 Matrix_in, arma::vec &Quaternion);
void Quaternion2Matrix(arma::vec4 const &Quaternion_in, arma::mat &Matrix_out);
arma::vec3 euler_angle(const arma::mat33 &TBD_in);
int sign(const double &variable);

#endif


#include <cmath>
#include <vector>
#include <catch2/catch.hpp>
#include <Eigen/Dense>
#include "factory.h"
#include "normal_dist.h"

TEST_CASE("Test different distribution types", "[Distributions]") {

  SECTION("Test CDF/ICDF values for normal distribution") {
    double mean = 0.0;
    double std_dev = 1.0;
    auto test_distribution =
        Factory<stochastic::Distribution, double, double>::instance()->create(
            "NormalDist", std::move(mean), std::move(std_dev));

    std::vector<double> locations = {0.0};

    auto probabilities = test_distribution->cumulative_dist_func(locations);
    auto calced_locations = test_distribution->inv_cumulative_dist_func(probabilities);

    REQUIRE(probabilities[0] == Approx(0.5).epsilon(0.01));
    REQUIRE(calced_locations[0] == Approx(0.0).epsilon(0.01));
  }

  SECTION("Test CDF/ICDF values for lognormal distribution") {
    double mean = 0.0;
    double std_dev = 0.25;
    auto test_distribution =
        Factory<stochastic::Distribution, double, double>::instance()->create(
            "LognormalDist", std::move(mean), std::move(std_dev));

    std::vector<double> locations = {1.0};

    auto probabilities = test_distribution->cumulative_dist_func(locations);
    auto calced_locations = test_distribution->inv_cumulative_dist_func(probabilities);

    REQUIRE(probabilities[0] == Approx(0.5).epsilon(0.01));
    REQUIRE(calced_locations[0] == Approx(1.0).epsilon(0.01));
  }

  SECTION("Test CDF/ICDF values for Beta distribution") {
    double alpha = 0.5;
    double beta = 0.5;
    auto test_distribution =
        Factory<stochastic::Distribution, double, double>::instance()->create(
            "BetaDist", std::move(alpha), std::move(beta));

    std::vector<double> locations = {0.5};

    auto probabilities = test_distribution->cumulative_dist_func(locations);
    auto calced_locations = test_distribution->inv_cumulative_dist_func(probabilities);

    REQUIRE(probabilities[0] == Approx(0.5).epsilon(0.01));
    REQUIRE(calced_locations[0] == Approx(0.5).epsilon(0.01));
  }

  SECTION("Test CDF/ICDF values for inverse Gaussian distribution") {
    double mean = 1.0;
    double std_dev = 1.0;
    auto test_distribution =
        Factory<stochastic::Distribution, double, double>::instance()->create(
            "InverseGaussianDist", std::move(mean), std::move(std_dev));

    std::vector<double> locations = {0.01, 0.675841, 20};

    auto probabilities = test_distribution->cumulative_dist_func(locations);
    auto calced_locations = test_distribution->inv_cumulative_dist_func(probabilities);

    REQUIRE(probabilities[0] + 1.0 == Approx(1.0).epsilon(0.01));
    REQUIRE(probabilities[1] == Approx(0.5).epsilon(0.01));
    REQUIRE(probabilities[2] == Approx(1.0).epsilon(0.01));
    REQUIRE(calced_locations[0] == Approx(locations[0]).epsilon(0.01));
    REQUIRE(calced_locations[1] == Approx(locations[1]).epsilon(0.01));
    REQUIRE(calced_locations[2] == Approx(locations[2]).epsilon(0.01));
  }

  SECTION("Test CDF/ICDF values for Student's t distribution") {
    double mean = 1.0;
    double scale = 0.25;
    double dof = 1.0;
    auto test_distribution =
        Factory<stochastic::Distribution, double, double, double>::instance()
            ->create("StudentstDist", std::move(mean), std::move(scale),
                     std::move(dof));

    std::vector<double> locations = {1.0};

    auto probabilities = test_distribution->cumulative_dist_func(locations);
    auto calced_locations = test_distribution->inv_cumulative_dist_func(probabilities);

    REQUIRE(probabilities[0]== Approx(0.5).epsilon(0.01));
    REQUIRE(calced_locations[0] == Approx(locations[0]).epsilon(0.01));
  }

  SECTION("Test CDF/ICDF values for uniform distribution") {
    double lower = 0.0;
    double upper = 1.0;
    auto test_distribution =
        Factory<stochastic::Distribution, double, double>::instance()->create(
            "UniformDist", std::move(lower), std::move(upper));

    std::vector<double> locations = {0.0, 0.5, 1.0};

    auto probabilities = test_distribution->cumulative_dist_func(locations);
    auto calced_locations =
        test_distribution->inv_cumulative_dist_func(probabilities);

    REQUIRE(probabilities[0] + 1.0 == Approx(1.0).epsilon(0.01));
    REQUIRE(calced_locations[0] + 1.0 == Approx(1.0).epsilon(0.01));
    REQUIRE(probabilities[1] == Approx(0.5).epsilon(0.01));
    REQUIRE(calced_locations[1] == Approx(0.5).epsilon(0.01));
    REQUIRE(probabilities[2] == Approx(1.0).epsilon(0.01));
    REQUIRE(calced_locations[2] == Approx(1.0).epsilon(0.01));
  }
}


/** @file
 *
 *  Declaration of the LikelihoodFieldTest class.
 */

#include <grid_map_lf/LikelihoodField.h>
#include <grid_map_core/GridMapMath.hpp>
#include <grid_map_core/iterators/GridMapIterator.hpp>

#include <gtest/gtest.h>

#include <boost/math/distributions/normal.hpp>

#include <iostream>
#include <cmath>

using namespace grid_map;
using namespace std;

namespace {
double g_sigma = 0.1;
}

TEST(LikelihoodField, EmptyMap)
{
  GridMap map({"layer"});
  map.setGeometry(Length(3.0, 3.0), 1.0, Position());
  map["layer"].setConstant(0);

  SignedDistanceField sdf;
  sdf.calculateSignedDistanceField(map, "layer");
  map.add("distance_field", sdf.getData());

  LikelihoodField lf;
  lf.calculateLikelihoodField(map, "distance_field", g_sigma);
  map.add("likelihood_field", lf.getData());

  for (grid_map::GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator)
  {
    EXPECT_EQ(map.at("likelihood_field", (*iterator)), 0);
  }
}

TEST(LikelihoodField, FullyOccupiedMap)
{
  GridMap map({"layer"});
  map.setGeometry(Length(3.0, 3.0), 1.0, Position());
  map["layer"].setConstant(1);

  SignedDistanceField sdf;
  sdf.calculateSignedDistanceField(map, "layer");
  map.add("distance_field", sdf.getData());

  LikelihoodField lf;
  lf.calculateLikelihoodField(map, "distance_field", g_sigma);
  map.add("likelihood_field", lf.getData());

  for (grid_map::GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator)
  {
    EXPECT_EQ(map.at("likelihood_field", (*iterator)), 1);
  }
}

TEST(LikelihoodField, MiddlePoint)
{
  GridMap map({"layer"});
  map.setGeometry(Length(3.0, 3.0), 1.0, Position());
  map["layer"].setConstant(0);
  map["layer"](1, 1) = 1; // set middle point to obstacle

  SignedDistanceField sdf;
  sdf.calculateSignedDistanceField(map, "layer");
  map.add("distance_field", sdf.getData());

  LikelihoodField lf;
  lf.calculateLikelihoodField(map, "distance_field", g_sigma);
  map.add("likelihood_field", lf.getData());

  boost::math::normal_distribution<double> normalDistribution(0, g_sigma);
  double normalization = pdf(normalDistribution, 0);

  EXPECT_NEAR(map["layer"](0, 0), pdf(normalDistribution, sqrt(2))/ normalization, 1e-5);
  EXPECT_NEAR(map["layer"](0, 1), pdf(normalDistribution, 1)/ normalization, 1e-5);
  EXPECT_NEAR(map["layer"](0, 2), pdf(normalDistribution, sqrt(2))/ normalization, 1e-5);
  EXPECT_NEAR(map["layer"](1, 0), pdf(normalDistribution, 1)/ normalization, 1e-5);
  EXPECT_NEAR(map["layer"](1, 1), pdf(normalDistribution, 0)/ normalization, 1e-5);
  EXPECT_NEAR(map["layer"](1, 2), pdf(normalDistribution, 1)/ normalization, 1e-5);
  EXPECT_NEAR(map["layer"](2, 0), pdf(normalDistribution, sqrt(2))/ normalization, 1e-5);
  EXPECT_NEAR(map["layer"](2, 1), pdf(normalDistribution, 1)/ normalization, 1e-5);
  EXPECT_NEAR(map["layer"](2, 2), pdf(normalDistribution, sqrt(2))/ normalization, 1e-5);
}

TEST(LikelihoodField, getLikelihoodAt)
{
  GridMap map({"layer"});
  map.setGeometry(Length(3.0, 3.0), 1.0, Position());
  map["layer"].setConstant(0);
  map["layer"](1, 1) = 1; // set middle point to obstacle

  SignedDistanceField sdf;
  sdf.calculateSignedDistanceField(map, "layer");
  map.add("distance_field", sdf.getData());

  LikelihoodField lf;
  lf.calculateLikelihoodField(map, "distance_field", g_sigma);
  map.add("likelihood_field", lf.getData());

  boost::math::normal_distribution<double> normalDistribution(0, g_sigma);
  double normalization = pdf(normalDistribution, 0);

  EXPECT_NEAR(lf.getLikelihoodAt(grid_map::Position(0, 0)),
          pdf(normalDistribution, sdf.getDistanceAt(grid_map::Position(0, 0))) / normalization, 1e5);
  EXPECT_NEAR(lf.getLikelihoodAt(grid_map::Position(0, 1)),
          pdf(normalDistribution, sdf.getDistanceAt(grid_map::Position(0, 1))) / normalization, 1e5);
  EXPECT_NEAR(lf.getLikelihoodAt(grid_map::Position(0, 2)),
          pdf(normalDistribution, sdf.getDistanceAt(grid_map::Position(0, 2))) / normalization, 1e5);
  EXPECT_NEAR(lf.getLikelihoodAt(grid_map::Position(1, 0)),
          pdf(normalDistribution, sdf.getDistanceAt(grid_map::Position(1, 0))) / normalization, 1e5);
  EXPECT_NEAR(lf.getLikelihoodAt(grid_map::Position(1, 1)),
          pdf(normalDistribution, sdf.getDistanceAt(grid_map::Position(1, 1))) / normalization, 1e5);
  EXPECT_NEAR(lf.getLikelihoodAt(grid_map::Position(1, 2)),
          pdf(normalDistribution, sdf.getDistanceAt(grid_map::Position(1, 2))) / normalization, 1e5);
  EXPECT_NEAR(lf.getLikelihoodAt(grid_map::Position(2, 0)),
          pdf(normalDistribution, sdf.getDistanceAt(grid_map::Position(2, 0))) / normalization, 1e5);
  EXPECT_NEAR(lf.getLikelihoodAt(grid_map::Position(2, 1)),
          pdf(normalDistribution, sdf.getDistanceAt(grid_map::Position(2, 1))) / normalization, 1e5);
  EXPECT_NEAR(lf.getLikelihoodAt(grid_map::Position(2, 2)),
          pdf(normalDistribution, sdf.getDistanceAt(grid_map::Position(2, 2))) / normalization, 1e5);
}

TEST(LikelihoodField, unknown_region)
{
  GridMap map({"layer"});
  map.setGeometry(Length(1.0, 1.0), 1.0, Position());
  map["layer"].setConstant(NAN);

  grid_map::LikelihoodField lf;
  lf.calculateLikelihoodField(map, "layer", 1.0);

  EXPECT_EQ(lf.getData()(0, 0), 0.5);
}


/*
Author: Rohan Chetan Thanki
Date created: 16-Oct-2021
*/

// This file contains functions which are used in the main file. This ensures that the main file is not cluttered

#include "Hedging_Portfolio.hpp"
#include <iostream>
#include <fstream>
#include <vector>

#include <boost/random.hpp>
#include <boost/math/distributions.hpp>
#include <boost/date_time.hpp>

using namespace std;

void testDelta ()
{
	const double K = 250;
	const double S = 248;
	const double r = 0.03;
	const double T = 1;
	const double sigma = 0.3;
	const double trueDelta = 0.5882;
	Hedging_Portfolio hpobj1(K, S, r, T, sigma, 'c');
	double computedDelta = hpobj1.computeDelta();
	if (abs(trueDelta - computedDelta) <= 0.01)
		cout << "Delta test case passed" << endl;
	else
		cout << "Delta test case failed" << endl;
}

void testImpliedVol()
{
	const double K = 250;
	const double S = 248;
	const double r = 0.03;
	const double T = 1;
	const double sigma = 0;

	const double marketPrice = 46.41;
	double maxVol = 100;
	const double trueImpliedVol = 0.45;

	Hedging_Portfolio hpobj1(K, S, r, T, sigma, 'c');
	hpobj1.setOptionPrice(marketPrice);
	double computedImpliedVol = hpobj1.computeImpliedVol(maxVol);

	if (abs(trueImpliedVol - computedImpliedVol) <= 0.01)
		cout << "Implied volatility test case passed" << endl;
	else
		cout << "Implied volatility test case failed" << endl;
}

void runAllTests()
{
	testDelta();
	testImpliedVol();
}


/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2021 INRIA.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
*/

/*! \file InvertMatrix.hpp

The following code inverts the matrix input using LU-decomposition with backsubstitution of unit vectors. Reference: Numerical Recipies in C, 2nd ed., by Press, Teukolsky, Vetterling & Flannery.

you can solve Ax=b using three lines of ublas code:

permutation_matrix<> piv;
lu_factorize(A, piv);
lu_substitute(A, piv, x);

*/
#ifndef INVERT_MATRIX_HPP
#define INVERT_MATRIX_HPP

// REMEMBER to update "lu.hpp" header includes from boost-CVS
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>

/** Matrix inversion routine.

    Uses lu_factorize and lu_substitute in uBLAS to invert a matrix

    \param input source matrix
    \param output inverted matrix.

*/
template<class T, class U, class V>
bool InvertMatrix(const boost::numeric::ublas::matrix<T, U, V>& input,
                  boost::numeric::ublas::matrix<T, U, V>& inverse)
{
  typedef boost::numeric::ublas::permutation_matrix<std::size_t> pmatrix;
  // create a working copy of the input
  boost::numeric::ublas::matrix<T, U, V> A(input);
  // create a permutation matrix for the LU-factorization
  pmatrix pm(A.size1());

  // perform LU-factorization
  int res = lu_factorize(A,pm);
  if(res != 0) return false;

  // create identity matrix of "inverse"
  inverse.assign(boost::numeric::ublas::identity_matrix<T>(A.size1()));

// backsubstitute to get the inverse
  lu_substitute(A, pm, inverse);

  return true;
}
#endif //INVERT_MATRIX_HPP


//==================================================================================================
/**
  EVE - Expressive Vector Engine
  Copyright : EVE Contributors & Maintainers
  SPDX-License-Identifier: MIT
**/
//==================================================================================================
#include "test.hpp"
#include <eve/module/core.hpp>
#include <eve/module/core.hpp>
#include <eve/module/math.hpp>
#include <eve/module/elliptic.hpp>
#include <boost/math/special_functions/ellint_1.hpp>

//==================================================================================================
// Types tests
//==================================================================================================
EVE_TEST_TYPES( "Check return types of ellint_1"
            , eve::test::simd::ieee_reals
            )
<typename T>(eve::as<T>)
{
  using v_t = eve::element_type_t<T>;

  TTS_EXPR_IS( eve::ellint_1(T())  , T);
  TTS_EXPR_IS( eve::ellint_1(v_t()), v_t);
  TTS_EXPR_IS( eve::ellint_1(T(), T())  , T);
  TTS_EXPR_IS( eve::ellint_1(v_t(), v_t()), v_t);
  TTS_EXPR_IS( eve::ellint_1(T(), v_t()), T);
  TTS_EXPR_IS( eve::ellint_1(v_t(), T()), T);
};

//==================================================================================================
// ellint_1  tests
//==================================================================================================
EVE_TEST( "Check behavior of ellint_1 on wide"
        , eve::test::simd::ieee_reals
        , eve::test::generate( eve::test::randoms(0, 1.0)
                             , eve::test::randoms(0, eve::pio_2))
        )
<typename T>(T const& k, T const& phi)
{
  using eve::detail::map;
  using v_t = eve::element_type_t<T>;

  TTS_ULP_EQUAL(eve::ellint_1(k)      , map([](auto e) -> v_t { return boost::math::ellint_1(e); }, k), 16);
  TTS_ULP_EQUAL(eve::ellint_1(phi, k) , map([](auto e, auto f) -> v_t { return boost::math::ellint_1(e, f); }, k, phi), 16);
};


#include <blitz/array.h>

BZ_USING_NAMESPACE(blitz)

void setupInitialConditions(Array<float,3>& P1, Array<float,3>& P2,
    Array<float,3>& P3, Array<float,3>& c, int N);

float acoustic3D_BlitzInterlacedCycled(int N, int niters)
{
    // Allocate the arrays as a group.  Blitz++ will interlace them in
    // memory, improving data locality.

    Array<float,3> P1, P2, P3, c;
    allocateArrays(shape(N,N,N), P1, P2, P3, c);
    Range I(1,N-2), J(1,N-2), K(1,N-2);

    setupInitialConditions(P1, P2, P3, c, N);

    for (int iter=0; iter < niters; ++iter)
    {
        P3(I,J,K) = (2-6*c(I,J,K)) * P2(I,J,K)
          + c(I,J,K)*(P2(I-1,J,K) + P2(I+1,J,K) + P2(I,J-1,K) + P2(I,J+1,K)
          + P2(I,J,K-1) + P2(I,J,K+1)) - P1(I,J,K);

        cycleArrays(P1, P2, P3);
    }

    return P1(N/2,N/2,N/2);
}


//
// Copyright (c) 2016-2019 CNRS, INRIA
//

#ifndef __pinocchio_math_quaternion_hpp__
#define __pinocchio_math_quaternion_hpp__

#include "pinocchio/math/fwd.hpp"
#include "pinocchio/math/comparison-operators.hpp"
#include "pinocchio/math/sincos.hpp"
#include "pinocchio/utils/static-if.hpp"
#include <boost/type_traits.hpp>

#include <Eigen/Geometry>

namespace pinocchio
{
  namespace quaternion
  {
    ///
    /// \brief Compute the minimal angle between q1 and q2.
    ///
    /// \param[in] q1 input quaternion.
    /// \param[in] q2 input quaternion.
    ///
    /// \return angle between the two quaternions
    ///
    template<typename D1, typename D2>
    typename D1::Scalar
    angleBetweenQuaternions(const Eigen::QuaternionBase<D1> & q1,
                            const Eigen::QuaternionBase<D2> & q2)
    {
      typedef typename D1::Scalar Scalar;
      const Scalar innerprod = q1.dot(q2);
      Scalar theta = math::acos(innerprod);
      static const Scalar PI_value = PI<Scalar>();

      theta = internal::if_then_else(internal::LT, innerprod, Scalar(0),
                                     PI_value - theta,
                                     theta);
      return theta;
    }

    ///
    /// \brief Check if two quaternions define the same rotations.
    /// \note Two quaternions define the same rotation iff q1 == q2 OR q1 == -q2.
    ///
    /// \param[in] q1 input quaternion.
    /// \param[in] q2 input quaternion.
    ///
    /// \return Return true if the two input quaternions define the same rotation.
    ///
    template<typename D1, typename D2>
    bool defineSameRotation(const Eigen::QuaternionBase<D1> & q1,
                            const Eigen::QuaternionBase<D2> & q2,
                            const typename D1::RealScalar & prec = Eigen::NumTraits<typename D1::Scalar>::dummy_precision())
    {
      return (q1.coeffs().isApprox(q2.coeffs(), prec) || q1.coeffs().isApprox(-q2.coeffs(), prec) );
    }

    /// Approximately normalize by applying the first order limited development
    /// of the normalization function.
    ///
    /// Only additions and multiplications are required. Neither square root nor
    /// division are used (except a division by 2). Let \f$ \delta = ||q||^2 - 1 \f$.
    /// Using the following limited development:
    /// \f[ \frac{1}{||q||} = (1 + \delta)^{-\frac{1}{2}} = 1 - \frac{\delta}{2} + \mathcal{O}(\delta^2) \f]
    ///
    /// The output is
    /// \f[ q_{out} = q \times \frac{3 - ||q_{in}||^2}{2} \f]
    ///
    /// The output quaternion is guaranted to statisfy the following:
    /// \f[ | ||q_{out}|| - 1 | \le \frac{M}{2} ||q_{in}|| ( ||q_{in}||^2 - 1 )^2 \f]
    /// where \f$ M = \frac{3}{4} (1 - \epsilon)^{-\frac{5}{2}} \f$
    /// and \f$ \epsilon \f$ is the maximum tolerance of \f$ ||q_{in}||^2 - 1 \f$.
    ///
    /// \warning \f$ ||q||^2 - 1 \f$ should already be close to zero.
    ///
    /// \note See
    /// http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html#title3
    /// to know the reason why the argument is const.
    template<typename D>
    void firstOrderNormalize(const Eigen::QuaternionBase<D> & q)
    {
      typedef typename D::Scalar Scalar;
      const Scalar N2 = q.squaredNorm();
#ifndef NDEBUG
      const Scalar epsilon = sqrt(sqrt(Eigen::NumTraits<Scalar>::epsilon()));
      typedef apply_op_if<less_than_or_equal_to_op,boost::is_floating_point<Scalar>::value,true> static_leq;
      assert(static_leq::op(math::fabs(N2-1.), epsilon));
#endif
      const Scalar alpha = ((Scalar)3 - N2) / Scalar(2);
      PINOCCHIO_EIGEN_CONST_CAST(D,q).coeffs() *= alpha;
#ifndef NDEBUG
      const Scalar M = Scalar(3) * math::pow(Scalar(1)-epsilon, ((Scalar)-Scalar(5))/Scalar(2)) / Scalar(4);
      assert(static_leq::op(math::fabs(q.norm() - Scalar(1)),
                            math::max(M * sqrt(N2) * (N2 - Scalar(1))*(N2 - Scalar(1)) / Scalar(2), Eigen::NumTraits<Scalar>::dummy_precision())));
#endif
    }

    /// Uniformly random quaternion sphere.
    template<typename Derived>
    void uniformRandom(const Eigen::QuaternionBase<Derived> & q)
    {
      typedef typename Derived::Scalar Scalar;

      // Rotational part
      const Scalar u1 = (Scalar)rand() / RAND_MAX;
      const Scalar u2 = (Scalar)rand() / RAND_MAX;
      const Scalar u3 = (Scalar)rand() / RAND_MAX;

      const Scalar mult1 = sqrt(Scalar(1)-u1);
      const Scalar mult2 = sqrt(u1);

      static const Scalar PI_value = PI<Scalar>();
      Scalar s2,c2; SINCOS(Scalar(2)*PI_value*u2,&s2,&c2);
      Scalar s3,c3; SINCOS(Scalar(2)*PI_value*u3,&s3,&c3);

      PINOCCHIO_EIGEN_CONST_CAST(Derived,q).w() = mult1 * s2;
      PINOCCHIO_EIGEN_CONST_CAST(Derived,q).x() = mult1 * c2;
      PINOCCHIO_EIGEN_CONST_CAST(Derived,q).y() = mult2 * s3;
      PINOCCHIO_EIGEN_CONST_CAST(Derived,q).z() = mult2 * c3;
    }

    namespace internal
    {

      template<typename Scalar, bool value = boost::is_floating_point<Scalar>::value>
      struct quaternionbase_assign_impl;

      template<Eigen::DenseIndex i>
      struct quaternionbase_assign_impl_if_t_negative
      {
        template<typename Scalar, typename Matrix3, typename QuaternionDerived>
        static inline void run(Scalar t,
                               Eigen::QuaternionBase<QuaternionDerived> & q,
                               const Matrix3 & mat)
        {
          using pinocchio::math::sqrt;

          Eigen::DenseIndex j = (i+1)%3;
          Eigen::DenseIndex k = (j+1)%3;

          t = sqrt(mat.coeff(i,i)-mat.coeff(j,j)-mat.coeff(k,k) + Scalar(1.0));
          q.coeffs().coeffRef(i) = Scalar(0.5) * t;
          t = Scalar(0.5)/t;
          q.w() = (mat.coeff(k,j)-mat.coeff(j,k))*t;
          q.coeffs().coeffRef(j) = (mat.coeff(j,i)+mat.coeff(i,j))*t;
          q.coeffs().coeffRef(k) = (mat.coeff(k,i)+mat.coeff(i,k))*t;
        }
      };

      struct quaternionbase_assign_impl_if_t_positive
      {
        template<typename Scalar, typename Matrix3, typename QuaternionDerived>
        static inline void run(Scalar t,
                               Eigen::QuaternionBase<QuaternionDerived> & q,
                               const Matrix3 & mat)
        {
          using pinocchio::math::sqrt;

          t = sqrt(t + Scalar(1.0));
          q.w() = Scalar(0.5)*t;
          t = Scalar(0.5)/t;
          q.x() = (mat.coeff(2,1) - mat.coeff(1,2)) * t;
          q.y() = (mat.coeff(0,2) - mat.coeff(2,0)) * t;
          q.z() = (mat.coeff(1,0) - mat.coeff(0,1)) * t;
        }
      };

      template<typename Scalar>
      struct quaternionbase_assign_impl<Scalar, true>
      {
        template<typename Matrix3, typename QuaternionDerived>
        static inline void run(Eigen::QuaternionBase<QuaternionDerived> & q,
                               const Matrix3 & mat)
        {
          using pinocchio::math::sqrt;

          Scalar t = mat.trace();
          if (t > Scalar(0.))
            quaternionbase_assign_impl_if_t_positive::run(t,q,mat);
          else
          {
            Eigen::DenseIndex i = 0;
            if (mat.coeff(1,1) > mat.coeff(0,0))
              i = 1;
            if (mat.coeff(2,2) > mat.coeff(i,i))
              i = 2;

            if(i==0)
              quaternionbase_assign_impl_if_t_negative<0>::run(t,q,mat);
            else if(i==1)
              quaternionbase_assign_impl_if_t_negative<1>::run(t,q,mat);
            else
              quaternionbase_assign_impl_if_t_negative<2>::run(t,q,mat);
          }
        }
      };

    } // namespace internal

    template<typename D, typename Matrix3>
    void assignQuaternion(Eigen::QuaternionBase<D> & quat,
                          const Eigen::MatrixBase<Matrix3> & R)
    {
      internal::quaternionbase_assign_impl<typename Matrix3::Scalar>::run(PINOCCHIO_EIGEN_CONST_CAST(D,quat),
                                                                          R.derived());
    }

  } // namespace quaternion

}
#endif //#ifndef __pinocchio_math_quaternion_hpp__


//#include <boost/test/unit_test.hpp>
//#include <iostream>
//
//#include "ssmkit/filter/kalman.hpp"
//
//using namespace PROJECT_NAME;
//
//BOOST_AUTO_TEST_SUITE(kalman_filter_test);
//
//BOOST_AUTO_TEST_CASE(one_step_test)
//{
//    constexpr double diff_tol = 0.0001;
//
//    arma::mat F {{1, 0}, {0, 1}};
//    arma::mat H ("1 0");
//    arma::mat B ("1; 1");
//
//    mlpack::distribution::GaussianDistribution w(arma::zeros<arma::vec>(2), arma::eye<arma::mat>(2,2));
//    mlpack::distribution::GaussianDistribution v(arma::zeros<arma::vec>(1), arma::eye<arma::mat>(1,1));
//
//    mlpack::distribution::GaussianDistribution x0(arma::zeros<arma::vec>(2), arma::eye<arma::mat>(2,2));
//
//    filter::Kalman kf(F,H,B,w,v);
//
//    // whithout control input
//    kf.initialize(x0);
//    BOOST_REQUIRE(arma::approx_equal(arma::vec({0,0}),kf.state().Mean(), "absdiff", diff_tol));
//    BOOST_REQUIRE(arma::approx_equal(arma::mat({{1,0},{0,1}}),kf.state().Covariance(), "absdiff", diff_tol));
//
//    kf.predict();
//    BOOST_REQUIRE(arma::approx_equal(arma::vec({0,0}),kf.predicted().Mean(), "absdiff", diff_tol));
//    BOOST_REQUIRE(arma::approx_equal(arma::mat({{2,0},{0,2}}),kf.predicted().Covariance(), "absdiff", diff_tol));
//
//    kf.filter(arma::vec({1}));
//    BOOST_REQUIRE(arma::approx_equal(arma::vec({0.6667,0}),kf.state().Mean(), "absdiff", diff_tol));
//    BOOST_REQUIRE(arma::approx_equal(arma::mat({{0.6667,0},{0,2}}),kf.state().Covariance(), "absdiff", diff_tol));
//
//    // with control input
//    kf.initialize(x0);
//    BOOST_REQUIRE(arma::approx_equal(arma::vec({0,0}),kf.state().Mean(), "absdiff", diff_tol));
//    BOOST_REQUIRE(arma::approx_equal(arma::mat({{1,0},{0,1}}),kf.state().Covariance(), "absdiff", diff_tol));
//
//    kf.predict(arma::vec({2}));
//    BOOST_REQUIRE(arma::approx_equal(arma::vec({2,2}),kf.predicted().Mean(), "absdiff", diff_tol));
//    BOOST_REQUIRE(arma::approx_equal(arma::mat({{2,0},{0,2}}),kf.predicted().Covariance(), "absdiff", diff_tol));
//
//    kf.filter(arma::vec({3}));
//    BOOST_REQUIRE(arma::approx_equal(arma::vec({2.6667,2}),kf.state().Mean(), "absdiff", diff_tol));
//    BOOST_REQUIRE(arma::approx_equal(arma::mat({{0.6667,0},{0,2}}),kf.state().Covariance(), "absdiff", diff_tol));
//}
//
//BOOST_AUTO_TEST_SUITE_END();


#include<Eigen/Dense>
#include<Eigen/Geometry>
#include<cstring>
#include <boost/iterator/iterator_concepts.hpp>
using namespace std;
namespace map3d{

template <typename PoseT>
struct edge{

  string Header;
  int a_ID;
  int b_ID;
  PoseT pose_ab;
  Eigen::Matrix<float,6,6> informationMatrix_ab;
};

struct pose
{
    Eigen::Vector3f t;
    Eigen::Quaternion<float> q;
};
template <typename PoseT>
struct vertix{
  string Header;
  int pose_ID;
  PoseT initialPose;
};
}


// Copyright Maarten L. Hekkelman, Radboud University 2008-2011.
//   Distributed under the Boost Software License, Version 1.0.
//       (See accompanying file LICENSE_1_0.txt or copy at
//             http://www.boost.org/LICENSE_1_0.txt)
//
// 3d routines

#include "mas.h"

#include <valarray>
#include <cmath>

#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH

#include "primitives-3d.h"
#include "matrix.h"

using namespace std;

const double
	kPI = 4 * std::atan(1.0);

// --------------------------------------------------------------------

MQuaternion Normalize(MQuaternion q)
{
	valarray<double> t(4);

	t[0] = q.R_component_1();
	t[1] = q.R_component_2();
	t[2] = q.R_component_3();
	t[3] = q.R_component_4();

	t *= t;

	double length = sqrt(t.sum());

	if (length > 0.001)
		q /= length;
	else
		q = MQuaternion(1, 0, 0, 0);

	return q;
}

// --------------------------------------------------------------------

double MPoint::Normalize()
{
	double length = mX * mX + mY * mY + mZ * mZ;
	if (length > 0)
	{
		length = sqrt(length);
		mX /= length;
		mY /= length;
		mZ /= length;
	}
	return length;
}

MPoint operator+(const MPoint& lhs, const MPoint& rhs)
{
	return MPoint(lhs.mX + rhs.mX, lhs.mY + rhs.mY, lhs.mZ + rhs.mZ);
}

MPoint operator-(const MPoint& lhs, const MPoint& rhs)
{
	return MPoint(lhs.mX - rhs.mX, lhs.mY - rhs.mY, lhs.mZ - rhs.mZ);
}

MPoint operator-(const MPoint& pt)
{
	return MPoint(-pt.mX, -pt.mY, -pt.mZ);
}

MPoint operator*(const MPoint& pt, double f)
{
	MPoint result(pt);
	result *= f;
	return result;
}

MPoint operator/(const MPoint& pt, double f)
{
	MPoint result(pt);
	result /= f;
	return result;
}

ostream& operator<<(ostream& os, const MPoint& pt)
{
	os << '(' << pt.mX << ',' << pt.mY << ',' << pt.mZ << ')';
	return os;
}

ostream& operator<<(ostream& os, const vector<MPoint>& pts)
{
	uint32 n = pts.size();
	os << '[' << n << ']';

	foreach (const MPoint& pt, pts)
	{
		os << pt;
		if (n-- > 1)
			os << ',';
	}

	return os;
}

// --------------------------------------------------------------------

double DihedralAngle(const MPoint& p1, const MPoint& p2, const MPoint& p3, const MPoint& p4)
{
	MPoint v12 = p1 - p2;	// vector from p2 to p1
	MPoint v43 = p4 - p3;	// vector from p3 to p4

	MPoint z = p2 - p3;		// vector from p3 to p2

	MPoint p = CrossProduct(z, v12);
	MPoint x = CrossProduct(z, v43);
	MPoint y = CrossProduct(z, x);

	double u = DotProduct(x, x);
	double v = DotProduct(y, y);

	double result = 360;
	if (u > 0 and v > 0)
	{
		u = DotProduct(p, x) / sqrt(u);
		v = DotProduct(p, y) / sqrt(v);
		if (u != 0 or v != 0)
			result = atan2(v, u) * 180 / kPI;
	}

	return result;
}

double CosinusAngle(const MPoint& p1, const MPoint& p2, const MPoint& p3, const MPoint& p4)
{
	MPoint v12 = p1 - p2;
	MPoint v34 = p3 - p4;

	double result = 0;

	double x = DotProduct(v12, v12) * DotProduct(v34, v34);
	if (x > 0)
		result = DotProduct(v12, v34) / sqrt(x);

	return result;
}

// --------------------------------------------------------------------

tuple<double,MPoint> QuaternionToAngleAxis(MQuaternion q)
{
	if (q.R_component_1() > 1)
		q = Normalize(q);

	// angle:
	double angle = 2 * acos(q.R_component_1());
	angle = angle * 180 / kPI;

	// axis:
	double s = sqrt(1 - q.R_component_1() * q.R_component_1());
	if (s < 0.001)
		s = 1;

	MPoint axis(q.R_component_2() / s, q.R_component_3() / s, q.R_component_4() / s);

	return make_tuple(angle, axis);
}

MPoint CenterPoints(vector<MPoint>& points)
{
	MPoint t;

	foreach (MPoint& pt, points)
	{
		t.mX += pt.mX;
		t.mY += pt.mY;
		t.mZ += pt.mZ;
	}

	t.mX /= points.size();
	t.mY /= points.size();
	t.mZ /= points.size();

	foreach (MPoint& pt, points)
	{
		pt.mX -= t.mX;
		pt.mY -= t.mY;
		pt.mZ -= t.mZ;
	}

	return t;
}

MPoint Centroid(vector<MPoint>& points)
{
	MPoint result;

	foreach (MPoint& pt, points)
		result += pt;

	result /= points.size();

	return result;
}

double RMSd(const vector<MPoint>& a, const vector<MPoint>& b)
{
	double sum = 0;
	for (uint32 i = 0; i < a.size(); ++i)
	{
		valarray<double> d(3);

		d[0] = b[i].mX - a[i].mX;
		d[1] = b[i].mY - a[i].mY;
		d[2] = b[i].mZ - a[i].mZ;

		d *= d;

		sum += d.sum();
	}

	return sqrt(sum / a.size());
}

// The next function returns the largest solution for a quartic equation
// based on Ferrari's algorithm.
// A depressed quartic is of the form:
//
//   x^4 + ax^2 + bx + c = 0
//
// (since I'm too lazy to find out a better way, I've implemented the
//  routine using complex values to avoid nan's as a result of taking
//  sqrt of a negative number)
double LargestDepressedQuarticSolution(double a, double b, double c)
{
	complex<double> P = - (a * a) / 12 - c;
	complex<double> Q = - (a * a * a) / 108 + (a * c) / 3 - (b * b) / 8;
	complex<double> R = - Q / 2.0 + sqrt((Q * Q) / 4.0 + (P * P * P) / 27.0);

	complex<double> U = pow(R, 1 / 3.0);

	complex<double> y;
	if (U == 0.0)
		y = -5.0 * a / 6.0 + U - pow(Q, 1.0 / 3.0);
	else
		y = -5.0 * a / 6.0 + U - P / (3.0 * U);

	complex<double> W = sqrt(a + 2.0 * y);

	// And to get the final result:
	// result = (±W + sqrt(-(3 * alpha + 2 * y ± 2 * beta / W))) / 2;
	// We want the largest result, so:

	valarray<double> t(4);

	t[0] = (( W + sqrt(-(3.0 * a + 2.0 * y + 2.0 * b / W))) / 2.0).real();
	t[1] = (( W + sqrt(-(3.0 * a + 2.0 * y - 2.0 * b / W))) / 2.0).real();
	t[2] = ((-W + sqrt(-(3.0 * a + 2.0 * y + 2.0 * b / W))) / 2.0).real();
	t[3] = ((-W + sqrt(-(3.0 * a + 2.0 * y - 2.0 * b / W))) / 2.0).real();

	return t.max();
}

MQuaternion AlignPoints(const vector<MPoint>& pa, const vector<MPoint>& pb)
{
	// First calculate M, a 3x3 matrix containing the sums of products of the coordinates of A and B
	matrix<double> M(3, 3, 0);

	for (uint32 i = 0; i < pa.size(); ++i)
	{
		const MPoint& a = pa[i];
		const MPoint& b = pb[i];

		M(0, 0) += a.mX * b.mX;	M(0, 1) += a.mX * b.mY;	M(0, 2) += a.mX * b.mZ;
		M(1, 0) += a.mY * b.mX;	M(1, 1) += a.mY * b.mY;	M(1, 2) += a.mY * b.mZ;
		M(2, 0) += a.mZ * b.mX;	M(2, 1) += a.mZ * b.mY;	M(2, 2) += a.mZ * b.mZ;
	}

	// Now calculate N, a symmetric 4x4 matrix
	symmetric_matrix<double> N(4);

	N(0, 0) =  M(0, 0) + M(1, 1) + M(2, 2);
	N(0, 1) =  M(1, 2) - M(2, 1);
	N(0, 2) =  M(2, 0) - M(0, 2);
	N(0, 3) =  M(0, 1) - M(1, 0);

	N(1, 1) =  M(0, 0) - M(1, 1) - M(2, 2);
	N(1, 2) =  M(0, 1) + M(1, 0);
	N(1, 3) =  M(0, 2) + M(2, 0);

	N(2, 2) = -M(0, 0) + M(1, 1) - M(2, 2);
	N(2, 3) =  M(1, 2) + M(2, 1);

	N(3, 3) = -M(0, 0) - M(1, 1) + M(2, 2);

	// det(N - λI) = 0
	// find the largest λ (λm)
	//
	// Aλ4 + Bλ3 + Cλ2 + Dλ + E = 0
	// A = 1
	// B = 0
	// and so this is a so-called depressed quartic
	// solve it using Ferrari's algorithm

	double C = -2 * (
		M(0, 0) * M(0, 0) + M(0, 1) * M(0, 1) + M(0, 2) * M(0, 2) +
		M(1, 0) * M(1, 0) + M(1, 1) * M(1, 1) + M(1, 2) * M(1, 2) +
		M(2, 0) * M(2, 0) + M(2, 1) * M(2, 1) + M(2, 2) * M(2, 2));

	double D = 8 * (M(0, 0) * M(1, 2) * M(2, 1) +
					M(1, 1) * M(2, 0) * M(0, 2) +
					M(2, 2) * M(0, 1) * M(1, 0)) -
			   8 * (M(0, 0) * M(1, 1) * M(2, 2) +
					M(1, 2) * M(2, 0) * M(0, 1) +
					M(2, 1) * M(1, 0) * M(0, 2));

	double E =
		(N(0,0) * N(1,1) - N(0,1) * N(0,1)) * (N(2,2) * N(3,3) - N(2,3) * N(2,3)) +
		(N(0,1) * N(0,2) - N(0,0) * N(2,1)) * (N(2,1) * N(3,3) - N(2,3) * N(1,3)) +
		(N(0,0) * N(1,3) - N(0,1) * N(0,3)) * (N(2,1) * N(2,3) - N(2,2) * N(1,3)) +
		(N(0,1) * N(2,1) - N(1,1) * N(0,2)) * (N(0,2) * N(3,3) - N(2,3) * N(0,3)) +
		(N(1,1) * N(0,3) - N(0,1) * N(1,3)) * (N(0,2) * N(2,3) - N(2,2) * N(0,3)) +
		(N(0,2) * N(1,3) - N(2,1) * N(0,3)) * (N(0,2) * N(1,3) - N(2,1) * N(0,3));

	// solve quartic
	double lm = LargestDepressedQuarticSolution(C, D, E);

	// calculate t = (N - λI)
	matrix<double> li = identity_matrix<double>(4) * lm;
	matrix<double> t = N - li;

	// calculate a matrix of cofactors for t
	matrix<double> cf(4, 4);

	const uint32 ixs[4][3] =
	{
		{ 1, 2, 3 },
		{ 0, 2, 3 },
		{ 0, 1, 3 },
		{ 0, 1, 2 }
	};

	uint32 maxR = 0;
	for (uint32 r = 0; r < 4; ++r)
	{
		const uint32* ir = ixs[r];

		for (uint32 c = 0; c < 4; ++c)
		{
			const uint32* ic = ixs[c];

			cf(r, c) =
				t(ir[0], ic[0]) * t(ir[1], ic[1]) * t(ir[2], ic[2]) +
				t(ir[0], ic[1]) * t(ir[1], ic[2]) * t(ir[2], ic[0]) +
				t(ir[0], ic[2]) * t(ir[1], ic[0]) * t(ir[2], ic[1]) -
				t(ir[0], ic[2]) * t(ir[1], ic[1]) * t(ir[2], ic[0]) -
				t(ir[0], ic[1]) * t(ir[1], ic[0]) * t(ir[2], ic[2]) -
				t(ir[0], ic[0]) * t(ir[1], ic[2]) * t(ir[2], ic[1]);
		}

		if (r > maxR and cf(r, 0) > cf(maxR, 0))
			maxR = r;
	}

	// NOTE the negation of the y here, why? Maybe I swapped r/c above?
	MQuaternion q(cf(maxR, 0), cf(maxR, 1), -cf(maxR, 2), cf(maxR, 3));
	q = Normalize(q);

	return q;
}


#include "phase_space.hpp"
#include <boost/math/quadrature/gauss_kronrod.hpp>
#include <gsl/gsl_sf_bessel.h>

using MomentaType = std::array<phase_space::LVector<double>, 4>;

/// Struct holding whatever parameters are needed
struct ModelParameters {
  double mx;
  std::array<double, 4> fsp_masses;
};

/// Function computing squared matrix element
auto msqrd(const MomentaType &momenta, const ModelParameters &params)
    -> double {
  return 1.0;
}

/// Compute zero-temperature cross section
auto cross_section(double cme, const ModelParameters &params) -> double {
  auto msqrd_ = [&params](const MomentaType &momenta) {
    return msqrd(momenta, params);
  };
  return phase_space::cross_section(msqrd_, cme, params.mx, params.mx,
                                    params.fsp_masses)
      .first;
}

/// Compute the thermal cross section for X + X -> A + B + C + D
///
/// Usual integal is:
///     <σv> = (2 π² T) ∫ ds σ[√s] (s - 4 m²) √s K₁[√s/T] / (4π m² T K₂[m/T])
/// Define: s = z² m², x = m /T. Then this becomes:
///     <σv> = x ∫ dz z² (z² - 4) σ[m z] K₁[x * z] / (4 K₂[x]²)
/// Next, define scaled bessel functions: K₁[x] = exp(-x) k₁[x]
/// and K₂[x] = exp(-x) k₂[x]. Then:
///     <σv> = x / (4 k₂[x]²) ∫ dz z² (z² - 4) σ[m z] k₁[x * z] exp(-x (z-2))
double thermal_cross_section(const double x, const ModelParameters &params) {
  using boost::math::quadrature::gauss_kronrod;

  const double den = 2.0 * gsl_sf_bessel_Kn_scaled(2, x);
  const double pre = x / (den * den);
  const double mx = params.mx;

  auto f = [x, &params, mx](double z) -> double {
    const double z2 = z * z;
    const double sig = cross_section(z * mx, params);
    const double ker =
        z2 * (z2 - 4.0) * gsl_sf_bessel_K1_scaled(x * z) * exp(-x * (z - 2.0));
    return sig * ker;
  };

  const double integral = gauss_kronrod<double, 15>::integrate(
      f, 4.0, std::numeric_limits<double>::infinity(), 5, 1e-8);

  return pre * integral;
}

int main() {

  ModelParameters params = {100.0, {1.0, 1.0, 1.0, 1.0}};
  std::cout << thermal_cross_section(1.0, params);
  return 0.0;
}


#include <Eigen/Dense>
#include <cmath>
#include <iostream>
#include "../include/ply.h"

// contruct the stiffness matrix of the ply in the ply coordinates. P
Eigen::Matrix3d build_Q(const Properties& p);

double to_radian(double angle_degree);

using std::cin; using std::cout; using std::endl;
using Eigen::Matrix3d;

ply::ply(const std::string material_label,
    const Properties material_properties,
    const double theta, const double thickness):
    material_label_(material_label),
    material_properties_(material_properties), theta_(theta),
    thickness_(thickness) {

    double angle_radian = to_radian(theta);
    double s = sin(angle_radian);
    double c = cos(angle_radian);
    Matrix3d T_stress_inv;
    Matrix3d T_strain;

    T_stress_inv << pow(c,2), pow(s,2), -2*s*c,
                    pow(s,2), pow(c,2),  2*s*c,
                    s*c     , -s*c    ,  pow(c,2) - pow(s,2);

    T_strain << pow(c,2), pow(s,2),  s*c,
                pow(s,2), pow(c,2), -s*c,
                -2*s*c     , 2*s*c  ,  pow(c,2) - pow(s,2);

    Matrix3d Q = build_Q(material_properties_);

    // Transformation from ply coordinates to laminate coordinates.
    Qbar_ = T_stress_inv * Q * T_strain;

}

Matrix3d build_Q(const Properties& p) {
    double D = 1 - p.E2 / p.E1 * pow(p.nu12, 2);
    Matrix3d Q;

    Q << p.E1/D       , p.nu12*p.E2/D, 0,
         p.nu12*p.E2/D, p.E2/D       , 0,
         0            , 0            , p.G12;

    return Q;
}

double to_radian(double angle_degree) {
    return angle_degree * M_PI/180;
}


#include <HElib/FHE.h>
#include <HElib/FHEContext.h>
#include <HElib/EncryptedArray.h>
#include <NTL/ZZ.h>
#include <NTL/ZZX.h>
#include <iostream>
#include <fstream>
void generate_poly(std::vector<NTL::ZZX> &polys,
                   const EncryptedArray *ea,
                   const long upper) {
    polys.resize(ea->size());
    long d = ea->getDegree();
    for (size_t i = 0; i < polys.size(); i++) {
        polys[i].SetLength(d);
        for (long j = 0; j < d; j++)
            NTL::SetCoeff(polys[i], j, NTL::RandomBnd(upper));
        // std::cout << NTL::deg(polys[i]) << "=?" << d << std::endl;
    }
}

long inner_prod(NTL::ZZX const& v, NTL::ZZX const& u,
                const long d,
                const long upper) {
    NTL::ZZ ip(0);
    // std::cout << v << " " << u << std::endl;
    for (long i = 0; i < d; i++) {
        // std::cout << NTL::coeff(v, i)  << "*" << NTL::coeff(u, d - i - 1) << std::endl;
        ip = ip + NTL::coeff(v, i) * NTL::coeff(u, d - i - 1);
    }
    return ip % upper;
}

int main(int argc, char *argv[]) {
    // long m = 32;
    // long p = 113;
    long m = NTL::RandomPrime_long(5, 20);
    long p = NTL::RandomPrime_long(13, 20);
    do {
          p = NTL::RandomPrime_long(13, 20);
     } while ((m % p) == 0);
    FHEcontext context(m, p, 1);
    auto G = context.alMod.getFactorsOverZZ()[0];
    // EncryptedArray *ea = new EncryptedArray(context, G);
    auto ea = context.ea;
    std::cout << m << " " << p << std::endl;
    std::cout << ea->size() << " " << ea->getDegree() << std::endl;
    buildModChain(context, 4);
    FHESecKey sk(context);
    sk.GenSecKey(64);

    std::vector<NTL::ZZX> slots, slots2;
    generate_poly(slots, ea, p);
    generate_poly(slots2, ea, p);
    for (size_t i = 0; i < slots.size(); i++) {
        std::cout << inner_prod(slots[i], slots2[i], ea->getDegree(), p) << std::endl;
    }
    std::cout << "\n";
    Ctxt ctx(sk), ctx2(sk);
    ea->skEncrypt(ctx, sk, slots);
    ea->skEncrypt(ctx2, sk, slots2);
    ctx *= ctx2;
    ea->decrypt(ctx, sk, slots);
    for (auto &ss : slots)
        std::cout << NTL::coeff(ss, ea->getDegree() - 1) << std::endl;
    return 0;
}


#include <iostream>
#include <random>
#include <acado_toolkit.hpp>
#include <acado_gnuplot.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real_distribution.hpp>

#define pi 3.14159

using namespace std;

static boost::random::mt19937 gen;

int gen_data(float bounds[8], int index, char thread);
float rand_gen(float lim[2]);

float rand_gen(float lim[2]){
  static boost::random::uniform_real_distribution<> dist(0, 1);
  return lim[0] + dist(gen)*(lim[1] - lim[0]);
}

int gen_data(float bounds[8], int index, char thread){
  string states_nm, parameters_nm, control_nm;
  bool flag = false;

  USING_NAMESPACE_ACADO

  DifferentialState        q1,q2,qd1,qd2;   // the differential states
  Control                  tau1,tau2;       // the control input u
  Parameter                T;
  DifferentialEquation     f( 0.0, 0.5 );

  //  -------------------------------------

  OCP ocp( 0.0, 0.5,5 );
  ocp.minimizeLagrangeTerm(1+tau1*tau1 + tau2*tau2);
  //ocp.minimizeMayerTerm(q1*q1 + q2*q2 + qd1*qd1 + qd2*qd2 );

  f << dot(q1) == qd1;
  f << dot(q2) == qd2;
  f << dot(qd1) == -(48*tau1 - 48*tau2 + 24*qd1*qd1*sin(q2) + 24*qd2*qd2*sin(q2) + 18*qd1*qd1*sin(2*q2) - 72*tau2*cos(q2) + 48*qd1*qd2*sin(q2))/(36*cos(q2)*cos(q2) - 64);
  f << dot(qd2) == (48*tau1 - 240*tau2 + 120*qd1*qd1*sin(q2) + 24*qd2*qd2*sin(q2) + 36*qd1*qd1*sin(2*q2) + 18*qd2*qd2*sin(2*q2) + 72*tau1*cos(q2) - 144*tau2*cos(q2) + 48*qd1*qd2*sin(q2) + 36*qd1*qd2*sin(2*q2))/(18*cos(2*q2) - 46);

  ocp.subjectTo(f);
  ocp.subjectTo(AT_START, q1 ==  bounds[0] );

  ocp.subjectTo(AT_START, q2 ==  bounds[1] );

  ocp.subjectTo(AT_START, qd1 ==  bounds[2] );
  ocp.subjectTo(AT_START, qd2 ==  bounds[3] );

  ocp.subjectTo(AT_END, q1 == bounds[4]);
  ocp.subjectTo(AT_END, q2 == bounds[5]);
  ocp.subjectTo(AT_END, qd1 == bounds[6]);
  ocp.subjectTo(AT_END, qd2 == bounds[7]);

  // bounds on the control input u
  ocp.subjectTo(-400 <= tau1 <= 400);
  ocp.subjectTo(-400 <= tau2 <= 400);

  //  -------------------------------------

  // Optimization algorithm
  OptimizationAlgorithm algorithm(ocp);
  algorithm.set( DISCRETIZATION_TYPE , SINGLE_SHOOTING);
  algorithm.set( INTEGRATOR_TYPE , INT_RK45);
  algorithm.set( HESSIAN_APPROXIMATION   , BLOCK_BFGS_UPDATE);
  algorithm.set( KKT_TOLERANCE   , 1e-1);
  algorithm.set( ABSOLUTE_TOLERANCE, 1e-1);
  algorithm.set( INTEGRATOR_TOLERANCE, 1e-1);
  algorithm.set( MAX_NUM_ITERATIONS, 1000);
  algorithm.set( MAX_NUM_INTEGRATOR_STEPS, 10000);

  algorithm.set(PRINT_COPYRIGHT,BT_FALSE);
  algorithm.set(PRINTLEVEL,LOW);
  algorithm.set(PRINT_INTEGRATOR_PROFILE,BT_FALSE);
  algorithm.set(PRINT_SCP_METHOD_PROFILE,BT_FALSE);
  bool return_code = algorithm.solve();

  // VariablesGrid grid;
  // algorithm.getDifferentialStates(grid);
  // DVector final_state(4), diff(4);
  // final_state = grid.getLastVector();

  // diff(0) = final_state[0] - bounds[4];
  // diff(1) = final_state[1] - bounds[5];
  // diff(2) = final_state[2] - bounds[6];
  // diff(3) = final_state[3] - bounds[7];

  // if (diff.getNorm(VN_L2) < 0.001){

  if (return_code == 0){
    flag = true;
    // cout << thread;
    states_nm = "states_" + to_string(index) + "_" + thread + ".txt";
    parameters_nm = "parameters_" + to_string(index) + "_" + thread + ".txt";
    control_nm = "control_" + to_string(index) + "_" + thread + ".txt";
    algorithm.getDifferentialStates(states_nm.c_str());
    algorithm.getObjectiveValue(parameters_nm.c_str());
    algorithm.getControls(control_nm.c_str());
  }

  clearAllStaticCounters();
  return flag;
}

int main(int argc, char const *argv[])
{
  float q_lims[2] = {0.0,2*pi};
  float qd_lims[2] = {-30.0,30.0};

  if(argc != 3){
    cout << "Incorrect number of arguments." << endl;
  }
  else{
    int num_iter = atoi(argv[1]);

    gen.seed(time(0) + int(*argv[2]));
    int i = 0;
    while(i < num_iter){
      float bounds[8] = {rand_gen(q_lims),rand_gen(q_lims),rand_gen(qd_lims),rand_gen(qd_lims),rand_gen(q_lims),rand_gen(q_lims),rand_gen(qd_lims),rand_gen(qd_lims)};
      cout << i << endl;
      cout << bounds[0] << " " << bounds[1] << " " << bounds[2] << " " << bounds[3] << " " << bounds[4] << " " << bounds[5] << " " << bounds[6] << " " << bounds[7] << endl;
      bool success = gen_data(bounds, i, *argv[2]);
      if(success){
        i++;
      }
    }
  }
  return 0;
}


#pragma once

#include <boost/multi_array.hpp>

namespace fuzzy
{
  inline int _edit_distance_char_nonempty(const char *s1, int n1, const char *s2, int n2) {
    boost::multi_array<int, 2> arr(boost::extents[n1+1][n2+1]);

    arr[0][0] = 0;
    for (int i = 1; i < n1 + 1; i++)
      arr[i][0] = arr[i-1][0] + 1;
    for (int j = 1; j < n2 + 1; j++)
      arr[0][j] = arr[0][j-1] + 1;

    for (int i = 1; i < n1 + 1; i++)
    {
      for (int j = 1; j < n2 + 1; j++)
      {
        int diff = 0;
        if (s1[i-1] != s2[j-1])
          diff = 1;
        arr[i][j] = std::min(std::min(arr[i - 1][j] + 1,
                                      arr[i][j - 1] + 1),
                             arr[i - 1][j - 1] + diff);
      }
    }
    return arr[n1][n2];
  }

  inline int _edit_distance_char(const char *s1, int n1, const char *s2, int n2) {
    if (n1==0 || n2==0) return n1+n2;
    return _edit_distance_char_nonempty(s1, n1, s2, n2);
  }
}


/* =========================================================================
   Copyright (c) 2010-2014, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
   Portions of this software are copyright by UChicago Argonne, LLC.

                            -----------------
                  ViennaCL - The Vienna Computing Library
                            -----------------

   Project Head:    Karl Rupp                   rupp@iue.tuwien.ac.at

   (A list of authors and contributors can be found in the PDF manual)

   License:         MIT (X11), see file LICENSE in the base directory
============================================================================= */

/** \example qr.cpp
*
*   This tutorial shows how the QR factorization of matrices from ViennaCL or Boost.uBLAS can be computed.
*
**/

// Activate ublas support in ViennaCL
#define VIENNACL_WITH_UBLAS

//
// Include necessary system headers
//
#include <iostream>

//
// ViennaCL includes
//
#include "viennacl/matrix.hpp"
#include "viennacl/linalg/prod.hpp"
#include "viennacl/linalg/qr.hpp"

//
// Boost includes
//
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>

/**
* A helper function comparing two matrices and returning the maximum entry-wise relative error encountered.
**/
template<typename MatrixType>
double check(MatrixType const & qr, MatrixType const & ref)
{
  bool do_break = false;
  double max_error = 0;
  for (std::size_t i=0; i<ref.size1(); ++i)
  {
    for (std::size_t j=0; j<ref.size2(); ++j)
    {
      if (qr(i,j) != 0.0 && ref(i,j) != 0.0)
      {
        double rel_err = fabs(qr(i,j) - ref(i,j)) / fabs(ref(i,j) );

        if (rel_err > max_error)
          max_error = rel_err;
      }

      /* Uncomment the following if you also want to check for NaNs.
      if (qr(i,j) != qr(i,j))
      {
        std::cout << "!!!" << std::endl;
        std::cout << "!!! NaN detected at i=" << i << " and j=" << j << std::endl;
        std::cout << "!!!" << std::endl;
        do_break = true;
        break;
      }*/
    }
    if (do_break)
      break;
  }
  return max_error;
}

/**
*  We set up a random matrix using Boost.uBLAS and use it to initialize a ViennaCL matrix.
*  Then we compute the QR factorization directly for the uBLAS matrix as well as the ViennaCL matrix.
**/
int main (int, const char **)
{
  typedef double               ScalarType;     //feel free to change this to 'double' if supported by your hardware
  typedef boost::numeric::ublas::matrix<ScalarType>              MatrixType;
  typedef viennacl::matrix<ScalarType, viennacl::column_major>   VCLMatrixType;

  std::size_t rows = 113;   // number of rows in the matrix
  std::size_t cols = 54;    // number of columns

  /**
  * Create uBLAS matrices with some random input data.
  **/
  MatrixType ublas_A(rows, cols);
  MatrixType Q(rows, rows);
  MatrixType R(rows, cols);

  // Some random data with a bit of extra weight on the diagonal
  for (std::size_t i=0; i<rows; ++i)
  {
    for (std::size_t j=0; j<cols; ++j)
    {
      ublas_A(i,j) = ScalarType(-1.0) + ScalarType((i+1)*(j+1))
                     + ScalarType( (rand() % 1000) - 500.0) / ScalarType(1000.0);

      if (i == j)
        ublas_A(i,j) += ScalarType(10.0);

      R(i,j) = 0.0;
    }

    for (std::size_t j=0; j<rows; ++j)
      Q(i,j) = ScalarType(0.0);
  }

  // keep initial input matrix for comparison
  MatrixType ublas_A_backup(ublas_A);

  /**
  *   Setup the matrix in ViennaCL and copy the data from the uBLAS matrix:
  **/
  VCLMatrixType vcl_A(ublas_A.size1(), ublas_A.size2());

  viennacl::copy(ublas_A, vcl_A);

  /**
  *
QR Factorization with Boost.uBLAS Matrices

  * Compute QR factorization of A. A is overwritten with Householder vectors. Coefficients are returned and a block size of 3 is used.
  * Note that at the moment the number of columns of A must be divisible by the block size
  **/

  std::cout << "--- Boost.uBLAS ---" << std::endl;
  std::vector<ScalarType> ublas_betas = viennacl::linalg::inplace_qr(ublas_A);  //computes the QR factorization

  /**
  *  Let us check for the correct result:
  **/
  viennacl::linalg::recoverQ(ublas_A, ublas_betas, Q, R);
  MatrixType ublas_QR = prod(Q, R);
  double ublas_error = check(ublas_QR, ublas_A_backup);
  std::cout << "Maximum relative error (ublas): " << ublas_error << std::endl;

  /**
  *
QR Factorization with Boost.uBLAS Matrices

  *  We now compute the QR factorization from a ViennaCL matrix. Internally it uses Boost.uBLAS for the panel factorization.
  **/
  std::cout << "--- Hybrid (default) ---" << std::endl;
  viennacl::copy(ublas_A_backup, vcl_A);
  std::vector<ScalarType> hybrid_betas = viennacl::linalg::inplace_qr(vcl_A);

  /**
  *  Let us check for the correct result:
  **/
  viennacl::copy(vcl_A, ublas_A);
  Q.clear(); R.clear();
  viennacl::linalg::recoverQ(ublas_A, hybrid_betas, Q, R);
  double hybrid_error = check(ublas_QR, ublas_A_backup);
  std::cout << "Maximum relative error (hybrid): " << hybrid_error << std::endl;

  /**
  *  That's it. Print a success message and exit.
  **/
  std::cout << "!!!! TUTORIAL COMPLETED SUCCESSFULLY !!!!" << std::endl;

  return EXIT_SUCCESS;
}


// To run this script, `cd` to the `./test/fixtures` directory and then execute in the terminal `runWandbox --file --compiler gcc-head --output output.json ./runner.cpp`.

#include <random>
#include <algorithm>
#include <iterator>
#include <vector>
#include <iostream>
#include <boost/math/special_functions/gamma.hpp>

using namespace std;

vector<double> linspace( double start, double end, int num ) {
	double delta = (end - start) / (num - 1);
	vector<double> arr( num - 1 );
	for ( int i = 0; i < num - 1; ++i ){
		arr[ i ] = start + delta * i;
	}
	arr.push_back( end );
	return arr;
}

void print_vector( vector<double> vec, bool last = false ) {
	cout << "[";
	for ( vector<double>::iterator it = vec.begin(); it != vec.end(); ++it ) {
		if ( vec.end() != it+1 ) {
			cout << setprecision (16) << *it;
			cout << ",";
		} else {
			cout << setprecision (16) << *it;
			cout << "]";
			if ( last == false ) {
				cout << ",";
			}
		}
	}
	return;
}

void print_results(
	vector<double> x,
	vector<double> s,
	vector<double> lower_regularized,
	vector<double> upper_regularized,
	vector<double> lower_unregularized,
	vector<double> upper_unregularized
) {
	cout << "{" << endl;
	cout << "  \"x\": ";
	print_vector( x );
	cout << "  \"s\": ";
	print_vector( s );
	cout << "  \"lower_regularized\": ";
	print_vector( lower_regularized );
	cout << "  \"upper_regularized\": ";
	print_vector( upper_regularized );
	cout << "  \"lower_unregularized\": ";
	print_vector( lower_unregularized );
	cout << "  \"upper_unregularized\": ";
	print_vector( upper_unregularized, true );
	cout << "}" << endl;
	return;
}

int main() {
	random_device rd;
	mt19937 g(rd());

	vector<double> x = linspace( 1.0, 40.0, 100 );
	shuffle( x.begin(), x.end(), g );
	vector<double> s = linspace( 1.0, 40.0, 100 );
	shuffle( s.begin(), s.end(), g );
	vector<double> lower_regularized;
	vector<double> upper_regularized;
	vector<double> lower_unregularized;
	vector<double> upper_unregularized;

	for ( int i = 0; i < 100; i++  ) {
		double arg1 = s[ i ];
		double arg2 = x[ i ];
		lower_regularized.push_back( boost::math::gamma_p( arg1, arg2 ) );
		upper_regularized.push_back( boost::math::gamma_q( arg1, arg2 ) );
		lower_unregularized.push_back( boost::math::tgamma_lower( arg1, arg2 ) );
		upper_unregularized.push_back( boost::math::tgamma( arg1, arg2 ) );
	}

	print_results( x, s, lower_regularized, upper_regularized, lower_unregularized, upper_unregularized );
	return 0;
}


/**
 * @file
 * @brief We build a tensor product mesh on the unit square and use pointwise
 * refinement to refine it.
 * @author Anian Ruoss
 * @date   2018-10-06 16:37:17
 * @copyright MIT License
 */

#include <boost/program_options.hpp>
#include <functional>
#include <iostream>
#include <vector>

#include <lf/refinement/mesh_hierarchy.h>
#include <lf/refinement/refutils.h>
#include "lf/io/io.h"
#include "lf/mesh/utils/utils.h"

using CodimMeshDataSet_t =
    std::shared_ptr<lf::mesh::utils::CodimMeshDataSet<bool>>;

bool PointInTriangle(const Eigen::MatrixXd &tria_coords,
                     const Eigen::Vector2d &point) {
  // calculate barycentric coordinates of point using affine transformation
  // from reference triangle: phi(x_hat) = alpha + beta * x_hat
  Eigen::Vector2d alpha = tria_coords.col(0);
  Eigen::Matrix2d beta;
  beta << tria_coords.col(1) - tria_coords.col(0),
      tria_coords.col(2) - tria_coords.col(0);

  Eigen::Vector2d loc_coords = beta.inverse() * (point - alpha);

  return (0 <= loc_coords(0) && 0 <= loc_coords(1) && loc_coords.sum() <= 1);
}

CodimMeshDataSet_t MarkMesh(
    const std::shared_ptr<const lf::mesh::Mesh> &mesh_ptr,
    const Eigen::MatrixXd &point) {
  // we use CodimMeshDataSet to store whether an edge has to be marked or not
  CodimMeshDataSet_t marked =
      lf::mesh::utils::make_CodimMeshDataSet<bool>(mesh_ptr, 1, false);

  // loop through all cells to check if it contains the point
  for (const lf::mesh::Entity &cell : mesh_ptr->Entities(0)) {
    const lf::geometry::Geometry *geom_ptr = cell.Geometry();
    lf::base::RefEl ref_el = cell.RefEl();
    const Eigen::MatrixXd &ref_el_coords(ref_el.NodeCoords());
    const Eigen::MatrixXd vtx_coords(geom_ptr->Global(ref_el_coords));

    // mark all edges if the point lies inside the cell
    if (ref_el == lf::base::RefEl::kTria()) {
      if (PointInTriangle(vtx_coords, point)) {
        for (const lf::mesh::Entity &edge : cell.SubEntities(1)) {
          marked->operator()(edge) = true;
        }
      }
    } else if (ref_el == lf::base::RefEl::kQuad()) {
      // split quadrilateral into two triangles and check each separately
      Eigen::MatrixXd tria1 = vtx_coords.block(0, 0, 2, 3);
      Eigen::MatrixXd tria2(2, 3);
      tria2 << vtx_coords.col(0), vtx_coords.col(2), vtx_coords.col(3);

      if (PointInTriangle(tria1, point) || PointInTriangle(tria2, point)) {
        for (const lf::mesh::Entity &edge : cell.SubEntities(1)) {
          marked->operator()(edge) = true;
        }
      }
    } else {
      std::cerr << "unknown cell geometry" << std::endl;
    }
  }

  return marked;
}

int main(int argc, char **argv) {
  // define allowed command line arguments:
  namespace po = boost::program_options;
  po::options_description desc("Allowed options");
  desc.add_options()("help", "Produce this help message")(
      "num_steps", po::value<size_t>()->default_value(5),
      "Number of refinement steps")(
      "pointwise", po::value<bool>()->default_value(true),
      "Whether to use pointwise refinement or not")(
      "point",
      po::value<std::vector<double>>()->multitoken()->default_value(
          std::vector<double>{.5, .5}, ".5, .5"),
      "Point coordinates in unit square (ignored if pointwise=false)");

  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, desc), vm);
  po::notify(vm);

  if (vm.count("help") != 0u) {
    std::cout << desc << std::endl;
    return 1;
  }

  size_t num_steps = vm["num_steps"].as<size_t>();
  bool pointwise = vm["pointwise"].as<bool>();
  std::vector<double> point_coords = vm["point"].as<std::vector<double>>();
  Eigen::Vector2d point(point_coords.data());

  using size_type = lf::base::size_type;
  using lf::mesh::utils::TikzOutputCtrl;

  std::shared_ptr<lf::mesh::hybrid2d::MeshFactory> mesh_factory_ptr =
      std::make_shared<lf::mesh::hybrid2d::MeshFactory>(2);

  // build single-cell tensor product mesh on unit square
  lf::mesh::hybrid2d::TPQuadMeshBuilder builder(mesh_factory_ptr);
  builder.setBottomLeftCorner(Eigen::Vector2d{0, 0});
  builder.setTopRightCorner(Eigen::Vector2d{1, 1});
  builder.setNoXCells(1);
  builder.setNoYCells(1);
  std::shared_ptr<lf::mesh::Mesh> mesh_ptr = builder.Build();

  // output mesh information
  const lf::mesh::Mesh &mesh = *mesh_ptr;
  lf::mesh::utils::PrintInfo(mesh, std::cout);
  std::cout << std::endl;

  // build mesh hierarchy
  lf::refinement::MeshHierarchy multi_mesh(mesh_ptr, mesh_factory_ptr);

  // mark edges of cells containing point
  auto marker = [](const lf::mesh::Mesh &mesh, const lf::mesh::Entity &edge,
                   CodimMeshDataSet_t mesh_data) -> bool {
    return mesh_data->operator()(edge);
  };

  for (int step = 0; step < num_steps; ++step) {
    // obtain pointer to mesh on finest level
    const size_type n_levels = multi_mesh.NumLevels();
    std::shared_ptr<const lf::mesh::Mesh> mesh_fine =
        multi_mesh.getMesh(n_levels - 1);

    // print number of entities of various co-dimensions
    std::cout << "Mesh on level " << n_levels - 1 << ": " << mesh_fine->Size(2)
              << " nodes, " << mesh_fine->Size(1) << " edges, "
              << mesh_fine->Size(0) << " cells," << std::endl;

    lf::mesh::utils::writeTikZ(
        *mesh_fine,
        std::string("refinement_mesh") + std::to_string(step) + ".txt",
        TikzOutputCtrl::RenderCells | TikzOutputCtrl::CellNumbering |
            TikzOutputCtrl::VerticeNumbering | TikzOutputCtrl::NodeNumbering |
            TikzOutputCtrl::EdgeNumbering);

    lf::io::writeMatplotlib(*mesh_fine, std::string("refinement_mesh") +
                                            std::to_string(step) + ".csv");

    if (pointwise) {
      CodimMeshDataSet_t marked_mesh = MarkMesh(mesh_fine, point);
      multi_mesh.MarkEdges(std::bind(marker, std::placeholders::_1,
                                     std::placeholders::_2, marked_mesh));
      multi_mesh.RefineMarked();
    } else {
      multi_mesh.RefineRegular();
    }
  }

  // generate MATLAB functions describing all levels of mesh hierarchy
  lf::refinement::WriteMatlab(multi_mesh, "pointwise_refinement");

  return 0;
}


/*
Copyright 2017 InitialDLab

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 <iostream>
#include <string>
#include <random>

#include <cstdlib>

#include <boost/timer/timer.hpp>
#include <boost/random/taus88.hpp>
#include <boost/random/uniform_smallint.hpp>

#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

constexpr int REPEAT = 1000000;

template <typename T>
void test(std::string const& name)
{
    T t;
    boost::timer::auto_cpu_timer _;
    float s = 0;
    for(int i = 0; i < REPEAT; ++i)
    {
        s += t.run();
    }
    std::cerr << name << ' ' << s << std::endl;
            std::cerr << std::endl;
}

struct Rand
{
    int run() { rand()%10000; }
};

struct Drand48
{
    int run() { (size_t)(drand48()*10000); }
};

struct GSLReal
{
    GSLReal() { _gsl_rng = gsl_rng_alloc (gsl_rng_taus); }

    gsl_rng *_gsl_rng = nullptr;
    int run() {
        return gsl_ran_flat(_gsl_rng, 0, 10000);
    }
};

template<typename RNG, typename DIST>
struct Dist
{
    RNG rng;
    int run() {
        DIST dist(0, 10000);
        dist(rng);
    }
};

template<typename RNG, typename DIST>
struct Dist_1
{
    RNG rng;
    DIST dist{0, 10000};
    int run() {
        dist(rng);
    }
};

template<typename RNG, typename DIST>
struct Dist_2
{
    int run() {
        RNG rng;
        DIST dist{0, 10000};
        dist(rng);
    }
};

void test_uni()
{
    test<Rand>("rand");
    test<Drand48>("drand48");

    using std_default = std::default_random_engine;
    using taus88 = boost::random::taus88;

    using std_uni = std::uniform_int_distribution<int>;
    using smallint = boost::random::uniform_smallint<int>;

    test<Dist<std_default, std_uni>>("uni_default");
    test<Dist_1<std_default, std_uni>>("uni_default_1");
    test<Dist<taus88, std_uni>>("uni_taus88");
    test<Dist_1<taus88, std_uni>>("uni_taus88_1");

    test<Dist<std_default, smallint>>("smallint_default");
    test<Dist_1<std_default, smallint>>("smallint_default_1");
    test<Dist<taus88, smallint>>("smallint_taus88");
    test<Dist_1<taus88, smallint>>("smallint_taus88_1");
}

template<typename RNG, typename DIST>
struct RealDist
{
    RNG rng;
    float run() {
        DIST dist(0, 1);
        return dist(rng);
    }
};

template<typename RNG, typename DIST>
struct RealDist_1
{
    RNG rng;
    DIST dist{0, 1};
    float run() {
        return dist(rng);
    }
};

template<typename RNG, typename DIST>
struct RealDist_2
{
    float run() {
        RNG rng;
        DIST dist{0, 1};
        return dist(rng);
    }
};

void test_real()
{
    using std_default = std::default_random_engine;
    using taus88 = boost::random::taus88;

    using std_real = std::uniform_real_distribution<float>;

    test<Rand>("rand");
    test<Drand48>("drand48");
    test<GSLReal>("GSL_real");

    test<RealDist<std_default, std_real>>("real_default");
    test<RealDist_1<std_default, std_real>>("real_default_1");
    test<RealDist<taus88, std_real>>("real_taus88");
    test<RealDist_1<taus88, std_real>>("real_taus88_1");
}

void test_binomial()
{
    using taus88 = boost::random::taus88;
    taus88 rng;
    for(size_t total = 1; total < 100; total += 10)
    {
        //for(double prob = 0.0; prob < 1; prob += 0.2)
        double prob = 0.001;
        {
            size_t s = 0;
            {
                boost::timer::auto_cpu_timer _;
                for(int i = 0; i < REPEAT; ++i)
                {
                    s += std::binomial_distribution<size_t>(total, prob)(rng);
                }
            }
            std::cerr << "binomial " << total << ' ' << prob << ' ' << s << std::endl;
            std::cerr << std::endl;
        }
        {
            size_t s = 0;
            {
                boost::timer::auto_cpu_timer _;
                std::uniform_real_distribution<float> coin_dist(0,1);
                for(int i = 0; i < REPEAT; ++i)
                {
                    for(size_t j = 0; j < total; ++j)
                        if(coin_dist(rng) < prob)
                            ++s;
                }
            }
            std::cerr << "coin " <<  total << ' ' << prob << ' ' << s << std::endl;
            std::cerr << std::endl;
        }
    }
}

int main()
{
//    test_uni();
    test_real();
//    test_binomial();
    return 0;
}


// system includes -----------------------------------------------
#include <boost/math/constants/constants.hpp>
#include <boost/program_options.hpp>
#include <iostream>
// own includes --------------------------------------------------
#include <base/eigen2hdf.hpp>
#include <base/init.hpp>
#include <fft/fft2.hpp>
#include <ridgelet/ridgelet_cell_array.hpp>
#include <ridgelet/ridgelet_frame.hpp>
#include <ridgelet/rt.hpp>

using namespace std;

const char* fname = "test_rt.h5";

typedef RT<> RT_t;

typedef RT_t::array_t array_t;
typedef RT_t::complex_array_t complex_array_t;
typedef RT_t::rt_coeff_t rt_coeff_t;

void dump_frc(const std::vector<rt_coeff_t>& f_rc,
              const RidgeletFrame& rt,
              string fname = "f_rc.h5")
{
  hid_t file = H5Fcreate(fname.c_str(), H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);
  for (unsigned int i = 0; i < f_rc.size(); ++i) {
    stringstream ss;
    ss << rt.lambdas()[i];
    string slam = ss.str();
    eigen2hdf::save(file, slam, f_rc[i]);
  }
  H5Fclose(file);
  cout << "\n\n ---- Written f(lambda, t) to `" << fname << "` ----\n\n";
}

int main(int argc, char* argv[])
{
  SOURCE_INFO();

  namespace po = boost::program_options;

  cout << "FFTW_BACKWARD: " << FFTW_BACKWARD << "\n";
  cout << "FFTW_FORWARD: " << FFTW_FORWARD << "\n";

  unsigned int Jx, Jy, rho_x, rho_y;

  po::options_description options("options");
  options.add_options()("help", "produce help message")
      ("Jx,i", po::value<unsigned int>(&Jx)->default_value(2), "Jx")
      ("Jy,j", po::value<unsigned int>(&Jy)->default_value(2), "Jy")
      ("rx,x", po::value<unsigned int>(&rho_x)->default_value(1), "rho_x")
      ("ry,y", po::value<unsigned int>(&rho_y)->default_value(1), "rho_x");
  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, options), vm);
  po::notify(vm);

  if (vm.count("help")) {
    std::cout << options << "\n";
    return 0;
  }

  cout << setw(20) << "Jx"
       << ": " << Jx << "\n"
       << setw(20) << "Jy"
       << ": " << Jy << "\n"
       << setw(20) << "rho_x"
       << ": " << rho_x << "\n"
       << setw(20) << "rho_y"
       << ": " << rho_y << "\n";

  RidgeletFrame frame(Jx, Jy, rho_x, rho_y);
  const unsigned int ncols = frame.Nx();  // #cols
  const unsigned int nrows = frame.Ny();  // #rows
  // FFT fft;
  RT_t rt(frame);
  complex_array_t Fh(nrows, ncols);
  complex_array_t Fh2(nrows, ncols);
  /* compute ridgelet transform  */
  std::vector<rt_coeff_t> rt_coeffs(frame.size());
  std::vector<rt_coeff_t> rt_coeffs2(frame.size());
  rt.rt(rt_coeffs, Fh);

  for (unsigned int i = 0; i < frame.size(); ++i) {
    rt_coeffs[i].setZero();
  }
  bool failed = false;

  cout << "Set single lambda rt coeff to random and check invertibility..."
       << "\n";
  for (unsigned int i = 0; i < frame.size(); ++i) {
    rt_coeffs[i].setRandom();
    rt.irt(Fh, rt_coeffs);
    // forward transform
    rt.rt(rt_coeffs2, Fh);
    // inverse transform
    rt.irt(Fh2, rt_coeffs2);
    // check that is invertible on half size
    auto Diff = ftcut(Fh2, nrows / 2, ncols / 2) - ftcut(Fh, nrows / 2, ncols / 2);
    double diff = Diff.abs().sum();
    if (diff > 1e-11) {
      cout << "::check inverse::" << frame.lambdas()[i] << ": " << diff << "\n";
      failed = true;
    }
  }

  if (!failed)
    cout << "TEST PASSSED"
         << "\n";
  else
    cout << "TEST FAILED"
         << "\n";

  return 0;
}