///////////////////////////////////////////////////////////////////
//  Copyright Christopher Kormanyos 2021.                        //
//  Distributed under the Boost Software License,                //
//  Version 1.0. (See accompanying file LICENSE_1_0.txt          //
//  or copy at http://www.boost.org/LICENSE_1_0.txt)             //
///////////////////////////////////////////////////////////////////

#include <cmath>

#if defined(__GNUC__)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wconversion"
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wsign-conversion"
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-parameter"
#endif

#if defined(__clang__) && !defined(__APPLE__)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-copy"
#endif

#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/bindings/decwide_t.hpp>

#include <math/wide_decimal/decwide_t_examples.h>

namespace math { namespace wide_decimal {

namespace detail {

template<typename FloatingPointType>
FloatingPointType sin_series(const FloatingPointType& x)
{
        FloatingPointType term        = x;
  const FloatingPointType x2          = x * x;
        FloatingPointType sum         = x;
        bool              term_is_neg = true;
  const FloatingPointType tol         = std::numeric_limits<FloatingPointType>::epsilon() * x;

  for(std::uint32_t k = 3U; k < UINT32_C(0xFFFF); k += 2U)
  {
    term *= x2;
    term /= std::uint32_t(k * std::uint32_t(k - 1U));

    if(term < tol)
    {
      break;
    }

    term_is_neg ? sum -= term : sum += term;

    term_is_neg = (!term_is_neg);
  }

  return sum;
}

template<typename FloatingPointType>
FloatingPointType cos_series(const FloatingPointType& x)
{
  const FloatingPointType x2          = x * x;
        FloatingPointType term        = x2 / 2U;
        FloatingPointType sum         = term;
        bool              term_is_neg = true;
  const FloatingPointType tol         = std::numeric_limits<FloatingPointType>::epsilon() * x;

  for(std::uint32_t k = 4U; k < UINT32_C(0xFFFF); k += 2U)
  {
    term *= x2;
    term /= std::uint32_t(k * std::uint32_t(k - 1U));

    if(term < tol)
    {
      break;
    }

    term_is_neg ? sum -= term : sum += term;

    term_is_neg = (!term_is_neg);
  }

  return 1U - sum;
}

}

template<typename FloatingPointType>
FloatingPointType sin(const FloatingPointType& x)
{
  using floating_point_type = FloatingPointType;

  floating_point_type s;

  if(x < 0)
  {
    s = -sin(-x);
  }
  else if(x > 0)
  {
    // Perform argument reduction and subsequent scaling of the result.

    // Given x = k * (pi/2) + r, compute n = (k % 4).

    // | n |  sin(x) |  cos(x) |  sin(x)/cos(x) |
    // |----------------------------------------|
    // | 0 |  sin(r) |  cos(r) |  sin(r)/cos(r) |
    // | 1 |  cos(r) | -sin(r) | -cos(r)/sin(r) |
    // | 2 | -sin(r) | -cos(r) |  sin(r)/cos(r) |
    // | 3 | -cos(r) |  sin(r) | -cos(r)/sin(r) |

    const std::uint32_t      k = (std::uint32_t) (x / boost::math::constants::half_pi<floating_point_type>());
    const std::uint_fast32_t n = (std::uint_fast32_t) (k % 4U);

    floating_point_type r = x - (boost::math::constants::half_pi<floating_point_type>() * k);

    const bool is_neg =  (n > 1U);
    const bool is_cos = ((n == 1U) || (n == 3U));

    std::uint_fast32_t n_angle_identity = 0U;

    static const floating_point_type one_tenth = floating_point_type(1U) / 10U;

    // Reduce the argument with factors of three until it is less than 1/10.
    while(r > one_tenth)
    {
      r /= 3U;

      ++n_angle_identity;
    }

    s = (is_cos ? detail::cos_series(r) : detail::sin_series(r));

    // Rescale the result with the triple angle identity for sine.
    for(std::uint_fast32_t t = 0U; t < n_angle_identity; ++t)
    {
      s = (s * 3U) - (((s * s) * s) * 4U);
    }

    s = fabs(s);

    if(is_neg)
    {
      s = -s;
    }
  }
  else
  {
    s = 0;
  }

  return s;
}

template<typename FloatingPointType>
FloatingPointType cos(const FloatingPointType& x)
{
  using floating_point_type = FloatingPointType;

  floating_point_type c;

  if(x < 0)
  {
    c = cos(-x);
  }
  else if(x > 0)
  {
    // Perform argument reduction and subsequent scaling of the result.

    // Given x = k * (pi/2) + r, compute n = (k % 4).

    // | n |  sin(x) |  cos(x) |  sin(x)/cos(x) |
    // |----------------------------------------|
    // | 0 |  sin(r) |  cos(r) |  sin(r)/cos(r) |
    // | 1 |  cos(r) | -sin(r) | -cos(r)/sin(r) |
    // | 2 | -sin(r) | -cos(r) |  sin(r)/cos(r) |
    // | 3 | -cos(r) |  sin(r) | -cos(r)/sin(r) |

    const std::uint32_t      k = (std::uint32_t) (x / boost::math::constants::half_pi<floating_point_type>());
    const std::uint_fast32_t n = (std::uint_fast32_t) (k % 4U);

    floating_point_type r = x - (boost::math::constants::half_pi<floating_point_type>() * k);

    const bool is_neg = ((n == 1U) || (n == 2U));
    const bool is_sin = ((n == 1U) || (n == 3U));

    std::uint_fast32_t n_angle_identity = 0U;

    static const floating_point_type one_tenth = floating_point_type(1U) / 10U;

    // Reduce the argument with factors of three until it is less than 1/10.
    while(r > one_tenth)
    {
      r /= 3U;

      ++n_angle_identity;
    }

    c = (is_sin ? detail::sin_series(r) : detail::cos_series(r));

    // Rescale the result with the triple angle identity for cosine.
    for(std::uint_fast32_t t = 0U; t < n_angle_identity; ++t)
    {
      c = (((c * c) * c) * 4U) - (c * 3U);
    }

    c = fabs(c);

    if(is_neg)
    {
      c = -c;
    }
  }
  else
  {
    c = floating_point_type(1U);
  }

  return c;
}

} }

namespace local
{
  template<class T>
  bool test_tgamma()
  {
     // N[Gamma[5/2], 120]
     const T control_tgamma_2_and_half("1.32934038817913702047362561250585888709816209209179034616035584238968346344327413603121299255390849906217011771821192800");

     // N[Gamma[5000/2], 120]
     const T control_tgamma_2_fifty   ("1.29314250436364309292832582080974738839793748706951226669917697084512949902204448379552716614841127978037140294127577317E490");

     const T tgamma_2_and_half = boost::math::tgamma(T(T(5)   / 2));
     const T tgamma_2_fifty    = boost::math::tgamma(T(T(500) / 2));

     const T closeness_2_and_half = fabs(1 - fabs(tgamma_2_and_half / control_tgamma_2_and_half));
     const T closeness_2_fifty    = fabs(1 - fabs(tgamma_2_fifty    / control_tgamma_2_fifty));

     const bool result_is_ok_2_and_half = (closeness_2_and_half < std::numeric_limits<T>::epsilon() * T(1.0E5L));
     const bool result_is_ok_2_fifty    = (closeness_2_fifty    < std::numeric_limits<T>::epsilon() * T(1.0E5L));

     const bool result_is_ok = (result_is_ok_2_and_half && result_is_ok_2_fifty);

     return result_is_ok;
  }
}

bool math::wide_decimal::example009b_boost_math_standalone()
{
  using wide_decimal_010_type = math::wide_decimal::decwide_t< 10U, std::uint32_t, void>;
  using wide_decimal_035_type = math::wide_decimal::decwide_t< 35U, std::uint32_t, void>;
  using wide_decimal_105_type = math::wide_decimal::decwide_t<105U, std::uint32_t, void>;

  const bool result_010_is_ok = local::test_tgamma<wide_decimal_010_type>();
  const bool result_035_is_ok = local::test_tgamma<wide_decimal_035_type>();
  const bool result_105_is_ok = local::test_tgamma<wide_decimal_105_type>();

  const bool result_is_ok = (   result_010_is_ok
                             && result_035_is_ok
                             && result_105_is_ok);

  return result_is_ok;
}

// Enable this if you would like to activate this main() as a standalone example.
#if 0

#include <iomanip>
#include <iostream>

int main()
{
  const bool result_is_ok = math::wide_decimal::example009b_boost_math_standalone();

  std::cout << "result_is_ok: " << std::boolalpha << result_is_ok << std::endl;
}

#endif

#if defined(__clang__) && !defined(__APPLE__)
#pragma GCC diagnostic pop
#endif

#if defined(__GNUC__)
#pragma GCC diagnostic pop
#pragma GCC diagnostic pop
#pragma GCC diagnostic pop
#endif


// MovementPatternFunctions.cpp : Defines the entry point for the console application.
//

/*
#include <iostream>
#include <vector>
#include <math.h>
#include <complex>
#include <algorithm>
#include <numeric>
#include <tuple>

#include <Eigen/Dense>
*/
#include "UDG_Hokuyo.h"
using namespace std;
using namespace Eigen;

const float PI = 3.14159265;

void scanningPattern_rotateToAngle(vector<float>& x_pos, vector<float>& y_pos, float xC, float yC, float rotationAngle){

	rotationAngle = rotationAngle*(PI / 180);

	for (int i = 0; i < static_cast<int>(x_pos.size()); i++)
	{
		x_pos[i] = cos(rotationAngle) * (x_pos[i] - xC) - sin(rotationAngle) * (y_pos[i] - yC) + xC;
		y_pos[i] = sin(rotationAngle) * (x_pos[i] - xC) + cos(rotationAngle) * (y_pos[i] - yC) + yC;
	}

}

void scanningPattern_horiz(vector<float>& x_vec, vector<float>& y_vec, vector<float>& z_vec, float xC, float yC, float radius, float height, float rotAngle, float startAngle, float endAngle, float deltaPoints){

	if (startAngle > endAngle){
		deltaPoints = -deltaPoints;
	}

	endAngle += deltaPoints;

	for (float i = startAngle; i < endAngle; i = i+ deltaPoints)
	{
		x_vec.push_back(xC + radius*cos(i*(PI / 180)));
		y_vec.push_back(yC + radius*sin(i*(PI / 180)));
		z_vec.push_back(height);
	}

	scanningPattern_rotateToAngle(x_vec, y_vec, xC, yC, rotAngle);

}

//float calculatePCA_rotAngle(float positions[][2], int size){
float calculatePCA_rotAngle(vector<double>& x_pos, vector<double>& y_pos){

	int size = x_pos.size();

	MatrixXf posMat(x_pos.size(), 2);

	for (int i = 0; i < size; i++)
	{
		posMat(i, 0) = x_pos[i];
		posMat(i, 1) = y_pos[i];
	}

	/*
	for (int i = 0; i < size; i++)
	{
		for (int j = 0; j < 2; j++)
		{
			posMat(i, j) = positions[i][j];
		}
	}
	*/

	float avg_x = 0.0;
	float sum_x = 0.0;
	float avg_y = 0.0;
	float sum_y = 0.0;
	for (int i = 0; i < size; i++)
	{
		sum_x += posMat(i, 0);
		sum_y += posMat(i, 1);
	}
	avg_x = ((float)sum_x) / size;
	avg_y = ((float)sum_y) / size;

	for (int i = 0; i < size; i++)
	{
		posMat(i, 0) -= avg_x;
		posMat(i, 1) -= avg_y;
	}

	int fact = size - 1;

	MatrixXf covMat(2, 2);
	covMat = (posMat.transpose()*posMat.conjugate()) / fact;

	/*
	std::cout << covMat(0, 0) << ' ';
	std::cout << covMat(0, 1) << std::endl;
	std::cout << covMat(1, 0) << ' ';
	std::cout << covMat(1, 1) << std::endl;
*/

	EigenSolver<MatrixXf> es(covMat);
	/*
	cout << "Eigenvalue:"
		<< endl << es.eigenvalues() << endl;

	cout << "Eigenvectors:"
		<< endl << es.eigenvectors() << endl;

	*/
	float eigenvalueCalc[2] = { es.eigenvalues()[0].real(), es.eigenvalues()[1].real() };

	float eigenvectorCalc[2][2] = { { es.eigenvectors()(0, 0).real(), es.eigenvectors()(0, 1).real() }, { es.eigenvectors()(1, 0).real(), es.eigenvectors()(1, 1).real() } };

	int index = -1;

	if (eigenvalueCalc[0] > eigenvalueCalc[1])
	{
		index = 0;
	}
	else
	{
		index = 1;
	}

	float maxEigenVec[2] = { eigenvectorCalc[0][index], eigenvectorCalc[1][index] };
	/*
	std::cout << maxEigenVec[0] << ' ';
	std::cout << maxEigenVec[1] << std::endl;
	*/
	float bladeRotAngle = atan2(maxEigenVec[0], maxEigenVec[1])*(180 / PI);

	if (bladeRotAngle < 0)
	{
		bladeRotAngle = bladeRotAngle + 180;
	}

	return bladeRotAngle;

}

void lidarData_thresholdByDistance(vector<int>& angles, vector<int>& distances, float minDist, float maxDist){

	std::vector<size_t> results;

	auto it = std::find_if(std::begin(distances), std::end(distances), [maxDist, minDist](int i){return i > maxDist || i < minDist; });
	while (it != std::end(distances)) {
		results.emplace_back(std::distance(std::begin(distances), it));
		it = std::find_if(std::next(it), std::end(distances), [maxDist, minDist](int i){return i > maxDist || i < minDist; });

	}

	for (size_t i = 0; i < results.size(); i++)
	{

		distances[results[i]] = -1;
		angles[results[i]] = -1;
	}

	distances.erase(remove(distances.begin(), distances.end(), -1), distances.end());
	angles.erase(remove(angles.begin(), angles.end(), -1), angles.end());

}

tuple<double, double> calculateMeans(vector<int>& angles, vector<int>& distances){

	double meanDist = -1;
	double meanAngle = -1;

	meanDist = accumulate(distances.begin(), distances.end(), 0.0) / distances.size();

	vector<double> sinAngles;
	vector<double> cosAngles;

	for (size_t i = 0; i < angles.size(); i++)
	{
		sinAngles.push_back(sin(angles[i] * (PI / 180)));
		cosAngles.push_back(cos(angles[i] * (PI / 180)));

	}

	double avgSin = accumulate(sinAngles.begin(), sinAngles.end(), 0.0) / sinAngles.size();

	double avgCos = accumulate(cosAngles.begin(), cosAngles.end(), 0.0) / cosAngles.size();

	if (avgSin>0 && avgCos>0)
	{
		meanAngle = atan(avgSin / avgCos);
	}
	else if (avgCos <0)
	{
		meanAngle = atan(avgSin / avgCos) + 180 * (PI / 180);
	}
	else if (avgSin <0 && avgCos >0)
	{
		meanAngle = atan(avgSin / avgCos) + 360 * (PI / 180);
	}

	meanAngle = meanAngle * (180 / PI);

	return make_tuple(meanAngle, meanDist);

}

void lidar_PolarToCart(double centerX, double centerY, vector<int>& angles, vector<int>& distances, vector<double>& posX, vector<double>& posY){

	for (size_t i = 0; i < distances.size(); i++)
	{
		posX.push_back(centerX + distances[i] * sin(angles[i] * (PI / 180)));
		posY.push_back(centerY + distances[i] * cos(angles[i] * (PI / 180)));
	}

}

tuple<double, double, double, int, int> getPathCenterFromMaxDist(vector<double>& x_pos, vector<double>& y_pos){

	double maxDistance = 0;
	double maxDistance_ind[2] = {-1,-1};

	double tempDist = 0;
	for (size_t i = 0; i < x_pos.size(); i++)
	{
		for (size_t j = 0; j < x_pos.size(); j++)
		{
			tempDist = sqrt(pow(x_pos[i] - x_pos[j], 2) + pow(y_pos[i] - y_pos[j], 2));
			if (tempDist > maxDistance)
			{
				maxDistance = tempDist;
				maxDistance_ind[0] = i;
				maxDistance_ind[1] = j;
			}

		}
	}

	int path_centerIndex = max(maxDistance_ind[0], maxDistance_ind[1]);
	double path_centerX = x_pos[path_centerIndex];
	double path_centerY = y_pos[path_centerIndex];

	return  make_tuple(maxDistance, path_centerX, path_centerY, maxDistance_ind[0], maxDistance_ind[1]);

}


#include <Eigen/Dense>
#include "numerical_gradient.h"

namespace MyDL{

    using namespace Eigen;

    MatrixXd numerical_gradient(double (*f)(MatrixXd), MatrixXd x){
        double h = 1e-4;
        int size_x = x.rows() * x.cols();
        MatrixXd grad(x.rows(), x.cols());

        for(int i = 0; i < size_x; i++){
            double tmp_val = x(i);
            double f_plus_h;
            double f_minus_h;

            // f(x + h)
            x(i) = tmp_val + h;
            f_plus_h = f(x);

            // f(x - h)
            x(i) = tmp_val - h;
            f_minus_h = f(x);

            grad(i) = (f_plus_h - f_minus_h) / (2*h);

            x(i) = tmp_val;
        }

        return grad;
    }

    // TwoLayerNetなどで用いるもの(ラムダ式使用・DNNのインスタンスに含まれるパラメータを参照して使用する場合)
    MatrixXd numerical_gradient(const std::function<double(MatrixXd)>& f, MatrixXd& x)
    {
        double h = 1e-4;
        int size_x = x.rows() * x.cols();
        MatrixXd grad(x.rows(), x.cols());

        for (int i = 0; i < size_x; i++)
        {
            double tmp_val = x(i);
            double f_plus_h;
            double f_minus_h;

            // f(x + h)
            x(i) = tmp_val + h;
            f_plus_h = f(x);

            // f(x - h)
            x(i) = tmp_val - h;
            f_minus_h = f(x);

            grad(i) = (f_plus_h - f_minus_h) / (2 * h);

            x(i) = tmp_val;
        }

        return grad;
    }
}


#include <Eigen/Core>
#include <iostream>
#include <LBFGS.h>

using Eigen::VectorXf;
using Eigen::MatrixXf;
using namespace LBFGSpp;

class Rosenbrock
{
private:
    int n;
    uint64_t ncalls = 0;
    float delta = 1e-7;
public:
    Rosenbrock(int n_) : n(n_) {}
    float operator()(const VectorXf& x, VectorXf& grad)
    {
        ncalls++;
        if (ncalls != 1) return getForward(x);

        float fx = 0.0;
        for(int i = 0; i < n; i += 2)
        {
            float t1 = 1.0 - x[i];
            float t2 = 10 * (x[i + 1] - x[i] * x[i]);
            grad[i + 1] = 20 * t2;
            grad[i]     = -2.0 * (x[i] * grad[i + 1] + t1);
            fx += t1 * t1 + t2 * t2;
        }
        return fx;
    }
    float getForward(const VectorXf& x)
    {
        float fx = 0.0;
        for(int i = 0; i < n; i += 2)
        {
            float t1 = 1.0 - x[i];
            float t2 = 10 * (x[i + 1] - x[i] * x[i]);
            fx += t1 * t1 + t2 * t2;
        }
        return fx;
    }
    float getDg(const VectorXf& drt, const VectorXf& x1, const float& fx)
    {
        float res = 0;
        VectorXf x2 = x1 + delta * drt;
        float fx2 = getForward(x2);
        res = ( fx2 - fx ) / delta;
        return res;
    }
    void getGrad(const VectorXf& x, VectorXf& grad)
    {
        for(int i = 0; i < n; i += 2)
        {
            float t1 = 1.0 - x[i];
            float t2 = 10 * (x[i + 1] - x[i] * x[i]);
            grad[i + 1] = 20 * t2;
            grad[i]     = -2.0 * (x[i] * grad[i + 1] + t1);
        }
    }
};

int main()
{
    const int n = 10;
    LBFGSParam<float> param;
    LBFGSSolver<float> solver(param);
    Rosenbrock fun(n);

    VectorXf x = VectorXf::Zero(n);
    float fx;
    int niter = solver.minimize(fun, x, fx);

    std::cout << niter << " iterations" << std::endl;
    std::cout << "x = \n" << x.transpose() << std::endl;
    std::cout << "f(x) = " << fx << std::endl;

    return 0;
}


static bool eigen_did_assert = false;
#define eigen_assert(X) if(!eigen_did_assert && !(X)){ std::cout << "### Assertion raised in " << __FILE__ << ":" << __LINE__ << ":\n" #X << "\n### The following would happen without assertions:\n"; eigen_did_assert = true;}

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

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
// intentionally remove indentation of snippet
{
MatrixXd X = MatrixXd::Random(5,5);
MatrixXd A = X + X.transpose();
cout << "Here is a random symmetric 5x5 matrix, A:" << endl << A << endl << endl;

SelfAdjointEigenSolver<MatrixXd> es(A);
cout << "The eigenvalues of A are:" << endl << es.eigenvalues() << endl;
cout << "The matrix of eigenvectors, V, is:" << endl << es.eigenvectors() << endl << endl;

double lambda = es.eigenvalues()[0];
cout << "Consider the first eigenvalue, lambda = " << lambda << endl;
VectorXd v = es.eigenvectors().col(0);
cout << "If v is the corresponding eigenvector, then lambda * v = " << endl << lambda * v << endl;
cout << "... and A * v = " << endl << A * v << endl << endl;

MatrixXd D = es.eigenvalues().asDiagonal();
MatrixXd V = es.eigenvectors();
cout << "Finally, V * D * V^(-1) = " << endl << V * D * V.inverse() << endl;

}
  return 0;
}


#include <iostream>
#include <stdio.h>
#include <stdlib.h>
#include <boost/python.hpp>
#include <boost/utility.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/python/extract.hpp>
#include <boost/python/list.hpp>
using namespace std;

class matrix
{
	int rows,cols;
	float a[10][10];
public:
	void dimensions(int r,int c)
	{
		rows=r;
		cols=c;
		for(int i=0;i<rows;i++)
			for(int j=0;j<cols;j++)
				a[i][j]=0;
	}
	void read( float x,int i, int j)
	{
		a[i][j]=x;

	}
	void display()
	{
		for(int i=0;i<rows;i++)
		{
			for(int j=0;j<cols;j++)
				cout<<a[i][j]<<"  ";
			cout<<endl;
		}
	}
	matrix operator + (matrix obj)
	{
		matrix sum;
		sum.dimensions(rows,cols);
		for(int i=0;i<rows;i++)
			for(int j=0;j<cols;j++)
				sum.a[i][j]=a[i][j]+obj.a[i][j];
		return sum;
	}
	matrix operator - (matrix obj)
	{
		matrix dif;
		dif.dimensions(rows,cols);
		for(int i=0;i<rows;i++)
			for(int j=0;j<cols;j++)
				dif.a[i][j]=a[i][j]-obj.a[i][j];
		return dif;
	}
	matrix operator * (matrix obj)	//only for square matrices
	{
		matrix product;
		product.dimensions(rows,cols);
		for(int i=0;i<rows;i++)
			for(int j=0;j<obj.cols;j++)
			{
				product.a[i][j]=0;
				for(int m=0;m<cols;m++)
					product.a[i][j]=product.a[i][j]+a[i][m]*obj.a[m][j];
			}
		return product;
	}
 	//int operator ++()
	//{
	//	return determinant(a,R);
	//}
	matrix operator & (matrix obj)
	{
		matrix inv;
		inv.dimensions(rows,cols);
		inverse(a,inv.a);
		return inv;

	}
	void cofactor(float mat[10][10], float temp[10][10], int p, int q, int n)
	{
    	int i = 0, j = 0;
    	for (int row = 0; row < n; row++)
        	for (int col = 0; col < n; col++)
            	if (row != p && col != q)
            	{
                	temp[i][j++] = mat[row][col];
                	if (j == n - 1)
                	{
                    	j = 0;
                    	i++;
                	}
            	}
	}
	float determinant(float mat[10][10], float n)
	{
    	float sum = 0;
    	if (n == 1)
     	   return mat[0][0];
 		float temp[10][10];
	 	float sign = 1;
    	for (int f = 0; f < n; f++)
    	{
   	    	cofactor(mat, temp, 0, f, n);
       		sum =sum+ sign * mat[0][f] * determinant(temp, n - 1);
        	sign = -sign;
    	}
 		return sum;
	}
	void adjoint(float A[10][10],float adj[10][10])
	{
    	if (rows == 1)
    	{
      		adj[0][0] = 1;
       		return;
    	}
    	float sign = 1;
    	float temp[10][10];
 		for (int i=0; i<rows; i++)
    	{
        	for (int j=0; j<rows; j++)
        	{
        	    cofactor(A, temp, i, j, rows);
        	    sign = ((i+j)%2==0)? 1: -1;
            	adj[j][i] = (sign)*(determinant(temp, rows-1));
        	}
    	}
	}
	bool inverse(float A[10][10], float inverse[10][10])
	{
    	float det = determinant(A, rows);
    	if (det == 0)
    	{
       		cout << "Inverse can't be found as matrix is singular"<<endl;
        	return false;
    	}
    	float adj[10][10];
    	adjoint(A, adj);
    	for (int i=0; i<rows; i++)
       		for (int j=0; j<rows; j++)
            	inverse[i][j] = adj[i][j]/det;
 		return true;
	}
};

using namespace boost::python;
BOOST_PYTHON_MODULE(matrix)
{
	class_<matrix>("matrix")
		.def("dimensions",&matrix::dimensions)
		.def("read",&matrix::read)
		.def("display",&matrix::display)
		.def("cofactor",&matrix::cofactor)
		.def("determinant",&matrix::determinant)
		.def("adjoint",&matrix::adjoint)
		.def("inverse",&matrix::inverse)
		.def(self+self)
		.def(self-self)
		.def(self*self)
//		.def(++self)
		.def(self&self)
	;
}


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

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  Matrix3f A(3,3);
A << 1,2,3,  4,5,6,  7,8,10;
Matrix<float,3,2> B;
B << 3,1, 3,1, 4,1;
Matrix<float,3,2> X;
X = A.fullPivLu().solve(B);
cout << "The solution with right-hand side (3,3,4) is:" << endl;
cout << X.col(0) << endl;
cout << "The solution with right-hand side (1,1,1) is:" << endl;
cout << X.col(1) << endl;

  return 0;
}


#include <boost/program_options.hpp>
#include <cmath>
#include <iostream>
#include <string>

static unsigned long long gcd(unsigned long long a, unsigned long long b) {
    unsigned long long c;
    while (a != 0) {
        c = a;
        a = b % a;
        b = c;
    }
    return b;
}

static bool is_prime(unsigned int number) {
    const unsigned int upper_limit = std::floor(std::sqrt(number));

    for (unsigned int a = 2; a <= upper_limit; a++) {
        if (number % a == 0) {
            return false;
        }
    }
    return true;
}

void output_coprimes(const unsigned int composite_number, const unsigned int length) {
    unsigned output_length  = 0;
    unsigned current_number = 2;

    while (output_length < length) {
        if (gcd(current_number, composite_number) == 1) {
            std::cout << current_number << "\n";
            output_length++;
        }
        current_number++;
    }
}

void output_primes(const unsigned int composite_number, const unsigned int length) {
    unsigned output_length  = 0;
    unsigned current_number = 2;

    while (output_length < length) {
        if (is_prime(current_number) && gcd(current_number, composite_number) == 1) {
            std::cout << current_number << "\n";
            output_length++;
        }
        current_number++;
    }
}

int main(int argc, char** argv) {
    namespace po = boost::program_options;
    po::options_description description("JKQ DDSIM by https://iic.jku.at/eda/ -- Allowed options");
    description.add_options()("help,h", "produce help message")("composite_number,N", po::value<unsigned int>()->required(),
                                                                "number of measurements on the final quantum state")("strategy,S", po::value<std::string>()->required(),
                                                                                "strategy for prime base generation (primes, coprimes)")("length,L", po::value<unsigned int>()->required(), "how many bases to generate");
    po::variables_map vm;
    try {
        po::store(po::parse_command_line(argc, argv, description), vm);
        if (vm.count("help")) {
            std::cout << description;
            return 0;
        }
        po::notify(vm);
    } catch (const po::error& e) {
        std::cerr << "[ERROR] " << e.what() << "! Try option '--help' for available commandline options.\n";
        std::exit(1);
    }

    if (vm["strategy"].as<std::string>() == "coprimes") {
        output_coprimes(vm["composite_number"].as<unsigned int>(), vm["length"].as<unsigned int>());
    } else if (vm["strategy"].as<std::string>() == "primes") {
        output_primes(vm["composite_number"].as<unsigned int>(), vm["length"].as<unsigned int>());
    } else {
        std::cerr << "Invalid stragety.\n";
    }
}


/*
author: Kris Gonzalez
date: 200118
objective: use eigen library, but compile using cmake. proof of concept to ensure this works.

src1: https://eigen.tuxfamily.org/dox/GettingStarted.html
src2: https://eigen.tuxfamily.org/dox/TopicCMakeGuide.html
KJG200118: get eigen by cloning this repo: https://gitlab.com/libeigen/eigen

*/

#include <iostream>
#include <Eigen/Dense>
using Eigen::MatrixXd;
int main()
{
  MatrixXd m(2,2);
  m(0,0) = 3;
  m(1,0) = 2.5;
  m(0,1) = -1;
  m(1,1) = m(1,0) + m(0,1);
  std::cout << m << std::endl;
}


#include <Eigen/Core>
#include "problem.h"
#include "random.h"
#include "so3.h"

#include <math.h>

void compute_error( const Eigen::Matrix3d &R, const Eigen::Vector3d &t,
                   const Eigen::Matrix3d &Rsoln, const Eigen::Vector3d &tsoln,
                  double &rot_angle_err, double &trans_angle_err, double &trans_scale_err )
{
    Eigen::Matrix3d Rdiff = R*Rsoln.transpose();
    Eigen::Vector3d rdiff = so3ln(Rdiff);
    rot_angle_err = rdiff.norm();

    trans_angle_err = acos(t.dot(tsoln)/t.norm()/tsoln.norm());

    if ( isnan(trans_angle_err) ) trans_angle_err = 0;
    trans_scale_err = tsoln.norm()/t.norm();
}

void generateProblem( double trans_mag, double angle, double noise, Problem &prob, int nrays )
{
    Eigen::Vector3d X;    // 3D points
    Eigen::Vector3d cu;   // camera centers for first multi-camera rig
    Eigen::Vector3d cv;   // camera centers for second multi-camera rig
    Eigen::Vector3d PX;   // 3D points after R,t transformation
    Ray u;                // observations in first multi-camera rig
    Ray v;                // observations in second multi-camera rig

    Eigen::Vector3d w = rand3f();
    w = w/w.norm();
    double theta = angle*M_PI/180;
    prob.R = so3exp(w*theta);                // random rotation
    prob.t = rand3f();                       // random translation
    while ( prob.t.norm() < 1e-10 ) prob.t = rand3f();
    prob.t = prob.t/prob.t.norm()*trans_mag;

    prob.ray_pairs.clear();

    for ( size_t i = 0; i < nrays; i++ ) {
        X = rand3f();    // sample random point
        while ( X.norm() == 0 ) X = rand3f();
        X = X/X.norm();
        double scale_factor = ((double)rand()/RAND_MAX) * 4 + 4;
        X *= scale_factor;

        cu = rand3f();        // sample random camera locations
        cv = rand3f(); // inter-camera
        //cv = cu;    // intra-camera

        PX = prob.R*X+prob.t;    // transform point
        u.x = X-cu;    // make observation rays
        u.c = cu;
        v.x = PX-cv;
        v.c = cv;

        if ( noise > 0. )    // add noise with requested standard deviation
        {
            double unorm = u.x.norm();
            Eigen::Vector3d uproj = u.x/u.x(2);
            uproj.head(2) += grand2f()*noise;
            uproj /= uproj.norm();
            uproj *= unorm;
            if ( (X-cu).dot(uproj) < 0 ) uproj = -uproj;
            u.x = uproj;

            double vnorm = v.x.norm();
            Eigen::Vector3d vproj = v.x/v.x(2);
            vproj.head(2) += grand2f()*noise;
            vproj /= vproj.norm();
            vproj *= vnorm;
            if ( (PX-cv).dot(vproj) < 0 ) vproj = -vproj;
            v.x = vproj;
        }

        prob.ray_pairs.push_back( RayPair( u, v ) );
    }
}


#include "sophus/se3.hpp"
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <cmath>
#include <iostream>

using namespace std;
using namespace Eigen;

/// Basic Sophus implementation

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

  // Rotation of 90 degrees about thez axis
  Matrix3d R = AngleAxisd(M_PI / 2, Vector3d(0, 0, 1)).toRotationMatrix();
  // Quaternion object
  Quaterniond q(R);
  Sophus::SO3d SO3_R(R);  // Sophus::SO3d Lie algebra object from rotation matrix
  Sophus::SO3d SO3_q(q);  // Same lie algebra object from quaternion
  // Print the SO3 matrix from both the lie algebra object
  cout << "SO(3) from matrix:\n" << SO3_R.matrix() << endl;
  cout << "SO(3) from quaternion:\n" << SO3_q.matrix() << endl;
  cout << "they are equal" << endl;

  // Log is the 3D vector Lie Algebra representation of the rotation matrix
  Vector3d so3 = SO3_R.log();
  cout << "so3 = " << so3.transpose() << endl;
  // hat representation which is the skew symmetric matrix
  cout << "so3 hat=\n" << Sophus::SO3d::hat(so3) << endl;
  // vee gets the vector from the skew symmetric matrix
  cout << "so3 hat vee= " << Sophus::SO3d::vee(Sophus::SO3d::hat(so3)).transpose() << endl;

  // Apply a left perturbation
  Vector3d update_so3(1e-4, 0, 0);  // A small perturbation
  // Get the lie group (Rotation matrix) from the rotation vector and apply the rotation
  Sophus::SO3d SO3_updated = Sophus::SO3d::exp(update_so3) * SO3_R;
  cout << "SO3 updated = \n" << SO3_updated.matrix() << endl;

  cout << "*******************************" << endl;
  // SE3 sophus
  Vector3d t(1, 0, 0);        // Translation by 1m
  Sophus::SE3d SE3_Rt(R, t);  // Lie Algebra from R,t SE(3)
  Sophus::SE3d SE3_qt(q, t);  // Lie Algebra from q,t SE(3)
  cout << "SE3 from R,t= \n" << SE3_Rt.matrix() << endl;
  cout << "SE3 from q,t= \n" << SE3_qt.matrix() << endl;
  // Lie Algebra for SE3 is a 6 Dof Vector
  typedef Eigen::Matrix<double, 6, 1> Vector6d;
  Vector6d se3 = SE3_Rt.log();
  cout << "se3 = " << se3.transpose() << endl;
  // Get the hat and recover the vector format of Lie Algebra from the hat
  cout << "se3 hat = \n" << Sophus::SE3d::hat(se3) << endl;
  cout << "se3 hat vee = " << Sophus::SE3d::vee(Sophus::SE3d::hat(se3)).transpose() << endl;

  // Left perturb SE3
  Vector6d update_se3;  // Small perturbation in roh
  update_se3.setZero();
  update_se3(0, 0) = 1e-4;
  Sophus::SE3d SE3_updated = Sophus::SE3d::exp(update_se3) * SE3_Rt;
  cout << "SE3 updated = " << endl << SE3_updated.matrix() << endl;

  return 0;
}


#include <cmath>
#include <random>
#include <Eigen/Dense>
#include "aggregate_gen_SA.hpp"
using namespace Eigen;
using namespace std;

//return pos_sph matrix
MatrixXd aggregate_gen_SA(const double& a, const int& num_sph, const double& kf, const double& Df, const double& tol){
    MatrixXd pos_sph= MatrixXd::Zero(3,num_sph);
    pos_sph.col(0) << -a,0,0;
    pos_sph.col(1) << a,0,0;

    Vector3d pos_cand(3);
    double rN;
    double phi;
    double u;
    ArrayXd dist_sph;

    for(int N=3; N <= num_sph; ++N){
        rN= N*N*a*a/(N-1)*pow(N/kf,2/Df)-N*a*a/(N-1)-N*a*a*pow((N-1)/kf,2/Df);
        rN= sqrt(rN);
        dist_sph.resize(N-1);

        while(1){
            phi= 2*M_PI*urf(re);
            u= 2*urf(re)-1;
            pos_cand << sqrt(1.0-u*u)*cos(phi), sqrt(1.0-u*u)*sin(phi), u;
            pos_cand *= rN;

            for(int i= 0; i < N-1; ++i){
                dist_sph(i)=(pos_sph.col(i)-pos_cand).norm();
            }

            if(dist_sph.minCoeff() < 2*a+tol && dist_sph.minCoeff() > 2*a){
                pos_sph.col(N-1)= pos_cand;
                pos_sph.colwise() -= pos_sph.rowwise().mean(); // translate the centroid to origin
                break;
            }
        }
    }

    // final check of the attached condition
    ArrayXd mindist_c(num_sph);
    for(int i= 0; i< num_sph; ++i){
        mindist_c(i)=((pos_sph.colwise()-pos_sph.col(i)).colwise().norm().array()-2*a).array().abs().minCoeff();
    }
    assert(mindist_c.maxCoeff() < tol);

    return pos_sph;
}


// https://www.boost.org/doc/libs/1_67_0/libs/math/example/numerical_derivative_example.cpp

// Copyright Christopher Kormanyos 2013.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or
// copy at http://www.boost.org/LICENSE_1_0.txt).

#ifdef _MSC_VER
#  pragma warning (disable : 4996) // assignment operator could not be generated.
#endif

# include <iostream>
# include <iomanip>
# include <limits>
# include <cmath>

#include <boost/static_assert.hpp>
#include <boost/type_traits/is_floating_point.hpp>
#include <boost/math/special_functions/next.hpp> // for float_distance

//[numeric_derivative_example
/*`The following example shows how multiprecision calculations can be used to
obtain full precision in a numerical derivative calculation that suffers from precision loss.

Consider some well-known central difference rules for numerically
computing the 1st derivative of a function [f'(x)] with [/x] real.

Need a reference here?  Introduction to Partial Differential Equations, Peter J. Olver
 December 16, 2012

Here, the implementation uses a C++ template that can be instantiated with various
floating-point types such as `float`, `double`, `long double`, or even
a user-defined floating-point type like __multiprecision.

We will now use the derivative template with the built-in type `double` in
order to numerically compute the derivative of a function, and then repeat
with a 5 decimal digit higher precision user-defined floating-point type.

Consider the function  shown below.
!!
(3)
We will now take the derivative of this function with respect to x evaluated
at x = 3= 2. In other words,

(4)

The expected result is

 0:74535 59924 99929 89880 . (5)
The program below uses the derivative template in order to perform
the numerical calculation of this derivative. The program also compares the
numerically-obtained result with the expected result and reports the absolute
relative error scaled to a deviation that can easily be related to the number of
bits of lost precision.

*/

/*` [note Rquires the C++11 feature of
[@http://en.wikipedia.org/wiki/Anonymous_function#C.2B.2B anonymous functions]
for the derivative function calls like `[]( const double & x_) -> double`.
*/

template <typename value_type,  typename function_type>
value_type derivative (const value_type x, const value_type dx, function_type function)
{
  /*! \brief Compute the derivative of function using a 3-point central difference rule of O(dx^6).
    \tparam value_type, floating-point type, for example: `double` or `cpp_dec_float_50`
    \tparam function_type

    \param x Value at which to evaluate derivative.
    \param dx Incremental step-size.
    \param function Function whose derivative is to computed.

    \return derivative at x.
  */

  BOOST_STATIC_ASSERT_MSG(false == std::numeric_limits<value_type>::is_integer, "value_type must be a floating-point type!");

  const value_type dx2(dx * 2U);
  const value_type dx3(dx * 3U);
  // Difference terms.
  const value_type m1 ((function (x + dx) - function(x - dx)) / 2U);
  const value_type m2 ((function (x + dx2) - function(x - dx2)) / 4U);
  const value_type m3 ((function (x + dx3) - function(x - dx3)) / 6U);
  const value_type fifteen_m1 (m1 * 15U);
  const value_type six_m2 (m2 * 6U);
  const value_type ten_dx (dx * 10U);
  return ((fifteen_m1 - six_m2) + m3) / ten_dx;  // Derivative.
} //

#include <boost/multiprecision/cpp_dec_float.hpp>
  using boost::multiprecision::number;
  using boost::multiprecision::cpp_dec_float;

// Re-compute using 5 extra decimal digits precision (22) than double (17).
#define MP_DIGITS10 unsigned (std::numeric_limits<double>::max_digits10 + 5)

typedef cpp_dec_float<MP_DIGITS10> mp_backend;
typedef number<mp_backend> mp_type;

int main()
{
  {
    const double d =
      derivative
      ( 1.5, // x = 3.2
        std::ldexp (1., -9), // step size 2^-9 = see below for choice.
        [](const double & x)->double // Function f(x).
        {
          return std::sqrt((x * x) - 1.) - std::acos(1. / x);
        }
      );

    // The 'exactly right' result is [sqrt]5 / 3 = 0.74535599249992989880.
    const double rel_error = (d - 0.74535599249992989880) / 0.74535599249992989880;
    const double bit_error = std::abs(rel_error) / std::numeric_limits<double>::epsilon();
    std::cout.precision (std::numeric_limits<double>::digits10); // Show all guaranteed decimal digits.
    std::cout << std::showpoint ; // Ensure that any trailing zeros are shown too.

    std::cout << " derivative : " << d << std::endl;
    std::cout << " expected   : " << 0.74535599249992989880 << std::endl;
    // Can compute an 'exact' value using multiprecision type.
    std::cout << " expected   : " << sqrt(static_cast<mp_type>(5))/3U << std::endl;
    std::cout << " bit_error : " << static_cast<unsigned long>(bit_error)  << std::endl;

    std::cout.precision(6);
    std::cout << "float_distance = " << boost::math::float_distance(0.74535599249992989880, d) << std::endl;

  }

  { // Compute using multiprecision type with an extra 5 decimal digits of precision.
    const mp_type mp =
      derivative(mp_type(mp_type(3) / 2U), // x = 3/2
        mp_type(mp_type(1) / 10000000U), // Step size 10^7.
        [](const mp_type & x)->mp_type
        {
          return sqrt((x * x) - 1.) - acos (1. / x); // Function
        }
    );

    const double d = mp.convert_to<double>(); // Convert to closest double.
    const double rel_error = (d - 0.74535599249992989880) / 0.74535599249992989880;
    const double bit_error = std::abs (rel_error) / std::numeric_limits<double>::epsilon();
    std::cout.precision (std::numeric_limits <double>::digits10); // All guaranteed decimal digits.
    std::cout << std::showpoint ; // Ensure that any trailing zeros are shown too.
    std::cout << " derivative : " << d << std::endl;
    // Can compute an 'exact' value using multiprecision type.
    std::cout << " expected   : " << sqrt(static_cast<mp_type>(5))/3U << std::endl;
    std::cout << " expected   : " << 0.74535599249992989880
    << std::endl;
    std::cout << " bit_error : "  << static_cast<unsigned long>(bit_error)  << std::endl;

    std::cout.precision(6);
    std::cout << "float_distance = " << boost::math::float_distance(0.74535599249992989880, d) << std::endl;

  }

} // int main()

/*`
The result of this program on a system with an eight-byte, 64-bit IEEE-754
conforming floating-point representation for `double` is:

 derivative : 0.745355992499951

 derivative : 0.745355992499943
 expected   : 0.74535599249993
 bit_error : 78

    derivative : 0.745355992499930
   expected   : 0.745355992499930
   bit_error : 0

The resulting bit error is 0. This means that the result of the derivative
calculation is bit-identical with the double representation of the expected result,
and this is the best result possible for the built-in type.

The derivative in this example has a known closed form. There are, however,
countless situations in numerical analysis (and not only for numerical deriva-
tives) for which the calculation at hand does not have a known closed-form
solution or for which the closed-form solution is highly inconvenient to use. In
such cases, this technique may be useful.

This example has shown how multiprecision can be used to add extra digits
to an ill-conditioned calculation that suffers from precision loss. When the result
of the multiprecision calculation is converted to a built-in type such as double,
the entire precision of the result in double is preserved.

 */

/*

  Description: Autorun "J:\Cpp\big_number\Debug\numerical_derivative_example.exe"
   derivative : 0.745355992499943
   expected   : 0.745355992499930
   expected   : 0.745355992499930
   bit_error : 78
  float_distance = 117.000
   derivative : 0.745355992499930
   expected   : 0.745355992499930
   expected   : 0.745355992499930
   bit_error : 0
  float_distance = 0.000000

 */


#include "interpolator.h"
#include <cmath>
#include <Eigen/Core>
#include <Eigen/LU>

using std::vector;
using Eigen::VectorXd;
using Eigen::MatrixXd;
using Eigen::FullPivLU;
using Eigen::Map;

namespace rbf
{

extern VectorXd solveLinearSystem(const MatrixXd& A, const VectorXd& y);

Interpolator::Interpolator(FunctionType functionType, const double epsilon) :
    functionType(functionType),
    epsilon(epsilon)
{
}

void Interpolator::reset()
{
    ys.clear();
    xs.clear();
    w.clear();
}

void Interpolator::addCenterPoint(const double y, const vector<double>& x)
{
    ys.push_back(y);
    xs.push_back(x);
}

void Interpolator::computeWeights(const bool useRegularization, const double lambda)
{
    assert(ys.size() == xs.size());

    const int dim = ys.size();

    MatrixXd O = MatrixXd::Zero(dim, dim);
    VectorXd y = Map<VectorXd>(&ys[0], dim);

    for (int i = 0; i < dim; ++ i)
    {
        for (int j = 0; j < dim; ++ j)
        {
            O(i, j) = getRbfValue(xs[i], xs[j]);
        }
    }

    MatrixXd A;
    VectorXd b;
    if (useRegularization)
    {
        MatrixXd O2 = MatrixXd::Zero(dim * 2, dim);
        for (int i = 0; i < dim; ++ i)
        {
            for (int j = 0; j < dim; ++ j)
            {
                O2(i, j) = O(i, j);
            }
        }
        const double coef = 0.5 * lambda;
        for (int i = 0; i < dim; ++ i)
        {
            O2(i + dim, i) = coef;
        }

        VectorXd y2 = VectorXd::Zero(dim * 2);
        for (int i = 0; i < dim; ++ i)
        {
            y2(i) = y(i);
        }

        A = O2.transpose() * O2;
        b = O2.transpose() * y2;
    }
    else
    {
        A = O;
        b = y;
    }

    const VectorXd x = solveLinearSystem(A, b);
    assert(x.rows() == dim);

    w.resize(dim);
    for (int i = 0; i < dim; ++ i)
    {
        w[i] = x(i);
    }
}

double Interpolator::getInterpolatedValue(const vector<double>& x) const
{
    assert(w.size() == xs.size());

    const int dim = w.size();

    double result = 0.0;
    for (int i = 0; i < dim; ++ i)
    {
        result += w[i] * getRbfValue(x, xs[i]);
    }

    return result;
}

double Interpolator::getRbfValue(const double r) const
{
    double result;
    switch (functionType)
    {
        case FunctionType::Gaussian:
            result = exp(- pow((epsilon * r), 2.0));
            break;
        case FunctionType::ThinPlateSpline:
            result = r * r * log(r);
            if (isnan(result))
            {
                result = 0.0;
            }
            break;
        case FunctionType::InverseQuadratic:
            result = 1.0 / (1.0 + pow((epsilon * r), 2.0));
            break;
        case FunctionType::BiharmonicSpline:
            result = r;
            break;
        default:
            break;
    }
    return result;
}

double Interpolator::getRbfValue(const vector<double>& xi, const vector<double>& xj) const
{
    assert (xi.size() == xj.size());

    const VectorXd xiVec = Map<const VectorXd>(&xi[0], xi.size());
    const VectorXd xjVec = Map<const VectorXd>(&xj[0], xj.size());

    return getRbfValue((xjVec - xiVec).norm());
}

VectorXd solveLinearSystem(const MatrixXd& A, const VectorXd& y)
{
    FullPivLU<MatrixXd> lu(A);
    return lu.solve(y);
}

}


//------------------------------------------------------------------------------
// \file BitSet_test.cpp
//------------------------------------------------------------------------------
#include "Cpp/Utilities/SuperBitSet.h"

#include <bitset>
#include <boost/test/unit_test.hpp>
#include <cmath>
#include <iostream>
#include <limits>
#include <string>
// https://stackoverflow.com/questions/20731/how-do-you-clear-a-stringstream-variable
#include <sstream>

using Cpp::Utilities::SuperBitSet;
using Cpp::Utilities::number_of_bits_in_a_byte;

BOOST_AUTO_TEST_SUITE(Cpp)
BOOST_AUTO_TEST_SUITE(Utilities)
BOOST_AUTO_TEST_SUITE(SuperBitSet_tests)

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(NumberOfBitsInAByteIs8)
{
  BOOST_TEST(number_of_bits_in_a_byte == 8);
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(BitSetWorks)
// cf. https://www.cs.utexas.edu/users/fussell/courses/cs429h/lectures/Lecture_3-429h.pdf
{
  BOOST_TEST_REQUIRE(sizeof(unsigned short) == 2); // 2 bytes long
  BOOST_TEST_REQUIRE(std::pow(2, 2*8) == 65536); // 2 bytes long, 8 bits in a byte

  std::bitset<sizeof(unsigned short) * 8> unsigned_short_bitset {15213};

  constexpr unsigned short x_0 {15213};
  constexpr short x_1 {15213};
  constexpr short y {-15213};

  //std::cout << unsigned_short_bitset.to_string() << '\n';
  BOOST_TEST(unsigned_short_bitset.to_string() == "0011101101101101");
  unsigned_short_bitset = x_0;
  BOOST_TEST(unsigned_short_bitset.to_string() == "0011101101101101");
  unsigned_short_bitset = x_1;
  BOOST_TEST(unsigned_short_bitset.to_string() == "0011101101101101");
  unsigned_short_bitset = y;
  BOOST_TEST(unsigned_short_bitset.to_string() == "1100010010010011");
}

// cf. https://www.geeksforgeeks.org/c-bitset-and-its-application/
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(DemonstrateStdBitSet)
{
  {
    // Setup
    std::stringstream string_stream;

    constexpr std::size_t M {32};
    // default constructor initializes with all bits 0
    std::bitset<M> bset1;

    // bset2 is initialized with bits of 20
    std::bitset<M> bset2 {20};

    // bset3 is initialized with bits of specified binary string
    std::bitset<M> bset3 {std::string{"1100"}};

    // std::cout prints exact bits representation of bitset
    std::cout << bset1 << std::endl; // 00000000000000000000000000000000
    std::cout << bset2 << std::endl; // 00000000000000000000000000010100
    std::cout << bset3 << std::endl; // 00000000000000000000000000001100

    // declaring set8 with capacity of 8 bits
    std::bitset<8> set8; // 00000000

    // setting first bit (or 6th index)
    set8[1] = 1; // 00000010
    set8[4] = set8[1]; // 00010010
    std::cout << set8 << std::endl;

    // count function returns number of set bits in bitset
    int numberof1 = set8.count();
  }
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(DemonstrateSuperBitSet)
{
  {
    SuperBitSet<2> bits2 {1};
    BOOST_TEST(bits2.to_string() == "01");
  }

  BOOST_TEST(true);
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(SuperBitSetIsLittleEndian)
{
  {
    // initializing 1st M=5 (rightmost, least significant) bit position
    SuperBitSet<5> bits {"11010"};
    BOOST_TEST(bits.to_string() == "11010");
    BOOST_TEST(bits[0] == 0);
    BOOST_TEST(bits[1] == 1);
    BOOST_TEST(bits[2] == 0);
    BOOST_TEST(bits[3] == 1);
    BOOST_TEST(bits[4] == 1);
  }
  {
    SuperBitSet<8> bits {0};
    BOOST_TEST(bits[0] == 0);
    BOOST_TEST(bits[1] == 0);
    BOOST_TEST(bits[2] == 0);
    BOOST_TEST(bits[3] == 0);
    BOOST_TEST(bits[4] == 0);
    BOOST_TEST(bits[5] == 0);
    BOOST_TEST(bits[6] == 0);
    BOOST_TEST(bits[7] == 0);
  }
  {
    SuperBitSet<8> bits {1};
    BOOST_TEST(bits[0] == 1);
    BOOST_TEST(bits[1] == 0);
    BOOST_TEST(bits[2] == 0);
    BOOST_TEST(bits[3] == 0);
    BOOST_TEST(bits[4] == 0);
    BOOST_TEST(bits[5] == 0);
    BOOST_TEST(bits[6] == 0);
    BOOST_TEST(bits[7] == 0);
  }
  {
    SuperBitSet<8> bits {2};
    BOOST_TEST(bits[0] == 0);
    BOOST_TEST(bits[1] == 1);
    BOOST_TEST(bits[2] == 0);
    BOOST_TEST(bits[3] == 0);
    BOOST_TEST(bits[4] == 0);
    BOOST_TEST(bits[5] == 0);
    BOOST_TEST(bits[6] == 0);
    BOOST_TEST(bits[7] == 0);
  }
  {
    SuperBitSet<8> bits {std::numeric_limits<uint8_t>::max() - 1};
    BOOST_TEST(bits[0] == 0);
    BOOST_TEST(bits[1] == 1);
    BOOST_TEST(bits[2] == 1);
    BOOST_TEST(bits[3] == 1);
    BOOST_TEST(bits[4] == 1);
    BOOST_TEST(bits[5] == 1);
    BOOST_TEST(bits[6] == 1);
    BOOST_TEST(bits[7] == 1);
  }
  {
    SuperBitSet<8> bits {std::numeric_limits<uint8_t>::max()};
    BOOST_TEST(bits[0] == 1);
    BOOST_TEST(bits[1] == 1);
    BOOST_TEST(bits[2] == 1);
    BOOST_TEST(bits[3] == 1);
    BOOST_TEST(bits[4] == 1);
    BOOST_TEST(bits[5] == 1);
    BOOST_TEST(bits[6] == 1);
    BOOST_TEST(bits[7] == 1);
  }
  {
    SuperBitSet<8> bits {std::numeric_limits<uint8_t>::max() + 1};
    BOOST_TEST(bits[0] == 0);
    BOOST_TEST(bits[1] == 0);
    BOOST_TEST(bits[2] == 0);
    BOOST_TEST(bits[3] == 0);
    BOOST_TEST(bits[4] == 0);
    BOOST_TEST(bits[5] == 0);
    BOOST_TEST(bits[6] == 0);
    BOOST_TEST(bits[7] == 0);
  }

  BOOST_TEST(true);
}

// Use examples from wikipedia, "Bitwise operation"

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
// NOT is also called the complement, or negation
BOOST_AUTO_TEST_CASE(SuperBitSetWorksWithBitwiseNot)
{
  // bitwise complement = two's complement of the value - 1. If 2's complement
  // arithmetic used, then NOT x = -x - 1
  // TODO: explore and understand the previous statement.
  {
    SuperBitSet<4> bits {7};
    BOOST_TEST(bits.to_string() == "0111");
    BOOST_TEST(bits.to_ulong() == 7);
    BOOST_TEST((~bits).to_string() == "1000");
    BOOST_TEST((~bits).to_ulong() == 8);
  }
  {
    SuperBitSet<8> bits {171};
    BOOST_TEST(bits.to_string() == "10101011");
    BOOST_TEST(bits.to_ulong() == 171);
    BOOST_TEST((~bits).to_string() == "01010100");
    BOOST_TEST((~bits).to_ulong() == 84);
  }
  // For unsigned ints, bitwise complement of number is "mirror reflection" of
  // number across half-way point of unsigned int's range;
  // e.g. for 8-bit unsigned int, NOT x = 255 - x, "flips" increasing range from
  // 0 to 255, to decreasing range from 255 to 0.
}

// cf. https://en.wikipedia.org/wiki/Bitwise_operation Arithmetic shift section.

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(SuperBitSetWorksWithLeftShift)
{
  {
    SuperBitSet<8> bits {"00010111"};
    BOOST_TEST_REQUIRE(bits.to_string() == "00010111");
    BOOST_TEST(bits.to_ulong() == 23);
    BOOST_TEST((bits << 1).to_string() == "00101110");
    BOOST_TEST((bits << 1).to_ulong() == 46);
    BOOST_TEST((bits << 2).to_string() == "01011100");
    BOOST_TEST((bits << 2).to_ulong() == 92);
  }
}

// cf. https://en.wikipedia.org/wiki/Bitwise_operation Arithmetic shift section.
// In a right arithmetic shift, sign bit (MSB in two's complement) is shifted in
// on left, preserves sign of operand. If MSB is 1, new 1 is copied into
// leftmost position.
// Logical right-shift inserts 0 into most significant bit, instead of copying
// sign bit; it's ideal for unsigned binary numbers, while
// arithmetic right-shift ideal for signed two's complement binary numbers.
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(SuperBitSetWorksWithRightShiftAsLogicalShift)
{
  {
    SuperBitSet<8> bits {"10010111"};
    BOOST_TEST_REQUIRE(bits.to_string() == "10010111");
    BOOST_TEST(bits.to_ulong() == 151);
    BOOST_TEST((bits >> 1).to_string() == "01001011");
    BOOST_TEST((bits >> 1).to_ulong() == 75);
    BOOST_TEST((bits >> 2).to_string() == "00100101");
    BOOST_TEST((bits >> 2).to_ulong() == 37);
  }
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(UseBitwiseLeftShiftToGetMax)
{
  // unsigned long long is 64 bits or 8 bytes.
  SuperBitSet<64> bits {1ull};
  BOOST_TEST(sizeof(unsigned long long) == 8);
  {
    BOOST_TEST_REQUIRE(
      bits.to_string() ==
        "0000000000000000000000000000000000000000000000000000000000000001");
    BOOST_TEST(bits.to_ullong() == 1ull);

    BOOST_TEST((bits << 0).to_string() ==
      "0000000000000000000000000000000000000000000000000000000000000001");
    BOOST_TEST((bits << 0).to_ullong() == 1);
    BOOST_TEST((bits << 1).to_string() ==
      "0000000000000000000000000000000000000000000000000000000000000010");
    BOOST_TEST((bits << 1).to_ullong() == 2);
    BOOST_TEST((bits << 4).to_string() ==
      "0000000000000000000000000000000000000000000000000000000000010000");
    BOOST_TEST((bits << 4).to_ullong() == 16);

    BOOST_TEST(SuperBitSet<8>{(bits << 0).to_ullong() - 1}.to_string() ==
      "00000000");
    BOOST_TEST(SuperBitSet<8>{(bits << 0).to_ullong() - 1}.to_ulong() == 0);
    BOOST_TEST(SuperBitSet<8>{(bits << 1).to_ullong() - 1}.to_string() ==
      "00000001");
    BOOST_TEST(SuperBitSet<8>{(bits << 1).to_ullong() - 1}.to_ulong() == 1);
    BOOST_TEST(SuperBitSet<8>{(bits << 4).to_ullong() - 1}.to_string() ==
      "00001111");
    BOOST_TEST(SuperBitSet<8>{(bits << 4).to_ullong() - 1}.to_ulong() == 15);
  }
}

BOOST_AUTO_TEST_SUITE_END() // SuperBitSet_tests
BOOST_AUTO_TEST_SUITE_END() // Utilities
BOOST_AUTO_TEST_SUITE_END() // Cpp


#include <vector>
#include <boost/format.hpp>
#include <algorithm>
#include <fstream>
#include <string>
#include <random>
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/random.hpp>

using boost::multiprecision::cpp_int;
template <typename Rng, size_t BitCount>
using bit_generator = boost::random::independent_bits_engine<Rng, BitCount, cpp_int>;

template<typename E>
E gcd(E a, E b) {
    E zero(0);
    while (b != zero) {
        a = a % b;
        std::swap(a, b);
    }
    return a;
}

template<typename T>
std::pair<T, T>
quotient_remainder(T a, T b) {
  return { a / b, a % b };
}

template <typename E>
std::pair<E, E> extended_gcd(E a, E b) {
    E x0(1);
    E x1(0);
    while (b != E(0)) {
        // compute new r and x
        std::pair<E, E> qr = quotient_remainder(a, b);
        E x2 = x0 - qr.first * x1;
        // shift r and x
        x0 = x1;
        x1 = x2;
        a = b;
        b = qr.second;
    }
    return {x0, a};
}

template<typename Integer>
constexpr
inline
bool odd(Integer x) {
  return (x & 1) == 1;
}

template<typename Integer>
constexpr
inline
bool even(Integer x) {
  return !odd(x);
}

template<typename Integer>
constexpr
inline
Integer half(Integer x) {
  // precondition(x >= 0)
  return x >> 1;
}

template <typename A, typename N, typename Op>
// requires (Domain<Op, A>)
A power_accumulate_semigroup(A r, A a, N n, Op op) {
    // precondition(n >= 0);
    if (n == 0) return r;
    while (true) {
        if (odd(n)) {
            r = op(r, a);
            if (n == 1) return r;
        }
        n = half(n);
        a = op(a, a);
    }
}

template <typename MultiplicativeSemigroup, typename Integer>
MultiplicativeSemigroup power_accumulate_semigroup(MultiplicativeSemigroup r, MultiplicativeSemigroup a, Integer n) {
    // precondition(n >= 0);
    if (n == 0) return r;
    while (true) {
        if (odd(n)) {
            r = r * a;
            if (n == 1) return r;
        }
        n = half(n);
        a = a * a;
    }
}

template <typename A, typename N, typename Op>
// requires (Domain<Op, A>)
A power_semigroup(A a, N n, Op op) {
    // precondition(n > 0);
    while (!odd(n)) {
        a = op(a, a);
        n = half(n);
    }
    if (n == 1) return a;
    // Auch
    return power_accumulate_semigroup(a, op(a, a), half(N{n - 1}), op);
}

template <typename MultiplicativeSemigroup, typename Integer>
MultiplicativeSemigroup power_semigroup(MultiplicativeSemigroup a, Integer n) {
    // precondition(n > 0);
    while (!odd(n)) {
        a = a * a;
        n = half(n);
    }
    if (n == 1) return a;
    return power_accumulate_semigroup(a, a * a, half(n - 1));
}

template<typename T>
struct modulo_multiply {
  T value;
  modulo_multiply() = default;
  modulo_multiply(const modulo_multiply &) = default;
  modulo_multiply(const T x) : value(x) {}
  T operator()(T x, T y) {
    return (x * y) % value;
  }
};

template<typename T>
std::pair<T, T> factorize(T x, T val) {
  // Bad code, can be better
  T k;
  for (k = 0; x % val == 0; x /= val, ++k);
  return std::make_pair(x, k);
}

template <typename I>
bool miller_rabin_test(I n, I q, I k, I w) {
    // precondition n > 1 && n - 1 = 2^kq && q is odd

    modulo_multiply<I> mmult(n);
    I x = power_semigroup(w, q, mmult);
    if (x == I(1) || x == n - I(1)) return true;
    for (I i(1); i < k; ++i) {

        // invariant x = w^{2^{i-1}q}

        x = mmult(x, x);
        if (x == n - I(1)) return true;
        if (x == I(1))     return false;
    }
    return false;
}

template <typename N>
N stein_gcd(N m, N n) {
    if (m < N(0)) m = -m;
    if (n < N(0)) n = -n;
    if (m == N(0)) return n;
    if (n == N(0)) return m;

    // m > 0 && n > 0

    int d_m = 0;
    while (even(m)) { m >>= 1; ++d_m;}

    int d_n = 0;
    while (even(n)) { n >>= 1; ++d_n;}

    // odd(m) && odd(n)

    while (m != n) {
      if (n > m) std::swap(n, m);
      m -= n;
      do m >>= 1; while (even(m));
    }

    // m == n

    return m << std::min(d_m, d_n);
}

template<typename Integer, typename RandomGenerator>
inline
Integer random_coprime(Integer x, RandomGenerator& rand) {
  Integer number = rand();
  while (number == 0) number = rand();

  Integer gcd_value = gcd(x, number);
  if (gcd_value == Integer{1}) {
    return number;
  }
  ++number;
  return number;
}

template<typename Integer, typename ProbeGenerator>
bool check_primarity(Integer n, ProbeGenerator& rand) {
  if (even(n)) { return false; }

  std::pair<Integer, Integer> qk = factorize(Integer{n - 1}, Integer{2});
  const constexpr size_t witnesses = 100;

  for (size_t i = 0; i < witnesses; ++i) {
    Integer w = random_coprime(n, rand);
    if (!miller_rabin_test(n, qk.first, qk.second, w)) {
      return false;
    }
  }
  return true;
}

template<typename PrimarityTest, typename ProbeGenerator, typename WitnessGenerator>
auto probable_prime(PrimarityTest primarity_test, ProbeGenerator& rand, WitnessGenerator& rand_wintness) {
  auto x = rand();
  while (!primarity_test(x, rand_wintness))  {
    x = rand();
  }
  return x;
}

template<typename ProbeGenerator, typename WitnessGenerator>
auto probable_prime(ProbeGenerator& rng, WitnessGenerator& wng) {
  return probable_prime([](const auto &x, auto& rnd) { return check_primarity(x, rnd); }, rng, wng);
}

template <typename I>
I multiplicative_inverse(I a, I n) {
    std::pair<I, I> p = extended_gcd(a, n);
    if (p.second != I(1)) return I(0);
    if (p.first < I(0)) return p.first + n;
    return p.first;
}

template<typename T0, typename T1, typename T2>
struct triple {
  T0 m0;
  T1 m1;
  T2 m2;
  triple(const T0 &m0, const T1 &m1, const T2 &m2) : m0(m0), m1(m1), m2(m2) {}
};

struct rsa_keychain {
  cpp_int n = 0;
  cpp_int public_key = 0;
  cpp_int private_key = 0;
  rsa_keychain(const cpp_int &n, const cpp_int &public_key, const cpp_int &private_key) : n(n),
                                                                                public_key(public_key),
                                                                                private_key(private_key) {}
  size_t block_power_of_2() const
  {
    cpp_int value;
    if (public_key == 0) { value = private_key; }
    else if (private_key == 0) { value = public_key; }
    else { value = std::min(public_key, private_key); }
    const cpp_int m = cpp_int{std::min(value, n)};
    return static_cast<size_t>(factorize(m, cpp_int{2}).first);
  }
};

template <typename ProbeGenerator, typename WitnessGenerator>
rsa_keychain rsa_key_generation(ProbeGenerator& p_generator, WitnessGenerator& w_generator) {
  typedef cpp_int Integer;
  while(true) {
  Integer p1 = probable_prime(p_generator, w_generator);
  Integer p2 = probable_prime(p_generator, w_generator);
  Integer n = p1 * p2;
  Integer phi = (p1 - 1) * (p2 - 1);
  Integer public_key = random_coprime(phi, p_generator);
  while (public_key >= phi) {
    public_key = random_coprime(phi, p_generator);
  }
  Integer private_key = multiplicative_inverse(public_key, phi);
  if (private_key != 0)
  return rsa_keychain(n, public_key, private_key);
  }
}

// implement using operator<<
std::pair<std::string, std::string> save(const rsa_keychain &rsa_data, const std::string &file_name) {
  auto first = file_name + ".public";
  std::ofstream public_file(first);
  public_file << std::hex << rsa_data.n << std::endl << rsa_data.public_key;
  auto second = file_name + ".public";
  std::ofstream private_file(second);
  private_file << std::hex << rsa_data.n << std::endl << rsa_data.private_key;
  return {first, second};
}

/*
template<typename OutputIterator>
OutputIterator encrypt(uint8_t *first, uint8_t *last, const cpp_int &n, const cpp_int &public_key, OutputIterator out) {
  constexpr size_t block_size = factorize(n, cpp_int{2});
  auto qr = quotient_remainder(size_t{last - first}, block_size);
  for (size_t i = 0; i != qr.first; ++i) {
    cpp_int val = *(uint64_t*)first;
  }
  return out;
}
*/

cpp_int crypt(cpp_int val, cpp_int n, cpp_int key) {
  return power_semigroup(val, key, modulo_multiply<cpp_int>(n));
}

int main() {
  //boost::mt19937 rng(42);
  bit_generator<std::random_device, 512> probe_generator, witness_generator;
  auto rsa_triple = rsa_key_generation(probe_generator, witness_generator);
  cpp_int value = 65;
  auto encrypted = crypt(value, rsa_triple.n, rsa_triple.public_key);
  auto decrypted = crypt(encrypted, rsa_triple.n, rsa_triple.private_key);
  std::cout << decrypted << std::endl;

}


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

#include <boost/test/included/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/math/concepts/real_concept.hpp>
#include <boost/math/special_functions/bessel.hpp>
#include <boost/math/quadrature/trapezoidal.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>

using boost::multiprecision::cpp_bin_float_50;
using boost::multiprecision::cpp_bin_float_100;
using boost::math::quadrature::trapezoidal;

template<class Real>
void test_constant()
{
    std::cout << "Testing constants are integrated correctly by the adaptive trapezoidal routine on type " << boost::typeindex::type_id<Real>().pretty_name()  << "\n";

    auto f = [](Real)->Real { return boost::math::constants::half<Real>(); };
    Real Q = trapezoidal<decltype(f), Real>(f, (Real) 0.0, (Real) 10.0);
    BOOST_CHECK_CLOSE(Q, 5.0, 100*std::numeric_limits<Real>::epsilon());
}

template<class Real>
void test_rational_periodic()
{
    using boost::math::constants::two_pi;
    using boost::math::constants::third;
    std::cout << "Testing that rational periodic functions are integrated correctly by trapezoidal rule on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";

    auto f = [](Real x)->Real { return 1/(5 - 4*cos(x)); };

    Real tol = 100*boost::math::tools::epsilon<Real>();
    Real Q = trapezoidal(f, (Real) 0.0, two_pi<Real>(), tol);

    BOOST_CHECK_CLOSE_FRACTION(Q, two_pi<Real>()*third<Real>(), 10*tol);
}

template<class Real>
void test_bump_function()
{
    std::cout << "Testing that bump functions are integrated correctly by trapezoidal rule on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";
    auto f = [](Real x)->Real {
        if( x>= 1 || x <= -1)
        {
            return (Real) 0;
        }
        return (Real) exp(-(Real) 1/(1-x*x));
    };
    Real tol = boost::math::tools::epsilon<Real>();
    Real Q = trapezoidal(f, (Real) -1, (Real) 1, tol);
    // 2*NIntegrate[Exp[-(1/(1 - x^2))], {x, 0, 1}, WorkingPrecision -> 210]
    Real Q_exp = boost::lexical_cast<Real>("0.44399381616807943782304892117055266376120178904569749730748455394704");
    BOOST_CHECK_CLOSE_FRACTION(Q, Q_exp, 15*tol);
}

template<class Real>
void test_zero_function()
{
    std::cout << "Testing that zero functions are integrated correctly by trapezoidal rule on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";
    auto f = [](Real)->Real { return (Real) 0;};
    Real tol = 100* boost::math::tools::epsilon<Real>();
    Real Q = trapezoidal(f, (Real) -1, (Real) 1, tol);
    BOOST_CHECK_SMALL(Q, 100*tol);
}

template<class Real>
void test_sinsq()
{
    std::cout << "Testing that sin(x)^2 is integrated correctly by the trapezoidal rule on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";
    auto f = [](Real x)->Real { return sin(10*x)*sin(10*x); };
    Real tol = 100* boost::math::tools::epsilon<Real>();
    Real Q = trapezoidal(f, (Real) 0, (Real) boost::math::constants::pi<Real>(), tol);
    BOOST_CHECK_CLOSE_FRACTION(Q, boost::math::constants::half_pi<Real>(), tol);

}

template<class Real>
void test_slowly_converging()
{
    using std::sqrt;
    std::cout << "Testing that non-periodic functions are correctly integrated by the trapezoidal rule, even if slowly, on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";
    // This function is not periodic, so it should not be fast to converge:
    auto f = [](Real x)->Real { using std::sqrt;  return sqrt(1 - x*x); };

    Real tol = sqrt(sqrt(boost::math::tools::epsilon<Real>()));
    Real error_estimate;
    Real Q = trapezoidal(f, (Real) 0, (Real) 1, tol, 15, &error_estimate);
    BOOST_CHECK_CLOSE_FRACTION(Q, boost::math::constants::half_pi<Real>()/2, 10*tol);
}

template<class Real>
void test_rational_sin()
{
    using std::pow;
    using std::sin;
    using boost::math::constants::two_pi;
    using boost::math::constants::half;
    std::cout << "Testing that a rational sin function is integrated correctly by the trapezoidal rule on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";
    Real a = 5;
    auto f = [=](Real x)->Real { using std::sin;  Real t = a + sin(x); return 1.0f / (t*t); };

    Real expected = two_pi<Real>()*a/pow(a*a - 1, 3*half<Real>());
    Real tol = 100* boost::math::tools::epsilon<Real>();
    Real Q = trapezoidal(f, (Real) 0, (Real) boost::math::constants::two_pi<Real>(), tol);
    BOOST_CHECK_CLOSE_FRACTION(Q, expected, tol);
}

BOOST_AUTO_TEST_CASE(trapezoidal_quadrature)
{
    test_constant<float>();
    test_constant<double>();
    test_constant<long double>();
    test_constant<boost::math::concepts::real_concept>();
    test_constant<cpp_bin_float_50>();
    test_constant<cpp_bin_float_100>();

    test_rational_periodic<float>();
    test_rational_periodic<double>();
    test_rational_periodic<long double>();
    test_rational_periodic<boost::math::concepts::real_concept>();
    test_rational_periodic<cpp_bin_float_50>();
    test_rational_periodic<cpp_bin_float_100>();

    test_bump_function<float>();
    test_bump_function<double>();
    test_bump_function<long double>();
    test_rational_periodic<boost::math::concepts::real_concept>();
    test_rational_periodic<cpp_bin_float_50>();

    test_zero_function<float>();
    test_zero_function<double>();
    test_zero_function<long double>();
    test_zero_function<boost::math::concepts::real_concept>();
    test_zero_function<cpp_bin_float_50>();
    test_zero_function<cpp_bin_float_100>();

    test_sinsq<float>();
    test_sinsq<double>();
    test_sinsq<long double>();
    test_sinsq<boost::math::concepts::real_concept>();
    test_sinsq<cpp_bin_float_50>();
    test_sinsq<cpp_bin_float_100>();

    test_slowly_converging<float>();
    test_slowly_converging<double>();
    test_slowly_converging<long double>();
    test_slowly_converging<boost::math::concepts::real_concept>();

    test_rational_sin<float>();
    test_rational_sin<double>();
    test_rational_sin<long double>();
    test_rational_sin<boost::math::concepts::real_concept>();
    test_rational_sin<cpp_bin_float_50>();

}