//  (C) Copyright Evan Miller 2020.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <iostream>
#include <boost/math/tools/ulps_plot.hpp>
#include <boost/core/demangle.hpp>
#include <boost/math/special_functions/jacobi_theta.hpp>

using boost::math::tools::ulps_plot;

int main() {
    using PreciseReal = long double;
    using CoarseReal = float;

    CoarseReal q = 0.5;

    auto jacobi_theta1_coarse = [=](CoarseReal z) {
        return boost::math::jacobi_theta1<CoarseReal>(z, q);
    };
    auto jacobi_theta1_precise = [=](PreciseReal z) {
        return boost::math::jacobi_theta1<PreciseReal>(z, q);
    };
    auto jacobi_theta2_coarse = [=](CoarseReal z) {
        return boost::math::jacobi_theta2<CoarseReal>(z, q);
    };
    auto jacobi_theta2_precise = [=](PreciseReal z) {
        return boost::math::jacobi_theta2<PreciseReal>(z, q);
    };
    auto jacobi_theta3_coarse = [=](CoarseReal z) {
        return boost::math::jacobi_theta3m1<CoarseReal>(z, q);
    };
    auto jacobi_theta3_precise = [=](PreciseReal z) {
        return boost::math::jacobi_theta3m1<PreciseReal>(z, q);
    };
    auto jacobi_theta4_coarse = [=](CoarseReal z) {
        return boost::math::jacobi_theta4m1<CoarseReal>(z, q);
    };
    auto jacobi_theta4_precise = [=](PreciseReal z) {
        return boost::math::jacobi_theta4m1<PreciseReal>(z, q);
    };

    int samples = 2500;
    int width = 800;
    PreciseReal clip = 100;

    std::string filename1 = "jacobi_theta1_" + boost::core::demangle(typeid(CoarseReal).name()) + ".svg";
    auto plot1 = ulps_plot<decltype(jacobi_theta1_precise), PreciseReal, CoarseReal>(jacobi_theta1_precise, 0.0, boost::math::constants::two_pi<CoarseReal>(), samples);
    plot1.clip(clip).width(width);
    std::string title1 = "jacobi_theta1(x, 0.5) ULP plot at " + boost::core::demangle(typeid(CoarseReal).name()) + " precision";
    plot1.title(title1);
    plot1.vertical_lines(10);
    plot1.add_fn(jacobi_theta1_coarse);
    plot1.write(filename1);

    std::string filename2 = "jacobi_theta2_" + boost::core::demangle(typeid(CoarseReal).name()) + ".svg";
    auto plot2 = ulps_plot<decltype(jacobi_theta2_precise), PreciseReal, CoarseReal>(jacobi_theta2_precise, 0.0, boost::math::constants::two_pi<CoarseReal>(), samples);
    plot2.clip(clip).width(width);
    std::string title2 = "jacobi_theta2(x, 0.5) ULP plot at " + boost::core::demangle(typeid(CoarseReal).name()) + " precision";
    plot2.title(title2);
    plot2.vertical_lines(10);
    plot2.add_fn(jacobi_theta2_coarse);
    plot2.write(filename2);

    std::string filename3 = "jacobi_theta3_" + boost::core::demangle(typeid(CoarseReal).name()) + ".svg";
    auto plot3 = ulps_plot<decltype(jacobi_theta3_precise), PreciseReal, CoarseReal>(jacobi_theta3_precise, 0.0, boost::math::constants::two_pi<CoarseReal>(), samples);
    plot3.clip(clip).width(width);
    std::string title3 = "jacobi_theta3m1(x, 0.5) ULP plot at " + boost::core::demangle(typeid(CoarseReal).name()) + " precision";
    plot3.title(title3);
    plot3.vertical_lines(10);
    plot3.add_fn(jacobi_theta3_coarse);
    plot3.write(filename3);

    std::string filename4 = "jacobi_theta4_" + boost::core::demangle(typeid(CoarseReal).name()) + ".svg";
    auto plot4 = ulps_plot<decltype(jacobi_theta4_precise), PreciseReal, CoarseReal>(jacobi_theta4_precise, 0.0, boost::math::constants::two_pi<CoarseReal>(), samples);
    plot4.clip(clip).width(width);
    std::string title4 = "jacobi_theta4m1(x, 0.5) ULP plot at " + boost::core::demangle(typeid(CoarseReal).name()) + " precision";
    plot4.title(title4);
    plot4.vertical_lines(10);
    plot4.add_fn(jacobi_theta4_coarse);
    plot4.write(filename4);
}


#include <cstdio>
#include <cstdlib>
#include <iostream>
#include <fstream>
#include <vector>
#include <chrono>

#include <getopt.h>

#include "Sampling_Matrix.hpp"
#include "Alpert_Matrix.hpp"
#include "Utils.hpp"
#include "ccstomp.hpp"

#include <boost/numeric/bindings/blas/blas.h>

using namespace std;

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

  if(argc == 1 ){
    cout << argv[0] << " usage is:" << endl;
    cout << argv[0] << " --seed 'seed_value' --reduction 'reduction_value' [mesh input_file] [data input_file]" << endl;
    cout << "If the data input file is provided it should have the same number of points as the mesh, if not a default data field will be assigned" << endl;
    cout << "Please refer to README.txt in this folder for more info" << endl;
    exit (0);
  }

  /*Seed is overwritten if set as a command line option*/
    unsigned seed = chrono::system_clock::now().time_since_epoch().count();
    chrono::duration <double> duration;

    int d, N;
    double R=10.0e0;
    int c;
    double alpha=1.0e0, beta=0.0e0;
    int inc=1;

    {
          static struct option long_options[] =
        {
          /* These options set a flag. */
          /* These options don’t set a flag.
             We distinguish them by their indices. */
          {"seed",  required_argument, 0, 's'},
          {"reduction",  required_argument, 0, 'r'},
          {0, 0, 0, 0}
        };
      /* getopt_long stores the option index here. */
      int option_index = 0;

      while (c = getopt_long (argc, argv, "s",
			      long_options, &option_index) != -1){

	if(option_index == 0 && optarg){
	  seed  = atoi(optarg);
	  cout << "Seed set to " << seed << endl;
	}
	if(option_index == 1 && optarg){
	  R  = atof(optarg);
	  cout << "Reduction set to " << R << endl;
	}
      }

    }

    if(optind >= argc ) {
      cout << "Expected an input file name " << endl;
      exit(0);
    }

    cout << "Reading mesh input file" << argv[optind] << endl;
    ifstream f(argv[optind]);
    f >> N >> d;

    cout << "Number of Points " << N << endl;
    cout << "Number of dimensions " << d << endl;
    cout << "R " << R << endl;

    int M=ceil(double(N)/R);
    int NR=N-M;

    vector<vector<double> >p(N, vector<double>(d));

    // Read mesh into array
    for (int i = 0; i < N; i++)
        for (int j = 0; j < d; j++)
            f >> p[i][j];

    f.close();

    // Setting Wavelet orders
    vector<int> ki(d);
    for (int j = 0; j < d; j++)
      ki[j] = 5;

    cout << "Constructing Alpert Matrix" << endl;
    boost::numeric::ublas::compressed_matrix <double>  U(N, N);
    chrono::system_clock::time_point t1 = chrono::system_clock::now();
    boostbuild_Alpert_matrix(p, ki,  U, -1, N);
    chrono::system_clock::time_point t2 = chrono::system_clock::now();
    duration=t2-t1;
    cout << "Total wavelet matrix construction time " << duration.count() << endl;

    boost::numeric::ublas::vector<double> x(N);
    boost::numeric::ublas::vector<double> w(N);
    boost::numeric::ublas::vector<double> xw(N);
    boost::numeric::ublas::vector<double> ys(N);

    // Setting data, this step can be replaced by some commands to read an external data file
    double pi=4.0e0*atan(1.0e0);
    if (argc>6) {
      cout << "Reading data input file" << argv[optind+1] << endl;
      f.open(argv[optind+1]);
      for (int i = 0; i < N; i++)
	f >> x(i);
    }
    else
      for (int i = 0; i < N; i++) x(i)=(4.0e0*sin(8.0e0*pi*p[i][0]))*(4.0e0*sin(7.0e0*pi*p[i][1]) )*3.0e0*sin(6.0e0*pi*p[i][0]);
//     for (int i = 0; i < N; i++) x[i]=(4.0*sin(2.0*pi*p[i][0]) - 4.0*sin(2.0*pi*p[i][1]))*3.0*sin(2.0*pi*p[i][0]);

    // Forward wavelet transform
    t1 = chrono::system_clock::now();
    boost::numeric::ublas::axpy_prod(U, x, w, true);

    // Sorting, wavelet compression and inverse wavelet transform
    vector<double> yv(N);
    vector<size_t> iyv(N);
    for (int i = 0; i < N; i++) {yv[i]=fabs(w(i));iyv[i]=i;}
    sortandindices(yv, iyv);
    for (int i = NR; i < N; i++) ys(iyv[i])=w(iyv[i]);
    t2 = chrono::system_clock::now();
    duration=t2-t1;
    cout << "Total wavelet compression time " << duration.count() << endl;

    // xw is the reconstruction by wavelets
    boost::numeric::ublas::axpy_prod(ys, U, xw, true);

    cout << "Constructing Bernoulli Sampling Matrix" << endl;
    boost::numeric::ublas::matrix<double, boost::numeric::ublas::column_major> P(N,M,0.0e0);
    t1 = chrono::system_clock::now();
    ConstructBernoulliMatrix(M, N, seed, P);
    t2 = chrono::system_clock::now();
    duration=t2-t1;
    cout << "Total compression matrix construction time " << duration.count() << endl;

    cout << "Performing CS compression" << endl;
    boost::numeric::ublas::vector<double> y(M);
    t1 = chrono::system_clock::now();
    BLAS_DGEMV("T", &N, &M, &alpha, &P(0,0), &N, &x(0), &inc, &beta, &y(0), &inc);
    t2 = chrono::system_clock::now();
    duration=t2-t1;
    cout << "Total CS compression time " << duration.count() << endl;

    cout << "Constructing Alpert-Bernoulli Matrix product" << endl;
    t1 = chrono::system_clock::now();
    build_Alpert_Sampling_product(U,P,P);
    t2 = chrono::system_clock::now();
    duration=t2-t1;
    cout << "Alpert-Compression matrix product time " << duration.count() << endl;

    // Computing incoherence between sampling and wavelet matrices
    double maxT=0.0;
    for (int j = 0; j < M; j++)
      for (int i = 0; i < N; i++)
	if (fabs(P(i,j))>maxT)
	  maxT=fabs(P(i,j));

    cout << "Performing StOMP" << endl;
    t1 = chrono::system_clock::now();
    boost::numeric::ublas::vector<double> ywp(N);
    int * asize= new int[2]; asize[0]=M; asize[1]=N;
    int * rsize= new int[2]; rsize[0]=M; rsize[1]=1;
    int niter=0, nywp=0;
    double *activeSet=NULL;
    double *aresult=NULL;
    // activeSet and aresult are the initial guess of the solution, here we assume the coefficient
    // vector is empty
    cc_solve_stomp(&P(0,0), &y(0), asize, rsize, 1.0e-8, 50, &niter, &nywp, &ywp(0), activeSet, aresult);

    boost::numeric::ublas::vector<double> xs(N);
    boost::numeric::ublas::axpy_prod(ywp, U, xs, true);
    t2 = chrono::system_clock::now();
    duration=t2-t1;
    cout << "StOMP time " << duration.count() << endl;

    // Computing NRMSE
    double es=0.0e0, ew=0.0e0;
    for (int i = 0; i < N; i++) {
      ew+=pow(x(i)-xw(i),2.0e0);
      es+=pow(x(i)-xs(i),2.0e0);
    }

    // Computing min and max values in the data x
    double mx=x(0);
    double mn=x(0);
    for (int i = 0; i < N; i++) {
      if (x(i)>mx)
	mx=x(i);
      if (x(i)<mn)
	mn=x(i);
    }

    es=sqrt(es/double(N))/(mx-mn);
    ew=sqrt(ew/double(N))/(mx-mn);

    cout << "Alpert Wavelets Compression NRMSE = " << ew << endl;
    cout << "CS Compression NRMSE = " << es << endl;
}


/*
 * Copyright Nick Thompson, 2017
 * Use, modification and distribution are subject to the
 * Boost Software License, Version 1.0. (See accompanying file
 * LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
 */
#define BOOST_TEST_MODULE catmull_rom_test

#include <array>
#include <random>
#include <boost/cstdfloat.hpp>
#include <boost/type_index.hpp>
#include <boost/test/included/unit_test.hpp>
#include <boost/test/tools/floating_point_comparison.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/math/interpolators/catmull_rom.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>
#include <boost/numeric/ublas/vector.hpp>

using std::abs;
using boost::multiprecision::cpp_bin_float_50;
using boost::math::catmull_rom;

template<class Real>
void test_alpha_distance()
{
    Real tol = std::numeric_limits<Real>::epsilon();
    std::array<Real, 3> v1 = {0,0,0};
    std::array<Real, 3> v2 = {1,0,0};
    Real alpha = 0.5;
    Real d = boost::math::detail::alpha_distance<std::array<Real, 3>>(v1, v2, alpha);
    BOOST_CHECK_CLOSE_FRACTION(d, 1, tol);

    d = boost::math::detail::alpha_distance<std::array<Real, 3>>(v1, v2, 0.0);
    BOOST_CHECK_CLOSE_FRACTION(d, 1, tol);

    d = boost::math::detail::alpha_distance<std::array<Real, 3>>(v1, v2, 1.0);
    BOOST_CHECK_CLOSE_FRACTION(d, 1, tol);

    v2[0] = 2;
    d = boost::math::detail::alpha_distance<std::array<Real, 3>>(v1, v2, alpha);
    BOOST_CHECK_CLOSE_FRACTION(d, pow(2, (Real)1/ (Real) 2), tol);

    d = boost::math::detail::alpha_distance<std::array<Real, 3>>(v1, v2, 0.0);
    BOOST_CHECK_CLOSE_FRACTION(d, 1, tol);

    d = boost::math::detail::alpha_distance<std::array<Real, 3>>(v1, v2, 1.0);
    BOOST_CHECK_CLOSE_FRACTION(d, 2, tol);
}

template<class Real>
void test_linear()
{
    std::cout << "Testing that the Catmull-Rom spline interpolates linear functions correctly on type "
              << boost::typeindex::type_id<Real>().pretty_name() << "\n";

    Real tol = 10*std::numeric_limits<Real>::epsilon();
    std::vector<std::array<Real, 3>> v(4);
    v[0] = {0,0,0};
    v[1] = {1,0,0};
    v[2] = {2,0,0};
    v[3] = {3,0,0};
    catmull_rom<std::array<Real, 3>> cr(std::move(v));

    // Test that the interpolation condition is obeyed:
    BOOST_CHECK_CLOSE_FRACTION(cr.max_parameter(), 3, tol);
    auto p0 = cr(0.0);
    BOOST_CHECK_SMALL(p0[0], tol);
    BOOST_CHECK_SMALL(p0[1], tol);
    BOOST_CHECK_SMALL(p0[2], tol);
    auto p1 = cr(1.0);
    BOOST_CHECK_CLOSE_FRACTION(p1[0], 1, tol);
    BOOST_CHECK_SMALL(p1[1], tol);
    BOOST_CHECK_SMALL(p1[2], tol);

    auto p2 = cr(2.0);
    BOOST_CHECK_CLOSE_FRACTION(p2[0], 2, tol);
    BOOST_CHECK_SMALL(p2[1], tol);
    BOOST_CHECK_SMALL(p2[2], tol);

    auto p3 = cr(3.0);
    BOOST_CHECK_CLOSE_FRACTION(p3[0], 3, tol);
    BOOST_CHECK_SMALL(p3[1], tol);
    BOOST_CHECK_SMALL(p3[2], tol);

    Real s = cr.parameter_at_point(0);
    BOOST_CHECK_SMALL(s, tol);

    s = cr.parameter_at_point(1);
    BOOST_CHECK_CLOSE_FRACTION(s, 1, tol);

    s = cr.parameter_at_point(2);
    BOOST_CHECK_CLOSE_FRACTION(s, 2, tol);

    s = cr.parameter_at_point(3);
    BOOST_CHECK_CLOSE_FRACTION(s, 3, tol);

    // Test that the function is linear on the interval [1,2]:
    for (double s = 1; s < 2; s += 0.01)
    {
        auto p = cr(s);
        BOOST_CHECK_CLOSE_FRACTION(p[0], s, tol);
        BOOST_CHECK_SMALL(p[1], tol);
        BOOST_CHECK_SMALL(p[2], tol);

        auto tangent = cr.prime(s);
        BOOST_CHECK_CLOSE_FRACTION(tangent[0], 1, tol);
        BOOST_CHECK_SMALL(tangent[1], tol);
        BOOST_CHECK_SMALL(tangent[2], tol);
    }

}

template<class Real>
void test_circle()
{
    using boost::math::constants::pi;
    using std::cos;
    using std::sin;

    std::cout << "Testing that the Catmull-Rom spline interpolates circles correctly on type "
              << boost::typeindex::type_id<Real>().pretty_name() << "\n";

    Real tol = 10*std::numeric_limits<Real>::epsilon();
    std::vector<std::array<Real, 2>> v(20*sizeof(Real));
    std::vector<std::array<Real, 2>> u(20*sizeof(Real));
    for (size_t i = 0; i < v.size(); ++i)
    {
        Real theta = ((Real) i/ (Real) v.size())*2*pi<Real>();
        v[i] = {cos(theta), sin(theta)};
        u[i] = v[i];
    }
    catmull_rom<std::array<Real, 2>> circle(std::move(v), true);

    // Interpolation condition:
    for (size_t i = 0; i < v.size(); ++i)
    {
        Real s = circle.parameter_at_point(i);
        auto p = circle(s);
        Real x = p[0];
        Real y = p[1];
        if (abs(x) < std::numeric_limits<Real>::epsilon())
        {
            BOOST_CHECK_SMALL(u[i][0], tol);
        }
        if (abs(y) < std::numeric_limits<Real>::epsilon())
        {
            BOOST_CHECK_SMALL(u[i][1], tol);
        }
        else
        {
            BOOST_CHECK_CLOSE_FRACTION(x, u[i][0], tol);
            BOOST_CHECK_CLOSE_FRACTION(y, u[i][1], tol);
        }
    }

    Real max_s = circle.max_parameter();
    for(Real s = 0; s < max_s; s += 0.01)
    {
        auto p = circle(s);
        Real x = p[0];
        Real y = p[1];
        BOOST_CHECK_CLOSE_FRACTION(x*x+y*y, 1, 0.001);
    }
}

template<class Real, size_t dimension>
void test_affine_invariance()
{
    std::cout << "Testing that the Catmull-Rom spline is affine invariant in dimension "
              << dimension << " on type "
              << boost::typeindex::type_id<Real>().pretty_name() << "\n";

    Real tol = 1000*std::numeric_limits<Real>::epsilon();
    std::vector<std::array<Real, dimension>> v(100);
    std::vector<std::array<Real, dimension>> u(100);
    std::mt19937_64 gen(438232);
    Real inv_denom = (Real) 100/( (Real) (gen.max)() + (Real) 2);
    for(size_t j = 0; j < dimension; ++j)
    {
        v[0][j] = gen()*inv_denom;
        u[0][j] = v[0][j];
    }

    for (size_t i = 1; i < v.size(); ++i)
    {
        for(size_t j = 0; j < dimension; ++j)
        {
            v[i][j] = v[i-1][j] + gen()*inv_denom;
            u[i][j] = v[i][j];
        }
    }
    std::array<Real, dimension> affine_shift;
    for (size_t j = 0; j < dimension; ++j)
    {
        affine_shift[j] = gen()*inv_denom;
    }

    catmull_rom<std::array<Real, dimension>> cr1(std::move(v));

    for(size_t i = 0; i< u.size(); ++i)
    {
        for(size_t j = 0; j < dimension; ++j)
        {
            u[i][j] += affine_shift[j];
        }
    }

    catmull_rom<std::array<Real, dimension>> cr2(std::move(u));

    BOOST_CHECK_CLOSE_FRACTION(cr1.max_parameter(), cr2.max_parameter(), tol);

    Real ds = cr1.max_parameter()/1024;
    for (Real s = 0; s < cr1.max_parameter(); s += ds)
    {
        auto p0 = cr1(s);
        auto p1 = cr2(s);
        auto tangent0 = cr1.prime(s);
        auto tangent1 = cr2.prime(s);
        for (size_t j = 0; j < dimension; ++j)
        {
            BOOST_CHECK_CLOSE_FRACTION(p0[j] + affine_shift[j], p1[j], tol);
            if (abs(tangent0[j]) > 5000*tol)
            {
                BOOST_CHECK_CLOSE_FRACTION(tangent0[j], tangent1[j], 5000*tol);
            }
        }
    }
}

template<class Real>
void test_helix()
{
    using boost::math::constants::pi;
    std::cout << "Testing that the Catmull-Rom spline interpolates helices correctly on type "
              << boost::typeindex::type_id<Real>().pretty_name() << "\n";

    Real tol = 0.001;
    std::vector<std::array<Real, 3>> v(400*sizeof(Real));
    for (size_t i = 0; i < v.size(); ++i)
    {
        Real theta = ((Real) i/ (Real) v.size())*2*pi<Real>();
        v[i] = {cos(theta), sin(theta), theta};
    }
    catmull_rom<std::array<Real, 3>> helix(std::move(v));

    // Interpolation condition:
    for (size_t i = 0; i < v.size(); ++i)
    {
        Real s = helix.parameter_at_point(i);
        auto p = helix(s);
        Real t = p[2];

        Real x = p[0];
        Real y = p[1];
        if (abs(x) < tol)
        {
            BOOST_CHECK_SMALL(cos(t), tol);
        }
        if (abs(y) < tol)
        {
            BOOST_CHECK_SMALL(sin(t), tol);
        }
        else
        {
            BOOST_CHECK_CLOSE_FRACTION(x, cos(t), tol);
            BOOST_CHECK_CLOSE_FRACTION(y, sin(t), tol);
        }
    }

    Real max_s = helix.max_parameter();
    for(Real s = helix.parameter_at_point(1); s < max_s; s += 0.01)
    {
        auto p = helix(s);
        Real x = p[0];
        Real y = p[1];
        Real t = p[2];
        BOOST_CHECK_CLOSE_FRACTION(x*x+y*y, (Real) 1, (Real) 0.01);
        if (abs(x) < 0.01)
        {
            BOOST_CHECK_SMALL(cos(t),  (Real) 0.05);
        }
        if (abs(y) < 0.01)
        {
            BOOST_CHECK_SMALL(sin(t), (Real) 0.05);
        }
        else
        {
            BOOST_CHECK_CLOSE_FRACTION(x, cos(t), (Real) 0.05);
            BOOST_CHECK_CLOSE_FRACTION(y, sin(t), (Real) 0.05);
        }
    }
}

template<class Real>
class mypoint3d
{
public:
    // Must define a value_type:
    typedef Real value_type;

    // Regular constructor:
    mypoint3d(Real x, Real y, Real z)
    {
        m_vec[0] = x;
        m_vec[1] = y;
        m_vec[2] = z;
    }

    // Must define a default constructor:
    mypoint3d() {}

    // Must define array access:
    Real operator[](size_t i) const
    {
        return m_vec[i];
    }

    // Array element assignment:
    Real& operator[](size_t i)
    {
        return m_vec[i];
    }

private:
    std::array<Real, 3>  m_vec;
};

// Must define the free function "size()":
template<class Real>
BOOST_CONSTEXPR std::size_t size(const mypoint3d<Real>& c)
{
    return 3;
}

template<class Real>
void test_data_representations()
{
    std::cout << "Testing that the Catmull-Rom spline works with multiple data representations.\n";
    mypoint3d<Real> p0(0.1, 0.2, 0.3);
    mypoint3d<Real> p1(0.2, 0.3, 0.4);
    mypoint3d<Real> p2(0.3, 0.4, 0.5);
    mypoint3d<Real> p3(0.4, 0.5, 0.6);
    mypoint3d<Real> p4(0.5, 0.6, 0.7);
    mypoint3d<Real> p5(0.6, 0.7, 0.8);

    // Tests initializer_list:
    catmull_rom<mypoint3d<Real>> cat({p0, p1, p2, p3, p4, p5});

    Real tol = 0.001;
    auto p = cat(cat.parameter_at_point(0));
    BOOST_CHECK_CLOSE_FRACTION(p[0], p0[0], tol);
    BOOST_CHECK_CLOSE_FRACTION(p[1], p0[1], tol);
    BOOST_CHECK_CLOSE_FRACTION(p[2], p0[2], tol);
    p = cat(cat.parameter_at_point(1));
    BOOST_CHECK_CLOSE_FRACTION(p[0], p1[0], tol);
    BOOST_CHECK_CLOSE_FRACTION(p[1], p1[1], tol);
    BOOST_CHECK_CLOSE_FRACTION(p[2], p1[2], tol);
}

template<class Real>
void test_random_access_container()
{
    std::cout << "Testing that the Catmull-Rom spline works with multiple data representations.\n";
    mypoint3d<Real> p0(0.1, 0.2, 0.3);
    mypoint3d<Real> p1(0.2, 0.3, 0.4);
    mypoint3d<Real> p2(0.3, 0.4, 0.5);
    mypoint3d<Real> p3(0.4, 0.5, 0.6);
    mypoint3d<Real> p4(0.5, 0.6, 0.7);
    mypoint3d<Real> p5(0.6, 0.7, 0.8);

    boost::numeric::ublas::vector<mypoint3d<Real>> u(6);
    u[0] = p0;
    u[1] = p1;
    u[2] = p2;
    u[3] = p3;
    u[4] = p4;
    u[5] = p5;

    // Tests initializer_list:
    catmull_rom<mypoint3d<Real>, decltype(u)> cat(std::move(u));

    Real tol = 0.001;
    auto p = cat(cat.parameter_at_point(0));
    BOOST_CHECK_CLOSE_FRACTION(p[0], p0[0], tol);
    BOOST_CHECK_CLOSE_FRACTION(p[1], p0[1], tol);
    BOOST_CHECK_CLOSE_FRACTION(p[2], p0[2], tol);
    p = cat(cat.parameter_at_point(1));
    BOOST_CHECK_CLOSE_FRACTION(p[0], p1[0], tol);
    BOOST_CHECK_CLOSE_FRACTION(p[1], p1[1], tol);
    BOOST_CHECK_CLOSE_FRACTION(p[2], p1[2], tol);
}

BOOST_AUTO_TEST_CASE(catmull_rom_test)
{
#if !defined(TEST) || (TEST == 1)
    test_data_representations<float>();
    test_alpha_distance<double>();

    test_linear<double>();
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
    test_linear<long double>();
#endif

    test_circle<float>();
    test_circle<double>();
#endif
#if !defined(TEST) || (TEST == 2)
    test_helix<double>();

    test_affine_invariance<double, 1>();
    test_affine_invariance<double, 2>();
    test_affine_invariance<double, 3>();
    test_affine_invariance<double, 4>();

    test_random_access_container<double>();
#endif
#if !defined(TEST) || (TEST == 3)
    test_affine_invariance<cpp_bin_float_50, 4>();
#endif
}


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

  @copyright 2016 NumScale SAS

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

/*!
  @ingroup group-constant
  @defgroup constant-Splitfactor Splitfactor (function template)

  Generates a constant able to split IEEE values for precision issues

  @headerref{<boost/simd/constant/splitfactor.hpp>}

  @par Description

  1.  @code
      template<typename T> T Splitfactor();
      @endcode

  2.  @code
      template<typename T> T Splitfactor( boost::simd::as_<T> const& target );
      @endcode

  Generates a value of type @c T that evaluates to the factor usable to split a IEEE754 value
  into two parts in order to provide precision guarantee for some functions (like
  [two_add](@ref real-two_add) or [two_prod](@ref real-two_prod).

  @par Parameters

  | Name                | Description                                                         |
  |--------------------:|:--------------------------------------------------------------------|
  | **target**          | a [placeholder](@ref type-as) value encapsulating the constant type |

  @par Return Value
  A value of type @c as_integer_t<T> that evaluates to

  | Type              | double                        | float         |
  |:------------------|:------------------------------|---------------|
  | **Values**        |   \f$2^{27}\f$                | \f$2^{13}\f$  |

  @par Requirements
  - **T** models IEEEValue
**/

#include <boost/simd/constant/scalar/splitfactor.hpp>
#include <boost/simd/constant/simd/splitfactor.hpp>

#endif


//
// Created by jcfei on 18-9-29.
//

#include <iostream>
#include <vector>
#include <complex>
#include <math.h>
#include "time.h"
using namespace std;
//**********************************//
#include <fftw3.h>
#include "mkl.h"
#include <omp.h>
#include <armadillo>
//**********************************//
#include "tensor.h"
#include "tensor.cpp"
#include "cp_als.cpp"
#include "tucker_hosvd.cpp"
//**********************************//

double gettime(){
    struct timeval tv;
    gettimeofday(&tv,NULL);
    return tv.tv_sec*1000+tv.tv_usec/1000.0; //time:s
};

using namespace std;
using namespace arma;

int main() {
    double t0,t1;
    int I=3;
    int rank=0.2*I; rank=1;

    Tensor<float> a(I,I,I);
    //initialization
    for (int i = 0; i < a.n1; ++i) {
        for (int j = 0; j < a.n2; ++j) {
            for(int k=0; k< a.n3; ++k) {
                a(i,j,k) = randu<float>();
            }
        }
    }

    t0=gettime();
    tucker_core<float> result_tucker;
    result_tucker = hosvd(a,rank,rank,rank);
    t1=gettime();
    cout << "time:" <<t1-t0 <<endl;

    t0=gettime();
    cp_mats<float> result;
    result = cp_als(a,rank);
    t1=gettime();
    cout << "time:" <<t1-t0 <<endl;

    return 0;

}


// gnuplot-c++ includes
#include <gnuplot-iostream.h>

// MLearn includes
#include <MLearn/Core>
#include <MLearn/Sampling/GaussianSampling.h>

// STL includes
#include <vector>
#include <tuple>
#include <string>
#include <cmath>

// Eigen includes
#include <Eigen/Core>
#include <Eigen/SVD>

int main(int argc, char** argv){
	std::srand((unsigned int) time(0));

	Gnuplot gp;

	using namespace MLearn;
	using namespace Sampling::Gaussian;

	typedef double float_type;
	typedef MLMatrix<float_type> Matrix;
	typedef MLVector<float_type> Vector;

	uint DIM = 2;
	uint N = 5000;

	// compute a random covariance matrix
	Matrix random = Matrix::Random(DIM, DIM);
	Eigen::JacobiSVD<Matrix> svd(random,
		Eigen::ComputeFullU | Eigen::ComputeFullV );
	Matrix covariance = svd.matrixU();
	Matrix eig_val = (svd.singularValues().cwiseAbs().cwiseSqrt()).asDiagonal();
	covariance = covariance*eig_val*eig_val;
	covariance = covariance*(svd.matrixU().transpose());
	// compute mean
	Vector mean = Vector::Random(DIM);
	mean.array() -= float_type(0.5);

	// sample
	Matrix samples = MultivariateGaussian<float_type>::sample(
		mean, covariance, N);

	// draw the samples
	std::vector<std::pair<double, double> > xy_pts;
	for(uint i = 0; i < N; ++i) {
		xy_pts.push_back(std::make_pair(samples(0, i), samples(1, i)));
	}
	gp << "set multiplot\n";
	gp << "set xrange [-5:5]\nset yrange [-5:5]\n";
	gp << "plot '-' with points notitle\n";
	gp.send1d(xy_pts);

	gp << "set parametric\n";
	gp << "set trange [0:2*pi]\n";
	gp << "fx(t)=" +
	      std::to_string(svd.matrixU()(0,0)*eig_val(0,0)*1.96) +
	      "*cos(t)+" +
	      std::to_string(svd.matrixU()(0,1)*eig_val(1,1)*1.96) +
	      "*sin(t)+" + std::to_string(mean(0)) + "\n";
	gp << "fy(t) = " +
	      std::to_string(svd.matrixU()(1,0)*eig_val(0,0)*1.96) +
	      "*cos(t) + " +
	      std::to_string(svd.matrixU()(1,1)*eig_val(1,1)*1.96) +
	      "*sin(t) + " + std::to_string(mean(1)) + "\n";
	gp << "plot fx(t), fy(t) linecolor rgb \"blue\" notitle\n";

	return 0;
}


/*
 *  Copyright (C) 2003,2005 by Jarno Elonen
 *
 *  TPSDemo is Free Software / Open Source with a very permissive
 *  license:
 *
 *  Permission to use, copy, modify, distribute and sell this software
 *  and its documentation for any purpose is hereby granted without fee,
 *  provided that the above copyright notice appear in all copies and
 *  that both that copyright notice and this permission notice appear
 *  in supporting documentation.  The authors make no representations
 *  about the suitability of this software for any purpose.
 *  It is provided "as is" without express or implied warranty.
 */

#include <boost/numeric/ublas/matrix.hpp>

#include "spline.h"
#include "ludecomposition.h"

#include <vector>
#include <cmath>

using namespace boost::numeric::ublas;

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

static double tps_base_func(double r)
{
  if ( r == 0.0 )
    return 0.0;
  else
    return r*r * log(r);
}

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

/*
 *  Calculate Thin Plate Spline (TPS) weights from
 *  control points.
 */
tpsdemo::Spline::Spline(const std::vector<Vec> & control_pts, double regularization)
  : p(control_pts.size()),
    control_points(control_pts),
    mtx_v(p+3, 1),
    mtx_orig_k(p, p)
{
  // You We need at least 3 points to define a plane
  if ( control_points.size() < 3 )
    throw std::runtime_error("need at least 3 points for thin plate spline");

  int id_number = rand() % 1000;

  //unsigned p = control_points.size();

  // Allocate the matrix and vector
  matrix<double> mtx_l(p+3, p+3);
  //matrix<double> mtx_v(p+3, 1);
  //matrix<double> mtx_orig_k(p, p);

  // Fill K (p x p, upper left of L) and calculate
  // mean edge length from control points
  //
  // K is symmetrical so we really have to
  // calculate only about half of the coefficients.
  double a = 0.0;
  for ( unsigned i=0; i<p; ++i )
  {
    for ( unsigned j=i+1; j<p; ++j )
    {
      Vec pt_i = control_points[i];
      Vec pt_j = control_points[j];
      pt_i.y = pt_j.y = 0;
      double elen = (pt_i - pt_j).len();
      mtx_l(i,j) = mtx_l(j,i) =
        mtx_orig_k(i,j) = mtx_orig_k(j,i) =
          tps_base_func(elen);
      a += elen * 2; // same for upper & lower tri
    }
  }
  a /= (double)(p*p);

  // Fill the rest of L
  for ( unsigned i=0; i<p; ++i )
  {
    // diagonal: reqularization parameters (lambda * a^2)
    mtx_l(i,i) = mtx_orig_k(i,i) =
      regularization * (a*a);

    // P (p x 3, upper right)
    mtx_l(i, p+0) = 1.0;
    mtx_l(i, p+1) = control_points[i].x;
    mtx_l(i, p+2) = control_points[i].z;

    // P transposed (3 x p, bottom left)
    mtx_l(p+0, i) = 1.0;
    mtx_l(p+1, i) = control_points[i].x;
    mtx_l(p+2, i) = control_points[i].z;
  }
  // O (3 x 3, lower right)
  for ( unsigned i=p; i<p+3; ++i )
    for ( unsigned j=p; j<p+3; ++j )
      mtx_l(i,j) = 0.0;

  // Fill the right hand vector V
  for ( unsigned i=0; i<p; ++i )
    mtx_v(i,0) = control_points[i].y;
  mtx_v(p+0, 0) = mtx_v(p+1, 0) = mtx_v(p+2, 0) = 0.0;

  // Solve the linear system "inplace"
  if (0 != LU_Solve(mtx_l, mtx_v))
  {
    throw SingularMatrixError();
  }
}

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

double tpsdemo::Spline::interpolate_height(double x, double z) const
{
  double h = mtx_v(p+0, 0) + mtx_v(p+1, 0)*x + mtx_v(p+2, 0)*z;

  Vec pt_i, pt_cur(x,0,z);
  for ( unsigned i=0; i<p; ++i )
  {
    pt_i = control_points[i];
    pt_i.y = 0;
    h += mtx_v(i,0) * tps_base_func( ( pt_i - pt_cur ).len());
  }
  return h;
}

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

double tpsdemo::Spline::compute_bending_energy() const
{
  matrix<double> w( p, 1 );
  for ( unsigned i=0; i<p; ++i )
    w(i,0) = mtx_v(i,0);
  matrix<double> be = prod( prod<matrix<double> >( trans(w), mtx_orig_k ), w );
  return be(0,0);
}


#include "camera.hpp"

#include <Eigen/Geometry>

Matrix4d worldFromCamera(const CameraExtrinsics& coordinates)
{
    auto world_from_camera = Matrix4d{Matrix4d::Identity()};
    world_from_camera.col(3) << coordinates.x, coordinates.y, coordinates.z, 1.0;
    using namespace Eigen;
    auto R = Matrix3d{};
    const auto R_flip  = AngleAxisd(3.14151965, Vector3d::UnitX());
    const auto R_yaw   = AngleAxisd(coordinates.yaw, Vector3d::UnitY());
    const auto R_pitch = AngleAxisd(coordinates.pitch, Vector3d::UnitX());
    R = R_flip * R_yaw * R_pitch;
    world_from_camera.topLeftCorner<3, 3>() = R;
    return world_from_camera;
}

Matrix4d cameraFromWorld(const CameraExtrinsics& coordinates)
{
    return worldFromCamera(coordinates).inverse();
}

Matrix4d imageFromCamera(const CameraIntrinsics& c)
{
    auto image_from_camera = Matrix4d{};
    image_from_camera <<
        c.fx, 0.0, c.cx, 0.0,
        0.0, c.fy, c.cy, 0.0,
        0.0, 0.0, 0.0, 1.0,
        0.0, 0.0, 1.0, 0.0;
    return image_from_camera;
}

CameraIntrinsics makeCameraIntrinsics(size_t width, size_t height)
{
    auto intrinsics = CameraIntrinsics{};
    intrinsics.fx = 0.5 * height;
    intrinsics.fy = 0.5 * height;
    intrinsics.cx = 0.5 * width;
    intrinsics.cy = 0.5 * height;
    intrinsics.width = width;
    intrinsics.height = height;
    return intrinsics;
}


/* -*- c-basic-offset: 4 indent-tabs-mode: nil -*-  vi:set ts=8 sts=4 sw=4: */

#include "cq/CQSpectrogram.h"

#include "dsp/Window.h"

#include <cmath>
#include <vector>
#include <iostream>

using std::vector;
using std::cerr;
using std::endl;

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(TestCQFrequency)

// The principle here is to feed a single windowed sinusoid into a
// small CQ transform and check that the output has its peak bin at
// the correct frequency.

// Set up fs/2 = 50, frequency range 10 -> 40 i.e. 2 octaves, fixed
// duration of 2 seconds
static const cq_float sampleRate = 100;
static const cq_float cqmin = 11.8921;
static const cq_float cqmax = 40;
static const cq_float bpo = 4;
static const int duration = sampleRate * 2;

// Threshold below which to ignore a column completely
static const cq_float threshold = 0.08;

int
binForFrequency(cq_float freq)
{
    int bin = (bpo * 2) - round(bpo * log2(freq / cqmin)) - 1;
    return bin;
}

void
checkCQFreqColumn(int i, vector<cq_float> column,
                  cq_float freq, CQSpectrogram::Interpolation interp)
{
    cq_float maxval = 0.0;
    int maxidx = -1;
    int height = column.size();

    int nonZeroHeight = ((i % 2 == 1) ? height/2 : height);

    for (int j = 0; j < nonZeroHeight; ++j) {
        if (j == 0 || column[j] > maxval) {
            maxval = column[j];
            maxidx = j;
        }
    }

    int expected = binForFrequency(freq);
    if (maxval < threshold) {
        return; // ignore these columns at start and end
    } else if (expected < nonZeroHeight && maxidx != expected) {
        cerr << "ERROR: In column " << i << " with interpolation " << interp
             << ", maximum value for frequency " << freq
             << "\n       found at index " << maxidx
             << " (expected index " << expected << ")" << endl;
        cerr << "column contains: ";
        for (int j = 0; j < height; ++j) {
            cerr << column[j] << " ";
        }
        cerr << endl;
        BOOST_CHECK_EQUAL(maxidx, expected);
    }
}

void
testCQFrequencyWith(CQParameters params,
                    CQSpectrogram::Interpolation interp,
                    cq_float freq)
{
    CQSpectrogram cq(params, interp);

    BOOST_CHECK_EQUAL(cq.getBinsPerOctave(), bpo);
    BOOST_CHECK_EQUAL(cq.getOctaves(), 2);
    BOOST_CHECK_CLOSE(cq.getBinFrequency(0), 40, 1e-10);
    BOOST_CHECK_CLOSE(cq.getBinFrequency(4), 20, 1e-10);
    BOOST_CHECK_CLOSE(cq.getBinFrequency(7), cqmin, 1e-3);

    vector<cq_float> input;
    for (int i = 0; i < duration; ++i) {
        input.push_back(sin((i * 2 * M_PI * freq) / sampleRate));
    }
    Window<cq_float>(HanningWindow, duration).cut(input.data());

    CQSpectrogram::RealBlock output = cq.process(input);
    CQSpectrogram::RealBlock rest = cq.getRemainingOutput();
    output.insert(output.end(), rest.begin(), rest.end());

    BOOST_CHECK_EQUAL(output[0].size(),
                      cq.getBinsPerOctave() * cq.getOctaves());

    for (int i = 0; i < int(output.size()); ++i) {
        checkCQFreqColumn(i, output[i], freq, interp);
    }
}

void
testCQFrequency(cq_float freq)
{
    vector<CQSpectrogram::Interpolation> interpolationTypes;
    interpolationTypes.push_back(CQSpectrogram::InterpolateZeros);
    interpolationTypes.push_back(CQSpectrogram::InterpolateHold);
    interpolationTypes.push_back(CQSpectrogram::InterpolateLinear);

    for (int k = 0; k < int(interpolationTypes.size()); ++k) {
        CQSpectrogram::Interpolation interp = interpolationTypes[k];
        CQParameters params(sampleRate, cqmin, cqmax, bpo);
        testCQFrequencyWith(params, interp, freq);
    }
}

BOOST_AUTO_TEST_CASE(freq_11) { testCQFrequency(11); }
BOOST_AUTO_TEST_CASE(freq_17) { testCQFrequency(17); }
BOOST_AUTO_TEST_CASE(freq_24) { testCQFrequency(24); }
BOOST_AUTO_TEST_CASE(freq_27) { testCQFrequency(27); }
BOOST_AUTO_TEST_CASE(freq_33) { testCQFrequency(33); }
BOOST_AUTO_TEST_CASE(freq_40) { testCQFrequency(40); }

BOOST_AUTO_TEST_SUITE_END()


// Copyright (C) 2012  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include <dlib/filtering.h>
#include <sstream>
#include <string>
#include <cstdlib>
#include <ctime>
#include <dlib/matrix.h>
#include <dlib/rand.h>

#include "tester.h"

namespace
{

    using namespace test;
    using namespace dlib;
    using namespace std;

    logger dlog("test.filtering");

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

    template <typename filter_type>
    double test_filter (
        filter_type kf,
        int size
    )
    {
        // This test has a point moving in a circle around the origin.  The point
        // also gets a random bump in a random direction at each time step.

        running_stats<double> rs;

        dlib::rand rnd;
        int count = 0;
        const dlib::vector<double,3> z(0,0,1);
        dlib::vector<double,2> p(10,10), temp;
        for (int i = 0; i < size; ++i)
        {
            // move the point around in a circle
            p += z.cross(p).normalize()/0.5;
            // randomly drop measurements
            if (rnd.get_random_double() < 0.7 || count < 4)
            {
                // make a random bump
                dlib::vector<double,2> pp;
                pp.x() = rnd.get_random_gaussian()/3;
                pp.y() = rnd.get_random_gaussian()/3;

                ++count;
                kf.update(p+pp);
            }
            else
            {
                kf.update();
                dlog << LTRACE << "MISSED MEASUREMENT";
            }
            // figure out the next position
            temp = (p+z.cross(p).normalize()/0.5);
            const double error = length(temp - rowm(kf.get_predicted_next_state(),range(0,1)));
            rs.add(error);

            dlog << LTRACE << temp << "("<< error << "): " << trans(kf.get_predicted_next_state());

            // test the serialization a few times.
            if (count < 10)
            {
                ostringstream sout;
                serialize(kf, sout);
                istringstream sin(sout.str());
                filter_type temp;
                deserialize(temp, sin);
                kf = temp;
            }
        }

        return rs.mean();

    }

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

    void test_kalman_filter()
    {
        matrix<double,2,2> R;
        R = 0.3, 0,
        0,  0.3;

        // the variables in the state are
        // x,y, x velocity, y velocity, x acceleration, and y acceleration
        matrix<double,6,6> A;
        A = 1, 0, 1, 0, 0, 0,
        0, 1, 0, 1, 0, 0,
        0, 0, 1, 0, 1, 0,
        0, 0, 0, 1, 0, 1,
        0, 0, 0, 0, 1, 0,
        0, 0, 0, 0, 0, 1;

        // the measurements only tell us the positions
        matrix<double,2,6> H;
        H = 1, 0, 0, 0, 0, 0,
        0, 1, 0, 0, 0, 0;

        kalman_filter<6,2> kf;
        kf.set_measurement_noise(R);
        matrix<double> pn = 0.01*identity_matrix<double,6>();
        kf.set_process_noise(pn);
        kf.set_observation_model(H);
        kf.set_transition_model(A);

        DLIB_TEST(equal(kf.get_observation_model() , H));
        DLIB_TEST(equal(kf.get_transition_model() , A));
        DLIB_TEST(equal(kf.get_measurement_noise() , R));
        DLIB_TEST(equal(kf.get_process_noise() , pn));
        DLIB_TEST(equal(kf.get_current_estimation_error_covariance() , identity_matrix(pn)));

        double kf_error = test_filter(kf, 300);

        dlog << LINFO << "kf error: "<< kf_error;
        DLIB_TEST_MSG(kf_error < 0.75, kf_error);
    }

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

    void test_rls_filter()
    {

        rls_filter rls(10, 0.99, 0.1);

        DLIB_TEST(rls.get_window_size() == 10);
        DLIB_TEST(rls.get_forget_factor() == 0.99);
        DLIB_TEST(rls.get_c() == 0.1);

        double rls_error = test_filter(rls, 1000);

        dlog << LINFO << "rls error: "<< rls_error;
        DLIB_TEST_MSG(rls_error < 0.75, rls_error);
    }

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

    class filtering_tester : public tester
    {
    public:
        filtering_tester (
        ) :
            tester ("test_filtering",
                    "Runs tests on the filtering stuff (rls and kalman filters).")
        {}

        void perform_test (
        )
        {
            test_rls_filter();
            test_kalman_filter();
        }
    } a;

}


#include "multiprecision/Multiprecision.h"
#include "multiprecision/MPRotate.h"
#include "multiprecision/MPReplicate.h"
#include "algebra/Vector.hpp"
#include "protocol/LR.hpp"
#include "utils/timer.hpp"
#include <NTL/ZZ.h>
#include <iostream>
#include <map>

int main() {
    long m, p, r, P;
    m = 5227;
    p = 67499;
    r = 1;
    P = 1;
    MPContext context(m, p, r, P);
    context.buildModChain(8);
    MPSecKey sk(context);
    MPPubKey pk(sk);
    MPEncArray ea(context);
    printf("going to pack with %ld slots\n", ea.slots());
    MDL::Vector<long> vec(13);
    MDL::Matrix<long> mat(3, 3);
    for (int i = 0; i < vec.dimension(); i++)
        vec[i] = i;
    mat[0][0] = 1; mat[0][1] = 2; mat[0][2] = 3;
    mat[1][0] = 2; mat[1][1] = 3; mat[1][2] = 4;
    mat[2][0] = 3; mat[2][1] = 4; mat[2][2] = 5;

	MPEncVector encVec(pk);
    MPEncMatrix encMat, encMat2;
    encVec.pack(vec, ea);
    encMat.pack(mat, pk, ea);
    encMat2.pack(mat, pk, ea);

    // auto rep = repeat(encVec, ea, pk, vec.size(), vec.size());
    // totalSums(rep, ea, vec.size());
    // {
    //     MDL::Vector<NTL::ZZ> res;
    //     rep.unpack(res, sk, ea);
    //     std::cout << res << "\n";
    // }
    // MDL::Timer timer;
    // timer.start();
    // encMat = encMat.dot(encMat2, ea, pk, 3);
    // timer.end();
    // MDL::Matrix<NTL::ZZ> zzMat(3, 3);
    // encMat.unpack(zzMat, sk, ea, true);
    // std::cout << zzMat << "\n";
    // printf("mult %f\n", timer.second());
    return 0;
}


/**
 * @file benchmark_eigen_solver.cpp
 * @author remzerrr (remi.helleboid@gmail.com)
 * @brief
 * @version 0.1
 * @date 2021-09-25
 *
 * @copyright Copyright (c) 2021
 *
 */

#include <benchmark/benchmark.h>

#include <iostream>
#include <random>

#include <Eigen/Core>
#include <Eigen/Sparse>

//  SIZE_SYSTEM is basically the number of nodes in the mesh
static constexpr std::size_t SIZE_SYSTEM = 10;
//  NUMBER_ITERATION is used to simulate the great number of time poisson equation will be solved in MonteCarlo process.
static constexpr std::size_t NUMBER_ITERATION = 10'000;

    Eigen::SparseMatrix<double>
    create_laplacian_matrix(const std::size_t size) {
    typedef Eigen::Triplet<double> T;
    std::vector<T>                 tripletList;
    const std::size_t              non_zero_estimation = 3 * size;
    tripletList.reserve(non_zero_estimation);
    //  Filling respectively the upper diagonal, the diagonal and the lower diagonal.
    for (std::size_t index_row = 1; index_row < size - 1; ++index_row) {
        tripletList.push_back(T(index_row, index_row + 1, -1.0));
        tripletList.push_back(T(index_row, index_row, 2.0));
        tripletList.push_back(T(index_row - 1, index_row, -1.0));
    }
    //  Filling the first and last line of the matrix
    tripletList.push_back(T(0, 0, 2.0));
    tripletList.push_back(T(size - 1, size - 1, 2.0));

    Eigen::SparseMatrix<double> MatrixPoisson(size, size);
    MatrixPoisson.setFromTriplets(tripletList.begin(), tripletList.end());

    std::cout << MatrixPoisson << std::endl;
    return MatrixPoisson;
}

Eigen::VectorXd create_random_vector(const std::size_t size) {
    Eigen::VectorXd RandomVector = Eigen::VectorXd::Random(size);
    return RandomVector;
}

static void EIGEN_LU_BENCH(benchmark::State &state) {
    Eigen::SparseMatrix<double> PoissonMatrix = create_laplacian_matrix(SIZE_SYSTEM);
    Eigen::VectorXd RandomVector = create_random_vector(SIZE_SYSTEM);

    Eigen::SparseLU<Eigen::SparseMatrix<double>, Eigen::COLAMDOrdering<int> > LU_Solver;
    LU_Solver.analyzePattern(PoissonMatrix);
    LU_Solver.factorize(PoissonMatrix);
    for (auto _ : state) {
        for (std::size_t iteration = 0; iteration < NUMBER_ITERATION; ++ iteration) {
            LU_Solver.solve(RandomVector);
        }
    }
}

BENCHMARK(EIGEN_LU_BENCH);
// Run the benchmark
BENCHMARK_MAIN();


/********************************************************************
	created:	2012/08/27
	created:	27:8:2012   8:40
	filename: 	E:\Sync\Dropbox\Codes\glycan_pipeline\GAG\src\GAGPL\SPECTRUM\IsotopicDistribution.cpp
	file path:	E:\Sync\Dropbox\Codes\glycan_pipeline\GAG\src\GAGPL\SPECTRUM
	file base:	IsotopicDistribution
	file ext:	cpp
	author:		Han Hu

	purpose:
*********************************************************************/
#include "GAGPL/SPECTRUM/IsotopicDistribution.h"
#include "GAGPL/MISC/Param.h"

#include <time.h>
#include <algorithm>
#include <sstream>
#include <fstream>

#include <boost/make_shared.hpp>
#include <boost/xpressive/xpressive.hpp>
#include <boost/lexical_cast.hpp>

namespace gag
{
	void IsotopicDistribution::createMonoPeak()
	{
		// Get the monoisotopic isotope for each element of the composition.
		monopeak.mz = _compo.getMass();

		const std::map<std::string, int> ele_count = _compo.get();

		std::map<std::string, int>::const_iterator it = ele_count.begin();

		for(; it != ele_count.end(); it++)
		{
			// For each element, get the value of prob^coef.
			PeriodicTable& ptable = PeriodicTable::Instance();
			Isotope iso = ptable.getIsotopeByRelativeShift(it->first);
			monopeak.intensity += it->second * log(iso.abundance);
		}
		monopeak.intensity = exp(monopeak.intensity);

	}

	double IsotopicDistribution::calculatePhiValue(size_t order)
	{
		double phi_value = 0.0;

		// For each element, get the phi factor.
		const std::map<std::string, int> ele_count = _compo.get();

		std::map<std::string, int>::const_iterator it = ele_count.begin();

		// Iterate over all elements.
		for(; it != ele_count.end(); it++)
		{
			phi_value += _iso_const.getNthElementPowerSum(it->first, order) * it->second;
		}

		return phi_value;
	}

	//PowerSumVec IsotopicDistribution::calculatePhiValueVec()
	//{
	//	PowerSumVec phi_value_vec;
	//	// phi(0) = 0.
	//	phi_value_vec.push_back(0);

	//	for(size_t i=1; i<=_var_num; i++)
	//	{
	//		phi_value_vec.push_back(this->calculatePhiValue(i));
	//	}
	//
	//	return phi_value_vec;
	//}

	EleSymPolyVec IsotopicDistribution::calculateProbabilityVec()
	{
		//clock_t t = clock(); clock_t last_t = t;
		PowerSumVec phi_vec = this->calculatePhiValueVec();
		//t = clock()-last_t; last_t = clock();
		//std::cout << "Step 1: calculating phi value vec: " << (double)t/CLOCKS_PER_SEC << "s\n";

		NewtonGirardFormulae newton(_compo.getMaxNumVariants());
		EleSymPolyVec prob_vec;
		newton.updateParameters(phi_vec, prob_vec);
		//t = clock()-last_t; last_t = clock();
		//std::cout << "Step 2: newton: " << (double)t/CLOCKS_PER_SEC << "s\n";

		for(size_t i=0; i<prob_vec.size(); i++)
		{
			int sign = (i%2 == 0 ? 1 : -1);
			// q(j) = q(0) * e(j) * (-1)^j.
			prob_vec[i] = prob_vec[i] * monopeak.intensity * sign;
		}

		return prob_vec;
	}

	double IsotopicDistribution::calculateModifiedPhiValue(const std::string& symbol, size_t order)
	{

		double phi_value = phi_value_vec.at(order) - _iso_const.getNthElementPowerSum(symbol, order) + _iso_const.getNthModifiedElementPowerSum(symbol, order);

		return phi_value;
	}

	PowerSumVec IsotopicDistribution::calculateModifiedPhiValueVec(const std::string& symbol)
	{
		PowerSumVec modified_phi_value_vec;
		// phi(0) = 1.
		modified_phi_value_vec.push_back(0);

		for(size_t i=1; i<=_var_num; i++)
		{
			modified_phi_value_vec.push_back(this->calculateModifiedPhiValue(symbol, i));
		}

		return modified_phi_value_vec;
	}

	std::vector<double> IsotopicDistribution::calculateCenterMassVec(const EleSymPolyVec& prob_vec)
	{
		std::vector<double> mass_vec;

		std::map<std::string, int> ele_count = _compo.get();

		PeriodicTable& ptable = PeriodicTable::Instance();

		NewtonGirardFormulae newton(_compo.getMaxNumVariants());

		std::map<std::string, EleSymPolyVec> esp_map;
		//clock_t t = clock();
		for(std::map<std::string, int>::const_iterator it = ele_count.begin();
			it != ele_count.end(); it++)
		{
			const Isotope& mono = ptable.getIsotopeByRelativeShift(it->first);

			PowerSumVec& ps_vec = this->calculateModifiedPhiValueVec(it->first);
			EleSymPolyVec esp_vec;
			newton.updateParameters(ps_vec, esp_vec);

			esp_map.insert(std::make_pair(it->first, esp_vec));
		}
		//t = clock()-t;
		//std::cout << "Modified Phi Vec: " << (double)t/CLOCKS_PER_SEC << "s\n";

		for(size_t i = 0; i <= _var_num; i++)
		{
			std::map<std::string, EleSymPolyVec>::iterator it = esp_map.begin();
			int sign = (i%2 == 0 ? 1 : -1);
			double center_mass = 0.0;
			for(; it != esp_map.end(); it++)
			{
				const Isotope& mono = ptable.getIsotopeByRelativeShift(it->first);

				center_mass += ele_count[it->first] * sign * it->second.at(i) * monopeak.intensity * mono.mass;
			}
			// m(j) = sum(m(jk) * p(jk))/sum(p(jk))
			mass_vec.push_back(center_mass/prob_vec.at(i));
		}

		return mass_vec;
	}

	void IsotopicDistribution::setOrder(int order)
	{
		// Controlling the number of variants, and prevent unnecessary calculation.
		size_t max_num = _compo.getMaxNumVariants();
		if(order == 0) {
			// Default value calculated by formula 2.
			int heu = std::max((int)ceil(abs(2 * (_compo.getMass() - _compo.getAverageMass()))), 50);
			_var_num = (size_t)heu > max_num ? max_num : (size_t)heu;
		} else if(order > 0)
			_var_num = (size_t)order > max_num ? max_num : (size_t)order;
		else
			throw std::runtime_error("Error: invalid order number.");

		_var_num--;

		// Adding elements from the composition into _iso_const.
		updateIsotopicConstants();
	}

	AggregatedIsotopicVariants IsotopicDistribution::getAggregatedIsotopicVariants(int charge /* = 0 */)
	{
		//clock_t t = clock(); clock_t last_t = t;
		EleSymPolyVec prob_vec = this->calculateProbabilityVec();

		//t = clock()-last_t; last_t = clock();
		//std::cout << "Prob Vec: " << (double)t/CLOCKS_PER_SEC << "s\n";

		std::vector<double> center_mass_vec = this->calculateCenterMassVec(prob_vec);
		//t = clock()-last_t; last_t = clock();
		//std::cout << "Mass Vec: " << (double)t/CLOCKS_PER_SEC << "s\n";

		AggregatedIsotopicVariants peakset;

		avg_mass = 0.0;
		double sum(0.0);

		for(size_t i=0; i<= _var_num; i++)
		{
			double adjusted_mz = (charge == 0 ? center_mass_vec.at(i): calculateMZ(center_mass_vec.at(i), charge));

			PeakPtr peak = boost::make_shared<Peak>(adjusted_mz, prob_vec.at(i));
			peakset.addPeak(peak);

			// Code for calculating avg_mass
			avg_mass += adjusted_mz * prob_vec.at(i);
			sum += prob_vec.at(i);
		}

		avg_mass /= sum;

		return peakset;

	}

	void IsotopicDistribution::updateIsotopicConstants()
	{
		const std::map<std::string, int> ele_count = _compo.get();

		std::map<std::string, int>::const_iterator it = ele_count.begin();

		// Build the constants for all elements.
		for(; it != ele_count.end(); it++)
			_iso_const.addElement(it->first);

		_iso_const.updateOrder(_var_num);
	}

	void IsotopicDistribution::updatePhiValueVec()
	{
		phi_value_vec.clear();
		phi_value_vec.push_back(0);
		for(size_t i=1; i<=_var_num; i++)
		{
			phi_value_vec.push_back(this->calculatePhiValue(i));
		}
	}

	double calculateMass( double mz, int charge)
	{
		// Be careful of the ion mode.
		param::Param& param = param::Param::Instance();
		double electron_mass = param.getParameter<double>("electron_mass").first;

		return abs(charge) * mz - charge * (Composition("H").getMass() -electron_mass);
	}

	double calculateMZ(double mass, int charge, int pre_charge /* = 0 */)
	{
		param::Param& param = param::Param::Instance();
		double electron_mass = param.getParameter<double>("electron_mass").first;

		int coef_h = (pre_charge == 0 ? charge : pre_charge);

		// TBD: this should be controlled by parameter.
		// int coef_e = (pre_charge == 0 ? 1 : 0);
		return (mass + coef_h * (Composition("H").getMass() - electron_mass))/abs(charge);
	}
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2010 Klaus Spanderen

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

/*! \file extendedornsteinuhlenbeckprocess.hpp
    \brief extended Ornstein-Uhlenbeck process
*/

#ifndef quantlib_extended_ornstein_uhlenbeck_process_hpp
#define quantlib_extended_ornstein_uhlenbeck_process_hpp

#include <ql/stochasticprocess.hpp>

#include <boost/function.hpp>

namespace QuantLib {

    class OrnsteinUhlenbeckProcess;

    //! Extended Ornstein-Uhlenbeck process class
    /*! This class describes the Ornstein-Uhlenbeck process governed by
        \f[
            dx = a (b(t) - x_t) dt + \sigma dW_t.
        \f]

        \ingroup processes
    */
    class ExtendedOrnsteinUhlenbeckProcess : public StochasticProcess1D {
      public:
        enum Discretization { MidPoint, Trapezodial, GaussLobatto };

        ExtendedOrnsteinUhlenbeckProcess(
                                Real speed, Volatility sigma, Real x0,
                                const boost::function<Real (Real)>& b,
                                Discretization discretization = MidPoint,
                                Real intEps = 1e-4);

        //! \name StochasticProcess interface
        //@{
        Real x0() const;
        Real speed() const;
        Real volatility() const;
        Real drift(Time t, Real x) const;
        Real diffusion(Time t, Real x) const;
        Real expectation(Time t0, Real x0, Time dt) const;
        Real stdDeviation(Time t0, Real x0, Time dt) const;
        Real variance(Time t0, Real x0, Time dt) const;
        //@}
      private:
        const Real speed_;
        const Volatility vol_;
        const boost::function<Real (Real)> b_;
        const Real intEps_;
        const boost::shared_ptr<OrnsteinUhlenbeckProcess> ouProcess_;
        const Discretization discretization_;
    };
}

#endif


#include <armadillo>
#include <chrono>
#include <random>
#include <stdlib.h>

using namespace std;
using namespace arma;

//Test cases
#define TEST_CREATE         1
#define TEST_SCALE          2
#define TEST_TRANSPOSE      3
#define TEST_ADD            4
#define TEST_SUB            5
#define TEST_DOT            6
#define TEST_DET            7
#define TEST_EIGEN          8
#define TEST_SVD            9
#define TEST_CHOL           10


#pragma once

#include <Eigen/Dense>
#include <Eigen/Core>
#include <autodiff/forward.hpp>
#include <autodiff/forward/eigen.hpp>
#include <cmath>

namespace ast
{
typedef autodiff::dual Real;
typedef autodiff::VectorXdual Vec;

typedef double RealV;
typedef Eigen::MatrixXd Mat;
typedef Eigen::VectorXd VecV;

constexpr RealV epsilon = 0.001;

using namespace Eigen;
using namespace autodiff;

//struct QBody : public Vec3
//{
//  QBody() : Vec3()
//  {}

//  QBody(Real th, Real x, Real y) : Vec3(th, x, y)
//  {}

//  template<typename OtherDerived>
//  QBody(const Eigen::MatrixBase<OtherDerived>& other) : Vec3(other)
//  { }

//  template<typename OtherDerived>
//  QBody& operator=(const Eigen::MatrixBase<OtherDerived>& other)
//  {
//     this->Vec3::operator=(other);
//     return *this;
//  }

//  Real& th() { return this->data()[0]; }

//  const Real& th() const { return this->data()[0]; }

//  Real& theta() { return this->data()[0]; }

//  const Real& theta() const { return this->data()[0]; }

//  Real& x() { return this->data()[1]; }

//  const Real& x() const { return this->data()[1]; }

//  Real& y() { return this->data()[2]; }

//  const Real& y() const { return this->data()[2]; }
//};

inline Real sat(Real x, Real lim)
{
  if(x > lim)
    return lim;
  else if(x < -lim)
    return - lim;
  else
    return x;
}

inline Real clipToRange(Real x, Real rMin, Real rMax)
{
  if(x < rMin)
    return  rMin;
  else if (x > rMax)
    return rMax;
  else return x;
}

inline Real sq(Real x)
{
  return x*x;
}

inline Real cube(Real x)
{
  return x*x*x;
}

inline int sign(Real val)
{
  return (0 < val) - (val < 0);
}

template <typename T>
inline Real distance(const T& a , const T& b)
{
  return (b-a).norm();
}

inline RealV continuousAngle(RealV angle, RealV lastAngle)
{
    auto dAngle = fmod(angle, 2*M_PI) - fmod(lastAngle, 2*M_PI);

    if (dAngle > M_PI)
        return lastAngle + dAngle - 2.0 * M_PI;
    else if (dAngle < -M_PI)
        return lastAngle + dAngle + 2.0 * M_PI;
    else
        return lastAngle + dAngle;
}

inline Real discontinuousAngle(Real angle)
{
    return atan2(sin(angle), cos(angle));
}

}


#include <armadillo>
#include <iostream>

using namespace arma;

int main() {
    mat A(3, 2, fill::randn);
    A.print("A:");
    mat B(2, 4, fill::randn);
    B.print("B:");
    mat C = A*B;
    C.print("A*B:");
    return 0;
}


/*
   Program to solve the two-dimensional Ising model
   with zero external field and no parallelization
   Parallel version using MPI
   The coupling constant J is set to J = 1
   Boltzmann's constant = 1, temperature has thus dimension energy
   Metropolis aolgorithm  is used as well as periodic boundary conditions.
   The code needs an output file on the command line and the variables mcs, nspins,
   initial temp, final temp and temp step.
   Run as
   ./executable Outputfile numberof spins number of MC cycles initial temp final temp tempstep
   ./test.x Lattice 100 10000000 2.1 2.4 0.01
   Compile and link as
   c++ -O3 -std=c++11 -Rpass=loop-vectorize -o Ising.x IsingModel.cpp -larmadillo -lomp
*/

#include <cmath>
#include <iostream>
#include <fstream>
#include <iomanip>
#include <cstdlib>
#include <random>
#include <armadillo>
#include <string>
#include "omp.h"
#define NUM_THREADS 4
using namespace  std;
using namespace arma;
// output file
ofstream ofile;

// inline function for PeriodicBoundary boundary conditions
inline int PeriodicBoundary(int i, int limit, int add) {
  return (i+limit+add) % (limit);
}
// Function to initialise energy and magnetization
void InitializeLattice(int, mat &, double&, double&);
// The metropolis algorithm including the loop over Monte Carlo cycles
void MetropolisSampling(int, int, double, vec &);
// prints to file the results of the calculations
void WriteResultstoFile(int, int, double, vec);

// Main program begins here

int main(int argc, char* argv[])
{
  string filename;
  int NSpins, MonteCarloCycles;
  double InitialTemp, FinalTemp, TempStep;
  if (argc <= 5) {
    cout << "Bad Usage: " << argv[0] <<
      " read output file, Number of spins, MC cycles, initial and final temperature and tempurate step" << endl;
    exit(1);
  }
  filename=argv[1];
  // Initializations
  NSpins = atoi(argv[2]); MonteCarloCycles = atoi(argv[3]); InitialTemp = atof(argv[4]);  FinalTemp = atof(argv[5]);  TempStep = atof(argv[6]);
  string fileout = filename;
  string argument = to_string(NSpins);
  fileout.append(argument);
  ofile.open(fileout);
  cout << "  C++/OpenMP version" << endl;
  cout << "  Ising model with OpenMP" << endl;
  omp_set_num_threads(NUM_THREADS);
    //  int thread_num = omp_get_max_threads ( );
  cout << "  The number of processors available = " << omp_get_num_procs ( ) << endl;
  // defining time and various variables needed for the integration
  double wtime = omp_get_wtime ( );
  // Monte Carlo cycles
  for (double Temperature = InitialTemp; Temperature <= FinalTemp; Temperature+=TempStep){
    vec ExpectationValues = zeros<mat>(5);
    // Start Monte Carlo computation and get local expectation values
    // This may need a fix!!
# pragma omp parallel for default(shared) reduction(+:ExpectationValues)
    MetropolisSampling(NSpins, MonteCarloCycles, Temperature, ExpectationValues);
    WriteResultstoFile(NSpins, MonteCarloCycles, Temperature, ExpectationValues);
  }
  wtime = omp_get_wtime ( ) - wtime;
  cout << "  Elapsed time in seconds = " << wtime << endl;
  ofile.close();  // close output file
  return 0;
}

// The Monte Carlo part with the Metropolis algo with sweeps over the lattice
void MetropolisSampling(int NSpins, int MonteCarloCycles, double Temperature, vec &ExpectationValues)
{
  // Initialize the seed and call the Mersienne algo
  std::random_device rd;
  std::mt19937_64 gen(rd());
  // Set up the uniform distribution for x \in [[0, 1]
  std::uniform_real_distribution<double> RandomNumberGenerator(0.0,1.0);
  // Initialize the lattice spin values
  mat SpinMatrix = zeros<mat>(NSpins,NSpins);
  //    initialize energy and magnetization
  double Energy = 0.;     double MagneticMoment = 0.;
  // initialize array for expectation values
  InitializeLattice(NSpins, SpinMatrix, Energy, MagneticMoment);
  // setup array for possible energy changes
  vec EnergyDifference = zeros<mat>(17);
  for( int de =-8; de <= 8; de+=4) EnergyDifference(de+8) = exp(-de/Temperature);
  // Start Monte Carlo experiments
  int AllSpins = NSpins*NSpins;
  for (int cycles = 1; cycles <= MonteCarloCycles; cycles++){
    // The sweep over the lattice, looping over all spin sites
    for(int Spins =0; Spins < AllSpins; Spins++) {
      int ix = (int) (RandomNumberGenerator(gen)*NSpins);
      int iy = (int) (RandomNumberGenerator(gen)*NSpins);
      int deltaE =  2*SpinMatrix(ix,iy)*
	(SpinMatrix(ix,PeriodicBoundary(iy,NSpins,-1))+
	 SpinMatrix(PeriodicBoundary(ix,NSpins,-1),iy) +
	 SpinMatrix(ix,PeriodicBoundary(iy,NSpins,1)) +
	 SpinMatrix(PeriodicBoundary(ix,NSpins,1),iy));
      if ( RandomNumberGenerator(gen) <= EnergyDifference(deltaE+8) ) {
	SpinMatrix(ix,iy) *= -1.0;  // flip one spin and accept new spin config
	MagneticMoment += 2.0*SpinMatrix(ix,iy);
	Energy += (double) deltaE;
      }
    }
    // update expectation values  for local node after a sweep through the lattice
    ExpectationValues(0) += Energy;    ExpectationValues(1) += Energy*Energy;
    ExpectationValues(2) += MagneticMoment;
    ExpectationValues(3) += MagneticMoment*MagneticMoment;
    ExpectationValues(4) += fabs(MagneticMoment);
  }
} // end of Metropolis sampling over spins

// function to initialise energy, spin matrix and magnetization
void InitializeLattice(int NSpins, mat &SpinMatrix,  double& Energy, double& MagneticMoment)
{
  // setup spin matrix and initial magnetization using cold start, all spins pointing up or down
  for(int x =0; x < NSpins; x++) {
    for (int y= 0; y < NSpins; y++){
      SpinMatrix(x,y) = 1.0; // spin orientation for the ground state
      MagneticMoment +=  (double) SpinMatrix(x,y);
    }
  }
  // setup initial energy
  for(int x =0; x < NSpins; x++) {
    for (int y= 0; y < NSpins; y++){
      Energy -=  (double) SpinMatrix(x,y)*
	(SpinMatrix(PeriodicBoundary(x,NSpins,-1),y) +
	 SpinMatrix(x,PeriodicBoundary(y,NSpins,-1)));
    }
  }
}// end function initialize

void WriteResultstoFile(int NSpins, int MonteCarloCycles, double temperature, vec ExpectationValues)
{
  double norm = 1.0/((double) (MonteCarloCycles));  // divided by  number of cycles
  double E_ExpectationValues = ExpectationValues(0)*norm;
  double E2_ExpectationValues = ExpectationValues(1)*norm;
  double M_ExpectationValues = ExpectationValues(2)*norm;
  double M2_ExpectationValues = ExpectationValues(3)*norm;
  double Mabs_ExpectationValues = ExpectationValues(4)*norm;
  // all expectation values are per spin, divide by 1/NSpins/NSpins
  double AllSpins = 1.0/((double) NSpins*NSpins);
  double HeatCapacity = (E2_ExpectationValues- E_ExpectationValues*E_ExpectationValues)*AllSpins/temperature/temperature;
  double MagneticSusceptibility = (M2_ExpectationValues - M_ExpectationValues*M_ExpectationValues)*AllSpins/temperature;
  ofile << setiosflags(ios::showpoint | ios::uppercase);
  ofile << setw(15) << setprecision(8) << temperature;
  ofile << setw(15) << setprecision(8) << E_ExpectationValues*AllSpins;
  ofile << setw(15) << setprecision(8) << HeatCapacity;
  ofile << setw(15) << setprecision(8) << M_ExpectationValues*AllSpins;
  ofile << setw(15) << setprecision(8) << MagneticSusceptibility;
  ofile << setw(15) << setprecision(8) << Mabs_ExpectationValues*AllSpins << endl;
} // end output function


#include <iostream>
#include <armadillo>
#include <sstream>
#include <vector>
#include <assert.h>
#include <cmath>
#include "multibody.h"

#define _USE_MATH_DEFINES

arma::mat bond(const int i, const int j, const int size){
	arma::mat adj = arma::zeros(size,size);
	adj(i,j) = 1.0;  adj(j,i) = 1.0;
	return calcLaplacian(adj);
}

//Calculates the Laplacian from the Adjacency matrix
arma::mat calcLaplacian(const arma::mat &adj) {
    return arma::diagmat(arma::sum(adj)) - adj;
}

//Calculates the radius of gyration matrix operator
arma::mat rg2mat(int N) {
    arma::mat adj = arma::ones(N, N) - arma::eye(N, N);
    return 1.0 / ((double)N*N)*calcLaplacian(adj);
}

//finds c^T lti c in 1D quickly
double GaussSystem::omatfast(int i, int j, arma::mat &m) {
    return m.at(i, i) + m.at(j, j) - 2.0*m.at(i, j);
}

arma::mat readMatrix(std::string line, double *dim, double *eps, double *a) {
        std::stringstream ss;
        ss.str(line);
        int width;
        ss >> width; ss >> *dim; ss >> *eps >> *a;
        arma::mat adjacency(width, width);
        int i = 0;
        while(1) {  //read in numbers
            double elem;
            ss >> elem;
            if (ss) {
                adjacency[i] = elem;
                i++;
            }
            else {
                break;
            }
        }
        assert(i == width*width);
        return adjacency;
}

/*
	Create the adjacency matrix for a linear polymer
*/
arma::mat linearAdjacencyMatrix(const int N) {
	arma::vec prototype = arma::zeros(N);
	prototype(1) = 1;
	return arma::toeplitz(prototype);
}

double GaussSystem::triomatfast(const int i, const int j, const int u, const int v, arma::mat &m) {
    //matrix assosciated with \delta(r_i - r_j) \delta(r_u - r_v)
    //matrix is a reference to a pre-allocated matrix.  This function will be called a butt-ton of times
    //so it should be fast if possible
    mat2d(0, 0) = m(i, i) + m(j, j) - m(i, j) - m(j, i); mat2d(0, 1) = m(i, u) + m(j, v) - m(i, v) - m(j, u);
    mat2d(1, 0) = m(u, i) + m(v, j) - m(v, i) - m(u, j); mat2d(1, 1) = m(u, u) + m(v, v) - m(u, v) - m(v, u);

    /*
    manual verification
    arma::mat cm = arma::zeros(10, 2);
    cm(i, 0) = 1; cm(j, 0) = -1;
    cm(u, 1) = 1; cm(v, 1) = -1;
    std::cout << cm.t()*m*cm;
    std::cout << mat2d << std::endl; */
    return arma::det(mat2d);
}

arma::mat GaussSystem::triomat(const int i, const int j, const int u, const int v, arma::mat &m) {
    //matrix assosciated with \delta(r_i - r_j) \delta(r_u - r_v)
    //matrix is a reference to a pre-allocated matrix.  This function will be called a butt-ton of times
    //so it should be fast if possible
    arma::mat rv(2,2);
    rv(0, 0) = m(i, i) + m(j, j) - m(i, j) - m(j, i); rv(0, 1) = m(i, u) + m(j, v) - m(i, v) - m(j, u);
    rv(1, 0) = m(u, i) + m(v, j) - m(v, i) - m(u, j); rv(1, 1) = m(u, u) + m(v, v) - m(u, v) - m(v, u);
    return rv;
}

double GaussSystem::threeBondIrreducible(arma::mat &m) {
	arma::mat rv = arma::zeros(3,3);
	int size = m.n_cols;	//size of matrix, it should be square
	arma::mat c = arma::zeros(size, 3); //c matrix for 3 delta functions
	arma::mat ct = arma::zeros(3, size); //c transpose
	double acc = 0; int dumb;
	for(int i = 0; i < N-1; i++){
		for(int j = i+1; j < N; j++) {
			for(int k = 0; k < N; k++){ if(k != i && k != j) {
				// Stuff
				acc += std::pow(triomatfast(i,j,j,k, m), -D/2.0);
			}}
		}
	}
	return acc;
}

GaussSystem::GaussSystem(arma::mat laplacian, double dimension, double seglen) {
    N = laplacian.n_cols;   //Matrix size.  Assume it is square
    D = dimension;
    a = seglen;
    lap = laplacian;
    lti = arma::inv(lap);
    rg2 = rg2mat(N);
    opm = lti*rg2*lti;
    rg20 = arma::trace(lti*rg2);
    mat2d = arma::zeros(2, 2);  //2x2 matrix for use in triomatfast
}

double GaussSystem::order1fractionalRg2O(const int i, const int j) {
	double o = omatfast(i, j, lti);
	double on = omatfast(i, j, opm);
	return 1 - on/o/rg20;
}

double GaussSystem::order2fractionalRg2O(const int i, const int j, const int p, const int q) {
	arma::mat o = triomat(i, j, p, q, lti);
	arma::mat on = triomat(i, j, p, q, opm);
	return 1 - arma::trace(arma::inv(o)*on)/rg20;
}

double GaussSystem::secondcorrection() {
	double acc = 0;
    for (int i = 0; i < N; i++) {
        for (int j = i+1; j < N; j++) {
            acc += std::pow(omatfast(i, j, lti), -D/2.0);
        }
    }
    return acc * std::pow(D/(2*M_PI*a*a), D/2.0);
}

/*
 Calculate third virial coefficient
 calculates pairs f_{i,j}f_{u,v} where i < j, v > j, u >= i
 a heavy goddamn calculation
*/
double GaussSystem::thirdcorrection() {
    double sum = 0;
    double factor = std::pow(D / (2*M_PI*a*a), D);
    double det;
    for (int i = 0; i < N - 1; i++) {
        for (int j = i + 1; j < N; j++) {
            for (int u = i; u < N - 1; u++) {
                for (int v = u + 1; v < N; v++) {
                    if (i < u || j < v) {
                        det = triomatfast(i, j, u, v, lti);
                        sum += std::pow(det, -D/2.0);
                    }
                }
            }
        }
    }
    return factor*sum;
}

//Calculates \alpha - 1
double GaussSystem::correction1() {
    double acc = 0;
    for (int i = 0; i < N; i++) {
        for (int j = i+1; j < N; j++) {
            acc += std::pow(omatfast(i, j, lti), -D/2.0);
        }
    }
    return acc;
}

//Calculates \alpha - 1
double GaussSystem::term1() {
    double acc = 0;
    for (int i = 0; i < N; i++) {
        for (int j = i+1; j < N; j++) {
            acc += std::pow(omatfast(i, j, lti), -D/2.0)*(1 - order1fractionalRg2O(i,j));
        }
    }
    return acc;
}

double GaussSystem::term2() {
    double acc = 0;
    for (int i = 0; i < N - 1; i++) {
        for (int j = i + 1; j < N; j++) {
            for (int u = i; u < N - 1; u++) {
                for (int v = u + 1; v < N; v++) {
                    if (i < u || j < v) {
                        acc += std::pow(triomatfast(i, j, u, v, lti), -D/2.0)*(order2fractionalRg2O(i,j,u,v) - 1);
                    }
                }
            }
        }
    }
    return correction1()*term1() + acc;
}

//Calculates \alpha - 1
double GaussSystem::alpham1() {
    double acc = 0;
    for (int i = 0; i < N; i++) {
        for (int j = i+1; j < N; j++) {
            acc += omatfast(i, j, opm)*std::pow(omatfast(i, j, lti), -(1.0*D + 2.0) / 2.0) ;
        }
    }
    return (2*std::pow(2.0*M_PI*a*a / D, -D / 2.0)/rg20)*acc;
}

double GaussSystem::isitz() {
    double acc = 0;
    for (int i = 0; i < N; i++) {
        for (int j = i+1; j < N; j++) {
            acc += std::pow(omatfast(i, j, lti), -1.0*D/2.0) ;
        }
    }
    return (2*std::pow(2.0*M_PI*a*a / D, -D / 2.0))*acc;
}

//next order in \alpha
//requires previous order correction as parameter
double GaussSystem::alpham2(double alpha1) {
	double term1 = 4*alpha1*secondcorrection();
	double term2 = 0;
    arma::mat ok(2,2);	//matrices needed for 3rd order calculation
    arma::mat oki(2,2);
    arma::mat okp(2,2);
    //Horrid calculation
    for (int i = 0; i < N - 1; i++) {
        for (int j = i + 1; j < N; j++) {
            for (int u = i; u < N - 1; u++) {
                for (int v = u + 1; v < N; v++) {
                    if (i < u || j < v) {
                        ok = triomat(i, j, u, v, lti); //c^T L^{-1} c
                        oki = arma::inv(ok);	//invert it
                        okp = triomat(i, j, u, v, opm); //c^T L^{-1} R_G^2 L^{-1}
                        term2 += std::pow(arma::det(ok), -D/2.0)*arma::trace(oki*okp);
                    }
                }
            }
        }
    }
    return term1 - 4*term2/rg20*2.0*M_PI*a*a / D ;

}

/*
Calculate <R_G^2>_0
*/
double GaussSystem::z0R() {
    if (_z0 < 0) {  //calculate z0 if have not done so already
        _z0 = z0();
    }
    return _z0*rg20;
}

/*
z0 returns the non-interacting partition function
*/
double GaussSystem::z0() {
    double num = std::pow(2.0*M_PI*a*a / D, (double)N);
    double den = arma::det(lap);
    _z0 = std::pow(num / den, D / 2.0);
    return _z0;
}


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

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

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

 /*!

    @ingroup group-trigonometric
    Function object implementing atan2 capabilities

    atan2 function.

    @par Semantic:

    For every parameters of floating type T:

    @code
    T r = atan2(y, x);
    @endcode

    is similar but not fully equivalent to:

    @code
    T r =  atan(y/x);;
    @endcode

    as it is quadrant aware.

    @par Notes

    - For any real arguments @c x and @c y not both equal to zero, <tt>atan2(y, xy)</tt>
    is the angle in radians between the positive x-axis of a plane and the point
    given by the coordinates  <tt>(y,x)</tt>.

    - It is also the angle in \f$[-\pi,\pi[\f$ for which
    \f$x/\sqrt{x^2+y^2}\f$ and \f$y/\sqrt{x^2+y^2}\f$
    are respectively the sine and the cosine.

    - Following IEEE norms
     -  If y is \f$\pm0\f$ and x is negative or -0,\f$\pm\pi\f$ is returned
     -  If y is \f$\pm0\f$ and x is positive or +0, \f$\pm0\f$ is returned
     -  If y is \f$\pm\infty\f$ and x is finite, \f$\pm\pi/2\f$ is returned
     -  If y is \f$\pm\infty\f$ and x is \f$-\infty\f$,\f$\pm3\pi/4\f$ is returned
     -  If y is \f$\pm\infty\f$ and x is \f$+\infty\f$, \f$\pm\pi/4\f$ is returned
     -  If x is \f$\pm0\f$ and y is negative, \f$-\pi/2\f$ is returned
     -  If x is \f$\pm0\f$ and y is positive, \f$+\pi/2\f$  is returned
     -  If x is \f$-\infty\f$ and y is finite and positive, \f$+\pi\f$ is returned
     -  If x is \f$-\infty\f$ and y is finite and negative, \f$-\pi\f$ is returned
     -  If x is \f$+\infty\f$ and y is finite and positive, +0 is returned
     -  If x is \f$+\infty\f$ and y is finite and negative, -0 is returned
     -  If either x is Nan or y is Nan, Nan is returned

    - If you want to gain some cycles a fast_ tag is provided. If you use it pairs (x, y)
     where both are null or both are infinite will produce a Nan result which in fact
     is not more absurd than the IEEE choices and will be conforming in all other cases.

    @par Decorators

    std_ for floating entries

  @see atan, atand, atanpi

  **/
  const boost::dispatch::functor<tag::atan2_> atan2 = {};
} }
#endif

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

#endif


/*
* eigen3_impl.cpp
*
*  Created on: 18 oct. 2012
*      Author: boubad
*/
/////////////////////////////////////////////////////////
#if defined (__INTEL_COMPILER)
#pragma warning (disable:2196)
#endif
/////////////////////////////
#include <Eigen/Eigenvalues>
///////////////////////////////////////////
#include <cassert>
#include <vector>
#include <algorithm>
#include <cmath>
//////////////////////////////////////////////
namespace info {
	//////////////////////////////////////////
	using namespace Eigen;
	///////////////////////////////////////////////
	const double EPSILON = 0.000001;
	////////////////////////////////////////////////
	typedef std::pair<size_t, double> MyPair;
	typedef Matrix<double, Dynamic, Dynamic> MyMatrix;
	//
	struct MyComparePairDescFunc : public std::binary_function<MyPair, MyPair, bool> {
		bool operator()(const MyPair &v1, const MyPair &v2) const {
			return (v1.second > v2.second);
		} // operator()
	};
	// MyComparePairFunc
	///////////////////////////////////////////////////
	extern bool info_compute_eigen_impl(const int n, const double *pData,
		double *pVals, double *pVecs, int *pNbFacts /*= nullptr */) {
		//
		assert(n > 0);
		assert(pData != nullptr);
		assert(pVals != nullptr);
		assert(pVecs != nullptr);
		//
		MyMatrix m(n, n);
		for (int i = 0; i < n; ++i) {
			for (int j = 0; j <= i; ++j) {
				double v = pData[n * i + j];
				if (std::abs(v) < EPSILON) {
					v = 0;
				}
				m(i, j) = v;
				if (j != i) {
					m(j, i) = v;
				}
			}	// j
		}	// i
		EigenSolver<MyMatrix> solver(m);
		typename EigenSolver<MyMatrix>::EigenvalueType val = solver.eigenvalues();
		int nx = (int)val.rows();
		int ny = (int)val.cols();
		MyMatrix rval(nx, ny);
		for (int i = 0; i < nx; ++i) {
			for (int j = 0; j < ny; ++j) {
				rval(i, j) = val(i, j).real();
			}	// j
		}	// i
		typename EigenSolver<MyMatrix>::EigenvectorsType vec =
			solver.eigenvectors();
		nx = (int)vec.rows();
		ny = (int)vec.cols();
		MyMatrix rvec(nx, ny);
		for (int i = 0; i < nx; ++i) {
			for (int j = 0; j < ny; ++j) {
				rvec(i, j) = vec(i, j).real();
			}	// j
		}	// i
		std::vector<MyPair> oPairs;
		for (int i = 0; i < n; ++i) {
			double xx = rval(i, 0);
			if (xx > 0) {
				MyPair o((size_t)i, xx);
				oPairs.push_back(o);
			}
		}				// i
		int nFacts = (int)oPairs.size();
		bool bRet = (nFacts > 0);
		if (bRet) {
			std::sort(oPairs.begin(), oPairs.end(), MyComparePairDescFunc());
			for (int i = 0; i < nFacts; ++i) {
				const MyPair &pp = oPairs[i];
				const size_t ii = pp.first;
				double tt = pp.second;
				if (std::abs(tt) < EPSILON) {
					tt = 0;
				}
				pVals[i] = tt;
				for (int j = 0; j < n; ++j) {
					double xx = rvec(j, ii);
					if (std::abs(xx) < EPSILON) {
						xx = 0;
					}
					pVecs[j * nFacts + i] = xx;
				}// j
			}				// i
		}				// bRet
		if (pNbFacts != nullptr) {
			*pNbFacts = nFacts;
		}
		return (bRet);
	}// compute_eigen_impl
	 //////////////////////////////////////////
}	// namespace info
	/////////////////////////////////////////////////


#include "bindings-math.h"

#include "../luaapi/context.h"
#include "../luaapi/types.h"
#include "../luaapi/macros.h"

#include "CVec.h"
#include "util/angle.h"
#include "util/math_func.h"
#include "CodeAttributes.h"

#include <cmath>
#include <cstdio>
#include <string>
#include <iostream>

using std::cerr;
using std::endl;
#include <boost/lexical_cast.hpp>
#include <boost/bind.hpp>
using boost::lexical_cast;

namespace LuaBindings
{

INLINE lua_Number luaL_checknumber(lua_State *L, int narg)
{
	lua_Number d = lua_tonumber(L, narg);
	//if(d == 0 && !lua_isnumber(L, narg))
	//	; // TODO: tag_error(L, narg, LUA_TNUMBER);
	return d;
}

/*! sqrt(n)

	Returns the squareroot of n.
*/
int l_sqrt(lua_State* L)
{
	lua_pushnumber(L, sqrt(luaL_checknumber(L, 1)));
	return 1;
}

/*! abs(n)

	Returns the absolute value of n.
*/
int l_abs(lua_State* L)
{
	lua_pushnumber(L, fabs(luaL_checknumber(L, 1)));
	return 1;
}

/*! floor(n)

	Returns the number n rounded down towards infinity.
*/
int l_floor(lua_State* L)
{
	lua_pushnumber(L, floor(luaL_checknumber(L, 1)));
	return 1;
}

int l_round(lua_State* L)
{
	int d = lua_tointeger(L, 2);
	char buffer[256];
	sprintf(buffer, "%.*f", d, lua_tonumber(L, 1));
	lua_pushstring(L, buffer);
	return 1;
}

/*! randomint(l, u)

	Returns a random integer in the interval [l, u].
*/
int l_randomint(lua_State* L)
{
	int l = lua_tointeger(L, 1);
	int u = lua_tointeger(L, 2);

	//lua_pushnumber(L, l + (unsigned int)(rndgen()) % (u - l + 1));
	lua_pushinteger(L, l + rndInt(u - l + 1));

	return 1;
}

/*! randomfloat(l, u)

	Returns a random floating point number in the interval [l, u].
*/
int l_randomfloat(lua_State* L)
{
	lua_Number l = luaL_checknumber(L, 1);
	lua_Number u = luaL_checknumber(L, 2);

	lua_pushnumber(L, l + rnd() * (u - l));

	return 1;
}

/*! to_time_string(v)

	Converts a frame count //v// to a string showing hours, minutes and seconds as HH:MM:SS.
*/
int l_toTimeString(lua_State* L)
{
	lua_Integer v = lua_tointeger(L, 1);

	lua_Integer sec = v / 100;

	char c[8];
	c[0] = ((sec / 36000) % 10) + '0';
	c[1] = ((sec / 3600) % 10) + '0';
	c[2] = ':';
	c[3] = ((sec / 600) % 6) + '0';
	c[4] = ((sec / 60) % 10) + '0';
	c[5] = ':';
	c[6] = ((sec / 10) % 6) + '0';
	c[7] = (sec % 10) + '0';
	lua_pushlstring(L, c, 8);

	return 1;
}

/*! vector_diff(x1, y1, x2, y2)

	Returns a tuple equal to (x2 - x1, y2 - y1)
*/
int l_vector_diff(lua_State* L)
{
	lua_pushnumber(L, lua_tonumber(L, 3) - lua_tonumber(L, 1));
	lua_pushnumber(L, lua_tonumber(L, 4) - lua_tonumber(L, 2));

	return 2;
}

/*! vector_distance(x1, y1, x2, y2)

	Returns the distance from (x1, y1) to (x2, y2)
*/
int l_vector_distance(lua_State* L)
{
	lua_Number vx = lua_tonumber(L, 3) - lua_tonumber(L, 1);
	lua_Number vy = lua_tonumber(L, 4) - lua_tonumber(L, 2);
	vx *= vx;
	vy *= vy;
	lua_pushnumber(L, sqrt(vx + vy));

	return 1;
}

/*! vector_direction(x1, y1, x2, y2)

	Returns the angle between (x1, y1) and (x2, y2)
	in degrees.
*/
int l_vector_direction(lua_State* L)
{
	lua_Number vx = lua_tonumber(L, 3) - lua_tonumber(L, 1);
	lua_Number vy = lua_tonumber(L, 4) - lua_tonumber(L, 2);

	lua_pushnumber(L, rad2deg((float)atan2(vx, -vy)) );

	return 1;
}

/*! vector_add(x1, y1, x2, y2)

	Returns a tuple equal to (x1 + x2, y1 + y2)
*/
int l_vector_add(lua_State* L)
{
	lua_pushnumber(L, lua_tonumber(L, 1) + lua_tonumber(L, 3));
	lua_pushnumber(L, lua_tonumber(L, 2) + lua_tonumber(L, 4));

	return 2;
}

/*! angle_diff(a, b)

	Returns the relative angle in (-180, 180) between
	angle //a// and //b// such that (a + angle_diff(a, b)) = b (mod 360)
*/
int l_angle_diff(lua_State* L)
{
	AngleDiff diff(AngleDiff(lua_tonumber(L, 1)).relative(AngleDiff(lua_tonumber(L, 2))));

	lua_pushnumber(L, diff.toDeg());

	return 1;
}

/*! angle_clamp(angle)

	Returns //angle// normalized to [0, 360).
*/
int l_angle_clamp(lua_State* L)
{
	//lua_Number ang = lua_tonumber(L, 1);
	Angle ang((double)lua_tonumber(L, 1));

	ang.clamp();

	lua_pushnumber(L, ang.toDeg());

	return 1;
}

/*! angle_vector(angle[, length = 1])

	Returns a tuple representing the angle
	with length //length//.
*/
int l_angle_vector(lua_State* L)
{
	Angle ang((double)lua_tonumber(L, 1));
	lua_Number len = 1.0;

	if(lua_gettop(L) >= 2)
		len = lua_tonumber(L, 2);

	Vec vec(ang, len);
	lua_pushnumber(L, vec.x);
	lua_pushnumber(L, vec.y);

	return 2;
}

void initMath(LuaContext& context)
{
	context.functions()
		("sqrt", l_sqrt)
		("abs", l_abs)
		("floor", l_floor)

		("round", l_round)

		("randomint", l_randomint)
		("randomfloat", l_randomfloat)

		("to_time_string", l_toTimeString)

		("vector_diff", l_vector_diff)
		("vector_distance", l_vector_distance)
		("vector_direction", l_vector_direction)
		("vector_add", l_vector_add)

		("angle_clamp", l_angle_clamp)
		("angle_diff", l_angle_diff)
		("angle_vector", l_angle_vector)
	;
}

}