#include "math_unit_test.hpp"

#include <boost/config.hpp>
#include <boost/multiprecision/number.hpp>
#include <boost/math/fft/multiprecision_complex.hpp>
#ifdef BOOST_MATH_USE_FLOAT128
#include <boost/multiprecision/complex128.hpp>
#endif
#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/multiprecision/cpp_complex.hpp>

#include <boost/math/fft/bsl_backend.hpp>
#if defined(__GNUC__)
#include <boost/math/fft/fftw_backend.hpp>
#include <boost/math/fft/gsl_backend.hpp>
#include <boost/multiprecision/mpfr.hpp>
#include <boost/multiprecision/mpc.hpp>
#endif
#include <vector>
#include <iterator>
#include <iostream>
#include <boost/core/demangle.hpp>
#include <boost/random.hpp>

using namespace boost::math::fft;

template<class T>
void print(const std::vector<T>& V)
{
  std::cout << "Size: " << V.size() << '\n';
  std::cout << "[";
  for(auto x: V)
    std::cout << x << ", ";
  std::cout<< "]\n";
}

template<class Backend, class Complex = std::complex<typename Backend::value_type>>
void test_r2c(int n,int tolerance=1)
{
  using Real    = typename Backend::value_type;
  // using Complex = boost::multiprecision::complex<Real>;
  const Real tol = tolerance*std::numeric_limits<Real>::epsilon();

  boost::random::mt19937 rng;
  boost::random::uniform_real_distribution<double> U(0.0,1.0);

  std::vector<Real> A(n);
  std::vector<Complex> B(A.size());

  for(unsigned int i=0;i<A.size();++i)
  {
    A[i] = U(rng);
    B[i] = Complex{A[i],0.0};
  }

  std::vector<Real> HC,iHC;
  Backend rplan(A.size());
  rplan.real_to_halfcomplex(A.begin(),A.end(),std::back_inserter(HC));
  rplan.halfcomplex_to_real(HC.begin(),HC.end(),std::back_inserter(iHC));
  //print(A);
  //print(HC);
  //print(iHC);

  std::vector<Complex> TB;
  bsl_dft<Complex> cplan(B.size());
  cplan.forward(B.begin(),B.end(),std::back_inserter(TB));

  using std::abs;

  // check if the inverse recovers the original array
  {
    Real diff{0.0};
    const Real inv_n = Real{1.0}/n;
    for(unsigned int i=0;i<A.size();++i)
    {
      diff += abs(A[i]-iHC[i]*inv_n);
    }
    diff /= A.size();
    CHECK_MOLLIFIED_CLOSE(Real{0.0},diff,tol);
  }
  // check if the halfcomplex contains the non-redundant complex components
  {
    Real diff{0.0};

    diff += abs(TB[0]-HC[0]);
    for(int i=1,j=n-1;i<=j;++i,--j)
    {
      diff += abs(TB[i].real()-HC[i]);

      if(i<j)
      diff += abs(TB[j].imag()-HC[j]);
    }
    diff /= n;
    CHECK_MOLLIFIED_CLOSE(Real{0.0},diff,tol);
  }
}

int main()
{
  // corner cases
#if defined(__GNUC__)
  test_r2c<fftw_rdft<double>>(1);
  test_r2c<gsl_rdft<double>>(1);
#endif
  test_r2c<bsl_rdft<double>>(1);

#ifdef BOOST_MATH_USE_FLOAT128
  test_r2c< bsl_rdft<boost::multiprecision::float128>,
            boost::multiprecision::complex128 >(1);
#endif
    test_r2c< bsl_rdft<boost::multiprecision::cpp_bin_float_100>,
              boost::multiprecision::cpp_complex_100 >(1);
    test_r2c< bsl_rdft<boost::multiprecision::cpp_bin_float_quad>,
              boost::multiprecision::cpp_complex_quad >(1);

  // primes
  for(auto n: std::vector<int>{2,3,5,7,11,13,17,19})
  {
#if defined(__GNUC__)
    test_r2c<fftw_rdft<double>>(n,4);
    test_r2c<gsl_rdft<double>>(n,16);
#endif
    test_r2c<bsl_rdft<double>>(n,4);

#ifdef BOOST_MATH_USE_FLOAT128
    test_r2c< bsl_rdft<boost::multiprecision::float128>,
              boost::multiprecision::complex128 >(n,4);
#endif
    test_r2c< bsl_rdft<boost::multiprecision::cpp_bin_float_100>,
              boost::multiprecision::cpp_complex_100 >(n,4);
    test_r2c< bsl_rdft<boost::multiprecision::cpp_bin_float_quad>,
              boost::multiprecision::cpp_complex_quad >(n,4);
  }

  // powers of two
  for(auto n: std::vector<int>{2,4,8,16,32,64,128})
  {
#if defined(__GNUC__)
    test_r2c<fftw_rdft<double>>(n,4);
    test_r2c<gsl_rdft<double>>(n,4);
#endif
    test_r2c<bsl_rdft<double>>(n,4);

#ifdef BOOST_MATH_USE_FLOAT128
    test_r2c< bsl_rdft<boost::multiprecision::float128>,
              boost::multiprecision::complex128 >(n,4);
#endif
    test_r2c< bsl_rdft<boost::multiprecision::cpp_bin_float_100>,
              boost::multiprecision::cpp_complex_100 >(n,4);
    test_r2c< bsl_rdft<boost::multiprecision::cpp_bin_float_quad>,
              boost::multiprecision::cpp_complex_quad >(n,4);
  }
  // composite
  for(auto n: std::vector<int>{6,9,10,12,14,15,18,20,21,22,24,25,26,27,28,30})
  {
#if defined(__GNUC__)
    test_r2c<fftw_rdft<double>>(n,8);
    test_r2c<gsl_rdft<double>>(n,16);
#endif
    test_r2c<bsl_rdft<double>>(n,4);

#ifdef BOOST_MATH_USE_FLOAT128
    test_r2c< bsl_rdft<boost::multiprecision::float128>,
              boost::multiprecision::complex128 >(n,4);
#endif
    test_r2c< bsl_rdft<boost::multiprecision::cpp_bin_float_100>,
              boost::multiprecision::cpp_complex_100 >(n,4);
    test_r2c< bsl_rdft<boost::multiprecision::cpp_bin_float_quad>,
              boost::multiprecision::cpp_complex_quad >(n,4);
  }

  // TODO: fix this
  // test_r2c< bsl_rdft<boost::multiprecision::mpfr_float_100>,
  //           boost::multiprecision::mpc_complex_100 >(n,4;
  return boost::math::test::report_errors();
}


#include <iostream>
#include <armadillo>
#include <tuple>
#include <cstdlib>     /* srand, rand */
#include <ctime>       /* time */
#include "logistic_function.hpp"
#include "neural.hpp"
using matrix=arma::Mat<double>;
size_t const max_epochs = 1000;
const double lambda = 0.8;
const double tolerance = 0.000001;
/**
Evaluate Output
 Given
  Input  data X(input_dim,1)
  hidden-layer (input_dim,hidden_layer_size)
  Output (hidden_layer_size,out_put_size)
 Compute
  Output of FFNN

*/
matrix Evaluate(matrix const& X, matrix const& hiddenLayer, matrix const& outputLayer)
	{
	matrix inp = X; //21*m
	matrix hidden = inp.t()*hiddenLayer; // m*21*21*4 -> m*4
	matrix output = LogisticFunction::fn(hidden)*outputLayer; // m*4 * 4 *3 -> m*3
	return LogisticFunction::fn(output).t();//3*m
	}

matrix EvaluateWithBias(matrix const& inp, matrix const& hiddenLayer, matrix const& outputLayer)
	{

	matrix hidden = hiddenLayer.t()*inp; //  4*m
	hidden = LogisticFunction::fn(hidden);//4*m
	matrix hiddenInput = arma::join_cols(arma::ones(1, inp.n_cols), hidden);//5*m
	matrix output = outputLayer.t()*hiddenInput; // (5.3)'*5.m -> 3.m
	return LogisticFunction::fn(output);
	}
/**
Calculate cost using cross entropy
*/
double CalcCost(matrix const& target, matrix const& output)
	{

	double sum = 0.0;
	size_t item_count = target.n_elem;
	for (size_t i = 0; i < item_count; ++i)
		{
		double y = target[i];
		double val = output[i];
		sum = sum - y*log(val) - (1 - y)*log(1 - val);
		}
	return sum / target.n_cols;
	}

/**Back Propogation
 Ignore lambda
 Problem statement:
 Given
  X the feature mector of m column vectors of size feature_size
  Y the result vector of m column vectors of label_size
  hidden_layer_size
  Theta1: weight vector of hidden_layer_size columns each of size (feature_size )
  Theta2: weight vector of label_size columns each of size (hidden_layer_size)
Compute
  cost using cross entropy
  Theta1_gardient: weight vector of hidden_layer_size columns each of size (feature_size )
  Theta2_gradient: weight vector of size label_size columns each of size (hidden_layer_size )
  ...using sigmoid

Updates:
	Adding bias

*/
std::tuple<double,matrix,matrix>
BackProp(
	 matrix & a1, matrix & y, matrix const& Theta1, matrix const& Theta2
	)
	{

	// This generates [0 1 2 3 ... (ElementCount(trainingData) - 1)]. The
	// sequence will be used to iterate through the training data.
	size_t m = a1.n_cols;

	matrix theta1_gradient(Theta1);
	theta1_gradient.zeros();
	matrix theta2_gradient(Theta2);
	theta2_gradient.zeros();

	matrix output(Theta2.n_cols, 1);

	for (size_t j = m; j > 0; --j)
		{
		int k = rand() % j;
		a1.swap_cols(j - 1, k);
		y.swap_cols(j - 1, k);
		}
	output = EvaluateWithBias(a1, Theta1, Theta2);
	double cost = CalcCost(y, output);

	matrix z2 = Theta1.t()*a1;
	matrix a2 = arma::join_cols(arma::ones(1, m), LogisticFunction::fn(z2));

	matrix z3 = Theta2.t()*a2;
	matrix a3 = LogisticFunction::fn(z3);
	matrix delta3 = a3 - y;
	theta2_gradient += a2*delta3.t();
	matrix delta2 = (Theta2*delta3) % arma::join_cols(arma::ones(1, m), LogisticFunction::deriv(a2.rows(1, a2.n_rows - 1)));
	delta2 = delta2.rows(1, delta2.n_rows-1);
	theta1_gradient += a1*delta2.t();
	theta1_gradient = theta1_gradient / m + lambda*arma::join_cols(arma::zeros(1, Theta1.n_cols), Theta1.rows(1, Theta1.n_rows - 1));
	theta2_gradient = theta2_gradient / m + lambda*arma::join_cols(arma::zeros(1, Theta2.n_cols), Theta2.rows(1, Theta2.n_rows - 1));
	return std::make_tuple(cost, theta1_gradient, theta2_gradient);
	}

/**
Train Network
 Ignore lambda and bias
 Given:
  X the feature mector of m column vectors of size feature_size
  Y the result vector of m column vectors of label_size
  hidden_layer_size
 Compute:
  Theta1: weight vector of hidden_layer_size columns each of size (feature_size + 1)
  Theta2: weight vector of size label_size columns each of size (hidden_layer_size +1)
 Update:
  Adding bias
*/

std::tuple<matrix,matrix>
TrainNetwork(
	matrix const& X, matrix y,size_t hidden_layer_size
	)
	{
	matrix theta1;
	theta1.randu(X.n_rows+1, hidden_layer_size);
	matrix theta2;
	theta2.randu(hidden_layer_size+1, y.n_rows);
	double eta =0.5;
	size_t m = X.n_cols;
	matrix a1 = arma::join_cols(arma::ones(1, m), X);
	double prev_cost = 1000.0;
	for(size_t epoch = 0; epoch < max_epochs; ++epoch)
		{
		matrix delta1,delta2;
		double cost=0.0;
		std::tie(cost,delta1,delta2) = BackProp(a1,y,theta1,theta2);
		theta1 = theta1 - eta *  delta1;
		theta2 = theta2 - eta *  delta2;
		//std::cout << epoch << ":" << cost << std::endl;
		if (abs(prev_cost - cost) < tolerance)
			break;
		prev_cost = cost;
		}
	return std::make_tuple(theta1,theta2);
	}

/**
Predict
	Given
	 Neural Network
	 Input sample
    Compute
	 Class it belongs to
*/
matrix Predict(matrix const& input, matrix const& hiddenLayer, matrix const& outputLayer)
	{
	matrix output = EvaluateWithBias(input, hiddenLayer, outputLayer);
	matrix result(output.n_rows, output.n_cols);
	for (size_t i = 0; i < output.n_cols; ++i)
		{
		matrix cur = output.unsafe_col(i);
		double max = cur[0];
		size_t index = 0;
		for (size_t j = 1; j < cur.n_elem; ++j)
			{
			if (cur[j] > max)
				{
				max = cur[j];
				index = j;
				}
			}
		for (size_t j = 0; j < output.n_rows; ++j)
			{
			result(j, i) = index == j ? 1.0 : 0.0;
			}
		}
	return result;
	}
double ComputeError(matrix const& result, matrix const& labels)
	{
	double error = 0.0;
	for (size_t j = 0; j < result.n_elem; ++j)
		error += abs(result[j] - labels[j]);
	return error / 2.0;
	}


//  (C) Copyright Nick Thompson 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)
//
// Deliberately contains some unicode characters:
//
// boost-no-inspect
//
#include <iostream>
#include <boost/math/constants/constants.hpp>
#include <boost/math/tools/simple_continued_fraction.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>

using boost::math::constants::root_two;
using boost::math::constants::phi;
using boost::math::constants::pi;
using boost::math::constants::e;
using boost::math::constants::zeta_three;
using boost::math::tools::simple_continued_fraction;

int main()
{
    using Real = boost::multiprecision::cpp_bin_float_100;
    auto phi_cfrac = simple_continued_fraction(phi<Real>());
    std::cout << "φ ≈ " << phi_cfrac << "\n\n";

    auto pi_cfrac = simple_continued_fraction(pi<Real>());
    std::cout << "π ≈ " << pi_cfrac << "\n";
    std::cout << "Known: [3; 7, 15, 1, 292, 1, 1, 1, 2, 1, 3, 1, 14, 2, 1, 1, 2, 2, 2, 2, 1, 84, 2, 1, 1, 15, 3, 13, 1, 4, 2, 6, 6, 99, 1, 2, 2, 6, 3, 5, 1, 1, 6, 8, 1, 7, 1, 2, 3, 7, 1, 2, 1, 1, 12, 1, 1, 1, 3, 1, 1, 8, 1, 1, 2, 1, 6, 1, 1, 5, 2, 2, 3, 1, 2, 4, 4, 16, 1, 161, 45, 1, 22, 1, 2, 2, 1, 4, 1, 2, 24, 1, 2, 1, 3, 1, 2, 1, ...]\n\n";

    auto rt_cfrac = simple_continued_fraction(root_two<Real>());
    std::cout << "√2 ≈ " << rt_cfrac << "\n\n";

    auto e_cfrac = simple_continued_fraction(e<Real>());
    std::cout << "e ≈ " << e_cfrac << "\n";

    // Correctness can be checked in Mathematica via: ContinuedFraction[Zeta[3], 500]
    auto z_cfrac = simple_continued_fraction(zeta_three<Real>());
    std::cout << "ζ(3) ≈ " << z_cfrac << "\n";
}


/*
* compile with flags:   g++ test.cc softmax_classifier.cc   ../datasets/datasets.cc -std=c++14 -larmadillo -I ../../include/
* author: Yuzhen Liu
* Date: 2019.3.29 10:55
*/

#include <iostream>
#include <armadillo>
#include <cmath>
#include <linear/linear_regressor.h>
#include <datasets/datasets.h>

using namespace std;
using namespace arma;

int main() {
    Datasets dataset = Datasets("boston");

    // Linear_Regressor linear_regressor = Linear_Regressor(0.02);
    Linear_Regressor linear_regressor = Linear_Regressor();
    linear_regressor.train(dataset.x, dataset.y);

    vec res = linear_regressor.predict(dataset.x);
    printf("The sum loss is:\n");
    vec dis = res - dataset.y;
    // dis.print();
    join_rows(res, dataset.y).print();
    cout << "The standard deviation is: " << stddev(dis) << endl;

    return 0;
}


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>

#include <CGAL/natural_neighbor_coordinates_2.h>
#include <CGAL/Interpolation_gradient_fitting_traits_2.h>
#include <CGAL/sibson_gradient_fitting.h>
#include <CGAL/interpolation_functions.h>

#include <CGAL/Triangulation_vertex_base_with_info_2.h>
#include <CGAL/Regular_triangulation_2.h>

#include <boost/iterator/function_output_iterator.hpp>

#include <iostream>
#include <iterator>
#include <utility>
#include <vector>

typedef CGAL::Exact_predicates_inexact_constructions_kernel    K;
typedef CGAL::Interpolation_gradient_fitting_traits_2<K>       Traits;

typedef K::FT                                                  Coord_type;
typedef K::Point_2                                             Bare_point;
typedef K::Weighted_point_2                                    Weighted_point;
typedef K::Vector_2                                            Vector;

template <typename V, typename G>
struct Value_and_gradient
{
  Value_and_gradient() : value(), gradient(CGAL::NULL_VECTOR) {}

  V value;
  G gradient;
};

typedef CGAL::Triangulation_vertex_base_with_info_2<
                Value_and_gradient<Coord_type, Vector>, K,
                CGAL::Regular_triangulation_vertex_base_2<K> > Vb;
typedef CGAL::Regular_triangulation_face_base_2<K> Fb;
typedef CGAL::Triangulation_data_structure_2<Vb, Fb>           Tds;
typedef CGAL::Regular_triangulation_2<K, Tds>                  Regular_triangulation;
typedef Regular_triangulation::Vertex_handle                   Vertex_handle;

int main()
{
  Regular_triangulation rt;

  auto value_function = [](const Vertex_handle& a) -> std::pair<Coord_type, bool>
  {
    return std::make_pair(a->info().value, true);
  };

  auto gradient_function = [](const Vertex_handle& a) -> std::pair<Vector, bool>
  {
    return std::make_pair(a->info().gradient, a->info().gradient != CGAL::NULL_VECTOR);
  };

  auto gradient_output_iterator
    = boost::make_function_output_iterator
    ([](const std::pair<Vertex_handle, Vector>& p)
     {
       p.first->info().gradient = p.second;
     });

  // parameters for spherical function:
  Coord_type a(0.25), bx(1.3), by(-0.7), c(0.2);
  for (int y=0; y<4; y++) {
    for (int x=0; x<4; x++) {
      Weighted_point p(Bare_point(x,y), (x-y)/3. /*weight*/);
      Vertex_handle vh = rt.insert(p);
      Coord_type value = a + bx*x + by*y + c*(x*x+y*y);
      vh->info().value = value;
    }
  }

  CGAL::sibson_gradient_fitting_rn_2(rt,
                                     gradient_output_iterator,
                                     CGAL::Identity<std::pair<Vertex_handle, Vector> >(),
                                     value_function,
                                     Traits());

  // coordinate computation
  Weighted_point p(Bare_point(1.6, 1.4), -0.3 /*weight*/);
  std::vector<std::pair<Vertex_handle, Coord_type> > coords;
  typedef CGAL::Identity<std::pair<Vertex_handle, Coord_type> > Identity;
  Coord_type norm = CGAL::regular_neighbor_coordinates_2(rt,
                                                         p,
                                                         std::back_inserter(coords),
                                                         Identity()).second;

  // Sibson interpolant: version without sqrt:
  std::pair<Coord_type, bool> res = CGAL::sibson_c1_interpolation_square(coords.begin(),
                                                                         coords.end(),
                                                                         norm,
                                                                         p,
                                                                         value_function,
                                                                         gradient_function,
                                                                         Traits());

  if(res.second)
    std::cout << "Tested interpolation on " << p
              << " interpolation: " << res.first << " exact: "
              << a + bx * p.x()+ by * p.y()+ c*(p.x()*p.x()+p.y()*p.y())
              << std::endl;
  else
    std::cout << "C^1 Interpolation not successful." << std::endl
              << " not all gradients are provided."  << std::endl
              << " You may resort to linear interpolation." << std::endl;

  return EXIT_SUCCESS;
}


#include <Eigen/LU>
#include <cmath>
#include <mathtoolbox/rbf-interpolation.hpp>

using Eigen::Map;
using Eigen::MatrixXd;
using Eigen::PartialPivLU;
using Eigen::VectorXd;
using std::function;
using std::vector;

mathtoolbox::RbfInterpolator::RbfInterpolator(const function<double(const double)>& rbf_kernel,
                                              const bool                            use_polynomial_term)
    : m_rbf_kernel(rbf_kernel), m_use_polynomial_term(use_polynomial_term)
{
}

void mathtoolbox::RbfInterpolator::SetData(const MatrixXd& X, const VectorXd& y)
{
    assert(y.rows() == X.cols());

    this->m_X = X;
    this->m_y = y;
}

void mathtoolbox::RbfInterpolator::CalcWeights(const bool use_regularization, const double lambda)
{
    const int num_data = m_y.rows();

    // Construct the symmetric matrix of RBF values
    MatrixXd Phi{num_data, num_data};
    for (int i = 0; i < num_data; ++i)
    {
        for (int j = i; j < num_data; ++j)
        {
            const double value = m_rbf_kernel((m_X.col(i) - m_X.col(j)).norm());

            Phi(i, j) = value;
            Phi(j, i) = value;
        }
    }

    if (m_use_polynomial_term)
    {
        const int dim = m_X.rows();

        MatrixXd P{num_data, dim + 1};

        P.block(0, 0, num_data, 1)   = MatrixXd::Ones(num_data, 1);
        P.block(0, 1, num_data, dim) = m_X.transpose();

        MatrixXd A = MatrixXd::Zero(num_data + dim + 1, num_data + dim + 1);

        A.block(0, 0, num_data, num_data)       = Phi;
        A.block(0, num_data, num_data, dim + 1) = P;
        A.block(num_data, 0, dim + 1, num_data) = P.transpose();

        VectorXd b = VectorXd::Zero(num_data + dim + 1);

        b.segment(0, num_data) = m_y;

        VectorXd solution;
        if (use_regularization)
        {
            const auto I = MatrixXd::Identity(num_data + dim + 1, num_data + dim + 1);

            solution = PartialPivLU<MatrixXd>(A.transpose() * A + lambda * I).solve(A.transpose() * b);
        }
        else
        {
            solution = PartialPivLU<MatrixXd>(A).solve(b);
        }

        m_w = solution.segment(0, num_data);
        m_v = solution.segment(num_data, dim + 1);
    }
    else
    {
        const auto     I = MatrixXd::Identity(num_data, num_data);
        const MatrixXd A = use_regularization ? Phi.transpose() * Phi + lambda * I : Phi;
        const VectorXd b = use_regularization ? Phi.transpose() * m_y : m_y;

        m_w = PartialPivLU<MatrixXd>(A).solve(b);
    }
}

double mathtoolbox::RbfInterpolator::CalcValue(const VectorXd& x) const
{
    assert(x.rows() == m_X.rows());

    const int num_data = m_w.rows();
    const int dim      = x.rows();

    // Calculate the distance for each data point via broadcasting
    const Eigen::VectorXd norms = (m_X.colwise() - x).colwise().norm();

    // Calculate the RBF value associated with each data point
    // TODO: This part can be further optimized for performance by vectorization
    VectorXd rbf_values{num_data};
    for (int i = 0; i < num_data; ++i)
    {
        rbf_values(i) = m_rbf_kernel(norms(i));
    }

    // Calculate the weighted sum using dot product
    const double rbf_term = m_w.dot(rbf_values);

    if (m_use_polynomial_term)
    {
        const double polynomial_term = m_v(0) + x.transpose() * m_v.segment(1, dim);

        return rbf_term + polynomial_term;
    }
    else
    {
        return rbf_term;
    }
}


// C/C++ includes
#include <cfloat>
#include <vector>
#include <iostream>

//Eigen includes
#include <Eigen/Core>

double _bin_size = 1;

/**
 * @brief returns all the voxels that are traversed by a ray going from start to end
 * @param start : continous world position where the ray starts
 * @param end   : continous world position where the ray end
 * @return vector of voxel ids hit by the ray in temporal order
 *
 * J. Amanatides, A. Woo. A Fast Voxel Traversal Algorithm for Ray Tracing. Eurographics '87
 */

std::vector<Eigen::Vector3i> voxel_traversal(Eigen::Vector3d ray_start, Eigen::Vector3d ray_end) {
  std::vector<Eigen::Vector3i> visited_voxels;

  // This id of the first/current voxel hit by the ray.
  // Using floor (round down) is actually very important,
  // the implicit int-casting will round up for negative numbers.
  Eigen::Vector3i current_voxel(std::floor(ray_start[0]/_bin_size),
                                std::floor(ray_start[1]/_bin_size),
                                std::floor(ray_start[2]/_bin_size));

  // The id of the last voxel hit by the ray.
  // TODO: what happens if the end point is on a border?
  Eigen::Vector3i last_voxel(std::floor(ray_end[0]/_bin_size),
                             std::floor(ray_end[1]/_bin_size),
                             std::floor(ray_end[2]/_bin_size));

  // Compute normalized ray direction.
  Eigen::Vector3d ray = ray_end-ray_start;
  //ray.normalize();

  // In which direction the voxel ids are incremented.
  double stepX = (ray[0] >= 0) ? 1:-1; // correct
  double stepY = (ray[1] >= 0) ? 1:-1; // correct
  double stepZ = (ray[2] >= 0) ? 1:-1; // correct

  // Distance along the ray to the next voxel border from the current position (tMaxX, tMaxY, tMaxZ).
  double next_voxel_boundary_x = (current_voxel[0]+stepX)*_bin_size; // correct
  double next_voxel_boundary_y = (current_voxel[1]+stepY)*_bin_size; // correct
  double next_voxel_boundary_z = (current_voxel[2]+stepZ)*_bin_size; // correct

  // tMaxX, tMaxY, tMaxZ -- distance until next intersection with voxel-border
  // the value of t at which the ray crosses the first vertical voxel boundary
  double tMaxX = (ray[0]!=0) ? (next_voxel_boundary_x - ray_start[0])/ray[0] : DBL_MAX; //
  double tMaxY = (ray[1]!=0) ? (next_voxel_boundary_y - ray_start[1])/ray[1] : DBL_MAX; //
  double tMaxZ = (ray[2]!=0) ? (next_voxel_boundary_z - ray_start[2])/ray[2] : DBL_MAX; //

  // tDeltaX, tDeltaY, tDeltaZ --
  // how far along the ray we must move for the horizontal component to equal the width of a voxel
  // the direction in which we traverse the grid
  // can only be FLT_MAX if we never go in that direction
  double tDeltaX = (ray[0]!=0) ? _bin_size/ray[0]*stepX : DBL_MAX;
  double tDeltaY = (ray[1]!=0) ? _bin_size/ray[1]*stepY : DBL_MAX;
  double tDeltaZ = (ray[2]!=0) ? _bin_size/ray[2]*stepZ : DBL_MAX;

  Eigen::Vector3i diff(0,0,0);
  bool neg_ray=false;
  if (current_voxel[0]!=last_voxel[0] && ray[0]<0) { diff[0]--; neg_ray=true; }
  if (current_voxel[1]!=last_voxel[1] && ray[1]<0) { diff[1]--; neg_ray=true; }
  if (current_voxel[2]!=last_voxel[2] && ray[2]<0) { diff[2]--; neg_ray=true; }
  visited_voxels.push_back(current_voxel);
  if (neg_ray) {
    current_voxel+=diff;
    visited_voxels.push_back(current_voxel);
  }

  while(last_voxel != current_voxel) {
    if (tMaxX < tMaxY) {
      if (tMaxX < tMaxZ) {
        current_voxel[0] += stepX;
        tMaxX += tDeltaX;
      } else {
        current_voxel[2] += stepZ;
        tMaxZ += tDeltaZ;
      }
    } else {
      if (tMaxY < tMaxZ) {
        current_voxel[1] += stepY;
        tMaxY += tDeltaY;
      } else {
        current_voxel[2] += stepZ;
        tMaxZ += tDeltaZ;
      }
    }
    visited_voxels.push_back(current_voxel);
  }
  return visited_voxels;
}

int main (int, char**) {
  Eigen::Vector3d ray_start(0,0,0);
  Eigen::Vector3d ray_end(3,2,2);
  std::cout << "Voxel size: " << _bin_size << std::endl;
  std::cout << "Starting position: " << ray_start.transpose() << std::endl;
  std::cout << "Ending position: " << ray_end.transpose() << std::endl;
  std::cout << "Voxel ID's from start to end:" << std::endl;
  std::vector<Eigen::Vector3i> ids = voxel_traversal(ray_start,ray_end);

  for (auto& i : ids) {
    std::cout << "> " << i.transpose() << std::endl;
  }
  std::cout << "Total number of traversed voxels: " << ids.size() << std::endl;
  return 0;
}


/**
 * @ file
 * @ brief NPDE homework TEMPLATE MAIN FILE
 * @ author
 * @ date
 * @ copyright Developed at SAM, ETH Zurich
 */

#define _USE_MATH_DEFINES

#include <Eigen/Core>
#include <cmath>
#include <fstream>
#include <iostream>

#include "fluxlimitedfv.h"

typedef std::numeric_limits<double> dbl;

using namespace FluxLimitedFV;

int main(int /*argc*/, char** /*argv*/) {
  std::cout.precision(dbl::max_digits10);

  /* ADVECTION PROBLEM */
  // Data
  double T = 1.0;
  unsigned int nb_timesteps[8] = {10, 20, 40, 80, 160, 320, 640, 1280};

  // Parameter coefficient
  double beta = 1.0;

  // Flux limiter function
  auto phi = [](double theta) {
    return (abs(theta) + theta) / (1.0 + abs(theta));
  };

  // Initial conditions A and B as lambda functions
  auto mu0_A_fn = [](double x) -> double {
    if (x < 1) {
      return 0.0;
    } else if (x < 2) {
      return pow(sin(0.5 * M_PI * (x - 1)), 2);
    } else {
      return 1.0;
    }
  };
  auto mu0_B_fn = [](double x) -> double {
    if ((1 < x) && (x < 2)) {
      return 1.0;
    } else {
      return 0.0;
    }
  };

  Eigen::VectorXd error_L1_mu0_A = Eigen::VectorXd::Zero(8);
  Eigen::VectorXd error_L1_mu0_B = Eigen::VectorXd::Zero(8);
  double tau, h;
  int N;
  Eigen::VectorXd fluxlimAdvection_sol_A, fluxlimAdvection_sol_B;

  for (int k = 0; k < 8; k++) {
    tau = T / nb_timesteps[k];
    h = 1.2 * tau;
    N = std::round(5.0 / h);

    // Discrete initial conditions
    Eigen::VectorXd mu0_A(N);
    for (int j = 0; j < N; j++) {
      mu0_A(j) = mu0_A_fn(h * j);
    }
    Eigen::VectorXd mu0_B(N);
    for (int j = 0; j < N; j++) {
      mu0_B(j) = mu0_B_fn(h * j);
    }

    fluxlimAdvection_sol_A =
        fluxlimAdvection(beta, mu0_A, h, tau, nb_timesteps[k], phi);
    fluxlimAdvection_sol_B =
        fluxlimAdvection(beta, mu0_B, h, tau, nb_timesteps[k], phi);

    // Computing the L1 errors
    double sum_A = 0.0;
    double sum_B = 0.0;
    for (int j = 0; j < N; j++) {
      sum_A = sum_A +
              std::abs(h * (fluxlimAdvection_sol_A(j) - mu0_A_fn(j * h - T)));
      sum_B = sum_B +
              std::abs(h * (fluxlimAdvection_sol_B(j) - mu0_B_fn(j * h - T)));
    }
    error_L1_mu0_A[k] = sum_A;
    error_L1_mu0_B[k] = sum_B;
  }

  std::cout << "" << std::endl;
  std::cout << "--------------" << std::endl;
  std::cout << "error_L1_mu0_A" << std::endl;
  std::cout << "--------------" << std::endl;
  std::cout << "M";
  std::cout << "\t error" << std::endl;
  for (int k = 0; k < 8; k++) {
    std::cout << nb_timesteps[k];
    std::cout << "\t";
    std::cout << error_L1_mu0_A[k] << std::endl;
  }
  std::cout << "\n" << std::endl;
  std::cout << "--------------" << std::endl;
  std::cout << "error_L1_mu0_B" << std::endl;
  std::cout << "--------------" << std::endl;
  std::cout << "M";
  std::cout << "\t error" << std::endl;
  for (int k = 0; k < 8; k++) {
    std::cout << nb_timesteps[k];
    std::cout << "\t";
    std::cout << error_L1_mu0_B[k] << std::endl;
  }
  std::cout << "" << std::endl;

  // Output results to csv files
  Eigen::VectorXd x_advection(N);
  for (int j = 0; j < N; j++) {
    x_advection(j) = j * h;
  }
  const static Eigen::IOFormat CSVFormat(Eigen::StreamPrecision,
                                         Eigen::DontAlignCols, ", ", "\n");
  std::ofstream fluxlimAdvection_sol_A_csv;
  fluxlimAdvection_sol_A_csv.open(CURRENT_BINARY_DIR
                                  "/fluxlimAdvection_sol_A.csv");
  fluxlimAdvection_sol_A_csv << x_advection.transpose().format(CSVFormat)
                             << std::endl;
  fluxlimAdvection_sol_A_csv
      << fluxlimAdvection_sol_A.transpose().format(CSVFormat) << std::endl;
  fluxlimAdvection_sol_A_csv.close();
  std::cout << "Generated " CURRENT_BINARY_DIR "/fluxlimAdvection_sol_A.csv"
            << std::endl;
  std::system("python3 " CURRENT_SOURCE_DIR "/plot_sol.py " CURRENT_BINARY_DIR
              "/fluxlimAdvection_sol_A.csv " CURRENT_BINARY_DIR
              "/fluxlimAdvection_sol_A.eps");

  std::ofstream fluxlimAdvection_sol_B_csv;
  fluxlimAdvection_sol_B_csv.open(CURRENT_BINARY_DIR
                                  "/fluxlimAdvection_sol_B.csv");
  fluxlimAdvection_sol_B_csv << x_advection.transpose().format(CSVFormat)
                             << std::endl;
  fluxlimAdvection_sol_B_csv
      << fluxlimAdvection_sol_B.transpose().format(CSVFormat) << std::endl;
  fluxlimAdvection_sol_B_csv.close();
  std::cout << "Generated " CURRENT_BINARY_DIR "/fluxlimAdvection_sol_B.csv"
            << std::endl;
  std::system("python3 " CURRENT_SOURCE_DIR "/plot_sol.py " CURRENT_BINARY_DIR
              "/fluxlimAdvection_sol_B.csv " CURRENT_BINARY_DIR
              "/fluxlimAdvection_sol_B.eps");

  /* BURGERS FLUX PROBLEM */
  // Discretization parameters
  T = 2.0;
  tau = T / nb_timesteps[7];
  h = 1.2 * tau;
  N = std::round(5.0 / h);

  // Initial conditions A and B as lambda functions
  auto mu0_fn = [](double x) -> double {
    if (x < 2) {
      return -1.0;
    } else if (x < 3) {
      return 1.0;
    } else {
      return -1.0;
    }
  };

  Eigen::VectorXd mu0(N);
  for (int j = 0; j < N; j++) {
    mu0(j) = mu0_fn(h * j);
  }

  Eigen::VectorXd fluxlimBurgers_sol =
      fluxlimBurgers(mu0, h, tau, nb_timesteps[6], phi);

  Eigen::VectorXd x_Burgers(N);
  for (int j = 0; j < N; j++) {
    x_Burgers(j) = j * h;
  }

  std::ofstream fluxlimBurgers_sol_csv;
  fluxlimBurgers_sol_csv.open(CURRENT_BINARY_DIR "/fluxlimBurgers_sol.csv");
  fluxlimBurgers_sol_csv << x_Burgers.transpose().format(CSVFormat)
                         << std::endl;
  fluxlimBurgers_sol_csv << fluxlimBurgers_sol.transpose().format(CSVFormat)
                         << std::endl;
  fluxlimBurgers_sol_csv.close();
  std::cout << "Generated " CURRENT_BINARY_DIR "/fluxlimBurgers_sol.csv"
            << std::endl;
  std::system("python3 " CURRENT_SOURCE_DIR "/plot_sol.py " CURRENT_BINARY_DIR
              "/fluxlimBurgers_sol.csv " CURRENT_BINARY_DIR
              "/fluxlimBurgers_sol.eps");

}  // main


#include "MLTest.h"

#include <mlpack/core.hpp>
#include <mlpack/methods/neighbor_search/neighbor_search.hpp>
#include <iostream>
//#include <armadillo>

using namespace mlpack;
using namespace mlpack::neighbor; // NeighborSearch and NearestNeighborSort
using namespace mlpack::metric; // ManhattanDistance

namespace mv
{

    MLTest::MLTest()
	{
	}

    MLTest::~MLTest()
	{
	}

	void MLTest::Run()
	{
        // Load the data from data.csv (hard-coded).  Use CLI for simple command-line
        // parameter handling.
        arma::mat data("0.339406815,0.843176636,0.472701471; \
                  0.212587646,0.351174901,0.81056695;  \
                  0.160147626,0.255047893,0.04072469;  \
                  0.564535197,0.943435462,0.597070812");
        data = data.t();

        // Use templates to specify that we want a NeighborSearch object which uses
        // the Manhattan distance.
        NeighborSearch<NearestNeighborSort, ManhattanDistance> nn(data);

        // Create the object we will store the nearest neighbors in.
        arma::Mat<size_t> neighbors;
        arma::mat distances; // We need to store the distance too.

        // Compute the neighbors.
        nn.Search(1, neighbors, distances);

        // Write each neighbor and distance using Log.
        for (size_t i = 0; i < neighbors.n_elem; ++i)
        {
            std::cout << "Nearest neighbor of point " << i << " is point "
                << neighbors[i] << " and the distance is " << distances[i] << "." << std::endl;
        }

        //arma::arma_rng::set_seed_random();
        //
        //// Create a 4x4 random matrix and print it on the screen
        //arma::Mat<double> A = arma::randu(4,4);
        //std::cout << "A:\n" << A << "\n";
        //
        //// Multiply A with his transpose:
        //std::cout << "A * A.t() =\n";
        //std::cout << A * A.t() << "\n";
        //
        //// Access/Modify rows and columns from the array:
        //A.row(0) = A.row(1) + A.row(3);
        //A.col(3).zeros();
        //std::cout << "add rows 1 and 3, store result in row 0, also fill 4th column with zeros:\n";
        //std::cout << "A:\n" << A << "\n";
        //
        //// Create a new diagonal matrix using the main diagonal of A:
        //arma::Mat<double>B = arma::diagmat(A);
        //std::cout << "B:\n" << B << "\n";
	}
}


#include "normal.hpp"

#include <gtest/gtest.h>

#include <boost/random/mersenne_twister.hpp>

using namespace MultidimensionalArray;
using namespace ProbabilityDistributions;

TEST(NormalTest, Likelihood) {
  boost::random::mt19937 rng;
  const unsigned int n_samples = 100;
  Normal<double> dist(0, 1);
  Array<double> samples;
  dist.sample(samples, n_samples, rng);

  double likelihood1 = dist.log_likelihood(samples);

  dist.MLE(samples);

  double likelihood2 = dist.log_likelihood(samples);

  EXPECT_GE(likelihood2, likelihood1);

  EXPECT_LT(0, dist.get_sigma());
}

TEST(NormalTest, MLE) {
  boost::random::mt19937 rng;
  const unsigned int n_samples = 100;
  Normal<double> dist(0, 1);
  Array<double> samples;
  dist.sample(samples, n_samples, rng);
  dist.MLE(samples);

  double mu = dist.get_mu(), sigma = dist.get_sigma();
  double eps = 1e-2;
  double ll = dist.log_likelihood(samples);

  dist.set_mu(mu + eps);
  EXPECT_GE(ll, dist.log_likelihood(samples));
  dist.set_mu(mu - eps);
  EXPECT_GE(ll, dist.log_likelihood(samples));
  dist.set_mu(mu);

  dist.set_sigma(sigma + eps);
  EXPECT_GE(ll, dist.log_likelihood(samples));
  dist.set_sigma(sigma - eps);
  EXPECT_GE(ll, dist.log_likelihood(samples));
  dist.set_sigma(sigma);
}

TEST(NormalTest, Samples) {
  boost::random::mt19937 rng;
  const unsigned int n_samples = 100;
  Normal<double> dist(0, 1);
  Array<double> samples;
  dist.sample(samples, n_samples, rng);
  for (size_t i = 0; i < n_samples; i++) {
    EXPECT_LT(-5, samples(i,0));
    EXPECT_GT(5, samples(i,0));
  }
}


#pragma once

#include <stdio.h>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Utils/IO/IOUtilities.hpp>
#include <iostream>

namespace myUtils {

// =============================================================================
// Matrix Utils
// =============================================================================
Eigen::MatrixXd hStack(const Eigen::MatrixXd& a_, const Eigen::MatrixXd& b_);
Eigen::MatrixXd vStack(const Eigen::MatrixXd& a_, const Eigen::MatrixXd& b_);
Eigen::MatrixXd vStack(const Eigen::VectorXd& a_, const Eigen::VectorXd& b_);
Eigen::MatrixXd deleteRow(const Eigen::MatrixXd& a_, int row);

// =============================================================================
// Simple Trajectory Generator
// =============================================================================
double smooth_changing(double ini, double end, double moving_duration,
                       double curr_time);
double smooth_changing_vel(double ini, double end, double moving_duration,
                           double curr_time);
double smooth_changing_acc(double ini, double end, double moving_duration,
                           double curr_time);
void getSinusoidTrajectory(double initTime_, const Eigen::VectorXd& midPoint_,
                           const Eigen::VectorXd& amp_,
                           const Eigen::VectorXd& freq_, double evalTime_,
                           Eigen::VectorXd& p_, Eigen::VectorXd& v_,
                           Eigen::VectorXd& a_);

// =============================================================================
// ETC
// =============================================================================

double computeAlphaGivenBreakFrequency(double hz, double dt);

double bind_half_pi(double);

bool isEqual(const Eigen::VectorXd a, const Eigen::VectorXd b,
             const double threshold = 0.00001);
double CropValue(double value, double min, double max, std::string source);
double CropValue(double value, double min, double max);

Eigen::VectorXd CropVector(Eigen::VectorXd value, Eigen::VectorXd min,
                           Eigen::VectorXd max, std::string source);

Eigen::MatrixXd CropMatrix(Eigen::MatrixXd value, Eigen::MatrixXd min,
                           Eigen::MatrixXd max, std::string source);

bool isInBoundingBox(const Eigen::VectorXd& val, const Eigen::VectorXd& lb,
                     const Eigen::VectorXd& ub);

Eigen::MatrixXd GetRelativeMatrix(const Eigen::MatrixXd value,
                                  const Eigen::MatrixXd min,
                                  const Eigen::MatrixXd max);

Eigen::VectorXd GetRelativeVector(const Eigen::VectorXd value,
                                  const Eigen::VectorXd min,
                                  const Eigen::VectorXd max);

Eigen::VectorXd eulerIntegration(const Eigen::VectorXd& x,
                                 const Eigen::VectorXd& xdot, double dt);

Eigen::VectorXd doubleIntegration(const Eigen::VectorXd& q,
                                  const Eigen::VectorXd& alpha,
                                  const Eigen::VectorXd& alphad, double dt);

Eigen::Matrix3d VecToso3(const Eigen::Vector3d& omg);
Eigen::MatrixXd Adjoint(const Eigen::MatrixXd& R, const Eigen::Vector3d& p);

double QuatToYaw(const Eigen::Quaternion<double> q);

// Euler ZYX
//     Represents either:
//     extrinsic XYZ rotations: Fixed-frame roll, then fixed-frame pitch, then fixed-frame yaw.
//     or intrinsic ZYX rotations: Body-frame yaw, body-frame pitch, then body-frame roll
//
//     The equation is similar, but the values for fixed and body frame rotations are different.
// World Orientation is R = Rz*Ry*Rx
Eigen::Quaterniond EulerZYXtoQuat(const double roll, const double pitch, const double yaw);

// Quaternion to Euler ZYX
Eigen::Vector3d QuatToEulerZYX(const Eigen::Quaterniond & quat_in);

// ZYX extrinsic rotation rates to world angular velocity
// angular vel = [wx, wy, wz]
Eigen::Vector3d EulerZYXRatestoAngVel(const double roll, const double pitch, const double yaw,
                                      const double roll_rate, const double pitch_rate, const double yaw_rate);

void avoid_quat_jump(const Eigen::Quaternion<double> &des_ori, Eigen::Quaternion<double> &act_ori);
}  // namespace myUtils


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

#include <cinttypes>
#include <ciso646>

#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include <boost/mpl/list.hpp>

#include <trng/linear_algebra.hpp>

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

BOOST_AUTO_TEST_SUITE(test_suite_linear_algebra)

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

BOOST_AUTO_TEST_SUITE(test_suite_vector)
BOOST_AUTO_TEST_CASE(test_basic) {
  {
    trng::vector<int, 6> v1{1, 2, 3, 4, 5, 6};
    trng::vector<int, 6> v2{1, 2, 3, 4, 5, 255};
    {
      const bool ok = v1 == v1;
      BOOST_TEST(ok, "vectors are equal");
    }
    {
      const bool ok = v1 != v2;
      BOOST_TEST(ok, "vectors do not equal");
    }
  }
  {
    auto init1 = [](size_t i) { return std::uint8_t(i); };
    auto init2 = [](size_t i) { return std::uint8_t(3 * i); };
    trng::vector<std::uint8_t, 256> v_256_1(init1);
    trng::vector<std::uint8_t, 256> v_256_2(init2);
    trng::vector<std::uint8_t, 256> v_256_3(v_256_1 + v_256_2);
    bool ok{true};
    for (std::size_t i{0}; i < 256; ++i) {
      ok = ok and (static_cast<std::uint8_t>(v_256_1(i) + v_256_2(i)) == v_256_3(i));
    }
    BOOST_TEST(ok, "sum vector ok");
  }
  {
    auto init = [](size_t i) { return std::uint8_t(i); };
    trng::vector<std::uint8_t, 256> v_256_1(init);
    trng::vector<std::uint8_t, 256> v_256_2(std::uint8_t(13) * v_256_1);
    bool ok{true};
    for (std::size_t i{0}; i < 256; ++i) {
      ok = ok and (static_cast<std::uint8_t>(13 * v_256_1(i)) == v_256_2(i));
    }
    BOOST_TEST(ok, "product vector ok");
  }
  {
    auto init = [](size_t i) { return std::uint8_t(i); };
    trng::vector<std::uint8_t, 256> v_256_1(init);
    trng::vector<std::uint8_t, 256> v_256_2(v_256_1 * std::uint8_t(13));
    bool ok{true};
    for (std::size_t i{0}; i < 256; ++i) {
      ok = ok and (static_cast<std::uint8_t>(13 * v_256_1(i)) == v_256_2(i));
    }
    BOOST_TEST(ok, "product vector ok");
  }
  {
    trng::matrix<int, 2> A{1, 3,  //
                           2, 4};
    trng::vector<int, 2> b{3, 7};
    trng::vector<int, 2> c{24, 34};
    auto c2 = A * b;
    const bool ok{c2 == c};
    BOOST_TEST(ok, "matrix vector product ok");
  }
  {
    trng::matrix<int, 2> A{1, 3,  //
                           2, 4};
    trng::matrix<int, 2> B{1, 3,  //
                           2, -4};
    trng::matrix<int, 2> C{7, -9,  //
                           10, -10};
    auto C2 = A * B;
    const bool ok{C2 == C};
    BOOST_TEST(ok, "matrix matrix product ok");
  }
  {
    trng::matrix<int, 2> A{1, 3,  //
                           2, 4};
    trng::matrix<int, 2> A_5{1069, 2337,  //
                             1558, 3406};
    auto A_5_2 = trng::power(A, 5);
    const bool ok{A_5_2 == A_5};
    BOOST_TEST(ok, "matrix matrix power ok");
  }
}
BOOST_AUTO_TEST_SUITE_END()

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

BOOST_AUTO_TEST_SUITE(test_suite_GF2)
BOOST_AUTO_TEST_CASE(test_add) {
  const trng::GF2 zero(false);
  const trng::GF2 one(true);
  BOOST_TEST(zero + zero == zero, "addition 0 + 0");
  BOOST_TEST(zero + one == one, "addition 0 + 1");
  BOOST_TEST(one + zero == one, "addition 1 + 0");
  BOOST_TEST(one + one == zero, "addition 1 + 1");
}

BOOST_AUTO_TEST_CASE(test_mult) {
  const trng::GF2 zero(false);
  const trng::GF2 one(true);
  BOOST_TEST((zero * zero == zero), "multiplication 0 * 0");
  BOOST_TEST((zero * one == zero), "multiplication 0 * 1");
  BOOST_TEST((one * zero == zero), "multiplication 1 * 0");
  BOOST_TEST((one * one == one), "multiplication 1 * 1");
}

BOOST_AUTO_TEST_CASE(test_matrix_power) {
  const trng::GF2 zero(false);
  const trng::GF2 one(true);
  trng::matrix<trng::GF2, 4> A{one,  one, zero, one,   //
                               one,  one, one,  zero,  //
                               one,  one, zero, zero,  //
                               zero, one, zero, one};
  trng::matrix<trng::GF2, 4> A_8{one,  one, zero, one,   //
                                 one,  one, one,  zero,  //
                                 one,  one, zero, zero,  //
                                 zero, one, zero, one};
  auto A_8_2 = trng::power(A, 8);
  const bool ok{A_8_2 == A_8};
  BOOST_TEST(ok, "matrix matrix power in GF2 ok");
}
BOOST_AUTO_TEST_SUITE_END()

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

BOOST_AUTO_TEST_SUITE_END()


//
// Created by rdelfin on 4/21/16.
//

#pragma once

#include <Eigen/Dense>

class MatUtil {
public:
    static Eigen::VectorXd vectorize(const Eigen::MatrixXd& mat);
    static Eigen::MatrixXd matricise(const Eigen::VectorXd& vec, long rows, long cols);
};


// Fractool
#include <fractool/macros.hpp>
#include <fractool/ftcore/generate_julia.hpp>

// Internal
#include <algorithm>
#include <complex>

// External
#include <boost/log/trivial.hpp>

/**
 * Run julia set generation algorithm
 */
void generate_julia(unsigned size_x, unsigned size_y, unsigned char* param_buffer)
{
    // Print message
    BOOST_LOG_TRIVIAL(info) << "Generating julia...";

    // Parameter
    std::complex<float> c(-0.4, 0.6);

    // Helper variables
    float gsc = 4.0;                            // Grid scale
    float x0 = (float)size_x/2;                 // Center x value
    float y0 = (float)size_y/2;                 // Center y value
    float scl = gsc / std::min(size_x, size_y); // Final scale factor
    BOOST_LOG_TRIVIAL(debug) << "Grid scale: " << gsc;
    BOOST_LOG_TRIVIAL(debug) << "Origin Point (x0, y0): (" << x0 << "," << y0 << ")";
    BOOST_LOG_TRIVIAL(debug) << "Final Scale: " << scl;

    // Run iteration algorithm
    std::complex<float> z;
    unsigned char n;
    for (unsigned j = 0; j < size_y; ++j) {
        for (unsigned i = 0; i < size_x; ++i) {
            // Initialize z
            z = std::complex<float>(i - x0, y0 - j) * scl;

            // Iteration
            n = 0;
            while (n < 255 && std::abs(z) < 2) {
                z = z*z + c;
                n += 1;
            }

            // Log output
            BOOST_LOG_TRIVIAL(trace)
                << "Pixel (" << i << "," << j << "): "
                << "c = " << c << " n = " << (int)n;

            // Set parameter
            param_buffer[ARRAY2D(size_x, i, j)] = n;
        }
    }
}


/*
 * Bayes++ the Bayesian Filtering Library
 * Copyright (c) 2002 Michael Stevens
 * See accompanying Bayes++.htm for terms and conditions of use.
 *
 * $Id$
 */

/*
 * Example of using Bayesian Filter Class to solve a simple problem.
 *
 * The example implements a simple quadratic observer.
 *  This tries to estimate the state of system while also trying to
 *  calibrate a simple linear model of the system which includes
 *  a scale factor and a bias.
 *  Estimating both the system state and a scale factor results in a
 *  quadratic (product of two states and therefore non-linear) observation.
 *  The system model is a 1D brownian motion with a known perturbation.
 */

#include "BayesFilter/infFlt.hpp"
#include "Test/random.hpp"
#include <cmath>
#include <iostream>
#include <boost/numeric/ublas/io.hpp>
#include <boost/random.hpp>

namespace
{
	namespace FM = Bayesian_filter_matrix;
	using namespace FM;

	// Choose Filtering Scheme to use
	typedef Bayesian_filter::Information_scheme FilterScheme;

	// Square
	template <class scalar>
	inline scalar sqr(scalar x)
	{
		return x*x;
	}

	// Random numbers from Boost
	Bayesian_filter_test::Boost_random localRng;

	// Constant Dimensions
	const unsigned NX = 3;			// Filter State dimension 	(SystemState, Scale, Bias)

	// Filter Parameters
	// Noise on observing system state
	const Float OBS_NOISE = 0.01;
	// Prediction Noise: no Prediction noise as pertubation is known
	const Float X_NOISE = 0.0;	// System State
	const Float S_NOISE = 0.0;	// Scale
	const Float B_NOISE = 0.0;	// Bias
	// Filter's Initial state uncertainty: System state is unknown
	const Float i_X_NOISE = 1000.;
	const Float i_S_NOISE = 0.1;
	const Float i_B_NOISE = 0.1;

}//namespace

/*
 * Prediction model
 * Linear state predict model with additive control input
 */
class QCpredict : public Bayesian_filter::Linrz_predict_model
{
	Float motion;
	mutable FM::Vec fx;
public:
	QCpredict();
		;
	void predict(const FM::Vec& u)
	{
		motion = u[0];
	}
	const FM::Vec& f(const FM::Vec& x) const
	{
		// Constant scale and bias, system state perturbed by control input
		fx = x;
		fx[0] += motion;
		return fx;
	};
};

QCpredict::QCpredict() : Bayesian_filter::Linrz_predict_model(NX, NX), fx(NX)
{
	FM::identity (Fx);

	// Setup constant noise model: G is identity
	q[0] = sqr(X_NOISE);
	q[1] = sqr(S_NOISE);
	q[2] = sqr(B_NOISE);
	FM::identity (G);
}

/*
 * Quadratic observation model
 */
class QCobserve : public Bayesian_filter::Linrz_uncorrelated_observe_model
{
	mutable FM::Vec z_pred;
public:
	QCobserve ();
	const FM::Vec& h(const FM::Vec& x) const
	{	// Quadratic Observation model
		z_pred[0] = x[0] * x[1] + x[2];
		return z_pred;
	};
	void state (const FM::Vec& x)
	// Linearised model, Jacobian of h at x
	{
		Hx(0,0) = x[1];
		Hx(0,1) = x[0];
		Hx(0,2) = 1.;
	}
};

QCobserve::QCobserve () :
	Bayesian_filter::Linrz_uncorrelated_observe_model(NX,1), z_pred(1)
{
	// Observation Noise variance
	Zv[0] = OBS_NOISE*OBS_NOISE;
}

int main()
{
	// Global setup for test output
	std::cout.flags(std::ios::scientific); std::cout.precision(6);

	// Setup the test filters
	FM::Vec x_true (NX);

	// True State to be observed
	x_true[0] = 10.;	// System State
	x_true[1] = 1.0;	// Scale
	x_true[2] = 0.0;	// Bias

	std::cout << "Quadratic Calibration" << std::endl;
	std::cout << "Init " << x_true << std::endl;

	// Construct Prediction and Observation model and Calibration filter
	QCpredict linearPredict;
	QCobserve nonlinObserve;
	FilterScheme obsAndCalib (NX);

	// Give the filter an true initial guess of the system state
	obsAndCalib.x[0] = x_true[0];
	obsAndCalib.x[1] = 1.;		// Assumed initial Scale
	obsAndCalib.x[2] = 0.;		// Assumed initial Bias
	obsAndCalib.X.clear();
	obsAndCalib.X(0,0) = sqr(i_X_NOISE);
	obsAndCalib.X(1,1) = sqr(i_S_NOISE);
	obsAndCalib.X(2,2) = sqr(i_B_NOISE);

	obsAndCalib.init ();

	// Iterate the filter with test observations
	FM::Vec u(1), z_true(1), z(1);
	for (unsigned i = 0; i < 100; i++ )
	{
		// Predict true state using Brownian control input
		localRng.normal (u);				// normally distributed
		x_true[0] += u[0];
		linearPredict.predict (u);

		// Predict filter with known perturbation
		obsAndCalib.predict (linearPredict);

		// True Observation: Quadratic observation model
		z_true[0] = x_true[0] * x_true[1] + x_true[2];

		// Observation with additive noise
		localRng.normal (z, z_true[0], OBS_NOISE);	// normally distributed mean z_true[0], stdDev OBS_NOISE.

		// Filter observation using model linearised at state estimate x
		nonlinObserve.state (obsAndCalib.x);
		obsAndCalib.observe (nonlinObserve, z);
	}

	// Update the filter to state and covariance are available
	obsAndCalib.update ();

	// Print everything: True, filter, covariance
	std::cout << "True " << x_true <<  std::endl;
	std::cout << "Calb " << obsAndCalib.x << std::endl;
	std::cout << obsAndCalib.X << std::endl;
	return 0;
}


#include <stan/math/rev/scal.hpp>
#include <gtest/gtest.h>
#include <boost/math/special_functions/gamma.hpp>
#include <test/unit/math/rev/scal/fun/nan_util.hpp>
#include <test/unit/math/rev/scal/util.hpp>

TEST(AgradRev,gamma_p_var_var) {
  AVAR a = 0.5;
  AVAR b = 1.0;
  AVAR f = gamma_p(a,b);
  EXPECT_FLOAT_EQ(boost::math::gamma_p(0.5,1.0),f.val());

  AVEC x = createAVEC(a,b);
  VEC g;
  f.grad(x,g);
  EXPECT_FLOAT_EQ(-0.389837, g[0]);
  EXPECT_FLOAT_EQ(boost::math::gamma_p_derivative(0.5,1.0), g[1]);

  a = -0.5;
  EXPECT_THROW(gamma_p(a,b), std::domain_error);

  b = -1.0;
  EXPECT_THROW(gamma_p(a,b), std::domain_error);
}
TEST(AgradRev,gamma_p_double_var) {
  double a = 0.5;
  AVAR b = 1.0;
  AVAR f = gamma_p(a,b);
  EXPECT_FLOAT_EQ(boost::math::gamma_p(0.5,1.0),f.val());

  AVEC x = createAVEC(b);
  VEC g;
  f.grad(x,g);
  EXPECT_FLOAT_EQ(boost::math::gamma_p_derivative(0.5,1.0), g[0]);

  a = -0.5;
  EXPECT_THROW(gamma_p(a,b), std::domain_error);

  b = -1.0;
  EXPECT_THROW(gamma_p(a,b), std::domain_error);
}
TEST(AgradRev,gamma_p_var_double) {
  AVAR a = 0.5;
  double b = 1.0;
  AVAR f = gamma_p(a,b);
  EXPECT_FLOAT_EQ(boost::math::gamma_p(0.5,1.0),f.val());

  AVEC x = createAVEC(a);
  VEC g;
  f.grad(x,g);
  EXPECT_FLOAT_EQ(-0.389837, g[0]);

  a = -0.5;
  EXPECT_THROW(gamma_p(a,b), std::domain_error);

  b = -1.0;
  EXPECT_THROW(gamma_p(a,b), std::domain_error);
}

struct gamma_p_fun {
  template <typename T0, typename T1>
  inline
  typename stan::return_type<T0,T1>::type
  operator()(const T0& arg1,
             const T1& arg2) const {
    return gamma_p(arg1,arg2);
  }
};

TEST(AgradRev, gamma_p_nan) {
  gamma_p_fun gamma_p_;
  test_nan(gamma_p_,0.5,1.0,false,true);
}

TEST(AgradRev, check_varis_on_stack) {
  AVAR a = 0.5;
  AVAR b = 1.0;
  test::check_varis_on_stack(stan::math::gamma_p(a, b));
  test::check_varis_on_stack(stan::math::gamma_p(a, 1.0));
  test::check_varis_on_stack(stan::math::gamma_p(0.5, b));
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// QuickBook Example

// Copyright (c) 2020 Digvijay Janartha, Hamirpur, India.

// Use, modification and distribution is 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)

//[covered_by
//` Checks if the first geometry is inside or on border the second geometry

#include <iostream>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>

namespace bg = boost::geometry; /*< Convenient namespace alias >*/

int main()
{
    // Checks if the first geometry is inside or on border the second geometry.
    bg::model::polygon<bg::model::d2::point_xy<double> > poly1;
    bg::read_wkt("POLYGON((0 2,0 3,2 4,1 2,0 2))", poly1);
    bg::model::polygon<bg::model::d2::point_xy<double> > poly2;
    bg::read_wkt("POLYGON((0 4,3 4,2 2,0 1,0 4))", poly2);
    bool check_covered = bg::covered_by(poly1, poly2);
    if (check_covered) {
         std::cout << "Covered: Yes" << std::endl;
    } else {
        std::cout << "Covered: No" << std::endl;
    }

    bg::model::polygon<bg::model::d2::point_xy<double> > poly3;
    bg::read_wkt("POLYGON((-1 -1,-3 -4,-7 -7,-4 -3,-1 -1))", poly3);
    check_covered = bg::covered_by(poly1, poly3);
    if (check_covered) {
         std::cout << "Covered: Yes" << std::endl;
    } else {
        std::cout << "Covered: No" << std::endl;
    }

    // This should return true since both polygons are same, so they are lying on each other.
    check_covered = bg::covered_by(poly1, poly1);
    if (check_covered) {
         std::cout << "Covered: Yes" << std::endl;
    } else {
        std::cout << "Covered: No" << std::endl;
    }

    return 0;
}

//]

//[covered_by_output
/*`
Output:
[pre
Covered: Yes

[$img/algorithms/covered_by.png]

Covered: No
Covered: Yes
]
*/
//]


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

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  Vector2f v = Vector2f::Random();
JacobiRotation<float> G;
G.makeGivens(v.x(), v.y());
cout << "Here is the vector v:" << endl << v << endl;
v.applyOnTheLeft(0, 1, G.adjoint());
cout << "Here is the vector J' * v:" << endl << v << endl;
  return 0;
}


/*
 * 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)
 */
#include "math_unit_test.hpp"
#include <boost/type_index.hpp>
#include <boost/math/special_functions/chebyshev.hpp>
#include <boost/math/special_functions/chebyshev_transform.hpp>
#include <boost/math/special_functions/sinc.hpp>

#if !defined(TEST1) && !defined(TEST2) && !defined(TEST3) && !defined(TEST4)
#  define TEST1
#  define TEST2
#  define TEST3
#  define TEST4
#endif

using boost::math::chebyshev_t;
using boost::math::chebyshev_t_prime;
using boost::math::chebyshev_u;
using boost::math::chebyshev_transform;

template<class Real>
void test_sin_chebyshev_transform()
{
    using boost::math::chebyshev_transform;
    using boost::math::constants::half_pi;
    using std::sin;
    using std::cos;
    using std::abs;

    Real tol = std::numeric_limits<Real>::epsilon();
    auto f = [](Real x)->Real { return sin(x); };
    Real a = 0;
    Real b = 1;
    chebyshev_transform<Real> cheb(f, a, b, tol);

    Real x = a;
    while (x < b)
    {
        Real s = sin(x);
        Real c = cos(x);
        CHECK_ABSOLUTE_ERROR(s, cheb(x), tol);
        CHECK_ABSOLUTE_ERROR(c, cheb.prime(x), 150*tol);
        x += static_cast<Real>(1)/static_cast<Real>(1 << 7);
    }

    Real Q = cheb.integrate();

    CHECK_ABSOLUTE_ERROR(1 - cos(static_cast<Real>(1)), Q, 100*tol);
}

template<class Real>
void test_sinc_chebyshev_transform()
{
    using std::cos;
    using std::sin;
    using std::abs;
    using boost::math::sinc_pi;
    using boost::math::chebyshev_transform;
    using boost::math::constants::half_pi;

    Real tol = 100*std::numeric_limits<Real>::epsilon();
    auto f = [](Real x) { return boost::math::sinc_pi(x); };
    Real a = 0;
    Real b = 1;
    chebyshev_transform<Real> cheb(f, a, b, tol/50);

    Real x = a;
    while (x < b)
    {
        Real s = sinc_pi(x);
        Real ds = (cos(x)-sinc_pi(x))/x;
        if (x == 0) { ds = 0; }

        CHECK_ABSOLUTE_ERROR(s, cheb(x), tol);
        CHECK_ABSOLUTE_ERROR(ds, cheb.prime(x), 10*tol);
        x += static_cast<Real>(1)/static_cast<Real>(1 << 7);
    }

    Real Q = cheb.integrate();
    //NIntegrate[Sinc[x], {x, 0, 1}, WorkingPrecision -> 200, AccuracyGoal -> 150, PrecisionGoal -> 150, MaxRecursion -> 150]
    Real Q_exp = boost::lexical_cast<Real>("0.94608307036718301494135331382317965781233795473811179047145477356668");
    CHECK_ABSOLUTE_ERROR(Q_exp, Q, tol);
}

//Examples taken from "Approximation Theory and Approximation Practice", by Trefethen
template<class Real>
void test_atap_examples()
{
    using std::sin;
    using std::exp;
    using std::sqrt;
    using boost::math::constants::half;
    using boost::math::sinc_pi;
    using boost::math::chebyshev_transform;
    using boost::math::constants::half_pi;

    Real tol = 10*std::numeric_limits<Real>::epsilon();
    auto f1 = [](Real x) { return ((0 < x) - (x < 0)) - x/2; };
    auto f2 = [](Real x) { Real t = sin(6*x); Real s = sin(x + exp(2*x));
                           Real u = (0 < s) - (s < 0);
                           return t + u; };

    //auto f3 = [](Real x) { return sin(6*x) + sin(60*exp(x)); };
    //auto f4 = [](Real x) { return 1/(1+1000*(x+half<Real>())*(x+half<Real>())) + 1/sqrt(1+1000*(x-Real(1)/Real(2))*(x-Real(1)/Real(2)));};
    Real a = -1;
    Real b = 1;
    chebyshev_transform<Real> cheb1(f1, a, b, tol);
    chebyshev_transform<Real> cheb2(f2, a, b, tol);
    //chebyshev_transform<Real> cheb3(f3, a, b, tol);

    Real x = a;
    while (x < b)
    {
        // f1 and f2 are not differentiable; standard convergence rate theorems don't apply.
        // Basically, the max refinements are always hit; so the error is not related to the precision of the type.
        Real acceptable_error = sqrt(tol);
        Real acceptable_error_2 = 9e-4;
        if (std::is_same<Real, long double>::value)
        {
            acceptable_error = 1.6e-5;
        }
        if (std::is_same<Real, double>::value)
        {
            acceptable_error *= 500;
        }
        CHECK_ABSOLUTE_ERROR(f1(x), cheb1(x), acceptable_error);

        CHECK_ABSOLUTE_ERROR(f2(x), cheb2(x), acceptable_error_2);
        x += static_cast<Real>(1)/static_cast<Real>(1 << 7);
    }
}

//Validate that the Chebyshev polynomials are well approximated by the Chebyshev transform.
template<class Real>
void test_chebyshev_chebyshev_transform()
{
    Real tol = 500*std::numeric_limits<Real>::epsilon();
    // T_0 = 1:
    auto t0 = [](Real) { return 1; };
    chebyshev_transform<Real> cheb0(t0, -1, 1);
    CHECK_ABSOLUTE_ERROR(2, cheb0.coefficients()[0], tol);

    Real x = -1;
    while (x < 1)
    {
        CHECK_ABSOLUTE_ERROR(1, cheb0(x), tol);
        CHECK_ABSOLUTE_ERROR(Real(0), cheb0.prime(x), tol);
        x += static_cast<Real>(1)/static_cast<Real>(1 << 7);
    }

    // T_1 = x:
    auto t1 = [](Real x) { return x; };
    chebyshev_transform<Real> cheb1(t1, -1, 1);
    CHECK_ABSOLUTE_ERROR(Real(1), cheb1.coefficients()[1], tol);

    x = -1;
    while (x < 1)
    {
        CHECK_ABSOLUTE_ERROR(x, cheb1(x), tol);
        CHECK_ABSOLUTE_ERROR(Real(1), cheb1.prime(x), tol);
        x += static_cast<Real>(1)/static_cast<Real>(1 << 7);
    }

    auto t2 = [](Real x) { return 2*x*x-1; };
    chebyshev_transform<Real> cheb2(t2, -1, 1);
    CHECK_ABSOLUTE_ERROR(Real(1), cheb2.coefficients()[2], tol);

    x = -1;
    while (x < 1)
    {
        CHECK_ABSOLUTE_ERROR(t2(x), cheb2(x), tol);
        CHECK_ABSOLUTE_ERROR(4*x, cheb2.prime(x), tol);
        x += static_cast<Real>(1)/static_cast<Real>(1 << 7);
    }
}

int main()
{
#ifdef TEST1
    test_chebyshev_chebyshev_transform<float>();
    test_sin_chebyshev_transform<float>();
    test_atap_examples<float>();
    test_sinc_chebyshev_transform<float>();
#endif
#ifdef TEST2
    test_chebyshev_chebyshev_transform<double>();
    test_sin_chebyshev_transform<double>();
    test_atap_examples<double>();
    test_sinc_chebyshev_transform<double>();
#endif
#ifdef TEST3
    test_chebyshev_chebyshev_transform<long double>();
    test_sin_chebyshev_transform<long double>();
    test_atap_examples<long double>();
    test_sinc_chebyshev_transform<long double>();
#endif
#ifdef TEST4
#ifdef BOOST_HAS_FLOAT128
    test_chebyshev_chebyshev_transform<__float128>();
    test_sin_chebyshev_transform<__float128>();
    test_atap_examples<__float128>();
    test_sinc_chebyshev_transform<__float128>();
#endif
#endif

    return boost::math::test::report_errors();
}


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

using namespace Eigen;
using namespace std;

int main()
{
  MatrixXf m(2,2);
  MatrixXf n(2,2);
  MatrixXf result(2,2);

  m << 1,2,
       3,4;
  n << 5,6,
       7,8;

  result = m * n;
  cout << "-- Matrix m*n: --" << endl << result << endl << endl;
  result = m.array() * n.array();
  cout << "-- Array m*n: --" << endl << result << endl << endl;
  result = m.cwiseProduct(n);
  cout << "-- With cwiseProduct: --" << endl << result << endl << endl;
  result = m.array() + 4;
  cout << "-- Array m + 4: --" << endl << result << endl << endl;
}


/** @file 16.cpp Problem 16: Power digit sum
 *
 * 2**15 = 32768 and the sum of its digits is 3 + 2 + 7 + 6 + 8 = 26.
 *
 * What is the sum of the digits of the number 2**1000?
 */

#include "cpp_int_util.hpp"                 // sum_digits

/// @cond
#include <boost/multiprecision/cpp_int.hpp> // cpp_int

#include <iostream>                         // cout
/// @endcond

using boost::multiprecision::cpp_int;

int sum(int n)
{
    cpp_int i = 1;
    for (int p = 0; p < n; ++p)
        i *= 2;
    return cpp_int_util::sum_digits(i);
}

int main()
{
    assert(sum(15) == 26);
    std::cout << sum(1000) << std::endl;
}


// #define BOOST_TEST_DYN_LINK
// #define BOOST_TEST_MODULE ShockFrontTests
#include <boost/test/unit_test.hpp>

#include <iostream>
#include <memory>

#include "common/services/shockFront.hpp"

namespace cs = ble::src::common::services;

namespace ble::tests::unit_tests::common::services::shock_front {

void case1()
{
    // arrange
    double expected = 0.33355000000000001;
    double kmu = 0.125; // = mw / moil;
    double n = 2.0;

    // act
    double actual = cs::shock_front::get_shock_front(n, kmu);

    // assert
    BOOST_CHECK_CLOSE(expected, actual, 1e-8);
}

void case2()
{
    // arrange
    double expected = 0.8163999999999999; // wxmaxima gives 0.8164965809277261;
    double kmu = 2.0; // = mw / moil;
    double n = 2.0;

    // act
    double actual = cs::shock_front::get_shock_front(n, kmu);

    // assert
    BOOST_CHECK_CLOSE(expected, actual, 1e-8);
}

}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/itl/itl.hpp>

template <typename At, typename Lt, typename Ut>
void dense_ilu_0(const At& As, const Lt& Ls, const Ut& Us)
{
    mtl::dense2D<double> LU(As);

    const std::size_t n= num_rows(LU);
    for (std::size_t i= 1; i < n; i++)
	for (std::size_t k= 0; k < i; k++) {
	    LU[i][k]/= LU[k][k];
	    for (std::size_t j= k + 1; j < n; j++)
		if (LU[i][j] != 0)
		    LU[i][j]-= LU[i][k] * LU[k][j];
	}
    std::cout << "Factorizing A = \n" << As << "-> LU = \n" << LU;
    // std::cout << "L = \n" << Ls << "\nU = \n" << Us;

    if (std::abs(LU[3][2] - Ls[3][2]) > 0.001) throw "Wrong value in L for sparse ILU(0) factorization";

    if (std::abs(LU[3][3] - 1. / Us[3][3]) > 0.001) throw "Wrong value in U for sparse ILU(0) factorization";
}

int main()
{
    // For a more realistic example set sz to 1000 or larger
    const int size = 3, N = size * size;

    typedef mtl::compressed2D<double>  matrix_type;
    mtl::compressed2D<double>          A(N, N), dia(N, N);
    laplacian_setup(A, size, size);
    // dia= 1.0; A+= dia;

    itl::pc::ilu_0<matrix_type>        P(A);
    mtl::dense_vector<double>          x(N, 1.0), b(N);

    if(size > 1 && size < 4)
	dense_ilu_0(A, P.get_L(), P.get_U());

    b = A * x;
    x= 0;

    itl::cyclic_iteration<double> iter(b, N, 1.e-6, 0.0, 5);
    bicgstab(A, x, b, P, iter);

    return 0;
}


// transition_dist.cpp
// (c) 2021-01 David Merrell
//
// Implementation of TransitionDist class
// and its subclasses.

#define __STDCPP_WANT_MATH_SPEC_FUNCS__ 1

#include "transition_dist.h"
#include "contingency_table.h"
#include <cmath>
#include <string>

//// If we're on __linux__, use the std::beta function
//#ifdef __linux__
//
//float my_beta(float a, float b){
//    return std::beta(a,b);
//}
//
//// If we're on __APPLE__ or _WIN32, use the boost::math::beta function
//// (which seems to be slower...)
//#else

#include <boost/math/special_functions/beta.hpp>
float my_beta(float a, float b){
    return boost::math::beta(a,b);
}

//#endif

////////////////////////////////
// Beta distribution
////////////////////////////////

float binom_coeff(int n, int k){
    return 1.0/((n+1.0)*my_beta(n-k+1,k+1));
}

float binom_prob(int N, float p, int x){
    return binom_coeff(N,x) * pow(p,x) * pow(1.0 - p, N-x);
}

std::vector<float> initialize_binom_probs(int N, float p){
    std::vector<float> probs = std::vector<float>(N+1, 0);
    for(int i=0; i < N+1; i++){
        probs[i] = binom_prob(N, p, i);
    }
    return probs;
}

BinomTransitionDist::BinomTransitionDist(float pr_a0, float pr_a1,
                                         float pr_b0, float pr_b1){
    prior_a0 = pr_a0;
    prior_a1 = pr_a1;
    prior_b0 = pr_b0;
    prior_b1 = pr_b1;
}

void BinomTransitionDist::set_state_action(ContingencyTable ct,
                                           short unsigned int a_size,
                                           short unsigned int b_size){
    // compute smoothed point estimates
    float a_smoothing = prior_a0 + prior_a1;
    float b_smoothing = prior_b0 + prior_b1;

    float p_a = (float(ct.a1) + prior_a1) / (float(ct.a0 + ct.a1) + a_smoothing);
    a_probs = initialize_binom_probs(a_size, p_a);

    float p_b = (float(ct.b1) + prior_b1) / (float(ct.b0 + ct.b1) + b_smoothing);
    b_probs = initialize_binom_probs(b_size, p_b);

}

////////////////////////////////
// Beta-Binomial distribution
////////////////////////////////

float beta_binom_prob(int N, float pr_0, float pr_1, int x){
    return binom_coeff(N,x) * my_beta(x+pr_1, N - x + pr_0) / my_beta(pr_1, pr_0);
}

std::vector<float> initialize_beta_binom_probs(int N, float prior_0, float prior_1){
    std::vector<float> probs = std::vector<float>(N+1, 0);
    for(int i=0; i < N+1; i++){
        probs[i] = beta_binom_prob(N, prior_0, prior_1, i);
    }
    return probs;
}

BetaBinomTransitionDist::BetaBinomTransitionDist(float pr_a0, float pr_a1,
                                                 float pr_b0, float pr_b1){
    prior_a0 = pr_a0;
    prior_a1 = pr_a1;
    prior_b0 = pr_b0;
    prior_b1 = pr_b1;
}

void BetaBinomTransitionDist::set_state_action(ContingencyTable ct,
                                          short unsigned int size_a,
                                          short unsigned int size_b){
    a_probs = initialize_beta_binom_probs(size_a,
                                          ct.a0 + prior_a0,
                                          ct.a1 + prior_a1);

    b_probs = initialize_beta_binom_probs(size_b,
                                          ct.b0 + prior_b0,
                                          ct.b1 + prior_b1);
}

/////////////////////////////////
// Factory method
/////////////////////////////////
TransitionDist* TransitionDist::make_transition_dist(std::string tr_dist_type,
                                                     float pr_a0, float pr_a1,
                                                     float pr_b0, float pr_b1){
    if (tr_dist_type == "binom"){
      return new BinomTransitionDist(pr_a0, pr_a1, pr_b0, pr_b1);
    }
    else if(tr_dist_type == "beta_binom"){
      return new BetaBinomTransitionDist(pr_a0, pr_a1, pr_b0, pr_b1);
    }
    else{
      std::cerr << tr_dist_type << " not a valid value for transition distribution." << std::endl;
      throw(1);
    }

}


//
//  EuropeanOption.cpp
//  VI.3 Option Pricing
//
//  Created by Zhehao Li on 2020/4/27.
//  Copyright © 2020 Zhehao Li. All rights reserved.
//

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

/* Private Memeber Functions */
// Call price
double EuropeanOption::CallPrice(double S) const{                   // S is the price of underlying assets
    double tmp = sig * sqrt(T);
    double d1 = ( log(S / K) + (b + (sig * sig) * 0.5 ) * T ) / tmp;
    double d2 = d1 - tmp;

    boost::math::normal_distribution<double> stdNormal(0.0, 1.0);   // Create a normal dstn object

    return ( S * exp((b - r) * T) * cdf(stdNormal, d1) ) - ( K * exp(- r * T) * cdf(stdNormal, d2) );
}

// Put price
double EuropeanOption::PutPrice(double S) const{
    double tmp = sig * sqrt(T);
    double d1 = ( log(S / K) + (b + (sig * sig) * 0.5 ) * T ) / tmp;
    double d2 = d1 - tmp;

    boost::math::normal_distribution<double> stdNormal(0.0, 1.0);   // Create a normal dstn object

    return ( K * exp(- r * T) * cdf(stdNormal, -d2) ) - ( S * exp((b - r) * T) * cdf(stdNormal, -d1) );
}

// Call delta
double EuropeanOption::CallDelta(double S) const{
    double tmp = sig * sqrt(T);
    double d1 = ( log(S / K) + (b + (sig * sig) * 0.5 ) * T ) / tmp;

    boost::math::normal_distribution<double> stdNormal(0.0, 1.0);   // Create a normal dstn object

    return exp((b - r) * T) * cdf(stdNormal, d1);
}

// Put delta
double EuropeanOption::PutDelta(double S) const{
    double tmp = sig * sqrt(T);
    double d1 = ( log(S / K) + (b + (sig * sig) * 0.5 ) * T ) / tmp;

    boost::math::normal_distribution<double> stdNormal(0.0, 1.0);   // Create a normal dstn object

    return exp((b - r) * T) * ( cdf(stdNormal, d1) - 1.0 );
}

// Gamma
double EuropeanOption::CallPutGamma(double S) const{
    double tmp = S * sig * sqrt(T);
    double d1 = ( log(S / K) + (b + (sig * sig) * 0.5 ) * T ) / tmp;

    boost::math::normal_distribution<double> stdNormal(0.0, 1.0);

    return exp((b - r) * T) * pdf(stdNormal, d1) / tmp;
}

/* Public Memeber Functions */
// Default constructor
EuropeanOption::EuropeanOption(): K(110.0), T(0.5), r(0.05), sig(0.2), b(0.05) {
    //
}

// Copy constructor
EuropeanOption::EuropeanOption(const EuropeanOption & option2) : K(option2.K), T(option2.T), r(option2.r), b(option2.b), sig(option2.sig) {
    //
}

// Destructor
EuropeanOption::~EuropeanOption() {}

// Assignment Operators
EuropeanOption & EuropeanOption::operator = (const EuropeanOption & option2) {
    if (this == &option2) {
        return *this;
    }
    else{
        Option::operator=(option2);
        K   = option2.K;
        T   = option2.T;
        r   = option2.r;
        b   = option2.b;
        sig = option2.sig;

        return *this;
    }
}

// Accessing functions
// Get the price of sensitvit of option
double EuropeanOption::getValue(optionOutput output, double S) const{

    switch (output) {
        case value:
            return Value(S);
        case delta:
            return Delta(S);
        case gama:
            return Gamma(S);
        default:
            return 0;
    }
}

// Compute the price of option
double EuropeanOption::Value(double S) const{
    if (optType == "C") {
        return CallPrice(S);
    }
    else{
        return PutPrice(S);
    }
}

// Compute the delta of option
double EuropeanOption::Delta(double S) const{
    if (optType == "C") {
        return CallDelta(S);
    }
    else{
        return PutDelta(S);
    }
}

// Compute the gamma of option
double EuropeanOption::Gamma(double S) const{
    return CallPutGamma(S);
}

// Put-Call Parity
double EuropeanOption::PutCallParity(double S){

    if (optType == "C") {
        double P = CallPrice(S) + K * exp(- r * T) - S;
        return P;
    }
    else{
        double C = PutPrice(S) + S - K * exp(- r * T);
        return C;
    }
}

// Check if Put-Call Party holds
bool EuropeanOption::PutCallParity(double CP, double S){

    double P = PutCallParity(S);

    if (fabs(P - CP) < 1e-4) {
        return true;
    }
    else{
        return false;
    }
}

// Modifier functions
// Change the type of option
void EuropeanOption::toggle(){

    if (optType == "C") {
        optType = "P";
    }
    else{
        optType = "C";
    }
}

void EuropeanOption::setValue(double val, optionInput input){

    switch (input) {
        case strike:
            K = val;
            break;
        case maturity:
            T = val;
            break;
        case rate:
            r = val;
            break;
        case cost:
            b = val;
            break;
        case volatility:
            sig = val;
            break;
        default:
            break;
    }
}


#include "angular_gauss.hpp"

#include <array>
#include <cassert>
#include <cmath>

#include <boost/math/special_functions/erf.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/matrix.hpp>

#include "types/types.hpp"

namespace ublas = boost::numeric::ublas;

void AngularGauss::InvMatrix(const ublas::matrix<double>& input_mat, ublas::matrix<double>& inverse_mat) {
    ublas::matrix<double> A(input_mat);

    ublas::permutation_matrix<std::size_t> pm(A.size1());
    int factorization_status = ublas::lu_factorize(A, pm);
    assert(factorization_status == 0);

    inverse_mat.assign(ublas::identity_matrix<double>(A.size1()));
    lu_substitute(A, pm, inverse_mat);
}

double AngularGauss::Det(const ublas::matrix<double>& m) const {
    assert(m.size1() == 3 && m.size2() == 3);

    const double a = m(0, 0);
    const double b = m(0, 1);
    const double c = m(0, 2);
    const double d = m(1, 0);
    const double e = m(1, 1);
    const double f = m(1, 2);
    const double g = m(2, 0);
    const double h = m(2, 1);
    const double k = m(2, 2);

    const double determinant =
        (a * ((e * k) - (f * h))) - (b * ((k * d) - (f * g))) + (c * ((d * h) - (e * g)));

    return determinant;
}

double AngularGauss::InnerProduct(const Vector& x, const Vector& y) const {
    double result = 0;
    for (int i = 0; i < 3; ++i)
        for (int j = 0; j < 3; ++j)
            result += lambda(i, j) * x.at(i) * y.at(j);

    return result;
}

AngularGauss::AngularGauss(const Vector& mean_vec, const ublas::matrix<double>& cov_mat) {
    assert(cov_mat.size1() == 3 && cov_mat.size2() == 3);

    std::copy(mean_vec.begin(), mean_vec.end(), mean.begin());
    lambda = ublas::matrix<double>(3,3);
    InvMatrix(cov_mat, lambda);
    det_lambda = Det(lambda);
    mean_norm  = std::sqrt(InnerProduct(mean, mean));
}

double AngularGauss::Calc(const CoordsOfPoint& u) const {
    double u_norm = std::sqrt(InnerProduct(u, u));
    double z      = InnerProduct(mean, u) / u_norm;

    double coeff = std::exp(-0.5 * mean_norm * mean_norm) * std::sqrt(det_lambda) /
                   (4 * M_PI * u_norm * u_norm * u_norm);

    return coeff * (z * std::sqrt(M_2_PI) +
                    std::exp(0.5 * z * z) * (1 + z * z) * (1 + boost::math::erf(z / M_SQRT2)));
}

const std::array<double, 3> AngularGauss::Mean(void) const {
    return std::array<double, 3>{mean};
}


#include <iostream>
#include <vector>
#include <queue>
#include <algorithm>
#include <boost/timer.hpp>
#include <cassert>

// O(NlogN)
int kthMin1(std::vector<int>& data, int k) {
    std::sort(data.begin(), data.end());

    return data[k - 1];
}

// O(NlogN)
int kthMin2(const std::vector<int>& data, int k) {
    std::priority_queue<int> pq;
    for (const auto& num : data) {
        pq.push(num);
        if (pq.size() > k) {
            pq.pop();
        }
    }

    return pq.top();
}

// O(N + klogN)
int kthMin3(std::vector<int>& data, int k) {
    std::make_heap(data.begin(), data.end(), std::greater<>());

    for (int i = k - 1; i > 0; i--) {
        std::pop_heap(data.begin(), data.end(), std::greater<>());
        data.pop_back();
    }

    return data.front();
}

int partition(std::vector<int>& data, int low, int high) {
    int i = low;
    int j = high + 1;
    int pivot = data[low];
    while (true) {
        while (data[++i] < pivot) {
            if (i == high) break;
        }

        while (data[--j] > pivot) {
            if (j == low) break;
        }

        if (i >= j) break;

        std::swap(data[i], data[j]);
    }

    std::swap(data[low], data[j]);

    return j;
}

// O(N) average case
int kthMin4(std::vector<int>& data, int k) {
    k--;
    int low = 0;
    int high = data.size() - 1;
    while (high > low) {
        int middle = partition(data, low, high);
        if (middle == k) return data[k];
        else if (middle > k) high = middle - 1;
        else low = middle + 1;
    }

    return data[low];
}

std::vector<std::vector<int>> createVectors(int row, int col) {
    static std::uniform_int_distribution<int> distribution(
            std::numeric_limits<int>::min(),
            std::numeric_limits<int>::max()
    );
    static std::default_random_engine generator;

    std::vector<int> data(col);
    std::generate(data.begin(), data.end(),
                  []() { return distribution(generator); });

    return {row, data};
}

int main() {
    const int NUMS_LENGTH = 100;
    auto test_cases = createVectors(4, NUMS_LENGTH);

    std::random_device device;
    std::default_random_engine engine(device());
    std::uniform_int_distribution<int> dist(1, 100);

    const int K = dist(engine);

    boost::timer t1;
    int k1 = kthMin1(test_cases[0], K);
    std::cout << t1.elapsed() * 1000 << std::endl;

    boost::timer t2;
    int k2 = kthMin2(test_cases[1], K);
    std::cout << t2.elapsed() * 1000 << std::endl;

    boost::timer t3;
    int k3 = kthMin3(test_cases[2], K);
    std::cout << t3.elapsed() * 1000 << std::endl;

    boost::timer t4;
    int k4 = kthMin4(test_cases[3], K);
    std::cout << t4.elapsed() * 1000 << std::endl;

    assert(k1 == k2 && k2 == k3 && k3 == k4);

    return 0;
}


#include <gtest/gtest.h>

#include "helpers.hh"

#include <Eigen/Core>
#include <Eigen/Geometry>

using namespace std;
using namespace Eigen;

const Vector3d origin(0, 0, 0);

TEST (EigenTests, transforms)
{
  // Combine multiple translations, rotations (and scalings) by multiplying
  // them together.

  // Rotate around X by 90 degrees, then transform 1 unit along +Z (which is
  // now pointing along -Y from the original frame due to the rotation.)

  Affine3d transform(AngleAxisd(M_PI/2, Vector3d::UnitX()) * Translation3d(0, 0, 1));

  // Use the right-hand-rule to determine the direction of rotations.

  // Matrix multiplication is not commutative. Multiplying a set of transforms
  // in different orders gives different results.

  // Calculate the transform's effect on a point in space by multiplying the
  // transform by the vector.

  // Eigen's vector types are columns. They may only be post-multiplied with
  // transformation matrices.

  // Even though a transformation matrix may be both pre and post multiplied
  // with a vector, Eigen only supports the latter.
  // You multiply T*V. V*T will not compile.

  EXPECT_TRUE ( VectorsEqual ( Vector3d(0, -1, 0), transform * origin ) );

  // The transform not only moves points, but rotates them.
  // Transforming non-origin values will demonstrate this.

  EXPECT_TRUE ( VectorsEqual ( Vector3d(0, -1, 1), transform * Vector3d::UnitY() ) );

  // It's more useful to think of transforms as operating upon coordinate
  // frames rather than points. Transforms map points in one coordinate space
  // to another.

  // Consider a transform from the agent's torso frame to the frame of the
  // left ankle. This simple example only involves translation, but the following
  // discussion holds in the presence of rotations too.

  Affine3d torsoLeftAnkle(Translation3d(
    -0.074/2,
    -0.005,
    -0.1222 - 0.093 - 0.093));

  // Multiplying by the origin gives:

  EXPECT_TRUE ( VectorsEqual ( Vector3d(-0.074/2, -0.005, -0.1222 - 0.093 - 0.093),
                               torsoLeftAnkle * origin ) );

  // In this case, the vector multiplied should be considered in the 'ankle' frame.
  // The result is position of the same point in the 'torso' frame.

  // A convention we use in the code is to name transforms "AB", where A is one
  // space, and B is another.

  // In this way:
  //   AB * Vb = Va

  // `torsoLeftAnkle * origin` gives the position of the ankle's origin in
  // the torso's frame:

  // A transform may be inverted.
  //   BA = AB.inverse()

  Affine3d leftAnkleTorso(torsoLeftAnkle.inverse());

  // `leftAnkleTorso * origin` gives the position of the torso's origin in
  // the left ankle's frame:

  EXPECT_TRUE ( VectorsEqual ( Vector3d(0.074/2, 0.005, 0.1222 + 0.093 + 0.093),
                               leftAnkleTorso * origin ) );

  // The transform may be deconstructed into translation and rotation components

  EXPECT_TRUE ( VectorsEqual ( Vector3d(0, -1, 0), transform.translation() ) );

  EXPECT_TRUE ( MatricesEqual ( AngleAxisd(M_PI/2, Vector3d::UnitX()).matrix(),
                                transform.rotation() ) );
}


/*
 * 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 chebyshev_transform_test

#include <boost/cstdfloat.hpp>
#include <boost/type_index.hpp>
#include <boost/test/included/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/math/special_functions/chebyshev.hpp>
#include <boost/math/special_functions/chebyshev_transform.hpp>
#include <boost/math/special_functions/sinc.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>

#if !defined(TEST1) && !defined(TEST2) && !defined(TEST3) && !defined(TEST4)
#  define TEST1
#  define TEST2
#  define TEST3
#  define TEST4
#endif

using boost::multiprecision::cpp_bin_float_quad;
using boost::multiprecision::cpp_bin_float_50;
using boost::multiprecision::cpp_bin_float_100;
using boost::math::chebyshev_t;
using boost::math::chebyshev_t_prime;
using boost::math::chebyshev_u;
using boost::math::chebyshev_transform;

template<class Real>
void test_sin_chebyshev_transform()
{
    using boost::math::chebyshev_transform;
    using boost::math::constants::half_pi;
    using std::sin;
    using std::cos;
    using std::abs;

    Real tol = 10*std::numeric_limits<Real>::epsilon();
    auto f = [](Real x) { return sin(x); };
    Real a = 0;
    Real b = 1;
    chebyshev_transform<Real> cheb(f, a, b, tol);

    Real x = a;
    while (x < b)
    {
        Real s = sin(x);
        Real c = cos(x);
        if (abs(s) < tol)
        {
            BOOST_CHECK_SMALL(cheb(x), 100*tol);
            BOOST_CHECK_CLOSE_FRACTION(c, cheb.prime(x), 100*tol);
        }
        else
        {
            BOOST_CHECK_CLOSE_FRACTION(s, cheb(x), 100*tol);
            if (abs(c) < tol)
            {
                BOOST_CHECK_SMALL(cheb.prime(x), 100*tol);
            }
            else
            {
                BOOST_CHECK_CLOSE_FRACTION(c, cheb.prime(x), 100*tol);
            }
        }
        x += static_cast<Real>(1)/static_cast<Real>(1 << 7);
    }

    Real Q = cheb.integrate();

    BOOST_CHECK_CLOSE_FRACTION(1 - cos(static_cast<Real>(1)), Q, 100*tol);
}

template<class Real>
void test_sinc_chebyshev_transform()
{
    using std::cos;
    using std::sin;
    using std::abs;
    using boost::math::sinc_pi;
    using boost::math::chebyshev_transform;
    using boost::math::constants::half_pi;

    Real tol = 500*std::numeric_limits<Real>::epsilon();
    auto f = [](Real x) { return boost::math::sinc_pi(x); };
    Real a = 0;
    Real b = 1;
    chebyshev_transform<Real> cheb(f, a, b, tol/50);

    Real x = a;
    while (x < b)
    {
        Real s = sinc_pi(x);
        Real ds = (cos(x)-sinc_pi(x))/x;
        if (x == 0) { ds = 0; }
        if (s < tol)
        {
            BOOST_CHECK_SMALL(cheb(x), tol);
        }
        else
        {
            BOOST_CHECK_CLOSE_FRACTION(s, cheb(x), tol);
        }

        if (abs(ds) < tol)
        {
            BOOST_CHECK_SMALL(cheb.prime(x), 5 * tol);
        }
        else
        {
            BOOST_CHECK_CLOSE_FRACTION(ds, cheb.prime(x), 300*tol);
        }
        x += static_cast<Real>(1)/static_cast<Real>(1 << 7);
    }

    Real Q = cheb.integrate();
    //NIntegrate[Sinc[x], {x, 0, 1}, WorkingPrecision -> 200, AccuracyGoal -> 150, PrecisionGoal -> 150, MaxRecursion -> 150]
    Real Q_exp = boost::lexical_cast<Real>("0.94608307036718301494135331382317965781233795473811179047145477356668");
    BOOST_CHECK_CLOSE_FRACTION(Q_exp, Q, tol);
}

//Examples taken from "Approximation Theory and Approximation Practice", by Trefethen
template<class Real>
void test_atap_examples()
{
    using std::sin;
    using boost::math::constants::half;
    using boost::math::sinc_pi;
    using boost::math::chebyshev_transform;
    using boost::math::constants::half_pi;

    Real tol = 10*std::numeric_limits<Real>::epsilon();
    auto f1 = [](Real x) { return ((0 < x) - (x < 0)) - x/2; };
    auto f2 = [](Real x) { Real t = sin(6*x); Real s = sin(x + exp(2*x));
                           Real u = (0 < s) - (s < 0);
                           return t + u; };

    auto f3 = [](Real x) { return sin(6*x) + sin(60*exp(x)); };

    auto f4 = [](Real x) { return 1/(1+1000*(x+half<Real>())*(x+half<Real>())) + 1/sqrt(1+1000*(x-.5)*(x-0.5));};
    Real a = -1;
    Real b = 1;
    chebyshev_transform<Real> cheb1(f1, a, b);
    chebyshev_transform<Real> cheb2(f2, a, b, tol);
    //chebyshev_transform<Real> cheb3(f3, a, b, tol);

    Real x = a;
    while (x < b)
    {
        //Real s = f1(x);
        if (sizeof(Real) == sizeof(float))
        {
           BOOST_CHECK_CLOSE_FRACTION(f1(x), cheb1(x), 4e-3);
        }
        else
        {
           BOOST_CHECK_CLOSE_FRACTION(f1(x), cheb1(x), 1.3e-5);
        }
        BOOST_CHECK_CLOSE_FRACTION(f2(x), cheb2(x), 5e-3);
        //BOOST_CHECK_CLOSE_FRACTION(f3(x), cheb3(x), 100*tol);
        x += static_cast<Real>(1)/static_cast<Real>(1 << 7);
    }
}

//Validate that the Chebyshev polynomials are well approximated by the Chebyshev transform.
template<class Real>
void test_chebyshev_chebyshev_transform()
{
    Real tol = 500*std::numeric_limits<Real>::epsilon();
    // T_0 = 1:
    auto t0 = [](Real) { return 1; };
    chebyshev_transform<Real> cheb0(t0, -1, 1);
    BOOST_CHECK_CLOSE_FRACTION(cheb0.coefficients()[0], 2, tol);

    Real x = -1;
    while (x < 1)
    {
        BOOST_CHECK_CLOSE_FRACTION(cheb0(x), 1, tol);
        BOOST_CHECK_SMALL(cheb0.prime(x), tol);
        x += static_cast<Real>(1)/static_cast<Real>(1 << 7);
    }

    // T_1 = x:
    auto t1 = [](Real x) { return x; };
    chebyshev_transform<Real> cheb1(t1, -1, 1);
    BOOST_CHECK_CLOSE_FRACTION(cheb1.coefficients()[1], 1, tol);

    x = -1;
    while (x < 1)
    {
        if (x == 0)
        {
            BOOST_CHECK_SMALL(cheb1(x), tol);
        }
        else
        {
            BOOST_CHECK_CLOSE_FRACTION(cheb1(x), x, tol);
        }
        BOOST_CHECK_CLOSE_FRACTION(cheb1.prime(x), 1, tol);
        x += static_cast<Real>(1)/static_cast<Real>(1 << 7);
    }

    auto t2 = [](Real x) { return 2*x*x-1; };
    chebyshev_transform<Real> cheb2(t2, -1, 1);
    BOOST_CHECK_CLOSE_FRACTION(cheb2.coefficients()[2], 1, tol);

    x = -1;
    while (x < 1)
    {
        BOOST_CHECK_CLOSE_FRACTION(cheb2(x), t2(x), tol);
        if (x != 0)
        {
            BOOST_CHECK_CLOSE_FRACTION(cheb2.prime(x), 4*x, tol);
        }
        else
        {
            BOOST_CHECK_SMALL(cheb2.prime(x), tol);
        }
        x += static_cast<Real>(1)/static_cast<Real>(1 << 7);
    }
}

BOOST_AUTO_TEST_CASE(chebyshev_transform_test)
{
#ifdef TEST1
    test_chebyshev_chebyshev_transform<float>();
    test_sin_chebyshev_transform<float>();
    test_atap_examples<float>();
    test_sinc_chebyshev_transform<float>();
#endif
#ifdef TEST2
    test_chebyshev_chebyshev_transform<double>();
    test_sin_chebyshev_transform<double>();
    test_atap_examples<double>();
    test_sinc_chebyshev_transform<double>();
#endif
#ifdef TEST3
    test_chebyshev_chebyshev_transform<long double>();
    test_sin_chebyshev_transform<long double>();
    test_atap_examples<long double>();
    test_sinc_chebyshev_transform<long double>();
#endif
#ifdef TEST4
#ifdef BOOST_HAS_FLOAT128
    test_chebyshev_chebyshev_transform<__float128>();
    test_sin_chebyshev_transform<__float128>();
    test_atap_examples<__float128>();
    test_sinc_chebyshev_transform<__float128>();
#endif
#endif
}


/***********************************************************************
This file is part of the librjmcmc project source files.

Copyright : Institut Geographique National (2008-2012)
Contributors : Mathieu Brédif, Olivier Tournaire, Didier Boldo
email : librjmcmc@ign.fr

This software is a generic C++ library for stochastic optimization.

This software is governed by the CeCILL license under French law and
abiding by the rules of distribution of free software. You can use,
modify and/or redistribute the software under the terms of the CeCILL
license as circulated by CEA, CNRS and INRIA at the following URL
"http://www.cecill.info".

As a counterpart to the access to the source code and rights to copy,
modify and redistribute granted by the license, users are provided only
with a limited warranty and the software's author, the holder of the
economic rights, and the successive licensors have only limited liability.

In this respect, the user's attention is drawn to the risks associated
with loading, using, modifying and/or developing or reproducing the
software by the user in light of its specific status of free software,
that may mean that it is complicated to manipulate, and that also
therefore means that it is reserved for developers and experienced
professionals having in-depth computer knowledge. Users are therefore
encouraged to load and test the software's suitability as regards their
requirements in conditions enabling the security of their systems and/or
data to be ensured and, more generally, to use and operate it in the
same conditions as regards security.

The fact that you are presently reading this means that you have had
knowledge of the CeCILL license and that you accept its terms.

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

#ifndef __GAMMA_DISTRIBUTION_HPP__
#define __GAMMA_DISTRIBUTION_HPP__

#include <boost/random/gamma_distribution.hpp>
#include <boost/math/distributions/gamma.hpp>

namespace rjmcmc {

    // beta^alpha * x^(alpha-1) * exp(-beta * x) / Gamma(alpha)
    class gamma_distribution {
    public:
        typedef double real_type;
        typedef boost::gamma_distribution<real_type>          rand_distribution_type;
        typedef boost::math::gamma_distribution<real_type>    math_distribution_type;

        gamma_distribution(real_type alpha, real_type beta)
            : m_rand(alpha,beta)
            , m_math(alpha,1./beta)
        {}

        // new/old: (mean^(n1-n0) * n0! / n1!
        real_type pdf_ratio(real_type x0, real_type x1) const
        {
            return pow(x1/x0,m_rand.alpha()-1)*exp(m_rand.beta()*(x0-x1));
        }

        real_type pdf(real_type x) const
        {
            return boost::math::pdf(m_math, x);
        }

        template<typename Engine>
        inline real_type operator()(Engine& e) const { return m_rand(e); }

    private:
        mutable rand_distribution_type m_rand;
        math_distribution_type m_math;
    };

}; // namespace rjmcmc

#endif // __GAMMA_DISTRIBUTION_HPP__


#include <dlib/global_optimization.h>
#include <dlib/matrix.h>
#include <dlib/svm.h>

#include <plot.h>

#include <iostream>
#include <random>
#include <vector>

using SampleType = dlib::matrix<double, 1, 1>;
using Samples = std::vector<SampleType>;
using Labels = std::vector<SampleType>;

Samples LinSpace(double s, double e, size_t n) {
  Samples x_values(n);
  double step = (e - s) / n;
  double v = s;
  for (size_t i = 0; i < n; ++i) {
    x_values[i](0, 0) = v;
    v += step;
  }
  x_values[n - 1] = e;

  return x_values;
}

std::pair<Samples, Labels> GenerateData(size_t num_samples, size_t seed) {
  Samples x_values(num_samples);
  Labels y_values(num_samples);

  std::mt19937 re(seed);
  std::normal_distribution<double> dist;

  for (size_t i = 0; i < num_samples; ++i) {
    auto x_val = dist(re);
    x_values[i](0, 0) = x_val;

    auto y_val = std::cos(M_PI * x_val) + (dist(re) * 0.3);
    y_values[i](0, 0) = y_val;
  }
  return {x_values, y_values};
}

int main(int /*argc*/, char** /*argv*/) {
  using namespace dlib;

  // Generate data
  Samples samples;
  Labels labels;
  std::tie(samples, labels) = GenerateData(1000, 435635);

  auto mm = std::minmax_element(
      samples.begin(), samples.end(),
      [](const auto& a, const auto& b) { return a(0, 0) < b(0, 0); });
  std::pair<double, double> x_minmax{*mm.first, *mm.second};

  // Normalize data
  vector_normalizer<SampleType> x_normalizer;
  x_normalizer.train(samples);
  std::for_each(samples.begin(), samples.end(), x_normalizer);

  vector_normalizer<SampleType> y_normalizer;
  y_normalizer.train(labels);
  std::for_each(labels.begin(), labels.end(), y_normalizer);

  std::vector<double> raw_labels(labels.size());
  for (size_t i = 0; i < labels.size(); ++i) {
    raw_labels[i] = labels[i](0, 0);
  }

  // Randomize data
  randomize_samples(samples, raw_labels);

  // Define cross validation function
  auto CrossValidationScore = [&](const double gamma, const double c,
                                  const double degree_in) {
    auto degree = std::floor(degree_in);
    using KernelType = dlib::polynomial_kernel<SampleType>;
    dlib::svr_trainer<KernelType> trainer;
    trainer.set_kernel(KernelType(gamma, c, degree));

    std::cout << "gamma: " << std::setw(11) << gamma << "  c: " << std::setw(11)
              << c << "  degree: " << std::setw(11) << degree;
    std::cout.flush();

    dlib::matrix<double> result = dlib::cross_validate_regression_trainer(
        trainer, samples, raw_labels, 10);
    std::cout << std::setw(11) << "  MSE: " << result(0, 0) << std::setw(11)
              << "  MAE: " << result(0, 2) << std::endl;

    return result(0, 0);
  };

  // Search for the best parameters
  auto result = find_min_global(
      CrossValidationScore,
      {0.01, 1e-8, 5},  // minimum values for gamma, c, and degree
      {0.1, 1, 15},     // maximum values for gamma, c, and degree
      max_function_calls(50));

  double gamma = result.x(0);
  double c = result.x(1);
  double degree = result.x(2);

  std::cout << " best cross validation score: " << result.y << std::endl;
  std::cout << " best gamma: " << gamma << "   best c: " << c
            << "    best degree: " << degree << std::endl;

  // Train model
  using KernelType = dlib::polynomial_kernel<SampleType>;
  dlib::svr_trainer<KernelType> trainer;
  trainer.set_kernel(KernelType(gamma, c, degree));
  auto descision_func = trainer.train(samples, raw_labels);

  // Plot perdictions
  auto new_samples = LinSpace(x_minmax.first, x_minmax.second, 50);
  std::for_each(new_samples.begin(), new_samples.end(), x_normalizer);

  std::vector<double> predictions(new_samples.size());
  std::transform(new_samples.begin(), new_samples.end(), predictions.begin(),
                 descision_func);

  plotcpp::Plot plt;
  plt.SetTerminal("png");
  plt.SetOutput("plot.png");
  plt.SetTitle("SVM Polynomial kernel regression");
  plt.SetXLabel("x");
  plt.SetYLabel("y");
  plt.SetAutoscale();
  plt.GnuplotCommand("set grid");

  std::vector<double> x_coords(samples.size());
  std::transform(samples.begin(), samples.end(), x_coords.begin(),
                 [](auto& m) { return m(0, 0); });

  std::vector<double> x_pred_coords(new_samples.size());
  std::transform(new_samples.begin(), new_samples.end(), x_pred_coords.begin(),
                 [](auto& m) { return m(0, 0); });

  plt.Draw2D(plotcpp::Points(x_coords.begin(), x_coords.end(),
                             raw_labels.begin(), "orig", "lc rgb 'black' pt 7"),
             plotcpp::Lines(x_pred_coords.begin(), x_pred_coords.end(),
                            predictions.begin(), "pred", "lc rgb 'red' lw 2"));
  plt.Flush();

  return 0;
}