#include <Eigen/Dense>
#include <unsupported/Eigen/MatrixFunctions>
#include <iostream>
#include <math.h>

//! \brief Compute powers of a square matrix using smart binary representation
//! \param[in,out] A matrix for which you want to compute $A^k$. $A^k$ is stored in $A$
//! \param[out] k integer for $A^k$
template <class Matrix>
void matPow(Matrix & A, unsigned int k) {
    Matrix X = Matrix::Identity(A.rows(), A.cols());

    // p is used as binary mask to check wether $k = \sum_{i = 0}^M b_i 2^i$ has 1 in the $i$-th binary digit
    // obtaining the binay representation of p can be done in many ways, here we use ~k & p to check i-th binary is 1
    unsigned int p = 1;
    // Cycle all the way up to the length of the binary representation of $k$
    for(unsigned int j = 1; j <= ceil(log2(k)); ++j) {
        if( ( ~k & p ) == 0 ) {
            X = X*A;
        }

        A = A*A;
        p = p << 1;
    }
    A = X;
}

int main(void) {
    // Check/Test with provided, complex, matrix
    unsigned int n = 3; // size of matrix
    unsigned int k = 9; // power

    double PI = M_PI; // from math.h
    std::complex<double> I = std::complex<double>(0,1); // imaginary unit

    Eigen::MatrixXcd A(n,n);

    for(unsigned int i = 0; i < n; ++i) {
        for(unsigned int j = 0; j < n; ++j) {
            A(i,j) = exp(2. * PI * I * (double) i * (double) j / (double) n) / sqrt((double) n);
        }
    }

    // Test with simple matrix/simple power
//     Eigen::MatrixXd A(2,2);
//     k = 3;
//     A << 1,2,3,4;

    // Output results
    std::cout << "A = " << A << std::endl;
    std::cout << "Eigen:" << std::endl << "A^" << k << " = " << A.pow(k) << std::endl;
    matPow(A, k);
    std::cout << "Ours:" << std::endl << "A^" << k << " = " << A <<std::endl;
}

Nummerical Methods for CSE/PS1/solutions_ps1/C++/matPow.cpp


//  experiment with the exponential and Poisson distributions
//
//
/*
* Level_8 Exercise 5:
* Test program for experimenting with the exponential and Poisson distributions
*
* work with exponential distribution instead of normal distribution
* Poisson distribution instead of gamma distribution
*
* @file TestNormalDistribution.cpp
* @author Chunyu Yuan
* @version 1.0 02/22/2021
*
*/

#include <boost/math/distributions/normal.hpp> //header file for using normal distributions
#include <boost/math/distributions/gamma.hpp>//header file for using gamma distributions
#include <boost/math/distributions.hpp> // For non-member functions of distributions

#include <vector> //library for using vector
#include <iostream> // Standard Input / Output Streams Library
using namespace std;

/*
* Controls operation of the program
* Return type of main() expects an int
*
* @function main()
* @param none
* @return 0
*/
int main()
{
	// Don't forget to tell compiler which namespace
	using namespace boost::math;

	// Distributional properties
	double x = 10.25;
	double scaleParameter = 0.5;
	exponential_distribution<> myExponential(scaleParameter);
	// Choose precision
	cout.precision(10); // Number of values behind the comma

	// properties
	cout << "\n***Exponential distribution: \n";
	cout << "pdf: " << pdf(myExponential, x) << endl;
	cout << "cdf: " << cdf(myExponential, x) << endl;
	cout << "mean: " << mean(myExponential) << endl;
	cout << "variance: " << variance(myExponential) << endl;
	cout << "median: " << median(myExponential) << endl;
	cout << "mode: " << mode(myExponential) << endl;
	cout << "kurtosis excess: " << kurtosis_excess(myExponential) << endl;
	cout << "kurtosis: " << kurtosis(myExponential) << endl;
	cout << "skewness: " << skewness(myExponential) << endl;
	cout << "characteristic function: " << chf(myExponential, x) << endl;
	cout << "hazard: " << hazard(myExponential, x) << endl;

	// Gamma distribution
//	double alpha = 3.0; // Shape parameter, k
	//double beta = 0.5;	// Scale parameter, theta
//	gamma_distribution<double> myGamma(alpha, beta);

	//Passion distribution
	double mean1 = 3.0;
	poisson_distribution<double> myPoisson(mean1);

	double val = 13.0;
	cout << "\n***Poisson distribution: \n";
	cout << "pdf: " << pdf(myPoisson, val) << endl;
	cout << "cdf: " << cdf(myPoisson, val) << endl;
	cout << "mean: " << mean(myPoisson) << endl;
	cout << "variance: " << variance(myPoisson) << endl;
	cout << "median: " << median(myPoisson) << endl;
	cout << "mode: " << mode(myPoisson) << endl;
	cout << "kurtosis excess: " << kurtosis_excess(myPoisson) << endl;
	cout << "kurtosis: " << kurtosis(myPoisson) << endl;
	cout << "skewness: " << skewness(myPoisson) << endl;
	cout << "characteristic function: " << chf(myPoisson, val) << endl;
	cout << "hazard: " << hazard(myPoisson, val) << endl;
	cout << "\n\n " << endl;
	vector<double> pdfList;
	vector<double> cdfList;

	double start = 0.0;
	double end = 10.0;
	long N = 30;		// Number of subdivisions

	val = 0.0;
	double h = (end - start) / double(N);

	for (long j = 1; j <= N; ++j)
	{
		pdfList.push_back(pdf(myPoisson, val));
		cdfList.push_back(cdf(myPoisson, val));

		val += h;
	}

	for (long j = 0; j < pdfList.size(); ++j)
	{
		cout << pdfList[j] << ", ";

	}

	cout << "***" << endl;

	for (long j = 0; j < cdfList.size(); ++j)
	{
		cout << cdfList[j] << ", ";

	}

	return 0;
}


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

using namespace std;
using namespace Eigen;

// Evaluate the Legendre polynomials and its derivatives in the vector x using the 3-term recursion formulae. The outputs are the matrices Lx and DLx.
void legvals(const VectorXd &x, MatrixXd &Lx, MatrixXd &DLx)
{
    int n = Lx.cols()-1;
    int N = x.size();
    for (int j=0; j<N; j++) {
        Lx(j,0) = 1.;
        Lx(j,1) = x(j);
        DLx(j,0) = 0;
        DLx(j,1) = 1.;
        for (int k=2; k<n+1; k++) {
            Lx(j,k) = (2*k-1.)/k*x(j)*Lx(j,k-1)-(k-1.)/k*Lx(j,k-2);
            DLx(j,k) = (2*k-1.)/k*Lx(j,k-1)+(2*k-1.)/k*x(j)*DLx(j,k-1)-(k-1.)/k*DLx(j,k-2);
        }
    }
}

// Evaluate P_n(x), for a scalar x and integer n.
double Pnx(double x, int n) {
    VectorXd Px(n+1);
    Px(0) = 1.; Px(1) = x;
    for (int k=2; k<n+1; k++)
        Px(k) = (2*k-1.)/k*x*Px(k-1)-(k-1.)/k*Px(k-2);
    return Px(n);
}

// Find the Gauss points using the secant method with regula falsi. The standard secant method may be obtained by commenting out lines 50 and 52.
MatrixXd gaussPts(int n, double rtol=1e-10, double atol=1e-12) {
    MatrixXd zeros(n,n);
    double x0, x1, f0, f1, s;
    for (int k=1; k<n+1; k++) {
        for (int j=1; j<k+1; j++) {
            // Initialise initial guesses.
            if (j==1) x0 = -1.;
            else      x0 = zeros(j-2,k-2);
            if (j==k) x1 = 1.;
            else      x1 = zeros(j-1,k-2);

            // Secant method
            f0 = Pnx(x0,k);
            for (int i=0; i<1e4; i++) {
                f1 = Pnx(x1,k);
                s = f1*(x1-x0)/(f1-f0);
                if (Pnx(x1 - s,k)*f1<0) {
                    x0 = x1; f0 = f1;
                }
                x1 = x1 - s;
                if ((abs(s)<max(atol,rtol*min(abs(x0),abs(x1)))))  {
                    zeros(j-1,k-1) = x1;
                    break;
                }
            }
        }
    }
    return zeros;
}

// Test the implementation.
int main(){
    int n = 8;
    MatrixXd zeros = gaussPts(n);
    cout<<"Zeros: "<<endl<< zeros <<endl;

    for (int k=1; k<n+1; k++) {
        VectorXd xi = zeros.block(0, k-1, k, 1);
        MatrixXd Lx(k,n+1), DLx(k,n+1);
        legvals(xi, Lx, DLx);
        cout<<"Values of the "<<k<<"-th polynomial in the calculated zeros: "<<endl;
        cout<<Lx.col(k).transpose() <<endl;
    }
}

Nummerical Methods for CSE/solutions/solutions_ps10/legendre.cpp


#include <vector>
#include <Eigen/Dense>
#include "univariate.h"
#include "multivariate.h"

namespace numerical_optimization {

// homework1 univariate optimization problems
namespace uni {
constexpr int number_functions = 5;

std::vector<function_t> construct_functions() {

    std::vector<function_t> functions;
    functions.resize(number_functions);
    functions[0] = [](float x){ return std::pow(x, 4)/4 + std::pow(x, 3) + std::pow(x, 2)*(9/2) - 10*x; };
    functions[1] = [](float x){ return std::sin(x) +x*x-10; };
    functions[2] = [](float x){ return -std::exp((-1*x*x)/(1.4*1.4)); };

    // not differentiable cases
    functions[3] = [](float x){ return std::abs(x-0.3); };
    functions[4] = [](float x){ return std::abs(std::log(x)); };

    return functions;
};

std::vector<Univariate> construct_methods(const std::vector<function_t> functions) {
    std::vector<Univariate> methods;
    for(auto func : functions) {
        methods.emplace_back(Univariate(func));
    }
    return methods;
};
/////////////////////////////////
} // the end of namespace uni //
////////////////////////////////

namespace multi {
template<typename Multi>
std::vector<Multi> construct_methods(std::vector<std::function<double(const Vector2d&)>> functions) {
    std::vector<Multi> methods;
    for(const auto& func:functions) {
        methods.emplace_back(Multi(func));
    }
    return methods;
};
///////////////////////////////////
} // the end of namespace multi ///
//////////////////////////////////

namespace hw2 {
constexpr int number_functions = 3;

using namespace multi;
using namespace Eigen;
using function_t = std::function<double(const Vector2d&)>;

std::vector<function_t> construct_functions() {
    std::vector<function_t> functions(number_functions);

    functions[0] = [](Vector2d var) {
        return std::pow((var[0]+2*var[1]-6), 2)
        + std::pow((2*var[0]+var[1]-6), 2);
    };

    functions[1] = [](Vector2d var) {
        return 50*std::pow((var[1]-var[0]*var[0]), 2)
        + std::pow((1.0-var[0]), 2);
    };

    functions[2] = [](Vector2d var) {
        return std::pow((1.5-var[0]+var[0]*var[1]), 2)
        + std::pow((2.25-var[0]+var[0]*var[1]*var[1]), 2)
        + std::pow((2.625-var[0]+var[0]*var[1]*var[1]*var[1]), 2);
    };

    return functions;
};
//////////////////////////////////
} // the end of namespace hw2  ///
//////////////////////////////////

namespace hw5 {
constexpr int number_functions = 3;

using namespace multi;
using namespace Eigen;
using function_t = std::function<double(const Vector2d&)>;

std::vector<function_t> construct_functions() {
    std::vector<function_t> functions(number_functions);

    functions[0] = [](Vector2d var) {
        return std::pow((var[0]+2*var[1]-7), 2)
        + std::pow((2*var[0]+var[1]-5), 2);
    };

    functions[1] = [](Vector2d var) {
        return 40*std::pow((var[1]-var[0]*var[0]), 2)
        + std::pow((1.0-var[0]), 2);
    };

    functions[2] = [](Vector2d var) {
        return std::pow((1.5-var[0]+var[0]*var[1]), 2)
        + std::pow((2.25-var[0]+var[0]*var[1]*var[1]), 2)
        + std::pow((2.625-var[0]+var[0]*var[1]*var[1]*var[1]), 2);
    };

    return functions;
};
//////////////////////////////////
}/// the end of namespace hw5 ////
//////////////////////////////////

namespace hw6 {
constexpr int number_functions = 2;

using namespace Eigen;
using function_t = std::function<double(const Vector4d&, const Vector3d&)>;

std::vector<function_t> construct_functions() {
    std::vector<function_t> functions(number_functions);

    functions[0] = [](Vector4d coeff, Vector3d vars) {
        return coeff[0]*vars[0] + coeff[1]*vars[1] + coeff[2]*vars[2] + coeff[3];
    };

    functions[1] = [](Vector4d coeff, Vector3d vars) {
        double value = -(pow(vars[0]-coeff[0], 2) + pow(vars[1]-coeff[1], 2) + pow(vars[2]-coeff[2], 2))/pow(coeff[3], 2);
        return std::exp(value);
    };

    return functions;
}
//////////////////////////////////
}/// the end of namespace hw6 ////
//////////////////////////////////

namespace hw7 {
// 1. $f(x) = 2(x-0.5)^2 + 1$
// 2. $f(x) = |x-0.5|(cos(12\phi[x-0.5])+1) + 1$
constexpr int number_functions = 2;

using namespace Eigen;
using function_t = std::function<double(const double&)>;

std::vector<function_t> construct_functions() {
    std::vector<function_t> functions(number_functions);

    functions[0] = [](double var) {
        return std::pow((var-0.5),2) + 1;
    };

    functions[1] = [](double var) {
        auto x = var - 0.5;
        return std::abs(x)*(std::cos(12*3.14*x)+1.2);
    };

    return functions;
}
//////////////////////////////////
}/// the end of namespace hw7 ////
//////////////////////////////////

/////////////////////////////////////////////////////
} // the end of namespace numerical_optimization ////
/////////////////////////////////////////////////////


#include <iostream>
#include <vector>

#include <Eigen/Dense>

double f1(const Eigen::Vector2d &x) {
	return x(0) * x(0) + 2. * x(1) * x(1) - 1.;
}

double f2(const Eigen::Vector2d &x) {
	return x(1) - x(0) * x(0);
}

Eigen::Vector2d f(const Eigen::Vector2d &x) {
	return Eigen::Vector2d(f1(x), f2(x));
}

Eigen::Matrix2d Jacobian(const Eigen::Vector2d &x) {
	Eigen::Matrix2d J;
	J <<  2*x(0), 4*x(1),
	     -2*x(0), 	  1;
	return J;
}

Eigen::Vector2d Newton(Eigen::Vector2d x, int n) {
	for(int i = 0; i < n; i++) {
		x += Jacobian(x).fullPivLu().solve(-f(x));
	}

	return x;
}

Eigen::IOFormat ShortFmt(Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", ", ", "", "", "[", "]");

int main() {
	int n = 100;
	std::vector<Eigen::Vector2d> startingPoints(3);

	startingPoints[0] = Eigen::Vector2d(-1., 1.);
	startingPoints[1] = Eigen::Vector2d(1., 1.);
	startingPoints[2] = Eigen::Vector2d(-2., -2.);

	std::cout << "---------------------------" << std::endl;
	for (const Eigen::Vector2d &x : startingPoints) {
		Eigen::Vector2d y = Newton(x, n);
		std::cout << "   x = " << x.format(ShortFmt) << std::endl;
		std::cout << "   y = " << y.format(ShortFmt) << std::endl;
		std::cout << "f(y) = " << f(y).format(ShortFmt) << std::endl;
		std::cout << "---------------------------" << std::endl;
	}

	return 0;
}


//////////////////////////////////////////////////////////////////////////////////////////////
/// \file Operations.cpp
/// \brief Implementation file for the SO3 Lie Group math functions.
/// \details These namespace functions provide implementations of the special orthogonal (SO)
///          Lie group functions that we commonly use in robotics.
///
/// \author Sean Anderson
//////////////////////////////////////////////////////////////////////////////////////////////

#include <lgmath/so3/Operations.hpp>

#include <Eigen/Dense>
#include <stdexcept>
#include <stdio.h>

namespace lgmath {
namespace so3 {

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Builds the 3x3 skew symmetric matrix
///
/// The hat (^) operator, builds the 3x3 skew symmetric matrix from the 3x1 vector:
///
/// v^ = [0.0  -v3   v2]
///      [ v3  0.0  -v1]
///      [-v2   v1  0.0]
///
/// See eq. 5 in Barfoot-TRO-2014 for more information.
//////////////////////////////////////////////////////////////////////////////////////////////
Eigen::Matrix3d hat(const Eigen::Vector3d& vector) {
  Eigen::Matrix3d mat;
  mat <<       0.0,  -vector[2],   vector[1],
         vector[2],         0.0,  -vector[0],
        -vector[1],   vector[0],         0.0;
  return mat;
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Builds a rotation matrix using the exponential map
///
/// This function builds a rotation matrix, C_ab, using the exponential map (from an axis-
/// angle parameterization).
///
///   C_ab = exp(aaxis_ba^),
///
/// where aaxis_ba is a 3x1 axis-angle vector, the magnitude of the angle of rotation
/// can be recovered by finding the norm of the vector, and the axis of rotation is the unit-
/// length vector that arises from normalization. Note that the angle around the axis,
/// aaxis_ba, is a right-hand-rule (counter-clockwise positive) angle from 'a' to 'b'.
///
/// Alternatively, we that note that
///
///   C_ba = exp(-aaxis_ba^) = exp(aaxis_ab^).
///
/// Typical robotics convention has some oddity when it comes using this exponential map in
/// practice. For example, if we wish to integrate the kinematics:
///
///   d/dt C = omega^ * C,
///
/// where omega is the 3x1 angular velocity, we employ the convention:
///
///   C_20 = exp(deltaTime*-omega^) * C_10,
///
/// Noting that omega is negative (left-hand-rule).
/// For more information see eq. 97 in Barfoot-TRO-2014.
//////////////////////////////////////////////////////////////////////////////////////////////
Eigen::Matrix3d vec2rot(const Eigen::Vector3d& aaxis_ba, unsigned int numTerms) {

  // Get angle
  const double phi_ba = aaxis_ba.norm();

  // If angle is very small, return Identity
  if(phi_ba < 1e-12) {
    return Eigen::Matrix3d::Identity();
  }

  if (numTerms == 0) {

    // Analytical solution
    Eigen::Vector3d axis = aaxis_ba/phi_ba;
    const double sinphi_ba = sin(phi_ba);
    const double cosphi_ba = cos(phi_ba);
    return cosphi_ba*Eigen::Matrix3d::Identity() +
           (1.0 - cosphi_ba)*axis*axis.transpose() +
           sinphi_ba*so3::hat(axis);

  } else {

    // Numerical solution (good for testing the analytical solution)
    Eigen::Matrix3d C_ab = Eigen::Matrix3d::Identity();

    // Incremental variables
    Eigen::Matrix3d x_small = so3::hat(aaxis_ba);
    Eigen::Matrix3d x_small_n = Eigen::Matrix3d::Identity();

    // Loop over sum up to the specified numTerms
    for (unsigned int n = 1; n <= numTerms; n++) {
      x_small_n = x_small_n*x_small/double(n);
      C_ab += x_small_n;
    }
    return C_ab;
  }
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Builds and returns both the rotation matrix and SO(3) Jacobian
///
/// Similar to the function 'vec2rot', this function builds a rotation matrix, C_ab, using an
/// equivalent expression to the exponential map, but allows us to simultaneously extract
/// the Jacobian of SO(3), which is also required in some cases.
///
///   J_ab = jac(aaxis_ba)
///   C_ab = exp(aaxis_ba^) = identity + aaxis_ba^ * J_ab
///
/// For more information see eq. 97 in Barfoot-TRO-2014.
//////////////////////////////////////////////////////////////////////////////////////////////
void vec2rot(const Eigen::Vector3d& aaxis_ba, Eigen::Matrix3d* out_C_ab,
             Eigen::Matrix3d* out_J_ab) {

  // Check pointers
  if (out_C_ab == NULL) {
    throw std::invalid_argument("Null pointer out_C_ab in vec2rot");
  }
  if (out_J_ab == NULL) {
    throw std::invalid_argument("Null pointer out_J_ab in vec2rot");
  }

  // Set Jacobian term
  *out_J_ab = so3::vec2jac(aaxis_ba);

  // Set rotation matrix
  *out_C_ab = Eigen::Matrix3d::Identity() + so3::hat(aaxis_ba) * (*out_J_ab);
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Compute the matrix log of a rotation matrix
///
/// Compute the inverse of the exponential map (the logarithmic map). This lets us go from
/// a 3x3 rotation matrix back to a 3x1 axis angle parameterization. In some cases, when the
/// rotation matrix is 'numerically off', this involves some 'projection' back to SO(3).
///
///   aaxis_ba = ln(C_ab)
///
/// where aaxis_ba is a 3x1 axis angle, where the axis is normalized and the magnitude of
/// the rotation can be recovered by finding the norm of the axis angle. Note that the
/// angle around the axis, aaxis_ba, is a right-hand-rule (counter-clockwise positive)
/// angle from 'a' to 'b'.
///
/// Alternatively, we that note that
///
///   aaxis_ab = -aaxis_ba = ln(C_ba) = ln(C_ab^T)
///
/// See Barfoot-TRO-2014 Appendix B2 for more information.
//////////////////////////////////////////////////////////////////////////////////////////////
Eigen::Vector3d rot2vec(const Eigen::Matrix3d& C_ab) {

  // Get angle
  const double phi_ba = acos(0.5*(C_ab.trace()-1.0));
  const double sinphi_ba = sin(phi_ba);

  if (fabs(sinphi_ba) > 1e-9) {

    // General case, angle is NOT near 0, pi, or 2*pi
    Eigen::Vector3d axis;
    axis << C_ab(2,1) - C_ab(1,2),
            C_ab(0,2) - C_ab(2,0),
            C_ab(1,0) - C_ab(0,1);
    return (0.5*phi_ba/sinphi_ba)*axis;

  } else if (fabs(phi_ba) > 1e-9) {

    // Angle is near pi or 2*pi
    // ** Note with this method we do not know the sign of 'phi', however since we know phi is
    //    close to pi or 2*pi, the sign is unimportant..

    // Find the eigenvalues and eigenvectors
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d > eigenSolver(C_ab);

    // Try each eigenvalue
    for (int i = 0; i < 3; i++) {

      // Check if eigen value is near +1.0
      if ( fabs(eigenSolver.eigenvalues()[i] - 1.0) < 1e-6 ) {

        // Get corresponding angle-axis
        Eigen::Vector3d aaxis_ba = phi_ba*eigenSolver.eigenvectors().col(i);
        return aaxis_ba;
      }
    }

    // Runtime error
    throw std::runtime_error("so3 logarithmic map failed to find an axis-angle, "
                             "angle was near pi, or 2*pi, but no eigenvalues were near 1");

  } else {

    // Angle is near zero
    return Eigen::Vector3d::Zero();
  }
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Builds the 3x3 Jacobian matrix of SO(3)
///
/// Build the 3x3 left Jacobian of SO(3).
///
/// For the sake of a notation, we assign subscripts consistence with the rotation,
///
///   J_ab = J(aaxis_ba),
///
/// although we note to the SO(3) novice that this Jacobian is not a rotation matrix, and
/// should be used with care.
///
/// For more information see eq. 98 in Barfoot-TRO-2014.
//////////////////////////////////////////////////////////////////////////////////////////////
Eigen::Matrix3d vec2jac(const Eigen::Vector3d& aaxis_ba, unsigned int numTerms) {

  // Get angle
  const double phi_ba = aaxis_ba.norm();
  if(phi_ba < 1e-12) {

    // If angle is very small, return Identity
    return Eigen::Matrix3d::Identity();
  }

  if (numTerms == 0) {

    // Analytical solution
    Eigen::Vector3d axis = aaxis_ba/phi_ba;
    const double sinTerm = sin(phi_ba)/phi_ba;
    const double cosTerm = (1.0-cos(phi_ba))/phi_ba;
    return sinTerm*Eigen::Matrix3d::Identity() +
           (1.0 - sinTerm)*axis*axis.transpose() +
           cosTerm*so3::hat(axis);
  } else {

    // Numerical solution (good for testing the analytical solution)
    Eigen::Matrix3d J_ab = Eigen::Matrix3d::Identity();

    // Incremental variables
    Eigen::Matrix3d x_small = so3::hat(aaxis_ba);
    Eigen::Matrix3d x_small_n = Eigen::Matrix3d::Identity();

    // Loop over sum up to the specified numTerms
    for (unsigned int n = 1; n <= numTerms; n++) {
      x_small_n = x_small_n*x_small/double(n+1);
      J_ab += x_small_n;
    }
    return J_ab;
  }
}

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Builds the 3x3 inverse Jacobian matrix of SO(3)
///
/// Build the 3x3 inverse left Jacobian of SO(3).
///
/// For the sake of a notation, we assign subscripts consistence with the rotation,
///
///   J_ab_inverse = J(aaxis_ba)^{-1},
///
/// although we note to the SO(3) novice that this Jacobian is not a rotation matrix, and
/// should be used with care. Also, please note that J_ab_inverse is not equivalent to J_ba:
///
///   J(aaxis_ba)^{-1} != J(-aaxis_ba)
///
/// For more information see eq. 99 in Barfoot-TRO-2014.
//////////////////////////////////////////////////////////////////////////////////////////////
Eigen::Matrix3d vec2jacinv(const Eigen::Vector3d& aaxis_ba, unsigned int numTerms) {

  // Get angle
  const double phi_ba = aaxis_ba.norm();
  if(phi_ba < 1e-12) {

    // If angle is very small, return Identity
    return Eigen::Matrix3d::Identity();
  }

  if (numTerms == 0) {

    // Analytical solution
    Eigen::Vector3d axis = aaxis_ba/phi_ba;
    const double halfphi = 0.5*phi_ba;
    const double cotanTerm = halfphi/tan(halfphi);
    return cotanTerm*Eigen::Matrix3d::Identity() +
           (1.0 - cotanTerm)*axis*axis.transpose() -
           halfphi*so3::hat(axis);
  } else {

    // Logic error
    if (numTerms > 20) {
      throw std::invalid_argument("Numerical vec2jacinv does not support numTerms > 20");
    }

    // Numerical solution (good for testing the analytical solution)
    Eigen::Matrix3d J_ab_inverse = Eigen::Matrix3d::Identity();

    // Incremental variables
    Eigen::Matrix3d x_small = so3::hat(aaxis_ba);
    Eigen::Matrix3d x_small_n = Eigen::Matrix3d::Identity();

    // Boost has a bernoulli package... but we shouldn't need more than 20
    Eigen::Matrix<double,21,1> bernoulli;
    bernoulli << 1.0, -0.5, 1.0/6.0, 0.0, -1.0/30.0, 0.0, 1.0/42.0, 0.0, -1.0/30.0,
                 0.0, 5.0/66.0, 0.0, -691.0/2730.0, 0.0, 7.0/6.0, 0.0, -3617.0/510.0,
                 0.0, 43867.0/798.0, 0.0, -174611.0/330.0;

    // Loop over sum up to the specified numTerms
    for (unsigned int n = 1; n <= numTerms; n++) {
      x_small_n = x_small_n*x_small/double(n);
      J_ab_inverse += bernoulli(n)*x_small_n;
    }
    return J_ab_inverse;
  }
}

} // so3
} // lgmath


#include "L2_norm.hpp"
#include "integrate.hpp"
#include <Eigen/Core>
#include <Eigen/Sparse>
#include <gtest/gtest.h>
#include <gtest/gtest.h>

TEST(TestL2Norm, ConstantTest) {
	// In this test, we check that the L2-norm function
	// is able to get a simple constant right

	const double constant = 4.0;
	auto         f = [=](double, double) {
		return constant;
	};

	// We only add one triangle:

	Eigen::MatrixXd vertices(3, 3);
	vertices << 0, 0, 0,
	    1, 0, 0,
	    0, 1, 0;

	Eigen::MatrixXi triangles(1, 3);
	triangles << 0, 1, 2;

	Eigen::VectorXd u(3);
	u << constant, constant, constant;

	const double error = computeL2Difference(vertices, triangles, u, f);

	ASSERT_EQ(0.0, error) << "L2-norm does not give 0 = ||C-C||_2";
}

TEST(TestL2Norm, ShapeFunctionTest) {
	// In this test, we check that the L2-norm function
	// is able to destinguish the shape functions

	for (int i = 0; i < 3; ++i) {
		// We only add one triangle:

		Eigen::MatrixXd vertices(3, 3);
		vertices << 0, 0, 0,
		    1, 0, 0,
		    0, 1, 0;

		Eigen::MatrixXi triangles(1, 3);
		triangles << 0, 1, 2;

		Eigen::VectorXd u(3);
		u << (i == 0), (i == 1), (i == 2);

		auto f = [i](double x, double y) {
			return lambda(i, x, y);
		};

		const double error = computeL2Difference(vertices, triangles, u, f);

		ASSERT_EQ(0.0, error) << "L2-norm does not give 0 = ||lambda_" << i << "-lambda_" << i << "||_2";
	}
}


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

using namespace std;
using namespace Eigen;

int main()
{
   Matrix2d A;
   A << 2, 1,
        2, 0.9999999999;
   FullPivLU<Matrix2d> lu(A);
   cout << "By default, the rank of A is found to be " << lu.rank() << endl;
   lu.setThreshold(1e-5);
   cout << "With threshold 1e-5, the rank of A is found to be " << lu.rank() << endl;
}


/**
 * This file contains summary statistics template functions for Eigen::Matrix data types.
 *
 * More functions are added once a need for the particular function necessitates their development. In other words,
 * until there is a use for developing a summary statistic, I will not implement it. For example, as of right now, there
 * is no kurtosis function, because there are no clients nor any other functions within the library requiring a kurtosis
 * function.
 */

#ifndef PROMETHEUS_SUMMARY_STATISTICS_HPP
#define PROMETHEUS_SUMMARY_STATISTICS_HPP

#include <Eigen/Dense>

// zach's-attempt-at-math
namespace zaamath {
    /**
     * computes the sample mean of a given dataset represented by a matrix whose columns represent different populations
     * @tparam T the datatype of the input matrix (int, float, double, custom, etc.)
     * @tparam RowIndexType the number of rows of the input datatype
     * @tparam ColIndexType the number of columns of the input datatype
     * @param data the data represented by an Eigen::Matrix (vector or matrix)
     * @return the sample mean of the data
     */
    template <class T, int RowIndexType, int ColIndexType>
    Eigen::Matrix<T, 1, ColIndexType> sample_mean(const Eigen::Matrix<T, RowIndexType, ColIndexType>& data) {
        return data.colwise().mean();
    }

    /**
     * computes the sample variance of a given dataset represented by a matrix whose columns represent different
     * populations
     * @tparam T the datatype of the input matrix (int, float, double, custom, etc.)
     * @tparam RowIndexType the number of rows of the input datatype
     * @tparam ColIndexType the number of columns of the input datatype
     * @param data the data represented by an Eigen::Matrix (vector or matrix)
     * @return the sample variance of the data
     */
    template <class T, int RowIndexType, int ColIndexType>
    Eigen::Matrix<T, 1, ColIndexType> sample_var(const Eigen::Matrix<T, RowIndexType, ColIndexType>& data) {
        auto means = data.colwise().mean();
        return (data.rowwise() - means).colwise().squaredNorm() / (data.rows() - 1);
    }

    /**
     * computes the sample standard deviation of a given dataset represented by a matrix whose columns represent
     * different populations
     * @tparam T the datatype of the input matrix (int, float, double, custom, etc.)
     * @tparam RowIndexType the number of rows of the input datatype
     * @tparam ColIndexType the number of columns of the input datatype
     * @param data the data represented by an Eigen::Matrix (vector or matrix)
     * @return the sample standard deviation of the data
     */
    template <class T, int RowIndexType, int ColIndexType>
    Eigen::Matrix<T, 1, ColIndexType> sample_std_dev(const Eigen::Matrix<T, RowIndexType, ColIndexType>& data) {
        auto var = sample_var(data);
        return var.array().sqrt();
    }

    /**
     * computes the range of a given dataset represented by a matrix whose columns represent different populations
     * @tparam T the datatype of the input matrix (int, float, double, custom, etc.)
     * @tparam RowIndexType the number of rows of the input datatype
     * @tparam ColIndexType the number of columns of the input datatype
     * @param data the data represnted by an Eigen::Matrix (vector or matrix)
     * @return the range of each column (max element - min element)
     */
    template <class T, int RowIndexType, int ColIndexType>
    Eigen::Matrix<T, 1, ColIndexType> range(const Eigen::Matrix<T, RowIndexType, ColIndexType>& data) {
        return data.colwise().maxCoeff() - data.colwise().minCoeff();
    }

};

#endif //PROMETHEUS_SUMMARY_STATISTICS_HPP


//  Diagonalizing tridiagonal Toeplitz matrix  with Lapack functions
//  Compile as c++ -O3 -o Tridiag.x TridiagToeplitz.cpp -larmadillo -llapack -lblas
#include <cmath>
#include <iostream>
#include <fstream>
#include <iomanip>
#include <armadillo>

using namespace  std;
using namespace  arma;

// Begin of main program

int main(int argc, char* argv[])
{
  int       i, j, Dim;
  double    RMin, RMax, Step, DiagConst, NondiagConst;
  RMin = 0.0; RMax = 1.0; Dim =20;
  mat Hamiltonian = zeros<mat>(Dim,Dim);
  // Integration step length
  Step    = RMax/ Dim;
  DiagConst = 2.0 / (Step*Step);
  NondiagConst =  -1.0 / (Step*Step);

  // Setting up tridiagonal matrix and diagonalization using Armadillo
  Hamiltonian(0,0) = DiagConst;
  Hamiltonian(0,1) = NondiagConst;
  for(i = 1; i < Dim-1; i++) {
    Hamiltonian(i,i-1)    = NondiagConst;
    Hamiltonian(i,i)    = DiagConst;
    Hamiltonian(i,i+1)    = NondiagConst;
  }
  Hamiltonian(Dim-1,Dim-2) = NondiagConst;
  Hamiltonian(Dim-1,Dim-1) = DiagConst;
  // diagonalize and obtain eigenvalues
  vec Eigval(Dim);
  eig_sym(Eigval, Hamiltonian);
  double pi = acos(-1.0);
  cout << "RESULTS:" << endl;
  cout << setiosflags(ios::showpoint | ios::uppercase);
  cout <<"Number of Eigenvalues = " << setw(15) << Dim << endl;
  cout << "Exact versus numerical eigenvalues:" << endl;
  for(int i = 0; i < Dim; i++) {
    double Exact = DiagConst+2*NondiagConst*cos((i+1)*pi/(Dim+1));
    cout << setw(15) << setprecision(8) << fabs(Eigval[i]-Exact) << endl;
  }
  return 0;
}  //  end of main function


#include <iostream>
#include <Eigen/Dense>
#include <math.h>
#include <string>
#include <fstream>
#include <service.cpp>
#include <profiler.h>

const static Eigen::IOFormat CSVFormat(Eigen::StreamPrecision, Eigen::DontAlignCols, "\t", "\n");

int main()
{
	// Ex. 1
    // Load and store data
    Eigen::MatrixXd matA(512,512);
    std::string filename = "data/Bild";
    size_t row = 512;
    size_t col = 512;
    int load = loadData(matA, filename, row, col);
    std::ofstream file;
    file.open("output/data.txt", std::ofstream::out | std::ofstream::trunc);
	file << "# Picture with k:\n" << matA.format(CSVFormat) << std::endl;
	file.close();

    // Compute U, W, V matrix
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(matA, Eigen::ComputeThinU | Eigen::ComputeThinV);
    Eigen::MatrixXd W = svd.singularValues().asDiagonal();
    Eigen::MatrixXd U = svd.matrixU();
    Eigen::MatrixXd V = svd.matrixV().transpose();

    // Perform k-approx with values in range array
    int range[3]={10,20,50};
	for(int i : range){
        // Use Eigen::seq to decrease dimension of W, and of U and V respectively
        Eigen::MatrixXd W_new = W(Eigen::seq(0,i), Eigen::seq(0,i));
        Eigen::MatrixXd U_new = U(Eigen::all, Eigen::seq(0,i));
        Eigen::MatrixXd V_new = V(Eigen::seq(0,i), Eigen::all);
        Eigen::MatrixXd mat_new = U_new * W_new* V_new;
        // Save new picture in txt file, to make a simple read out in python
        std::ofstream file;
        file.open("output/data"+std::to_string(i)+".txt", std::ofstream::out | std::ofstream::trunc);
        file << "# Picture with k:"+std::to_string(i)+"\n" << mat_new.format(CSVFormat) << std::endl;
        file.close();
    }

    // Ex. 2

    // Number of dimension N linear from 1 to 1000
    Profiler::init(3);
    int anzahl = 1000;
    int start = 1;

    // Vectors for the building of the random NxN matrix, for the LU decomposition and the solve function
    Eigen::VectorXd random_times(anzahl - start);
    Eigen::VectorXd LU_times(anzahl - start);
    Eigen::VectorXd solve_times(anzahl - start);

    for (int N = start; N < anzahl; N++)
    {
        //All timers are reseted
        Profiler::resetAll();
        //Timer for random initialisation and saving
        Profiler::start(0);
        Eigen::MatrixXd M = Eigen::MatrixXd::Random(N, N);
          Profiler::stop(0);
        random_times[N - start] = Profiler::getTimeInS(0);
        Eigen::VectorXd b = Eigen::VectorXd::Random(N);
        //Timer for LU decomposition without saving
        Profiler::start(1);
        M.partialPivLu();
        Profiler::stop(1);
        LU_times[N - start] = Profiler::getTimeInS(1);
        Eigen::PartialPivLU<Eigen::MatrixXd> mat(M);
        //Timer for solve function without saving
        Profiler::start(2);
        Eigen::VectorXd x = mat.solve(b);
        Profiler::stop(2);
        solve_times[N - start] = Profiler::getTimeInS(2);
    }
    // Store vectors in file
    Eigen::MatrixXd store(anzahl-start, 3);
    store << random_times, LU_times, solve_times;
    std::ofstream file2;
    file2.open("output/times.txt", std::ofstream::out | std::ofstream::trunc);
	file2 << "# Times: Random, LU, Solve\n" << store.format(CSVFormat) << std::endl;
	file2.close();

    //Ex. 3
    // Initialize Matrix A with a1 - a10 as vector space
    Eigen::VectorXd a1(4), a2(4), a3(4), a4(4), a5(4), a6(4), a7(4), a8(4), a9(4), a10(4);
    a1 << 4., 1., 2., 4.;
    a2 << 1., 2., 6., 2.;
    a3 << 2., 6., 9., 8.;
    a4 << 5., 3., 8., 6.;
    a5 << 8., 2., 4., 8.;
    a6 << 6., 7., 11., 12.;
    a7 << 3., 8., 15., 10.;
    a8 << -2., 5., 7., 4.;
    a9 << 12,  3., 6., 12.;
    a10 << 3., -1., -4., 2.;

    Eigen::MatrixXd A(4, 10);
    A << a1, a2, a3, a4, a5, a6, a7, a8, a9, a10;

    // find orthonormal basis of a1-a10 with singular value decomposition
    Eigen::JacobiSVD<Eigen::MatrixXd> svd2(A, Eigen::ComputeThinU | Eigen::ComputeThinV);
    Eigen::MatrixXd W2 = svd2.singularValues().asDiagonal();
    Eigen::MatrixXd U2 = svd2.matrixU();
    Eigen::MatrixXd V2 = svd2.matrixV().transpose();

    std::ofstream file3;
    file3.open("output/basis.txt", std::ofstream::trunc);
    // basis vectors are the columns of U with w_i != 0
    for (int i = 0; i< 4; i++){
        if(W2(i,i) >= 1e-10){
            file3 << "Value W_" << i << " is not zero. The corresponding vector is\n" << U2.col(i).format(CSVFormat) << "\n\n";
        }
    }
    file3 << "\nDie U-Matrix" << U2.format(CSVFormat) << std::endl;
    file3 << "\nDie W-Matrix" << W2.format(CSVFormat) << std::endl;
    file3.close();

    return 0;
}


#include <iostream>
#include <set>

#include <Eigen/Core>

#include <cannon/math/primes.hpp>
#include <cannon/log/registry.hpp>

using namespace cannon::math;
using namespace cannon::log;

/*!
 * The sequence of triangle numbers is generated by adding the natural numbers. So the 7th triangle number would be 1 + 2 + 3 + 4 + 5 + 6 + 7 = 28. The first ten terms would be
 *
 *     1, 3, 6, 10, 15, 21, 28, 36, 45, 55
 *
 * Let us list the factors of the first seven triangle numbers:
 *
 *     1: 1
 *     3: 1, 3
 *     6: 1, 2, 3, 6
 *     10: 1, 2, 5, 10
 *     15: 1, 3, 5, 15
 *     21: 1, 3, 7, 21
 *     28: 1, 2, 4, 7, 14, 28
 *
 * We can see that 28 is the first triangle number to have over five divisors.
 *
 * What is the value of the first triangle number to have over five hundred divisors?
 */

unsigned int get_num_factors(unsigned long n) {
  auto prime_factors = get_prime_factorization(n);
  unsigned int num_factors = 1;

  for (auto it = prime_factors.begin(); it != prime_factors.end(); ) {
    unsigned long factor = *it;

    // Given a prime factorization of the input number, all possible factors
    // are all possible products of the factors
    num_factors *= prime_factors.count(factor) + 1;

    // Go to next unique entry
    do {
      ++it;
    } while (it != prime_factors.end() && *it == factor);
  }

  return num_factors;
}

unsigned long find_triangle_number_with_divisors(unsigned int num_divisors) {
  unsigned long prime_upper = 100;
  auto primes = get_primes_up_to(prime_upper);

  unsigned int n = 1;
  while (true) {
    unsigned int triangle = n * (n + 1) / 2;

    unsigned int n_factors = get_num_factors(triangle);

    if (n_factors > num_divisors) {
      return triangle;
    }

    ++n;
  }

}

int main(int argc, char** argv) {
  if (argc != 2) {
    std::cerr << "This script takes only one argument: the number of divisors to search for."  << std::endl;
    return 1;
  }

  std::cout << find_triangle_number_with_divisors(std::stoul(argv[1])) << std::endl;
}


// C++ includes
#include <iostream>
using namespace std;

// Eigen includes
#include <Eigen/Core>
using namespace Eigen;

// autodiff include
#include <autodiff/forward.hpp>
#include <autodiff/forward/eigen.hpp>
using namespace autodiff;

// The scalar function for which the gradient is needed
dual f(const VectorXdual& x)
{
    return x.cwiseProduct(x).sum(); // sum([x(i) * x(i) for i = 1:5])
}

int main()
{
    VectorXdual x(5);    // the input vector x with 5 variables
    x << 1, 2, 3, 4, 5;  // x = [1, 2, 3, 4, 5]

    dual u;  // the output scalar u = f(x) evaluated together with gradient below

    VectorXd g = gradient(f, wrt(x), at(x), u);  // evaluate the function value u and its gradient vector g = du/dx

    cout << "u = " << u << endl;    // print the evaluated output u
    cout << "g = \n" << g << endl;  // print the evaluated gradient vector g = du/dx
}


#include <iostream>
#include <Eigen/Dense>
#include "../Optimizer/optimizer"
using namespace std;
using namespace Eigen;
using namespace Optimizer;

double func (double x) {
    // This function will return the square after adding 10 to the given number.
    return pow(x + 10, 2);
}

double SumSquares (VectorXd x) {
    return x.squaredNorm();
}

double Matyas (Vector2d x) {
    return 0.26 * (pow(x(0),2) + pow(x(1),2)) - 0.48 * x(0) * x(1);
}

double Dixon (VectorXd x) {
	double val = 0;
	for (int i = 0; i < x.size() - 1; ++i)
		val += 100 * pow(x(i + 1) - pow(x(i), 2), 2) + pow(x(i) - 1, 2);
	return val;
}

double Himmelblau(Vector2d x)
{
    return pow((pow(x(0),2) + x(1) -11),2) + pow((x(0) + pow(x(1),2) - 7),2);
}

double func_cust(Vector2d x) {
    return pow(x(0)-10, 3) + pow(x(1)-20, 3);
}

double ineq1(Vector2d x){
    return - (pow(x(0)-5, 2) + pow(x(1)-5, 2) + 100);
}

double ineq2(Vector2d x){
    return - (pow(x(0)-5, 2) + pow(x(1)-5, 2) - 82.81);
}

int main () {

    cout << "Using Function: (x + 10)^2 for single variable algorithms testing." << endl;
    double ipt = 5.4;

    cout << "Test Bounding Phase:" << endl;
    Vector2d range = BoundingPhase(func, ipt);
    cout << "Range from bounding Phase for initial point :" << ipt << endl;
    cout << range << endl;

    cout << "Test Exhaustive Search:" << endl;
    range = Exhaustive(func, ipt);
    cout << "Range from Exhaustive Search for initial point :" << ipt << endl;
    cout << range << endl;

    cout << "Derivatives at " << ipt << endl;
    cout << Derivative(func, ipt) << endl;

    cout << "Finding optimal point using above range for Newton Rapshon Method." << endl;
    cout << "Optimal Point is: ";
    cout << NewtonRapshon (func, range) << endl;

    cout << "Finding optimal point using above range for Secant Method." << endl;
    cout << "Optimal Point is: ";
    cout << Secant (func, range) << endl;

    cout << "Finding optimal point using above range for Golden Section Search Method." << endl;
    cout << "Optimal Point is: ";
    cout << GoldenSection (func, range) << endl;

    cout << "Finding optimal point using above range for Interval Halving Method." << endl;
    cout << "Optimal Point is: ";
    cout << IntervalHalving (func, range) << endl;

    cout << "Finding optimal point using above range for Fibonnaci Search Method." << endl;
    cout << "Optimal Point is: ";
    cout << Fibonacci (func, range) << endl;

    cout << "Testing SVOptimize on (x + 10)^2 with initial point 5.4." << endl;
    cout << "The Optimal Point obtained is: ";
    cout << SVOptimize(func, 5.4) << endl;

    cout << "Testing Bisection method on (x + 10)^2" << endl;
    cout << "The Optimal Point obtained is: ";
    cout << Bisection(func, Eigen::Vector2d(-20,1) ,0.001, 10000) << endl;

    cout << "Testing Gradient function using SumSquares with 3 variables" << endl;
    cout << Gradient(SumSquares, Vector3d(3, 3, 4)) << endl;

    cout << "Testing Hessian function using SumSquares with 3 variables" << endl;
    cout << Hessian(SumSquares, Vector3d(3, 3, 4)) << endl;

    Vector3d x(4, -3, 7);

    cout << "Testing MVOptimize on SumSquares with initial point (4, -3, 7)." << endl;
    cout << "The Optimal Point obtained is: ";
    cout << MVOptimize(SumSquares, x, 200,  MVO::NEWTON) << endl;

    cout << "Testing DFP on sum squared function with 3 variables and initial point (4, -3, 7)." << endl;
    cout << "The Optimal Point obtained is: " << endl;
    cout << DFP(SumSquares, x) << endl;

    cout << "Testing Newton's method on sum squared function with 3 variables and initial point (4, -3, 7)." << endl;
    cout << "The Optimal Point obtained is: " << endl;
    cout << Newton(SumSquares, x) << endl;

    cout << "Testing Cauchy's method on sum squared function with 3 variables and initial point (4, -3, 7)." << endl;
    cout << "The Optimal Point obtained is: " << endl;
    cout << Cauchy(SumSquares, x) << endl;

    cout << "Testing Marquardt's method on sum squared function with 3 variables and initial point (4, -3, 7)." << endl;
    cout << "The Optimal Point obtained is: " << endl;
    cout << Marquardt(SumSquares, x) << endl;

    cout << "Testing Conjugate Gradient method on sum squared function with 3 variables and initial point (4, -3, 7)." << endl;
    cout << "The Optimal Point obtained is: " << endl;
    cout << ConjugateGradient(SumSquares, x) << endl;

    cout << "Testing Conjugate Gradient method on Matyas function with 2 variables and initial point (-3, 7)." << endl;
    cout << "The Optimal Point obtained is: " << endl;
    cout << ConjugateGradient(Matyas, Vector2d(-3, 7)) << endl;

    cout << "Testing Conjugate Gradient method on Dixon function with initial point (-3, 8, 0)." << endl;
    cout << "The Optimal Point obtained is: " << endl;
    cout << ConjugateGradient(Dixon, Vector3d(-3, 8, 0), 30000) << endl;

    cout << "Testing Simplex method on the Himmelblau function with two variables" << endl;
    cout << "The Optimal Point obtained is: " << endl;
    VectorXd vec(2);
    vec(0) = 1;
    vec(1) = 2;
    cout << Simplex(Himmelblau,vec,10000, 2, 0.5) << endl;

    cout << "Testing Penalty Function Method with custom function" << endl;
    cout << "The Optimal Point obtained is: " << endl;
    Vector2d x1(2, 2);
    vector<std::function<double (Eigen::VectorXd)>> eq_const;
    vector<std::function<double (Eigen::VectorXd)>> ineq_const{[](Vector2d x) { return pow(x(0) - 5, 2) + pow(x(1), 2) - 26; },
    [](Vector2d x) { return x(0); }, [](Vector2d x) { return x(1); }};
    cout << PenaltyConstrained(Himmelblau, x1, ineq_const, eq_const) << endl;

    cout << "Testing Multiplier Constrained Method with custom function" << endl;
    cout << "The Optimal Point obtained is: " << endl;
    cout << MultiplierConstrained(Himmelblau, x1, ineq_const, eq_const) << endl;

    cout << "Testing Data Save Methods" << endl;
    Optimum point1;
    point1.path = Hessian(SumSquares, Vector3d(3, 3, 4));
    point1.SaveAsTxt();
    point1.SaveAsCsv();
    cout << "TEST SUCCESSFUL" << endl;
}


#pragma once

#include <boost/math/constants/constants.hpp>

#include "fmmtl/meta/integer_sequence.hpp"

/** Computes the ith Chebyshev node of an N-sized quadrature on [-1/2, 1/2] */
template <typename T>
constexpr T chebyshev_node(std::size_t i, std::size_t N) {
  using boost::math::constants::pi;
  return
      (i     == 0  ) ? -0.5 :    // left point
      (i     == N-1) ?  0.5 :    // right point
      (2*i+1 == N  ) ?  0.0 :    // N is odd and i is middle
      (i     <  N/2) ? -std::cos(     i  * pi<T>()/(N-1)) / 2 :  // Force symmetry
      (i     >= N/2) ?  std::cos((N-1-i) * pi<T>()/(N-1)) / 2 :
      T();
}

template <typename T, typename Seq>
struct ChebyshevImpl;

template <typename T, std::size_t... Is>
struct ChebyshevImpl<T,fmmtl::index_sequence<Is...>> {
  static constexpr std::size_t N = sizeof...(Is);
  static constexpr T x[N] = { chebyshev_node<T>(Is, N)... };
};
template <typename T, std::size_t... Is>
constexpr T ChebyshevImpl<T,fmmtl::index_sequence<Is...>>::x[];

/** Precompute N Chebyshev nodes of type T in the range [-1/2, 1/2] */
template <typename T, std::size_t N>
struct Chebyshev : ChebyshevImpl<T,fmmtl::make_index_sequence<N>> {};


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

double F(Eigen::VectorXd x) {
    return pow(x(0), 4.0) + pow(x(0), 2.0) * x(1) + 0.5 * pow(x(0), 2.0) + 3 * x(0) * x(1) + pow(x(1), 2.0);
}

Eigen::VectorXd J(Eigen::VectorXd x) {
    int x_size = x.size();

    Eigen::VectorXd jacobian(x_size);

    jacobian(0) = 4 * pow(x(0), 3.0) + 2 * x(0) * x(1) + x(0) + 3 * x(1);
    jacobian(1) = pow(x(0), 2.0) + 3 * x(0) + 2 * x(1);

    return jacobian;
}

Eigen::MatrixXd H(Eigen::VectorXd x) {
    int x_size = x.size();

    Eigen::MatrixXd hessian(x_size, x_size);

    hessian(0, 0) = 12 * pow(x(0), 2.0) + 2 * x(1) + 1;
    hessian(0, 1) = 2 * x(0) + 3;
    hessian(1, 0) = 2 * x(0) + 3;
    hessian(1, 1) = 2;

    return hessian;
}

int main() {
    Eigen::Vector2d x0;

    x0(0) = -0.5;
    x0(1) = -0.5;

    double epsilon = 0.00001;
    double alpha = 0.3;
    double beta = 0.5;

    Eigen::VectorXd x = DM::Newton(x0, epsilon, alpha, beta, F, J, H);

    std::cout << "The solution is: " << x << std::endl;
    std::cout << "The objective is: " << F(x) << std::endl;
}


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

#include <iostream>
#include <iomanip>
#include <vector>
#include <array>
#include <forward_list>
#include <algorithm>
#include <random>
#include <boost/core/lightweight_test.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/math/statistics/univariate_statistics.hpp>
#include <boost/math/statistics/bivariate_statistics.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/multiprecision/cpp_complex.hpp>

using boost::multiprecision::cpp_bin_float_50;
using boost::multiprecision::cpp_complex_50;

/*
 * Test checklist:
 * 1) Does it work with multiprecision?
 * 2) Does it work with .cbegin()/.cend() if the data is not altered?
 * 3) Does it work with ublas and std::array? (Checking Eigen and Armadillo will make the CI system really unhappy.)
 * 4) Does it work with std::forward_list if a forward iterator is all that is required?
 * 5) Does it work with complex data if complex data is sensible?
 */

using  boost::math::statistics::means_and_covariance;
using  boost::math::statistics::covariance;

template<class Real>
void test_covariance()
{
    std::cout << std::setprecision(std::numeric_limits<Real>::digits10+1);
    Real tol = std::numeric_limits<Real>::epsilon();
    using std::abs;

    // Covariance of a single thing is zero:
    std::array<Real, 1> u1{8};
    std::array<Real, 1> v1{17};
    auto [mu_u1, mu_v1, cov1] = means_and_covariance(u1, v1);

    BOOST_TEST(abs(cov1) < tol);
    BOOST_TEST(abs(mu_u1 - 8) < tol);
    BOOST_TEST(abs(mu_v1 - 17) < tol);

    std::array<Real, 2> u2{8, 4};
    std::array<Real, 2> v2{3, 7};
    auto [mu_u2, mu_v2, cov2] = means_and_covariance(u2, v2);

    BOOST_TEST(abs(cov2+4) < tol);
    BOOST_TEST(abs(mu_u2 - 6) < tol);
    BOOST_TEST(abs(mu_v2 - 5) < tol);

    std::vector<Real> u3{1,2,3};
    std::vector<Real> v3{1,1,1};

    auto [mu_u3, mu_v3, cov3] = means_and_covariance(u3, v3);

    // Since v is constant, covariance(u,v) = 0 against everything any u:
    BOOST_TEST(abs(cov3) < tol);
    BOOST_TEST(abs(mu_u3 - 2) < tol);
    BOOST_TEST(abs(mu_v3 - 1) < tol);
    // Make sure we pull the correct symbol out of means_and_covariance:
    cov3 = covariance(u3, v3);
    BOOST_TEST(abs(cov3) < tol);

    cov3 = covariance(v3, u3);
    // Covariance is symmetric: cov(u,v) = cov(v,u)
    BOOST_TEST(abs(cov3) < tol);

    // cov(u,u) = sigma(u)^2:
    cov3 = covariance(u3, u3);
    Real expected = Real(2)/Real(3);

    BOOST_TEST(abs(cov3 - expected) < tol);

    std::mt19937 gen(15);
    // Can't template standard library on multiprecision, so use double and cast back:
    std::uniform_real_distribution<double> dis(-1.0, 1.0);
    std::vector<Real> u(500);
    std::vector<Real> v(500);
    for(size_t i = 0; i < u.size(); ++i)
    {
        u[i] = (Real) dis(gen);
        v[i] = (Real) dis(gen);
    }

    Real mu_u = boost::math::statistics::mean(u);
    Real mu_v = boost::math::statistics::mean(v);
    Real sigma_u_sq = boost::math::statistics::variance(u);
    Real sigma_v_sq = boost::math::statistics::variance(v);

    auto [mu_u_, mu_v_, cov_uv] = means_and_covariance(u, v);
    BOOST_TEST(abs(mu_u - mu_u_) < tol);
    BOOST_TEST(abs(mu_v - mu_v_) < tol);

    // Cauchy-Schwartz inequality:
    BOOST_TEST(cov_uv*cov_uv <= sigma_u_sq*sigma_v_sq);
    // cov(X, X) = sigma(X)^2:
    Real cov_uu = covariance(u, u);
    BOOST_TEST(abs(cov_uu - sigma_u_sq) < tol);
    Real cov_vv = covariance(v, v);
    BOOST_TEST(abs(cov_vv - sigma_v_sq) < tol);

}

template<class Real>
void test_correlation_coefficient()
{
    using boost::math::statistics::correlation_coefficient;

    Real tol = std::numeric_limits<Real>::epsilon();
    std::vector<Real> u{1};
    std::vector<Real> v{1};
    Real rho_uv = correlation_coefficient(u, v);
    BOOST_TEST(abs(rho_uv - 1) < tol);

    u = {1,1};
    v = {1,1};
    rho_uv = correlation_coefficient(u, v);
    BOOST_TEST(abs(rho_uv - 1) < tol);

    u = {1, 2, 3};
    v = {1, 2, 3};
    rho_uv = correlation_coefficient(u, v);
    BOOST_TEST(abs(rho_uv - 1) < tol);

    u = {1, 2, 3};
    v = {-1, -2, -3};
    rho_uv = correlation_coefficient(u, v);
    BOOST_TEST(abs(rho_uv + 1) < tol);

    rho_uv = correlation_coefficient(v, u);
    BOOST_TEST(abs(rho_uv + 1) < tol);

    u = {1, 2, 3};
    v = {0, 0, 0};
    rho_uv = correlation_coefficient(v, u);
    BOOST_TEST(abs(rho_uv) < tol);

    u = {1, 2, 3};
    v = {0, 0, 3};
    rho_uv = correlation_coefficient(v, u);
    // mu_u = 2, sigma_u^2 = 2/3, mu_v = 1, sigma_v^2 = 2, cov(u,v) = 1.
    BOOST_TEST(abs(rho_uv - sqrt(Real(3))/Real(2)) < tol);
}

int main()
{
    test_covariance<float>();
    test_covariance<double>();
    test_covariance<long double>();
    test_covariance<cpp_bin_float_50>();

    test_correlation_coefficient<float>();
    test_correlation_coefficient<double>();
    test_correlation_coefficient<long double>();
    test_correlation_coefficient<cpp_bin_float_50>();

    return boost::report_errors();
}


#include <iostream>
using namespace std;
#include <ctime>
// Eigen 部分
#include <Eigen/Core>
// 稠密矩阵的代数运算（逆，特征值等）
#include <Eigen/Dense>

#define MATRIX_SIZE 50

/****************************
* 本程序演示了 Eigen 基本类型的使用
****************************/

int main( int argc, char** argv )
{
    // Eigen 中所有向量和矩阵都是Eigen::Matrix，它是一个模板类。它的前三个参数为：数据类型，行，列
    // 声明一个2*3的float矩阵
    Eigen::Matrix<float, 2, 3> matrix_23;

    // 同时，Eigen 通过 typedef 提供了许多内置类型，不过底层仍是Eigen::Matrix
    // 例如 Vector3d 实质上是 Eigen::Matrix<double, 3, 1>，即三维向量
    Eigen::Vector3d v_3d;
	// 这是一样的
    Eigen::Matrix<float,3,1> vd_3d;

    // Matrix3d 实质上是 Eigen::Matrix<double, 3, 3>
    Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero(); //初始化为零
    // 如果不确定矩阵大小，可以使用动态大小的矩阵
    Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > matrix_dynamic;
    // 更简单的
    Eigen::MatrixXd matrix_x;
    // 这种类型还有很多，我们不一一列举

    // 下面是对Eigen阵的操作
    // 输入数据（初始化）
    matrix_23 << 1, 2, 3, 4, 5, 6;
    // 输出
    cout << matrix_23 << endl;

    // 用()访问矩阵中的元素
    for (int i=0; i<2; i++) {
        for (int j=0; j<3; j++)
            cout<<matrix_23(i,j)<<"\t";
        cout<<endl;
    }

    // 矩阵和向量相乘（实际上仍是矩阵和矩阵）
    v_3d << 3, 2, 1;
    vd_3d << 4,5,6;
    // 但是在Eigen里你不能混合两种不同类型的矩阵，像这样是错的
    // Eigen::Matrix<double, 2, 1> result_wrong_type = matrix_23 * v_3d;
    // 应该显式转换
    Eigen::Matrix<double, 2, 1> result = matrix_23.cast<double>() * v_3d;
    cout << result << endl;

    Eigen::Matrix<float, 2, 1> result2 = matrix_23 * vd_3d;
    cout << result2 << endl;

    // 同样你不能搞错矩阵的维度
    // 试着取消下面的注释，看看Eigen会报什么错
    // Eigen::Matrix<double, 2, 3> result_wrong_dimension = matrix_23.cast<double>() * v_3d;

    // 一些矩阵运算
    // 四则运算就不演示了，直接用+-*/即可。
    matrix_33 = Eigen::Matrix3d::Random();      // 随机数矩阵
    cout << matrix_33 << endl << endl;

    cout << matrix_33.transpose() << endl;      // 转置
    cout << matrix_33.sum() << endl;            // 各元素和
    cout << matrix_33.trace() << endl;          // 迹
    cout << 10*matrix_33 << endl;               // 数乘
    cout << matrix_33.inverse() << endl;        // 逆
    cout << matrix_33.determinant() << endl;    // 行列式

    // 特征值
    // 实对称矩阵可以保证对角化成功
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver ( matrix_33.transpose()*matrix_33 );
    cout << "Eigen values = \n" << eigen_solver.eigenvalues() << endl;
    cout << "Eigen vectors = \n" << eigen_solver.eigenvectors() << endl;

    // 解方程
    // 我们求解 matrix_NN * x = v_Nd 这个方程
    // N的大小在前边的宏里定义，它由随机数生成
    // 直接求逆自然是最直接的，但是求逆运算量大

    Eigen::Matrix< double, MATRIX_SIZE, MATRIX_SIZE > matrix_NN;
    matrix_NN = Eigen::MatrixXd::Random( MATRIX_SIZE, MATRIX_SIZE );
    Eigen::Matrix< double, MATRIX_SIZE,  1> v_Nd;
    v_Nd = Eigen::MatrixXd::Random( MATRIX_SIZE,1 );

    clock_t time_stt = clock(); // 计时
    // 直接求逆
    Eigen::Matrix<double,MATRIX_SIZE,1> x = matrix_NN.inverse()*v_Nd;
    cout <<"time use in normal invers is " << 1000* (clock() - time_stt)/(double)CLOCKS_PER_SEC << "ms"<< endl;

	// 通常用矩阵分解来求，例如QR分解，速度会快很多
    time_stt = clock();
    x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
    cout <<"time use in Qr compsition is " <<1000*  (clock() - time_stt)/(double)CLOCKS_PER_SEC <<"ms" << endl;

    return 0;
}


/*
 * MIT License
 * Copyright (c) 2020 Anirudh Topiwala
 * Author: Anirudh Topiwala
 * Create Date: 2020-08
 * Last Edit Date: 2020-08
 *
 * @brief Check if a Point is Inside , On or Outside a given Polygon
 *        Implements two funtions:
 *        1) Check if a point is inside a polygon using windinding number
 *            algorithm.
 *        2) A simpler way to check if a point is inside a convex polygon.
 *
 * A more detailed explanation can be found on my blog post
 * "Is the Point Inside the Polygon"
 * (https://medium.com/@topiwala.anirudh/is-the-point-inside-the-polygon-574b86472119)
 *
 */

#include <Eigen/Dense>
#include <cmath>
#include <iostream>
#include <unordered_map>
#include <vector>

/**
 * @brief The result can be used to test if the query point lies on the left or
 *        right side of the line formed by pt1 and pt2 when viewed in
 *        anticlockwise  direction.
 *
 * @param pt1: First point to form equation of line.
 * @param pt2: Second point to form equation of line.
 * @param query_point: Query point
 * @return: > 0: Query point lies on left of the line.
 *          = 0: Query point lies on the line.
 *          < 0: Query point lies on right of the line.
 */
double substitute_point_in_line(const Eigen::Vector2d &pt1,
                                const Eigen::Vector2d &pt2,
                                const Eigen::Vector2d &query_point) {
    return ((query_point.y() - pt1.y()) * (pt2.x() - pt1.x())) -
           ((query_point.x() - pt1.x()) * (pt2.y() - pt1.y()));
};

/**
 * @brief Check if a point lies inside, on or outside a convex polygon.
 *
 * For a convex polygon, if the sides of the polygon can be considered as a path
 * from first vertex. Then, a query point is said to be inside the polygon if it
 * lies on the same side of all the line segments making up the path.
 *
 * @param query_point Point to check.
 * @param vertices Vertices making up the polygon.
 * @return  = 1: query_point lies inside the polygon.
 *          = 0: query_point lies on the polygon.
 *          =-1: query_point lies outside the polygon.
 */
int is_point_inside_convex_polygon(const Eigen::Vector2d &query_point,
                                   std::vector<Eigen::Vector2d> &vertices) {
    const int num_sides_of_polygon = vertices.size();
    int count_same_side_results = 0;
    // Iterate over each side.
    for (size_t i = 0; i < num_sides_of_polygon; ++i) {
        const auto point_in_line = substitute_point_in_line(
            vertices[i], vertices[(i + 1) % num_sides_of_polygon], query_point);

        // Check if the point lies on the polygon.
        if (point_in_line == 0) {
            return point_in_line;
        }

        count_same_side_results += point_in_line > 0;
    }
    return (std::abs(count_same_side_results) == num_sides_of_polygon) ? 1 : -1;
}

/**
 * @brief Check if a point lies inside, on or outside any polygon.
 *
 * Winding number algorithm can be used to check if any point lies inside a
 * polygon. A more detailed explanation can be found in the blog post. The link
 * is attached at the top of the file.
 *
 *
 * @param query_point Point to check.
 * @param vertices Vertices making up the polygon in anticlockwise direction.
 * @return  = 1: query_point lies inside the polygon.
 *          = 0: query_point lies on the polygon.
 *          =-1: query_point lies outside the polygon.
 */
int is_point_inside_polygon(const Eigen::Vector2d &query_point,
                            std::vector<Eigen::Vector2d> &vertices) {
    int wn = 0;  // the  winding number counter
    const int num_sides_of_polygon = vertices.size();

    for (size_t i = 0; i < num_sides_of_polygon; ++i) {
        const auto point_in_line = substitute_point_in_line(
            vertices[i], vertices[(i + 1) % num_sides_of_polygon], query_point);

        // Check if the point lies on the polygon.
        if (point_in_line == 0) {
            return 0;
        }
        if (vertices[i].y() <= query_point.y()) {
            // Upward crossing.
            if (vertices[(i + 1) % num_sides_of_polygon].y() >
                query_point.y()) {
                if (point_in_line > 0) {
                    ++wn;  // query point is left of edge
                }
            }
        } else {
            // Downward crossing.
            if (vertices[(i + 1) % num_sides_of_polygon].y() <
                query_point.y()) {
                if (point_in_line < 0) {
                    --wn;  // query point is right of edge
                }
            }
        }
    }
    return (wn != 0) ? 1 : -1;  // Point is inside polygon only if wn != 0
}

int main() {
    // Map to make printing easier.
    std::unordered_map<int, std::string> get_value{
        {-1, "outisde"}, {0, "on"}, {1, "inside"}};
    // Non Convex Polygon
    {
        std::cout << "For Non Convex Polygon...\n" << std::endl;
        // Vetices need to be in anticlockwise direction to fix the notion of
        // left and right of edges made in the comments.
        std::vector<Eigen::Vector2d> vertices{{0, 0}, {3, 1}, {6, 0}, {3, 5}};
        std::vector<Eigen::Vector2d> query_points{
            {3, 2}, {3, 6}, {3, 1}, {0, 0}, {15, 20}};
        for (const auto &point : query_points) {
            std::cout << "Point: " << point.transpose() << " lies "
                      << get_value[is_point_inside_polygon(point, vertices)]
                      << " the polygon." << std::endl;
        }
    }

    std::cout << "\n";

    // Convex Polygons
    std::cout << "For Convex Polygon..." << std::endl;
    // Triangle
    {
        std::cout << "For Triangle\n" << std::endl;
        std::vector<Eigen::Vector2d> vertices{{0, 0}, {6, 0}, {3, 5}};
        std::vector<Eigen::Vector2d> query_points{
            {3, 2}, {3, 6}, {3, 5}, {0, 0}, {15, 20}};
        for (const auto &point : query_points) {
            std::cout << "Point: " << point.transpose() << " lies "
                      << get_value[is_point_inside_polygon(point, vertices)]
                      << " the polygon." << std::endl;
        }
    }

    std::cout << "\n";

    // Quadrilateral
    {
        std::cout << "For convex Quadrilateral\n" << std::endl;
        std::vector<Eigen::Vector2d> vertices{{0, 0}, {6, 0}, {6, 6}, {-1, 10}};
        std::vector<Eigen::Vector2d> query_points{
            {3, 2}, {3, 6}, {3, 5}, {0, 0}, {15, 20}};
        for (const auto &point : query_points) {
            std::cout << "Point: " << point.transpose() << " lies "
                      << get_value[is_point_inside_polygon(point, vertices)]
                      << " the polygon." << std::endl;
        }
    }
}


#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Jacobi>
#include <cmath>
#include <math.h>

using namespace Eigen;

void calc_V(const Eigen::Matrix2f& C, Eigen::Matrix2f& V) {

	Eigen::Matrix2f Sigma;
	float tau, t_2, c_2, s_2;

	tau = (C(1, 1) - C(0, 0)) / (2 * C(1, 0));
	if (tau>0) {
		t_2 = tau - sqrt(1 + tau*tau);
	}
	else {
		t_2 = tau + sqrt(1 + tau*tau);
	}
	c_2 = 1 / sqrt(1 + t_2*t_2);
	s_2 = t_2*c_2;
	V << c_2, -s_2, s_2, c_2;
}

void signconvention(Matrix2f& U, Matrix2f& V, Vector2f& sigma) {
	Vector2f tempvec;
	float tempf;
	if (sigma(0)<0 && sigma(1)<0) {
		U = -U;
		sigma = -sigma;
	}
	if (sigma(0)<0) {
		tempvec = U.col(0);
		U.col(0) << U.col(1);
		U.col(1) = tempvec;
		tempf = sigma(0);
		sigma(0) = sigma(1);
		sigma(1) = tempf;
		tempvec = V.col(0);
		V.col(0) << V.col(1);
		V.col(1) = tempvec;

	}
	else if (sigma(0) >= 0 && sigma(1) >= 0) {
		if (sigma(1) > sigma(0)) {
			tempvec = U.col(0);
			U.col(0) << U.col(1);
			U.col(1) = tempvec;
			tempf = sigma(0);
			sigma(0) = sigma(1);
			sigma(1) = tempf;
			tempvec = V.col(0);
			V.col(0) << V.col(1);
			V.col(1) = tempvec;
		}
	}

}

void My_SVD(const Eigen::Matrix2f& F, Eigen::Matrix2f& U, Eigen::Matrix2f& sigma, Eigen::Matrix2f& V)
{
	Eigen::Matrix2f C,A,Sigma;
	Eigen::Vector2f b,v,sigma_vec;
	Eigen::JacobiRotation<float> G;
	float a, c_U, s_U;

	C = F.transpose()*F;
	calc_V(C,V);

	A = F*V;

	G.makeGivens(A(0,0),A(1,0));
	v<<1,0;
    v.applyOnTheLeft(0,1,G);
    c_U = v(0);
    s_U = -v(1);
    U<<c_U,s_U,-s_U,c_U;
    Sigma = A;
    Sigma.applyOnTheLeft(0,1,G.adjoint());
    //std::cout<<Sigma;
	sigma_vec(0) = Sigma(0,0);
	sigma_vec(1) = Sigma(1,1);

	signconvention(U, V, sigma_vec);

	sigma << sigma_vec(0), 0,
		0, sigma_vec(1);

}

int main() {

	Eigen::Matrix2f F, U, sigma, V;

	F << 10, 2,
		3, 4;

	My_SVD(F, U, sigma, V);

	std::cout << U << std::endl << std::endl;

	std::cout << U*U.transpose() << std::endl << std::endl;

	std::cout << V << std::endl << std::endl;

	std::cout << V*V.transpose() << std::endl << std::endl;

	std::cout << sigma << std::endl << std::endl;

	std::cout << U*sigma*V.adjoint() << std::endl << std::endl;

	std::cout << F << std::endl;

	system("pause");

	return 0;
}


////
//// Copyright (C) 2016 SAM (D-MATH) @ ETH Zurich
//// Author(s): lfilippo <filippo.leonardi@sam.math.ethz.ch>
//// Contributors: tille, jgacon, dcasati
//// This file is part of the NumCSE repository.
////
#include <Eigen/Dense>
#include <iostream>

using namespace Eigen;

/* \brief Performs Gram-Schidt orthonormalization
 * Given a matrix $\mathbf{A}$ of linearly independent columns,
 * returns the result of a Gram-Schmidt orthonormalization.
 * Unstable GS algorithm: output is prone to cancellation issues.
 * \param[in] $\mathbf{A}$ Matrix of linearly independent columns
 * \return Matrix with ONB of $span(a_1, \cdots, a_n)$ as columns
 */

MatrixXd gram_schmidt(const MatrixXd &A) {
	// We create a matrix Q with the same size and data of A
	MatrixXd Q(A);

	for(int k = 0; k < A.cols(); k++) {
		VectorXd sum = VectorXd::Zero(A.cols());

		for(int j = 0; j < k; j++) {
			sum += (Q.col(j).dot(A.col(k))) * Q.col(j);
		}

		Q.col(k) = A.col(k) - sum;
		Q.col(k).normalize();
	}

	return Q;
}

int main(void) {

	// Orthonormality test
	unsigned int n = 4;
	MatrixXd A = MatrixXd::Random(n, n);
	MatrixXd Q = gram_schmidt(A);

	// "How far is Q from being orthonormal?"
	MatrixXd I =  Q.transpose() * Q ; 	// for orthogonal matrices this should be the identity
	std::cout << I << std::endl;

	// Error has to be small, but not zero (why?)
	double err = (I - MatrixXd::Identity(n, n)).norm();
	std::cout << "Error is: " << err << std::endl;

	return 0;
}


/*
 * prime_test.cpp: Math Utility #6. Determine if the given integer is prime, then
 * find and output the next prime starting from the same integer.
 *
 * Version:     1.0.0
 * License:     MIT License (see LICENSE.txt for more details)
 * Author:      Joshua Morrison (MrM21632)
 * Last Edited: 1/17/2018, 5:00pm
 */

#include <cstdint>
#include <cstdio>
#include <cstdlib>
#include <ctime>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/random_device.hpp>
#include <boost/random/uniform_int_distribution.hpp>

// Constant value used for testing primality; ensures we won't run into any
// unepxected results.
#define TEST_NUM (30)

// Constants for random number generation
boost::random::random_device rd;
boost::random::mt19937 mt(rd());

/**
 *  @brief Modular Addition
 *
 *  @param [in] a Summand
 *  @param [in] b Summand
 *  @param [in] n Modulus
 *  @return (a + b) mod n.
 *
 *  @details Performs modular addition on two integers with a given modulus.
 *           This implementation is written so as to avoid integer overflow.
 */
uint64_t mod_add(uint64_t a, uint64_t b, uint64_t n) {
    return ((a % n) + (b % n)) % n;
}

/**
 *  @brief Modular Multiplication
 *
 *  @param [in] a Multiplicand
 *  @param [in] b Multiplier
 *  @param [in] n Modulus
 *  @return ab mod n.
 *
 *  @details Performs modular multiplication on two integers with a given
 *           modulus. This implementation is written so as to avoid integer
 *           overflow.
 */
uint64_t mod_mult(uint64_t a, uint64_t b, uint64_t n) {
    uint64_t r = 0;  // Remainder, to be returned

    // If a is larger than the modulus, we need to reduce it in order to avoid
    // integer overflow.
    if (a >= n) a %= n;

    // This loop performs modular addition on r until the multiplier is 0.
    while (b > 0) {
        if (b & 1)  // Equivalent to "b % 2 == 1"
            r = mod_add(r, a, n);

        a = (a * 2) % n;
        b >>= 1;    // Equivalent to "b /= 2"
    }

    // (a * b) mod n == ((a mod n) * (b mod n)) mod n. At this point, we have
    // calculated r = (a mod n) * (b mod n), so we now need to reduce r by mod
    // n, then return that result.
    return r % n;
}

/**
 *  @brief Modular Exponentiation
 *
 *  @param [in] a Base
 *  @param [in] b Exponent
 *  @param [in] n Modulus
 *  @return a^b mod n.
 *
 *  @details Performs modular exponentiation on two integers with a given
 *           modulus. This implementation is written so as to avoid integer
 *           overflow.
 */
uint64_t mod_pow(uint64_t a, uint64_t b, uint64_t n) {
    uint64_t r = 1;  // Remainder, to be returned

    // If a is larger than the modulus, we need to reduce it in order to avoid
    // integer overflow.
    if (a >= n) a %= n;

    // This loop performs modular multiplication on both the remainder and the
    // base, until our exponent is 0.
    while (b > 0) {
        if (b & 1)  // Equivalent to "b % 2 == 1"
            r = mod_mult(r, a, n);

        a = mod_mult(a, a, n);
        b >>= 1;    // Equivalent to "b /= 2"
    }

    return r;
}

/**
 *  @brief Miller-Rabin Primality Test
 *
 *  @param [in] n Number to test for primality; must be odd
 *  @param [in] d A divisor of n-1; must be odd
 *  @return Returns true if n is likely prime, or false otherwise.
 *
 *  @details Performs a non-deterministic primality test on a given integer to
 *           determine if it is composite or (likely) prime.
 */
bool miller_rabin(uint64_t n, uint64_t d) {
    boost::random::uniform_int_distribution<uint64_t> dist(2, n - 2);

    uint64_t a = dist(mt);          // Randomly select a from [2, n-2]
    uint64_t x = mod_pow(a, d, n);  // Let x = a^d mod n

    // Base Case: if a^d mod n is 1 or n-1, then we can already assume n is
    // prime, so we can return true.
    if (x == 1 || x == n - 1)
        return true;

    // This loop performs modular multiplication on x until d is equal to n-1.
    while (d != (n - 1)) {
        x = mod_mult(x, x, n);
        d <<= 1;  // Equivalent to "d *= 2"

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

    // If we reach this point, we can safely say that n is not prime, so we
    // return false.
    return false;
}

/**
 *  @brief Main Primality Test Algorithm
 *
 *  @param [in] n Number to test for primality; must be odd
 *  @param [in] k Number of times to repeat the test
 *  @return Returns true if n is likely prime, or false otherwise.
 *
 *  @details Performs a series of non-deterministic primality tests on a given
 *           integer to determine if it is composite or (likely) prime. If any
 *           test fails, we immediately assume compositeness.
 */
bool is_prime(uint64_t n, uint64_t k) {
    // Base cases
    if (n <= 1) return false;
    if (n <= 3) return true;
    if (!(n & 1)) return false;  // Equivalent to "n % 2 == 0"

    // At this point, we know n is odd and greater than 1. Now, our goal is to
    // find an integer d such that n-1 = d * 2^r, where r >= 1. The loop divides
    // d by 2 until it is no longer even - at that point, we will have the value
    // we desire.
    uint64_t d = n - 1;
    while (!(d & 1))  // Equivalent to "d % 2 == 0"
        d >>= 1;      // Equivalent to "d /= 2"

    // This loop calls miller_rabin(n, d) k times. If any of the tests fail, we
    // know that n is not prime, so we return false.
    for (uint64_t i = 0; i < k; ++i) {
        if (!miller_rabin(n, d))
            return false;
    }

    // If we reach this point, we can safely assume (NOT guarantee!) n is prime,
    // so we return true.
    return true;
}

/**
 *  @brief Find the Next Prime
 *
 *  @param [in] n Start point for prime search
 *  @return The next prime number after n.
 *
 *  @details Looks for the next prime number to occur after the given integer.
 */
uint64_t next_prime(uint64_t n) {
    // This loop runs for all i in [n+1, inf).
    //
    // Because there is consistently a marginal number of integers between any
    // two primes, this process will execute quickly.
    for (uint64_t i = n + 1; ; ++i) {
        if (is_prime(i, TEST_NUM))
            return i;
    }
}

int main(int argc, char **argv) {
    if (argc != 2) {
        std::printf("Usage: prime_test n\n");
        std::printf("Test the given number for primality, then find the next prime number.\n\n");
        std::printf("n\t\tNumber to test; in range [0, 2^64)\n");
        std::exit(EXIT_FAILURE);
    }

    char *e;
    uint64_t n = std::strtoull(argv[1], &e, 10);

    clock_t start = std::clock();
    bool is_n_prime = is_prime(n, TEST_NUM);
    uint64_t next = next_prime(n);
    clock_t end = std::clock();
    double time = static_cast<double>(end - start) / CLOCKS_PER_SEC;

    std::printf("%llu is %s.\n", n, (is_n_prime ? "PRIME" : "NOT PRIME"));
    std::printf("The next prime is %llu.\n", next);
    std::printf("Process took %.6f seconds.\n", time);
}


#include <Eigen/Geometry>
#include "Kabsch.hpp"
// Given two sets of 3D points, find the rotation + translation + scale
// which best maps the first set to the second.
// Source: http://en.wikipedia.org/wiki/Kabsch_algorithm

// The input 3D points are stored as columns.
Eigen::Affine3d Find3DAffineTransform(Eigen::Matrix3Xd in, Eigen::Matrix3Xd out) {

  // Default output
  Eigen::Affine3d A;
  A.linear() = Eigen::Matrix3d::Identity(3, 3);
  A.translation() = Eigen::Vector3d::Zero();

  if (in.cols() != out.cols())
    throw "Find3DAffineTransform(): input data mis-match";

  // First find the scale, by finding the ratio of sums of some distances,
  // then bring the datasets to the same scale.
  double dist_in = 0, dist_out = 0;
  for (int col = 0; col < in.cols()-1; col++) {
    dist_in  += (in.col(col+1) - in.col(col)).norm();
    dist_out += (out.col(col+1) - out.col(col)).norm();
  }
  if (dist_in <= 0 || dist_out <= 0)
    return A;
  double scale = dist_out/dist_in;
  out /= scale;

  //  printf("scale %lf\n", scale);

  // Find the centroids then shift to the origin
  Eigen::Vector3d in_ctr = Eigen::Vector3d::Zero();
  Eigen::Vector3d out_ctr = Eigen::Vector3d::Zero();
  for (int col = 0; col < in.cols(); col++) {
    in_ctr  += in.col(col);
    out_ctr += out.col(col);
  }
  in_ctr /= in.cols();
  out_ctr /= out.cols();
  for (int col = 0; col < in.cols(); col++) {
    in.col(col)  -= in_ctr;
    out.col(col) -= out_ctr;
  }

  // SVD
  Eigen::MatrixXd Cov = in * out.transpose();
  Eigen::JacobiSVD<Eigen::MatrixXd> svd(Cov, Eigen::ComputeThinU | Eigen::ComputeThinV);

  // Find the rotation
  double d = (svd.matrixV() * svd.matrixU().transpose()).determinant();
  if (d > 0)
    d = 1.0;
  else
    d = -1.0;
  Eigen::Matrix3d I = Eigen::Matrix3d::Identity(3, 3);
  I(2, 2) = d;
  Eigen::Matrix3d R = svd.matrixV() * I * svd.matrixU().transpose();

  // The final transform
  A.linear() = scale * R;
  A.translation() = scale*(out_ctr - R*in_ctr);

  return A;
}

Eigen::Affine3d Find3DAffineTransformSameScale(Eigen::Matrix3Xd in, Eigen::Matrix3Xd out)
{
    // Default output
    Eigen::Affine3d A;
    A.linear() = Eigen::Matrix3d::Identity(3, 3);
    A.translation() = Eigen::Vector3d::Zero();

    if (in.cols() != out.cols())
        throw "Find3DAffineTransform(): input data mis-match";

    // Find the centroids then shift to the origin
    Eigen::Vector3d in_ctr = Eigen::Vector3d::Zero();
    Eigen::Vector3d out_ctr = Eigen::Vector3d::Zero();
    for (int col = 0; col < in.cols(); col++) {
        in_ctr  += in.col(col);
        out_ctr += out.col(col);
    }
    in_ctr /= in.cols();
    out_ctr /= out.cols();
    for (int col = 0; col < in.cols(); col++) {
        in.col(col)  -= in_ctr;
        out.col(col) -= out_ctr;
    }

    // SVD
    Eigen::MatrixXd Cov = in * out.transpose();
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(Cov, Eigen::ComputeThinU | Eigen::ComputeThinV);

    // Find the rotation
    double d = (svd.matrixV() * svd.matrixU().transpose()).determinant();
    if (d > 0)
        d = 1.0;
    else
        d = -1.0;
    Eigen::Matrix3d I = Eigen::Matrix3d::Identity(3, 3);
    I(2, 2) = d;
    Eigen::Matrix3d R = svd.matrixV() * I * svd.matrixU().transpose();

    // The final transform
    A.linear() = R;
    A.translation() = (out_ctr - R*in_ctr);

    return A;
}

// A function to test Find3DAffineTransform()

void TestFind3DAffineTransform(){

  // Create datasets with known transform
  Eigen::Matrix3Xd in(3, 100), out(3, 100);
  Eigen::Quaternion<double> Q(1, 3, 5, 2);
  Q.normalize();
  Eigen::Matrix3d R = Q.toRotationMatrix();
  double scale = 2.0;
  for (int row = 0; row < in.rows(); row++) {
    for (int col = 0; col < in.cols(); col++) {
      in(row, col) = log(2*row + 10.0)/sqrt(1.0*col + 4.0) + sqrt(col*1.0)/(row + 1.0);
    }
  }
  Eigen::Vector3d S;
  S << -5, 6, -27;
  for (int col = 0; col < in.cols(); col++)
    out.col(col) = scale*R*in.col(col) + S;

  Eigen::Affine3d A = Find3DAffineTransform(in, out);

  // See if we got the transform we expected
  if ( (scale*R-A.linear()).cwiseAbs().maxCoeff() > 1e-13 ||
       (S-A.translation()).cwiseAbs().maxCoeff() > 1e-13)
    throw "Could not determine the affine transform accurately enough";
}


///////////////////////////////////////////////////////////////////////////////
//      Copyright Christopher Kormanyos 2020.
// 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)
//

// This example exercises Boost.Multiprecision in concurrent
// multi-threaded environments. To do so a loop involving
// non-trivial calculations of numerous function values
// has been set up within both concurrent as well as
// sequential running environments. In particular,
// this example uses an AGM method to do a "from the ground up"
// calculation of logarithms. The logarithm functions values
// are compared with the values from Boost.Multiprecision's
// specific log functions for the relevant backends.
// The log GM here is not optimized or intended for
// high-performance work, but can be taken as an
// interesting example of an AGM iteration if helpful.

// This example has been initially motivated in part
// by discussions in:
// https://github.com/boostorg/multiprecision/pull/211

// We find the following performance data here:
// https://github.com/boostorg/multiprecision/pull/213
//
// cpp_dec_float:
// result_is_ok_concurrent: true, calculation_time_concurrent: 18.1s
// result_is_ok_sequential: true, calculation_time_sequential: 48.5s
//
// cpp_bin_float:
// result_is_ok_concurrent: true, calculation_time_concurrent: 18.7s
// result_is_ok_sequential: true, calculation_time_sequential: 50.4s
//
// gmp_float:
// result_is_ok_concurrent: true, calculation_time_concurrent: 3.3s
// result_is_ok_sequential: true, calculation_time_sequential: 12.4s
//
// mpfr_float:
// result_is_ok_concurrent: true, calculation_time_concurrent: 0.6s
// result_is_ok_sequential: true, calculation_time_sequential: 1.9s

#include <array>
#include <atomic>
#include <cstddef>
#include <cstdint>
#include <iomanip>
#include <iostream>
#include <limits>
#include <thread>
#include <vector>

#include <boost/math/constants/constants.hpp>
#include <boost/math/special_functions/prime.hpp>

#define BOOST_MULTIPRECISION_EXERCISE_THREADING_BACKEND_CPP_DEC_FLOAT 101
#define BOOST_MULTIPRECISION_EXERCISE_THREADING_BACKEND_GMP_FLOAT 102
#define BOOST_MULTIPRECISION_EXERCISE_THREADING_BACKEND_CPP_BIN_FLOAT 103
#define BOOST_MULTIPRECISION_EXERCISE_THREADING_BACKEND_MPFR_FLOAT 104

#if !defined(BOOST_MULTIPRECISION_EXERCISE_THREADING_BACKEND_TYPE)
#define BOOST_MULTIPRECISION_EXERCISE_THREADING_BACKEND_TYPE \
    BOOST_MULTIPRECISION_EXERCISE_THREADING_BACKEND_CPP_DEC_FLOAT
//#define BOOST_MULTIPRECISION_EXERCISE_THREADING_BACKEND_TYPE
//BOOST_MULTIPRECISION_EXERCISE_THREADING_BACKEND_CPP_BIN_FLOAT #define
//BOOST_MULTIPRECISION_EXERCISE_THREADING_BACKEND_TYPE BOOST_MULTIPRECISION_EXERCISE_THREADING_BACKEND_GMP_FLOAT #define
//BOOST_MULTIPRECISION_EXERCISE_THREADING_BACKEND_TYPE BOOST_MULTIPRECISION_EXERCISE_THREADING_BACKEND_MPFR_FLOAT
#endif

#if (BOOST_MULTIPRECISION_EXERCISE_THREADING_BACKEND_TYPE == \
     BOOST_MULTIPRECISION_EXERCISE_THREADING_BACKEND_CPP_DEC_FLOAT)
#include <nil/crypto3/multiprecision/cpp_dec_float.hpp>

using big_float_type = nil::crypto3::multiprecision::number<nil::crypto3::multiprecision::cpp_dec_float<501>,
                                                            nil::crypto3::multiprecision::et_off>;

#elif (BOOST_MULTIPRECISION_EXERCISE_THREADING_BACKEND_TYPE == \
       BOOST_MULTIPRECISION_EXERCISE_THREADING_BACKEND_CPP_BIN_FLOAT)
#include <nil/crypto3/multiprecision/cpp_bin_float.hpp>

using big_float_type = nil::crypto3::multiprecision::number<nil::crypto3::multiprecision::cpp_bin_float<501>,
                                                            nil::crypto3::multiprecision::et_off>;

#elif (BOOST_MULTIPRECISION_EXERCISE_THREADING_BACKEND_TYPE == \
       BOOST_MULTIPRECISION_EXERCISE_THREADING_BACKEND_GMP_FLOAT)
#include <nil/crypto3/multiprecision/gmp.hpp>

using big_float_type = nil::crypto3::multiprecision::number<nil::crypto3::multiprecision::gmp_float<501>,
                                                            nil::crypto3::multiprecision::et_off>;

#elif (BOOST_MULTIPRECISION_EXERCISE_THREADING_BACKEND_TYPE == \
       BOOST_MULTIPRECISION_EXERCISE_THREADING_BACKEND_MPFR_FLOAT)
#include <nil/crypto3/multiprecision/mpfr.hpp>

using big_float_type = nil::crypto3::multiprecision::number<nil::crypto3::multiprecision::mpfr_float_backend<501>,
                                                            nil::crypto3::multiprecision::et_off>;

#else
#error BOOST_MULTIPRECISION_EXERCISE_THREADING_BACKEND_TYPE is undefined.
#endif

namespace boost {
    namespace multiprecision {
        namespace exercise_threading {

            namespace detail {

                namespace my_concurrency {
                    template<typename index_type, typename callable_function_type>
                    void parallel_for(index_type start, index_type end, callable_function_type parallel_function) {
                        // Estimate the number of threads available.
                        static const unsigned int number_of_threads_hint = std::thread::hardware_concurrency();

                        static const unsigned int number_of_threads_total =
                            ((number_of_threads_hint == 0U) ? 4U : number_of_threads_hint);

                        // Use only 3/4 of the available cores.
                        static const unsigned int number_of_threads =
                            number_of_threads_total - (number_of_threads_total / 8U);

                        std::cout << "Executing with " << number_of_threads << " threads" << std::endl;

                        // Set the size of a slice for the range functions.
                        index_type n = index_type(end - start) + index_type(1);

                        index_type slice =
                            static_cast<index_type>(std::round(n / static_cast<float>(number_of_threads)));

                        slice = (std::max)(slice, index_type(1));

                        // Inner loop.
                        auto launch_range = [&parallel_function](index_type index_lo, index_type index_hi) {
                            for (index_type i = index_lo; i < index_hi; ++i) {
                                parallel_function(i);
                            }
                        };

                        // Create the thread pool and launch the jobs.
                        std::vector<std::thread> pool;

                        pool.reserve(number_of_threads);

                        index_type i1 = start;
                        index_type i2 = (std::min)(index_type(start + slice), end);

                        for (index_type i = 0U; ((index_type(i + index_type(1U)) < number_of_threads) && (i1 < end));
                             ++i) {
                            pool.emplace_back(launch_range, i1, i2);

                            i1 = i2;

                            i2 = (std::min)(index_type(i2 + slice), end);
                        }

                        if (i1 < end) {
                            pool.emplace_back(launch_range, i1, end);
                        }

                        // Wait for the jobs to finish.
                        for (std::thread& thread_in_pool : pool) {
                            if (thread_in_pool.joinable()) {
                                thread_in_pool.join();
                            }
                        }
                    }
                }    // namespace my_concurrency

                template<typename FloatingPointType, typename UnsignedIntegralType>
                FloatingPointType pown(const FloatingPointType& b, const UnsignedIntegralType& p) {
                    // Calculate (b ^ p).

                    using local_floating_point_type = FloatingPointType;
                    using local_unsigned_integral_type = UnsignedIntegralType;

                    local_floating_point_type result;

                    if (p == local_unsigned_integral_type(0U)) {
                        result = local_floating_point_type(1U);
                    } else if (p == local_unsigned_integral_type(1U)) {
                        result = b;
                    } else if (p == local_unsigned_integral_type(2U)) {
                        result = b;
                        result *= b;
                    } else {
                        result = local_floating_point_type(1U);

                        local_floating_point_type y(b);

                        for (local_unsigned_integral_type p_local(p); p_local != local_unsigned_integral_type(0U);
                             p_local >>= 1U) {
                            if ((static_cast<unsigned>(p_local) & 1U) != 0U) {
                                result *= y;
                            }

                            y *= y;
                        }
                    }

                    return result;
                }

                const std::vector<std::uint32_t>& primes() {
                    static const std::vector<std::uint32_t> my_primes = []() -> std::vector<std::uint32_t> {
                        std::vector<std::uint32_t> local_primes(10000U);

                        // Get exactly 10,000 primes.
                        for (auto i = 0U; i < local_primes.size(); ++i) {
                            local_primes[i] = boost::math::prime(i);
                        }

                        return local_primes;
                    }();

                    return my_primes;
                }

            }    // namespace detail

            template<typename FloatingPointType>
            FloatingPointType log(const FloatingPointType& x) {
                // Use an AGM method to compute the logarithm of x.

                // For values less than 1 invert the argument and
                // remember (in this case) to negate the result below.
                const bool b_negate = (x < 1);

                const FloatingPointType xx = ((b_negate == false) ? x : 1 / x);

                // Set a0 = 1
                // Set b0 = 4 / (x * 2^m) = 1 / (x * 2^(m - 2))

                FloatingPointType ak(1U);

                const float n_times_factor =
                    static_cast<float>(static_cast<float>(std::numeric_limits<FloatingPointType>::digits10) * 1.67F);
                const float lgx_over_lg2 = std::log(static_cast<float>(xx)) / std::log(2.0F);

                std::int32_t m = static_cast<std::int32_t>(n_times_factor - lgx_over_lg2);

                // Ensure that the resulting power is non-negative.
                // Also enforce that m >= 8.
                m = (std::max)(m, static_cast<std::int32_t>(8));

                FloatingPointType bk = detail::pown(FloatingPointType(2), static_cast<std::uint32_t>(m));

                bk *= xx;
                bk = 4 / bk;

                FloatingPointType ak_tmp(0U);

                using std::sqrt;

                // Determine the requested precision of the upcoming iteration in units of digits10.
                const FloatingPointType target_tolerance =
                    sqrt(std::numeric_limits<FloatingPointType>::epsilon()) / 100;

                for (std::int32_t k = static_cast<std::int32_t>(0); k < static_cast<std::int32_t>(64); ++k) {
                    using std::fabs;

                    // Check for the number of significant digits to be
                    // at least half of the requested digits. If at least
                    // half of the requested digits have been achieved,
                    // then break after the upcoming iteration.
                    const bool break_after_this_iteration =
                        ((k > static_cast<std::int32_t>(4)) && (fabs(1 - fabs(ak / bk)) < target_tolerance));

                    ak_tmp = ak;
                    ak += bk;
                    ak /= 2;

                    if (break_after_this_iteration) {
                        break;
                    }

                    bk *= ak_tmp;
                    bk = sqrt(bk);
                }

                // We are now finished with the AGM iteration for log(x).

                // Compute log(x) = {pi / [2 * AGM(1, 4 / 2^m)]} - (m * ln2)
                // Note at this time that (ak = bk) = AGM(...)

                // Retrieve the value of pi, divide by (2 * a) and subtract (m * ln2).
                const FloatingPointType result = boost::math::constants::pi<FloatingPointType>() / (ak * 2) -
                                                 (boost::math::constants::ln_two<FloatingPointType>() * m);

                return ((b_negate == true) ? -result : result);
            }

        }    // namespace exercise_threading
    }        // namespace multiprecision
}    // namespace boost

template<typename FloatingPointType>
bool log_agm_concurrent(float& calculation_time) {
    const std::size_t count = nil::crypto3::multiprecision::exercise_threading::detail::primes().size();

    std::vector<FloatingPointType> log_results(count);
    std::vector<FloatingPointType> log_control(count);

    std::atomic_flag log_agm_lock = ATOMIC_FLAG_INIT;

    std::size_t concurrent_log_agm_count = 0U;

    const std::clock_t start = std::clock();

    nil::crypto3::multiprecision::exercise_threading::detail::my_concurrency::parallel_for(
        std::size_t(0U),
        log_results.size(),
        [&log_results, &log_control, &concurrent_log_agm_count, &log_agm_lock](std::size_t i) {
            while (log_agm_lock.test_and_set()) {
                ;
            }
            ++concurrent_log_agm_count;
            if ((concurrent_log_agm_count % 100U) == 0U) {
                std::cout << "log agm concurrent at index " << concurrent_log_agm_count << " of " << log_results.size()
                          << ". Total processed so far: " << std::fixed << std::setprecision(1)
                          << (100.0F * float(concurrent_log_agm_count)) / float(log_results.size()) << "%."
                          << "\r";
            }
            log_agm_lock.clear();

            const FloatingPointType dx =
                (FloatingPointType(1U) / (nil::crypto3::multiprecision::exercise_threading::detail::primes()[i]));
            const FloatingPointType x = boost::math::constants::catalan<FloatingPointType>() + dx;

            const FloatingPointType lr = nil::crypto3::multiprecision::exercise_threading::log(x);
            const FloatingPointType lc = nil::crypto3::multiprecision::log(x);

            log_results[i] = lr;
            log_control[i] = lc;
        });

    calculation_time = static_cast<float>(std::clock() - start) / static_cast<float>(CLOCKS_PER_SEC);

    std::cout << std::endl;

    std::cout << "Checking results concurrent: ";

    bool result_is_ok = true;

    for (std::size_t i = 0U; i < log_results.size(); ++i) {
        using std::fabs;

        const FloatingPointType close_fraction = fabs(1 - (log_results[i] / log_control[i]));

        result_is_ok &= (close_fraction < std::numeric_limits<FloatingPointType>::epsilon() * 1000000U);
    }

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

    return result_is_ok;
}

template<typename FloatingPointType>
bool log_agm_sequential(float& calculation_time) {
    const std::size_t count = nil::crypto3::multiprecision::exercise_threading::detail::primes().size();

    std::vector<FloatingPointType> log_results(count);
    std::vector<FloatingPointType> log_control(count);

    std::atomic_flag log_agm_lock = ATOMIC_FLAG_INIT;

    const std::clock_t start = std::clock();

    for (std::size_t i = 0U; i < log_results.size(); ++i) {
        const std::size_t sequential_log_agm_count = i + 1U;

        if ((sequential_log_agm_count % 100U) == 0U) {
            std::cout << "log agm sequential at index " << sequential_log_agm_count << " of " << log_results.size()
                      << ". Total processed so far: " << std::fixed << std::setprecision(1)
                      << (100.0F * float(sequential_log_agm_count)) / float(log_results.size()) << "%."
                      << "\r";
        }

        const FloatingPointType dx =
            (FloatingPointType(1U) / (nil::crypto3::multiprecision::exercise_threading::detail::primes()[i]));
        const FloatingPointType x = boost::math::constants::catalan<FloatingPointType>() + dx;

        log_results[i] = nil::crypto3::multiprecision::exercise_threading::log(x);
        log_control[i] = nil::crypto3::multiprecision::log(x);
    }

    calculation_time = static_cast<float>(std::clock() - start) / static_cast<float>(CLOCKS_PER_SEC);

    std::cout << std::endl;

    std::cout << "Checking results sequential: ";

    bool result_is_ok = true;

    for (std::size_t i = 0U; i < log_results.size(); ++i) {
        using std::fabs;

        const FloatingPointType close_fraction = fabs(1 - (log_results[i] / log_control[i]));

        result_is_ok &= (close_fraction < std::numeric_limits<FloatingPointType>::epsilon() * 1000000U);
    }

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

    return result_is_ok;
}

int main() {
    std::cout << "Calculating " << nil::crypto3::multiprecision::exercise_threading::detail::primes().size()
              << " primes" << std::endl;

    float calculation_time_concurrent;
    const bool result_is_ok_concurrent = log_agm_concurrent<big_float_type>(calculation_time_concurrent);

    float calculation_time_sequential;
    const bool result_is_ok_sequential = log_agm_sequential<big_float_type>(calculation_time_sequential);

    std::cout << std::endl;

    std::cout << "result_is_ok_concurrent: " << std::boolalpha << result_is_ok_concurrent
              << ", calculation_time_concurrent: " << std::fixed << std::setprecision(1) << calculation_time_concurrent
              << "s" << std::endl;

    std::cout << "result_is_ok_sequential: " << std::boolalpha << result_is_ok_sequential
              << ", calculation_time_sequential: " << std::fixed << std::setprecision(1) << calculation_time_sequential
              << "s" << std::endl;
}


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

using namespace Eigen;
using namespace std;
int main()
{
  Vector3d v(1,2,3);
  Vector3d w(0,1,2);

  cout << "Dot product: " << v.dot(w) << endl;
  double dp = v.adjoint()*w; // automatic conversion of the inner product to a scalar
  cout << "Dot product via a matrix product: " << dp << endl;
  cout << "Cross product:\n" << v.cross(w) << endl;
}