//####### Test module for vec functions ####################################

//Define Module name
 #define BOOST_TEST_MODULE "math/vec_functions"

//Include Boost unit tests library & library for floating point comparison
#include <boost/test/unit_test.hpp>
#include <boost/test/tools/floating_point_comparison.hpp>

#include "vec_functions.hpp"

using namespace picsar::multi_physics::math;

//Tolerance for double precision calculations
const double double_tolerance = 1.0e-12;

//Tolerance for single precision calculations
const float float_tolerance = 1.0e-4;

//Templated tolerance
template <typename T>
T constexpr tolerance()
{
    if(std::is_same<T,float>::value)
        return float_tolerance;
    else
        return double_tolerance;
}

// ------------- Tests --------------

// ***Test norm_square

template<typename RealType>
void test_norm_square()
{
    const auto c0 = static_cast<RealType>(1.0);
    const auto c1 = static_cast<RealType>(2.0);
    const auto c2 = static_cast<RealType>(3.0);
    const auto expected = c0*c0 + c1*c1 + c2*c2;

    const auto vec = vec3<RealType>{c0, c1, c2};

    BOOST_CHECK_SMALL((norm_square(vec)-expected)/expected, tolerance<RealType>());
}

BOOST_AUTO_TEST_CASE( picsar_vec_functions_norm_square )
{
    test_norm_square<double>();
    test_norm_square<float>();
}

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

// ***Test norm

template<typename RealType>
void test_norm()
{
    const auto c0 = static_cast<RealType>(1.0);
    const auto c1 = static_cast<RealType>(2.0);
    const auto c2 = static_cast<RealType>(3.0);
    const auto expected = static_cast<RealType>(sqrt(c0*c0 + c1*c1 + c2*c2));

    const auto vec = vec3<RealType>{c0, c1, c2};

    BOOST_CHECK_SMALL((norm(vec)-expected)/expected, tolerance<RealType>());
}

BOOST_AUTO_TEST_CASE( picsar_vec_functions_norm )
{
    test_norm<double>();
    test_norm<float>();
}

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

// ***Test dot product

template<typename RealType>
void test_dot()
{
    const auto a0 = static_cast<RealType>(1.0);
    const auto a1 = static_cast<RealType>(2.0);
    const auto a2 = static_cast<RealType>(3.0);
    const auto b0 = static_cast<RealType>(-3.0);
    const auto b1 = static_cast<RealType>(-5.0);
    const auto b2 = static_cast<RealType>(-8.0);
    const auto expected = a0*b0 + a1*b1 + a2*b2;

    const auto veca = vec3<RealType>{a0, a1, a2};
    const auto vecb = vec3<RealType>{b0, b1, b2};

    BOOST_CHECK_SMALL((dot(veca,vecb)-expected)/expected, tolerance<RealType>());
}

BOOST_AUTO_TEST_CASE( picsar_vec_functions_dot )
{
    test_dot<double>();
    test_dot<float>();
}

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

// ***Test cross product

template<typename RealType>
void test_cross()
{
    const auto a0 = static_cast<RealType>(1.0);
    const auto a1 = static_cast<RealType>(2.0);
    const auto a2 = static_cast<RealType>(3.0);
    const auto b0 = static_cast<RealType>(-3.0);
    const auto b1 = static_cast<RealType>(-5.0);
    const auto b2 = static_cast<RealType>(-8.0);
    const auto exp0 = a1*b2 - a2*b1;
    const auto exp1 = a2*b0 - a0*b2;
    const auto exp2 = a0*b1 - a1*b0;

    const auto veca = vec3<RealType>{a0, a1, a2};
    const auto vecb = vec3<RealType>{b0, b1, b2};
    const auto veca_x_vecb = cross(veca, vecb);
    const auto expected = std::array<RealType,3>{exp0, exp1, exp2};

    for(int i = 0; i < 3; i++){
        if(expected[i] != 0.0)
            BOOST_CHECK_SMALL((veca_x_vecb[i]-expected[i])/expected[i], tolerance<RealType>());
        else
            BOOST_CHECK_SMALL(veca_x_vecb[i]-expected[i], tolerance<RealType>());
    }
}

BOOST_AUTO_TEST_CASE( picsar_vec_functions_cross )
{
    test_cross<double>();
    test_cross<float>();
}

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

// ***Test scalar product

template<typename RealType>
void test_vec_times_scalar()
{
    const auto a0 = static_cast<RealType>(1.0);
    const auto a1 = static_cast<RealType>(-2.0);
    const auto a2 = static_cast<RealType>(3.0);
    const auto c = static_cast<RealType>(-0.5);

    const auto exp0 = a0*c;
    const auto exp1 = a1*c;
    const auto exp2 = a2*c;

    const auto veca = vec3<RealType>{a0, a1, a2};

    const auto v_times_c = veca*c;
    const auto exptected = std::array<RealType,3>{exp0, exp1, exp2};

    for(int i = 0; i < 3; i++){
        if(exptected[i] != 0.0)
            BOOST_CHECK_SMALL((v_times_c[i]-exptected[i])/exptected[i], tolerance<RealType>());
        else
            BOOST_CHECK_SMALL(v_times_c[i]-exptected[i], tolerance<RealType>());
    }
}

BOOST_AUTO_TEST_CASE( picsar_vec_functions_vec_times_scalar )
{
    test_vec_times_scalar<double>();
    test_vec_times_scalar<float>();
}

template<typename RealType>
void test_scalar_times_vec()
{
    const auto a0 = static_cast<RealType>(1.0);
    const auto a1 = static_cast<RealType>(-2.0);
    const auto a2 = static_cast<RealType>(3.0);
    const auto c = static_cast<RealType>(-0.5);

    const auto exp0 = a0*c;
    const auto exp1 = a1*c;
    const auto exp2 = a2*c;

    const auto veca = vec3<RealType>{a0, a1, a2};

    const auto c_times_v = c*veca;
    const auto exptected = std::array<RealType,3>{exp0, exp1, exp2};

    for(int i = 0; i < 3; i++){
        if(exptected[i] != 0.0)
            BOOST_CHECK_SMALL((c_times_v[i]-exptected[i])/exptected[i], tolerance<RealType>());
        else
            BOOST_CHECK_SMALL(c_times_v[i]-exptected[i], tolerance<RealType>());
    }
}

BOOST_AUTO_TEST_CASE( picsar_vec_functions_scalar_times_vec )
{
    test_scalar_times_vec<double>();
    test_scalar_times_vec<float>();
}

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

// ***Test division by scalar

template<typename RealType>
void test_vec_div_scalar()
{
    const auto a0 = static_cast<RealType>(1.0);
    const auto a1 = static_cast<RealType>(-2.0);
    const auto a2 = static_cast<RealType>(3.0);
    const auto c = static_cast<RealType>(-0.5);

    const auto exp0 = a0/c;
    const auto exp1 = a1/c;
    const auto exp2 = a2/c;

    const auto veca = vec3<RealType>{a0, a1, a2};

    const auto v_div_c = veca/c;
    const auto exptected = std::array<RealType,3>{exp0, exp1, exp2};

    for(int i = 0; i < 3; i++){
        if(exptected[i] != 0.0)
            BOOST_CHECK_SMALL((v_div_c[i]-exptected[i])/exptected[i], tolerance<RealType>());
        else
            BOOST_CHECK_SMALL(v_div_c[i]-exptected[i], tolerance<RealType>());
    }
}

BOOST_AUTO_TEST_CASE( picsar_vec_functions_vec_div_scalar )
{
    test_vec_div_scalar<double>();
    test_vec_div_scalar<float>();
}

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

// ***Test vector addition

template<typename RealType>
void test_add()
{
    const auto a0 = static_cast<RealType>(1.0);
    const auto a1 = static_cast<RealType>(2.0);
    const auto a2 = static_cast<RealType>(3.0);
    const auto b0 = static_cast<RealType>(-3.0);
    const auto b1 = static_cast<RealType>(-5.0);
    const auto b2 = static_cast<RealType>(-8.0);
    const auto exp0 = a0 + b0;
    const auto exp1 = a1 + b1;
    const auto exp2 = a2 + b2;

    const auto veca = vec3<RealType>{a0, a1, a2};
    const auto vecb = vec3<RealType>{b0, b1, b2};
    const auto veca_p_vecb = veca + vecb;
    const auto expected = std::array<RealType,3>{exp0, exp1, exp2};

    for(int i = 0; i < 3; i++){
        if(expected[i] != 0.0)
            BOOST_CHECK_SMALL((veca_p_vecb[i]-expected[i])/expected[i], tolerance<RealType>());
        else
            BOOST_CHECK_SMALL(veca_p_vecb[i]-expected[i], tolerance<RealType>());
    }
}

BOOST_AUTO_TEST_CASE( picsar_vec_functions_add )
{
    test_add<double>();
    test_add<float>();
}

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

// ***Test vector subraction

template<typename RealType>
void test_subtract()
{
    const auto a0 = static_cast<RealType>(1.0);
    const auto a1 = static_cast<RealType>(2.0);
    const auto a2 = static_cast<RealType>(3.0);
    const auto b0 = static_cast<RealType>(-3.0);
    const auto b1 = static_cast<RealType>(-5.0);
    const auto b2 = static_cast<RealType>(-8.0);
    const auto exp0 = a0 - b0;
    const auto exp1 = a1 - b1;
    const auto exp2 = a2 - b2;

    const auto veca = vec3<RealType>{a0, a1, a2};
    const auto vecb = vec3<RealType>{b0, b1, b2};
    const auto veca_m_vecb = veca - vecb;
    const auto expected = std::array<RealType,3>{exp0, exp1, exp2};

    for(int i = 0; i < 3; i++){
        if(expected[i] != 0.0)
            BOOST_CHECK_SMALL((veca_m_vecb[i]-expected[i])/expected[i], tolerance<RealType>());
        else
            BOOST_CHECK_SMALL(veca_m_vecb[i]-expected[i], tolerance<RealType>());
    }
}

BOOST_AUTO_TEST_CASE( picsar_vec_functions_subtract )
{
    test_subtract<double>();
    test_subtract<float>();
}

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


#include <Eigen/SVD>

using namespace Eigen;

namespace Geometry
{

	/// Compute the pseudo-inverse of a matrix
	Eigen::MatrixXd pseudoInverse(const Eigen::MatrixXd& A)
	{
		JacobiSVD<MatrixXd> svd(A, ComputeFullU | ComputeFullV);
		double  tolerance=1e-6;
		int u=(int)A.rows(), v=(int)A.cols(), n=std::min<int>(u,v);
		MatrixXd sigma(v,u);
		sigma.setZero();
		sigma.topLeftCorner(n,n)=svd.singularValues().asDiagonal();
		for (int i=0;i<n;i++)
			if (sigma(i,i)<tolerance) sigma(i,i)=0;
			else sigma(i,i)=1.0/sigma(i,i);
		MatrixXd pinv=svd.matrixV()*sigma*svd.matrixU().transpose();
		return pinv;
	}

	/// Compute the pseudo-inverse and null-space of a matrix
	void pseudoInverseAndNullspace(const Eigen::Matrix<double,3,4>& P, Eigen::Matrix<double,4,3>& Pinv, Eigen::Vector4d& C)
	{
		JacobiSVD<MatrixXd> svd(P, ComputeFullU | ComputeFullV);
		double  tolerance=1e-6;
		int u=(int)P.rows(), v=(int)P.cols(), n=std::min<int>(u,v);
		MatrixXd sigma(v,u);
		sigma.setZero();
		sigma.topLeftCorner(n,n)=svd.singularValues().asDiagonal();
		for (int i=0;i<n;i++)
			if (sigma(i,i)<tolerance) sigma(i,i)=0;
			else sigma(i,i)=1.0/sigma(i,i);
		Pinv=svd.matrixV()*sigma*svd.matrixU().transpose();
		auto V=svd.matrixV();
		C=V.col(V.cols()-1);
	}

	/// Compute right null-space of A
	Eigen::VectorXd nullspace(const Eigen::MatrixXd& A)
	{
		JacobiSVD<MatrixXd> svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV);
		auto V=svd.matrixV();
		return V.col(V.cols()-1);
	}

	// Enforce rank deficiency
	Eigen::MatrixXd makeRankDeficient(const Eigen::MatrixXd& A)
	{
		JacobiSVD<MatrixXd> svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV);
		VectorXd sigma=svd.singularValues();
		sigma[sigma.size()-1]=0;
		return svd.matrixU()*sigma.asDiagonal()*svd.matrixV().transpose();
	}

	// Compute left null-space of A
	Eigen::VectorXd nullspace_left(const Eigen::MatrixXd& A)
	{
		return nullspace(A.transpose());
	}

} // namespace Geometry


//------------------------------------------------------------------------------
/// \file AdditiveIntegers_tests.cpp
/// \ref https://www.cs.utexas.edu/users/fussell/courses/cs429h/lectures/Lecture_2-429h.pdf
//------------------------------------------------------------------------------
#include "Algebra/Groups/AdditiveIntegers.h"

#include <boost/test/unit_test.hpp>
#include <cmath>

using Algebra::Groups::AbelianGroups::AdditiveInteger;

BOOST_AUTO_TEST_SUITE(Algebra)
BOOST_AUTO_TEST_SUITE(Groups)
BOOST_AUTO_TEST_SUITE(AbelianGroups)
BOOST_AUTO_TEST_SUITE(AdditiveIntegers_tests)

BOOST_AUTO_TEST_SUITE(Interface)

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(IdentityReturnsZero)
{
  const AdditiveInteger<int> additive_integer_int {2};
  BOOST_TEST(additive_integer_int.identity().value() == 0);
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(IdentityReturnsAdditiveInverse)
{
  const AdditiveInteger<int> additive_integer_int {2};
  BOOST_TEST(additive_integer_int.inverse().value() == -2);
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(GroupLawInheritedAndDoesAbelianGroupAddition)
{
  const AdditiveInteger<int> a {-3};
  const AdditiveInteger<int> b {5};
  BOOST_TEST(a.group_law(b).value() == 2);
  BOOST_TEST(b.group_law(a).value() == 2);
}

BOOST_AUTO_TEST_SUITE_END() // Interface

BOOST_AUTO_TEST_SUITE(Operations)

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(GroupLawInheritedAndDoesAbelianGroupAddition)
{
  const AdditiveInteger<int> a {1};
  const AdditiveInteger<int> b {2};
  BOOST_TEST((a + b).value() == 3);
}

BOOST_AUTO_TEST_SUITE_END() // Operations

BOOST_AUTO_TEST_SUITE_END() // AdditiveIntegers_tests
BOOST_AUTO_TEST_SUITE_END() // AbelianGroups
BOOST_AUTO_TEST_SUITE_END() // Groups
BOOST_AUTO_TEST_SUITE_END() // Algebra


#include "hypothesis.h"
#include <math.h>
#include <boost/math/quadrature/gauss.hpp>
#include <boost/math/distributions/chi_squared.hpp>
using namespace std;

Hypothesis::Hypothesis() = default;

// Hypothesis test for the avarage, we use the library Boost to integrate the function
// that corresponds for the Normal Cumulative Distribution Function, we use gaussian
// quadrature method to integrate passing the lambda function that corresponds to the
// Normal. We followed "Curso de Planejamento Experimental - FEMEC - UFU" available
// on Moodle to every step to do.
double Hypothesis::testAverage(double sampleAvg, double sampleStdDev, unsigned long sampleNumElements,
                               double confidencelevel, double avg, H1Comparition comp) {
    using namespace boost::math::quadrature;

    double stdError = sampleStdDev / sqrt(sampleNumElements);

    double z_calc = (sampleAvg - avg) / stdError;

    auto f = [](const double &x) {
        double constant = 1 / sqrt(2 * M_PI);
        double e = exp(pow(x, 2));
        double sqrtE = 1 / sqrt(e);
        return constant * sqrtE;
    };

    double Q = gauss<double, 10000>::integrate(f, -INFINITY, z_calc);

    double alpha = confidencelevel / 100;

    testHypothesisNull(comp, alpha, Q);
    return Q;

}

// Really similar to the previous method, we used the same things and followed
// the document on Moodle.
double Hypothesis::testProportion(double sampleProp, unsigned long sampleNumElements, double confidencelevel,
                                  double prop, H1Comparition comp) {
    using namespace boost::math::quadrature;

    double z_cal = ((sampleProp - prop) / sqrt((prop * (1 - prop)) / sampleNumElements));

    auto f = [](const double &x) {
        double constant = 1 / sqrt(2 * M_PI);
        double e = exp(pow(x, 2));
        double sqrtE = 1 / sqrt(e);
        return constant * sqrtE;
    };

    double Q = gauss<double, 10000>::integrate(f, -INFINITY, z_cal);

    double alpha = confidencelevel / 100;

    testHypothesisNull(comp, alpha, Q);
    return Q;
}

// This one is different from the rest, following "Curso de estatistica.pdf" on moodle,
// we need to use chi square method to calculate p-value, so after some research we used
// an implementation available online.
double Hypothesis::testVariance(double sampleVar, unsigned long sampleNumElements, double confidencelevel,
                                double var, H1Comparition comp) {
    using namespace boost::math::quadrature;

    int df = (int)sampleNumElements - 1;
    double chi_cal = (df * sampleVar) / var;

    double chi = ChiSquare(chi_cal, df);

    double alpha = confidencelevel / 100;

    double value_calc = 1 - chi;

    testHypothesisNull(comp, alpha, value_calc);
    return chi;
}

// Hypothesis test acording to p-value and confidence level and the type of comparation.
void Hypothesis::testHypothesisNull(H1Comparition comp, double alpha, double value_calc){

    double value_critical = 1 - alpha;

    if (comp == H1Comparition::DIFFERENT) {

        double value_inf = alpha / 2;
        double value_sup = value_critical + value_inf;

        if (value_calc > value_sup or value_calc < value_inf) {
            printf("\n   # REJECT H0 => NOT EQUAL WITH %0.4f OF CONFIDENCE LEVEL\n\n", alpha);
        } else {
            printf("\n   # ACCEPT H0 => EQUAL WITH %0.4f OF CONFIDENCE LEVEL\n\n", alpha);
        }

    } else if (comp == H1Comparition::LESS_THAN) {

        if (value_calc < alpha) {
            printf("\n   # REJECT H0 => LESS THAN WITH %0.4f OF CONFIDENCE LEVEL\n\n", alpha);
        } else {
            printf("\n   # ACCEPT H0 => NOT LESS THAN WITH %0.4f OF CONFIDENCE LEVEL\n\n", alpha);
        }

    } else {

        if (value_calc > value_critical) {
            printf("\n   # REJECT H0 => GREATER THAN WITH %0.4f OF CONFIDENCE LEVEL\n\n", alpha);
        } else {
            printf("\n   # ACCEPT H0 => NOT GREATER THAN WITH %0.4f OF CONFIDENCE LEVEL\n\n", alpha);
        }

    }
}

// Reference: https://jamesmccaffrey.wordpress.com/2010/11/01/programmatically-computing-chi-square-critical-values
double Hypothesis::ChiSquare(double x, int df) {
    // x = a computed chi-square value
    // df = degrees of freedom
    using namespace boost::math::quadrature;

    if (x <= 0.0 || df < 1) {
        printf("parameter x must be positive and parameter df must be 1 or greater");
    };

    double a = 0; // 299 variable names
    double y = 0;
    double s = 0;
    double z = 0;
    double e = 0;
    double c;

    bool even; // is df even?

    a = 0.5 * x;
    if (df % 2 == 0) even = true; else even = false;

    if (df > 1) y = Exp(-a); // ACM update remark (4)

    auto f = [](const double &x) {
        double constant = 1 / sqrt(2 * M_PI);
        double ex = exp(pow(x, 2));
        double sqrtE = 1 / sqrt(ex);
        return constant * sqrtE;
    };

    if (even) s = y; else s = 2.0 * gauss<double, 10000>::integrate(f, -INFINITY, -sqrt(x));

    if (df > 2)
    {
        x = 0.5 * (df - 1.0);
        if (even) z = 1.0; else z = 0.5;
        if (a > 40.0) // ACM remark (5)
        {
            if (even) e = 0.0; else e = 0.5723649429247000870717135; // log(sqrt(pi))
            c = log(a); // log base e
            while (z <= x)
            {
                e = log(z) + e;
                s = s + Exp(c * z - a - e); // ACM update remark (6)
                z = z + 1.0;
            }
            return s;
        } // a > 40.0
        else
        {
            if (even) e = 1.0; else e = 0.5641895835477562869480795 / sqrt(a); // (1 / sqrt(pi))
            c = 0.0;
            while (z <= x)
            {
                e = e * (a / z); // ACM update remark (7)
                c = c + e;
                z = z + 1.0;
            }
            return c * y + s;
        }
    }
    else
    {
        return s;
    }
}

// Reference: https://jamesmccaffrey.wordpress.com/2010/11/01/programmatically-computing-chi-square-critical-values/
double Hypothesis::Exp(double x) { // ACM update remark (3)
    if (x < -40.0) // 40.0 is a magic number. ACM update remark (8)
        return 0.0;
    else
        return exp(x);
}


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

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  MatrixXf m = MatrixXf::Random(3,2);
cout << "Here is the matrix m:" << endl << m << endl;
JacobiSVD<MatrixXf> svd(m, ComputeThinU | ComputeThinV);
cout << "Its singular values are:" << endl << svd.singularValues() << endl;
cout << "Its left singular vectors are the columns of the thin U matrix:" << endl << svd.matrixU() << endl;
cout << "Its right singular vectors are the columns of the thin V matrix:" << endl << svd.matrixV() << endl;
Vector3f rhs(1, 0, 0);
cout << "Now consider this rhs vector:" << endl << rhs << endl;
cout << "A least-squares solution of m*x = rhs is:" << endl << svd.solve(rhs) << endl;

  return 0;
}


#include <ctime>
#include <gtest/gtest.h>
#include <Eigen/Eigen>
#include <Eigen/Geometry>

#include "slick/math/se2.h"
#include "slick/test/unittest/util.h"

using namespace std;
using namespace Eigen;
using namespace slick;

// constructors tests ==========================================================
template <typename T>
void DefaultConstructor_Test() {
  // for double precision
  SE2Group<T> trans;
  EXPECT_MATRIX_NEAR(Eigen::Matrix<T, 2, 2>::Identity(), trans.get_rotation().get_matrix(),
                     Gap<T>());
  EXPECT_MATRIX_NEAR(Eigen::Matrix<T, 2, 1>::Zero(), trans.get_translation(),
                     Gap<T>());
}

TEST(SE2Test, DefaultConstructor) {
  DefaultConstructor_Test<double>();
  DefaultConstructor_Test<float>();
}

template <typename T>
void FromSO2VectorConstructor_Test() {
  std::srand(std::time(0));  // use current time as seed for random generator
  auto angle = T(std::rand());

  Eigen::Matrix<T, 2, 2> mrot;
  mrot = Eigen::Rotation2D<T>(angle);

  Eigen::Matrix<T, 2, 1> t = Eigen::Matrix<T, 2, 1>::Random();

  SO2Group<T> rot(angle);
  SE2Group<T> trans(rot, t);

  EXPECT_MATRIX_NEAR(mrot, trans.get_rotation().get_matrix(),
                     Gap<T>());
  EXPECT_MATRIX_NEAR(t, trans.get_translation(),
                     Gap<T>());
}

TEST(SE2Test, FromSO2VectorConstructor) {
  FromSO2VectorConstructor_Test<double>();
  FromSO2VectorConstructor_Test<float>();
}

template <typename T>
void FromVectorConstructor_Test() {
  std::srand(std::time(0));  // use current time as seed for random generator
  auto angle = T(std::rand());

  Eigen::Matrix<T, 2, 2> mrot;
  mrot = Eigen::Rotation2D<T>(angle);

  Eigen::Matrix<T, 2, 1> t = Eigen::Matrix<T, 2, 1>::Random();

  SE2Group<T> trans(Eigen::Matrix<T, 3, 1>(t[0], t[1], angle));

  EXPECT_MATRIX_NEAR(mrot, trans.get_rotation().get_matrix(),
                     Gap<T>());
}

TEST(SE2Test, FromVectorConstructor) {
  FromVectorConstructor_Test<double>();
  FromVectorConstructor_Test<float>();
}

// template <typename T>
// void FromListInitializerConstructor_Test() {
//   SE2Group<T> rot{1, 0, 0, 1};
//   EXPECT_MATRIX_NEAR(Eigen::Matrix<T, 2, 2>::Identity(), rot.get_matrix(),
//                      Gap<T>());

//   EXPECT_NEAR(0.0, rot.ln(), Gap<T>());
// }

// TEST(SE2Test, FromListInitializerConstructor) {
//   FromListInitializerConstructor_Test<double>();
//   FromListInitializerConstructor_Test<float>();
// }

// se2 specific functions ======================================================
template <typename T>
void Ln_Test() {
  std::srand(std::time(0));  // use current time as seed for random generator
  auto angle = T(std::rand()) / T(RAND_MAX) * 2 * M_PI;
  Eigen::Matrix<T, 2, 1> t = Eigen::Matrix<T, 2, 1>::Random();
  SE2Group<T> trans(Eigen::Matrix<T, 3, 1>(t[0], t[1], angle));

  auto est = trans.ln();
  est[2] = (est[2] > 0) ? est[2] : 2 * M_PI + est[2];
  EXPECT_NEAR(angle, est[2], Gap<T>());
}

TEST(SE2Test, Ln) {
  Ln_Test<double>();
  Ln_Test<float>();
}

template <typename T>
void Inverse_Test() {
  std::srand(std::time(0));  // use current time as seed for random generator
  auto angle = T(std::rand());
  Eigen::Matrix<T, 2, 1> t = Eigen::Matrix<T, 2, 1>::Random();
  SE2Group<T> trans(Eigen::Matrix<T, 3, 1>(t[0], t[1], angle));
  Eigen::Matrix<T, 2, 2> m_inv = trans.get_rotation().get_matrix().inverse();
  EXPECT_MATRIX_NEAR(m_inv, trans.inverse().get_rotation().get_matrix(), Gap<T>());
  // EXPECT_MATRIX_NEAR(-t, trans.inverse().get_translation(), Gap<T>());
}

TEST(SE2Test, Inverse) {
  Inverse_Test<double>();
  Inverse_Test<float>();
}

template <typename T>
void SE2RightHandMulOperator_Test() {
  std::srand(std::time(0));  // use current time as seed for random generator
  auto angle = T(std::rand());

  Eigen::Matrix<T, 3, 3> mtrans1 = Eigen::Matrix<T, 3, 3>::Identity();
  Eigen::Matrix<T, 2, 2> m;
  m = Eigen::Rotation2D<T>(angle);
  mtrans1.block(0, 0, 2, 2) = m;
  Eigen::Matrix<T, 2, 1> t = Eigen::Matrix<T, 2, 1>::Random();
  SE2Group<T> trans1(Eigen::Matrix<T, 3, 1>(t[0], t[1], angle));
  mtrans1.block(0, 2, 2, 1) = trans1.get_translation();

  std::srand(std::time(0));  // use current time as seed for random generator
  angle = T(std::rand());
  Eigen::Matrix<T, 3, 3> mtrans2 = Eigen::Matrix<T, 3, 3>::Identity();
  m = Eigen::Rotation2D<T>(angle);
  mtrans2.block(0, 0, 2, 2) = m;
  t = Eigen::Matrix<T, 2, 1>::Random();
  SE2Group<T> trans2(Eigen::Matrix<T, 3, 1>(t[0], t[1], angle));
  mtrans2.block(0, 2, 2, 1) = trans2.get_translation();

  EXPECT_MATRIX_NEAR((mtrans1 * mtrans2).block(0, 0, 2, 3),
    (trans1 * trans2).get_matrix(), Gap<T>());
}

TEST(SE2Test, SE2RightHandMulOperator) {
  SE2RightHandMulOperator_Test<double>();
  SE2RightHandMulOperator_Test<float>();
}

template <typename T>
void MatrixRightHandMulOperator_Test() {
  std::srand(std::time(0));  // use current time as seed for random generator
  auto angle = T(std::rand());

  Eigen::Matrix<T, 3, 3> mtrans = Eigen::Matrix<T, 3, 3>::Identity();
  Eigen::Matrix<T, 2, 2> m;
  m = Eigen::Rotation2D<T>(angle);
  mtrans.block(0, 0, 2, 2) = m;
  Eigen::Matrix<T, 2, 1> t = Eigen::Matrix<T, 2, 1>::Random();
  SE2Group<T> trans(Eigen::Matrix<T, 3, 1>(t[0], t[1], angle));
  mtrans.block(0, 2, 2, 1) = trans.get_translation();

  Eigen::Matrix<T, 3, 3> mrand = Eigen::Matrix<T, 3, 3>::Random();
  EXPECT_MATRIX_NEAR(mtrans * mrand, trans * mrand, Gap<T>());
}

TEST(SE2Test, MatrixRightHandMulOperator) {
  MatrixRightHandMulOperator_Test<double>();
  MatrixRightHandMulOperator_Test<float>();
}

template <typename T>
void MatrixLeftHandMulOperator_Test() {
  std::srand(std::time(0));  // use current time as seed for random generator
  auto angle = T(std::rand());

  Eigen::Matrix<T, 3, 3> mtrans = Eigen::Matrix<T, 3, 3>::Identity();
  Eigen::Matrix<T, 2, 2> m;
  m = Eigen::Rotation2D<T>(angle);
  mtrans.block(0, 0, 2, 2) = m;
  Eigen::Matrix<T, 2, 1> t = Eigen::Matrix<T, 2, 1>::Random();
  SE2Group<T> trans(Eigen::Matrix<T, 3, 1>(t[0], t[1], angle));
  mtrans.block(0, 2, 2, 1) = trans.get_translation();

  Eigen::Matrix<T, 3, 3> mrand = Eigen::Matrix<T, 3, 3>::Random();
  EXPECT_MATRIX_NEAR(mrand*mtrans , mrand*trans, Gap<T>());
}

TEST(SE2Test, MatrixLeftHandMulOperator) {
  MatrixLeftHandMulOperator_Test<double>();
  MatrixLeftHandMulOperator_Test<float>();
}

int main(int argc, char** argv) {
  std::vector<char*> vars(argc+1);
  for (int i = 0; i < argc; ++i)
    vars[i] = argv[i];
  char ca[50];
  sprintf(ca, "--gtest_repeat=%d", NSAMPLES);
  vars[argc] = ca;
  argc++;
  ::testing::InitGoogleTest(&argc, &vars.front());
  return RUN_ALL_TESTS();
}


#include <rapidcheck.h>
// #include "driver/StatusCode.hpp"
#include "3rd/json.hpp"

#include <vector>
#include <algorithm>
#include <iostream>
#include <unistd.h> // for usleep
#include <vector>
#include <cmath>
#include <complex>
#include <iostream>
#include <eigen3/Eigen/Core>
#include <eigen3/unsupported/Eigen/FFT>
#include <boost/multiprecision/cpp_dec_float.hpp>

using namespace std;

using boost::multiprecision::cpp_dec_float_50;

void twoSinTest() {
    const double pi = 2.*std::acos(0.);
    double Fs = 1000;            // Sampling frequency
    double T = 1 / Fs;           // Sampling period
    int L = 1000;               // Length of signal
    std::vector<double> timebuf(L);
    typedef std::complex<double> C;
    std::vector<C> freqbuf;

    double t = 0;
    for (int i = 0; i < L; i++) {
        // time domain has 2 sin waves
        timebuf[i] = 0.7*std::sin(2 * pi * 50 * t) + std::sin(2 * pi * 120 * t);
        t += T;
    }

    Eigen::FFT<double> fft;
    fft.fwd(freqbuf, timebuf);
    cout << freqbuf.size() << endl;

    for (int i = 0; i < freqbuf.size(); i++) {
        const C &f = freqbuf[i];
        // cout << i << " " << f << endl;
        cout << f << ",";
    }
    cout << endl;
}

int main() {

    twoSinTest();

    return 0;
}


//           Copyright Matthew Pulver 2018 - 2019.
// Distributed under the Boost Software License, Version 1.0.
//      (See accompanying file LICENSE_1_0.txt or copy at
//           https://www.boost.org/LICENSE_1_0.txt)

#include <boost/math/differentiation/autodiff.hpp>
#include <iostream>

template<typename T>
T fourth_power(T x)
{
    x *= x;
    return x *= x;
}

int main()
{
    using namespace boost::math::differentiation;

    constexpr int Order=5; // The highest order derivative to be calculated.
    const autodiff_fvar<double,Order> x = make_fvar<double,Order>(2.0); // Find derivatives at x=2.
    const autodiff_fvar<double,Order> y = fourth_power(x);
    for (int i=0 ; i<=Order ; ++i)
        std::cout << "y.derivative("<<i<<") = " << y.derivative(i) << std::endl;
    return 0;
}
/*
Output:
y.derivative(0) = 16
y.derivative(1) = 32
y.derivative(2) = 48
y.derivative(3) = 48
y.derivative(4) = 24
y.derivative(5) = 0
**/


#include <Eigen/Dense>
#include <fmt/format.h>
#include <itp/timer>
#include <iostream>
#include <cstdio>
#include <algorithm>

extern"C" {
	//LU decomoposition of a general matrix
	void dgetrf_(int* M, int* N, double* A, int* lda, int* IPIV, int* INFO);
	//generate inverse of a matrix given its LU decomposition
	void dgetri_(int* N, double* A, int* lda, int* IPIV, double* WORK, int* lwork, int* INFO);
}

void inverse_Lapack(double* A, double* B, int N)
{
	std::copy_n(A, N*N, B);
	int* IPIV = new int[N + 1];
	int LWORK = N * N;
	double* WORK = new double[LWORK];
	int INFO;
	dgetrf_(&N, &N, B, &N, IPIV, &INFO);
	dgetri_(&N, B, &N, IPIV, WORK, &LWORK, &INFO);
	delete[] IPIV;
	delete[] WORK;
}

int main()
{
	int N;
	fmt::print("Please input number: ");
	std::cin >> N;

	Eigen::MatrixXd mat = Eigen::MatrixXd::Random(N, N) * 10;
	Eigen::MatrixXd inv = mat;
	itp::Timer timer;

	timer.start();
	inv = mat.inverse();
	timer.stop();
	fmt::print("{}, Eigen time : {}s\n", inv(N / 2, N / 2), timer.span());

	timer.start();
	inverse_Lapack(mat.data(), inv.data(), N);
	timer.stop();
	fmt::print("{}, Lapack time: {}s\n", inv(N / 2, N / 2), timer.span());
}


#include <vector>
#include <cassert>

#include <iostream>

#include <Eigen/Dense>
#include <Eigen/Sparse>

class NatCSI {
public:
    NatCSI(const std::vector<double> & t, const std::vector<double> & y)
        : t(t), y(y), h(t.size()-1), c(t.size()) {
        // Size check
        assert( ( t.size() == y.size() ) && "Error: mismatched size of t and y!");

        // m is the number of conditions (t goes from t_0 to t_n)
        // WARNING: m = n+1
        m = t.size();

        // Vector containing increments (from the right)
        for(int i = 0; i < (m - 1); ++i) {
            h(i) = t[i+1] - t[i];
            // Check that t is sorted
            assert( ( h(i) > 0 ) && "Error: array t must be sorted!");
        }

        // System matrix and rhs as in 3.5.9, we remove first and last row (known by natural contition)
        Eigen::SparseMatrix<double> A(m,m);
        Eigen::VectorXd b(m);

        // WARNING: sparse reserve space
        A.reserve(3);

        // Fill in natural conditions 3.5.10 for matrix
        A.coeffRef(0,0) = 2 / h(0);
        A.coeffRef(0,1) = 1 / h(0);
        A.coeffRef(m-1,m-2) = 1 / h(m-2);
        A.coeffRef(m-1,m-1) = 2 / h(m-2);

        // Reuse computation for rhs
        double bold = (y[1] - y[0]) / (h(0)*h(0));
        b(0) = 3*bold; // Fill in natural conditions 3.5.10
        // Fill matrix A and rhs b
        for(int i = 1; i < m-1; ++i) {
            // PRecompute b_i
            double hinv = 1./h(i);

            // Fill in a
            A.coeffRef(i,i-1) = hinv;
            A.coeffRef(i,i) = 2./h(i) + 2./h(i-1);
            A.coeffRef(i,i+1) = hinv;

            // Reuse computation for rhs b
            double bnew = (y[i+1] - y[i]) / (h(i)*h(i));
            b(i) = 3. * (bnew + bold);
            bold = bnew;
        }
        b(m-1) = 3*bold; // Fill in natural conditions 3.5.10
        // Compress the matrix
        A.makeCompressed();
        std::cout << A;

        // Factorize A and solve system A*c(1:end) = b
        Eigen::SparseLU<Eigen::SparseMatrix<double>> lu;
        lu.compute(A);
        c = lu.solve(b);

    }

    double operator() (double x) const {
        // Assert that x \in (t_0, t_n)
        assert( ( t[0] <= x && x <= t[m-1] ) && "Error: x must be in (t_0, t_n)");

        // Find j s.t. x \in (t_j,t_j+1)
        // as with the previous sheet (PS 6), we perform an efficient binary search on the data (log. complexity)
        auto j = (std::lower_bound(t.begin(), t.end(), x) - t.begin()) - 1;
        if( j == -1 ) j++;

        // Precompute tau and evaluate spline (3.5.5)
        double tau = (x - t[j]) / h(j);
        double tau2 = tau*tau;
        double tau3 = tau2*tau;
        return y[j] *         ( 1. - 3. * tau2 + 2 * tau3 ) +
               y[j+1] *           ( 3 * tau2 - 2 * tau3 ) +
               h(j) * c(j) *  ( tau - 2 * tau2 + tau3 ) +
               h(j) * c(j+1) *    ( - tau2 + tau3 );

    }

private:
    // Provided nodes and values to compute spline, same size Eigen vectors
    std::vector<double> t, y;
    // Difference t(i)-t(i-1) and coefficients of spline (s'(t_j))
    Eigen::VectorXd h, c;
    // Size of t, y and c  = m. h has size m-1
    int m;
};

int main() {

    int n = 8, m = 100;
    std::vector<double> t;
    std::vector<double> y;
    t.resize(n);
    y.resize(n);
    for(int i = 0; i < n; ++i) {
        t[i] = (double) i / (n-1);
        y[i] = cos(t[i]);
    }

    NatCSI N(t,y);

    Eigen::VectorXd x = Eigen::VectorXd::LinSpaced(m, 0, 1);
    for(int i = 0; i < x.size(); ++i) {
        x(i) = N(x(i));
    }
    std::cout << x << std::endl;
}

Nummerical Methods for CSE/PS8/solutions_ps8/natcsi.cpp


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

void gramSchmidtOrthogonalization(Eigen::MatrixXd &matrix,Eigen::MatrixXd &orthonormalMatrix)
{
/*
    In this method you make every column perpendicular to it's previous columns,
    here if a and b are representation vector of two columns, c=b-((b.a)/|a|).a
        ^
       /
    b /
     /
    /
    ---------->
        a
        ^
       /|
    b / |
     /  | c
    /   |
    ---------->
        a
    you just have to normilze every vector after make it perpendicular to previous columns
    so:
    q1=a.normalized();
    q2=b-(b.q1).q1
    q2=q2.normalized();
    q3=c-(c.q1).q1 - (c.q2).q2
    q3=q3.normalized();
    Now we have Q, but we want A=QR so we just multiply both side by Q.transpose(), since Q is orthonormal, Q*Q.transpose() is I
    A=QR;
    Q.transpose()*A=R;
*/
    Eigen::VectorXd col;
    for(int i=0;i<matrix.cols();i++)
    {
        col=matrix.col(i);
        col=col.normalized();
        for(int j=0;j<i-1;j++)
        {
            //orthonormalMatrix.col(i)
        }

        orthonormalMatrix.col(i)=col;
    }
    Eigen::MatrixXd A(4,3);

    A<<1,2,3,-1,1,1,1,1,1,1,1,1;
    Eigen::Vector4d a=A.col(0);
    Eigen::Vector4d b=A.col(1);
    Eigen::Vector4d c=A.col(2);

    Eigen::Vector4d q1=  a.normalized();
    Eigen::Vector4d q2=b-(b.dot(q1))*q1;
    q2=q2.normalized();

    Eigen::Vector4d q3=c-(c.dot(q1))*q1 - (c.dot(q2))*q2;
    q3=q3.normalized();

    std::cout<< "q1:"<<std::endl;
    std::cout<< q1<<std::endl;
    std::cout<< "q2"<<std::endl;
    std::cout<< q2<<std::endl;
    std::cout<< "q3:"<<std::endl;
    std::cout<< q3<<std::endl;

    Eigen::MatrixXd Q(4,3);
    Q.col(0)=q1;
    Q.col(1)=q2;
    Q.col(2)=q3;

    Eigen::MatrixXd R(3,3);
    R=Q.transpose()*(A);

    std::cout<<"Q"<<std::endl;
    std::cout<< Q<<std::endl;

    std::cout<<"R"<<std::endl;
    std::cout<< R.unaryExpr(std::ptr_fun(exp))<<std::endl;

    //MatrixXd A(4,3), thinQ(4,3), Q(4,4);

    Eigen::MatrixXd thinQ(4,3), q(4,4);

    //A.setRandom();
    Eigen::HouseholderQR<Eigen::MatrixXd> qr(A);
    q = qr.householderQ();
    thinQ.setIdentity();
    thinQ = qr.householderQ() * thinQ;
    std::cout << "Q computed by Eigen" << "\n\n" << thinQ << "\n\n";
    std::cout << q << "\n\n" << thinQ << "\n\n";

}

void gramSchmidtOrthogonalizationExample()
{
    Eigen::MatrixXd matrix(3,4),orthonormalMatrix(3,4) ;
    matrix=Eigen::MatrixXd::Random(3,4);////A.setRandom();

    gramSchmidtOrthogonalization(matrix,orthonormalMatrix);
}

int main()
{

}


#include <Eigen/Core>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

// Solve equation Ax=b
MatrixXf LinearEquationSolve(const Ref<const MatrixXf>& A, const Ref<const VectorXf>& b, string type = "QR") {
    int MATRIX_SIZE = b.rows();
    Matrix<float, MATRIX_SIZE, 1> x;
    if (type == "Reverse") {
        x = A.inverse() * b;
    }
    else if (type == "Cholesky")
    {
        x = A.ldlt().solve(b);
    }
    else
    {
        x = A.colPivHouseholderQr().solve(b);
    }

    return x;
}


//
// Created by jhwangbo on 23.09.16.
// This function is to solve linear equation Ax=b

#ifndef RAI_CONJUGATEGRADIENT_HPP
#define RAI_CONJUGATEGRADIENT_HPP
#include <functional>
#include <Eigen/Core>
#include <iostream>

namespace rai{
//namespace Math{

template <typename Derived>
Derived conjugateGradient(std::function< void(Eigen::Matrix<Derived, -1, 1>&,
                                           Eigen::Matrix<Derived, -1, 1>&)> eval,
                       Eigen::Matrix<Derived, -1, 1> b,
                       int iterN, Derived tol, Eigen::Matrix<Derived, -1, 1>& sol)
{
  int dim = b.rows();
  Eigen::Matrix<Derived, -1, 1> x, p, r, Ap, Ar, pnew, Apnew;
  x.setZero(dim); Ap.setZero(dim); Ar.setZero(dim); pnew.setZero(dim);
  r = b;
  p = b;

  Derived rdotr = r.squaredNorm();
  Derived newrdotr=0;
  int i;
  for(i = 0; i < iterN && i < dim; i++){
    eval(p, Ap);
    Derived a = rdotr / p.dot(Ap);
    x += a * p;
    r -= a * Ap;
    newrdotr = r.dot(r);
    if (newrdotr < tol)
      break;
    Derived b = newrdotr/rdotr;
    p = r + b * p;
    rdotr = newrdotr;
  }
  sol = x;
  return newrdotr;
};

}
//}

#endif //RAI_CONJUGATEGRADIENT_HPP


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

using namespace std;
using namespace Eigen;

int main()
{
  VectorXf v(2);
  MatrixXf m(2,2), n(2,2);

  v << -1,
       2;

  m << 1,-2,
       -3,4;

  cout << "v.squaredNorm() = " << v.squaredNorm() << endl;
  cout << "v.norm() = " << v.norm() << endl;
  cout << "v.lpNorm<1>() = " << v.lpNorm<1>() << endl;
  cout << "v.lpNorm<Infinity>() = " << v.lpNorm<Infinity>() << endl;

  cout << endl;
  cout << "m.squaredNorm() = " << m.squaredNorm() << endl;
  cout << "m.norm() = " << m.norm() << endl;
  cout << "m.lpNorm<1>() = " << m.lpNorm<1>() << endl;
  cout << "m.lpNorm<Infinity>() = " << m.lpNorm<Infinity>() << endl;
}


#include <Eigen/Dense>

#include <iostream>
#include <iomanip>

#include <cmath>

//! \brief One step of autonomous IVP y'' + y' + y = 0, [y(0), y'(0)] = z0 using SDIRK method
//! Use SDIRK method for first order ode z' = f(z). Steps of size h.
//! \tparam StateType type of solution space y and initial data y0
//! \param[in] z0 initial data z(0)
//! \param[in] h size of the step
//! \param[in] gamma parameter
//! \return next step z1
template <class StateType>
StateType sdirkStep(const StateType & z0, double h, double gamma) {
    // Matrix A for evaluation of f
    Eigen::Matrix2d A;
    A << 0.,1.,-1.,-1.;

    // Reuse factorization
    auto A_lu = (Eigen::Matrix2d::Identity()-h*gamma*A).partialPivLu();
    Eigen::Vector2d az = A*z0;

    // Increments
    Eigen::Vector2d k1 = A_lu.solve(az);
    Eigen::Vector2d k2 = A_lu.solve(az + h*(1-2*gamma)*A*k1);

    // Next step
    return z0 + h*0.5*(k1 + k2);
}

//! \brief Solve autonomous IVP y'' + y' + y = 0, [y(0), y'(0)] = z0 using SDIRK method
//! Use SDIRK method for first order ode z' = f(z), with N equidistant steps
//! \tparam StateType type of solution space z = [y,y']! and initial data z0 = [y(0), y'(0)]
//! \param[in] z0 initial data z(0)
//! \param[in] N number of equidistant steps
//! \param[in] T final time of simulation
//! \param[in] gamma parameter
//! \return vector containing each step of z_k (y and y')
template <class StateType>
std::vector<StateType> sdirkSolve(const StateType & z0, unsigned int N, double T, double gamma) {
    // Solution vector
    std::vector<StateType> res(N+1);
    // Equidistant step size
    const double h = T/N;

    // Push initial data
    res.at(0) = z0;

    // Main loop
    for(unsigned int i = 1; i <= N; ++i) {
        res.at(i) = sdirkStep(res.at(i-1), h, gamma);
    }

    return res;
}

int main() {
    // Initial data z0 = [y(0), y'(0)]
    Eigen::Vector2d z0;
    z0 << 1,0;
    // Final time
    const double T = 10;
    // Parameter
    const double gamma = (3.+std::sqrt(3.)) / 6.;
    // Mesh sizes
    std::vector<int> N = {20, 40, 80, 160, 320, 640, 1280, 2560, 5120, 10240};
    // Exact solution (only y(t)) given z0 = [y(0), y'(0)] and t
    auto yex = [&z0] (double t) {
        return 1./3.*std::exp(-t/2.) * ( 3.*z0(0) * std::cos( std::sqrt(3.)*t/2. ) +
        std::sqrt(3.)*z0(0) * std::sin( std::sqrt(3.)*t/2. ) +
        2.*std::sqrt(3.)*z0(1) * std::sin( std::sqrt(3.)*t/2. ) );
    };

    // Store old error for rate computation
    double errold = 0;
    std::cout << std::setw(15) << "n" << std::setw(15) << "maxerr" << std::setw(15) << "rate" << std::endl;
    // Loop over all meshes
    for(unsigned int i = 0; i < N.size(); ++i) {
        int n = N.at(i);
        // Get solution
        auto sol =  sdirkSolve(z0, n, T, gamma);
        // Compute error
        double err = std::abs(sol.back()(0) - yex(T));

        // I/O
        std::cout << std::setw(15) << n << std::setw(15) << err;
        if(i > 0) std::cout << std::setw(15) << std::log2(errold /err);
        std::cout << std::endl;

        // Store old error
        errold = err;
    }
}

Nummerical Methods for CSE/PS14/solutions_ps14/sdirk.cpp


//
// Created by Harold on 2021/4/11.
//

#ifndef M_MATH_M_FFT_EIGEN_HPP
#define M_MATH_M_FFT_EIGEN_HPP

#include <Eigen/Dense>
#include <eigen3/unsupported/Eigen/FFT>
#include <complex>

#ifdef _OPENMP
#include <omp.h>
#endif

#include "m_circshift_eigen.hpp"

namespace M_MATH {
    // 2D fft, requires fft shift
    template<typename Derived>
    Eigen::Matrix<std::complex<typename Derived::Scalar>, Eigen::Dynamic, Eigen::Dynamic>
    ForwardFFT(Eigen::MatrixBase<Derived> const& mat) {
        auto rows = mat.rows();
        auto cols = mat.cols();
        Eigen::FFT<typename Derived::Scalar> fft;
        Eigen::Matrix<std::complex<typename Derived::Scalar>, Eigen::Dynamic, Eigen::Dynamic> res(rows, cols);
        // need apply 1d fft on both rows and cols
        for (auto i = 0; i < rows; ++i) {
            Eigen::Matrix<std::complex<typename Derived::Scalar>, Eigen::Dynamic, 1> tmp(cols);
            fft.fwd(tmp, mat.row(i).eval());
            res.row(i) = tmp;
        }
        for (auto i = 0; i < cols; ++i) {
            Eigen::Matrix<std::complex<typename Derived::Scalar>, 1, Eigen::Dynamic> tmp(rows);
            fft.fwd(tmp, res.col(i).eval());
            res.col(i) = tmp;
        }
        return res;
    }

    // 2D ifft, requires fft shift
    template<typename Scalar>
    Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>
    BackwardFFT(Eigen::Matrix<std::complex<Scalar>, Eigen::Dynamic, Eigen::Dynamic> const& mat) {
        auto rows = mat.rows();
        auto cols = mat.cols();
        Eigen::FFT<Scalar> fft;
        Eigen::Matrix<std::complex<Scalar>, Eigen::Dynamic, Eigen::Dynamic> tmpMat(rows, cols);
        Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> res(rows, cols);
        // need apply 1d fft on both rows and cols
        for (auto i = 0; i < rows; ++i) {
            Eigen::Matrix<std::complex<Scalar>, Eigen::Dynamic, 1> tmp(cols);
            fft.inv(tmp, mat.row(i).eval());
            tmpMat.row(i) = tmp;
        }
        for (auto i = 0; i < cols; ++i) {
            Eigen::Matrix<std::complex<Scalar>, 1, Eigen::Dynamic> tmp(rows);
            fft.inv(tmp, tmpMat.col(i).eval());
            res.col(i) = tmp.real();
        }
        return res;
    }

    // OMP one is slower according to benchmark result (see bench/fft_bench.cpp)
    template<typename Derived>
    Eigen::Matrix<std::complex<typename Derived::Scalar>, Eigen::Dynamic, Eigen::Dynamic>
    ForwardFFT_OMP(Eigen::MatrixBase<Derived> const& mat) {
        auto rows = mat.rows();
        auto cols = mat.cols();
        Eigen::FFT<typename Derived::Scalar> fft;
        Eigen::Matrix<std::complex<typename Derived::Scalar>, Eigen::Dynamic, Eigen::Dynamic> res(rows, cols);
        // need apply 1d fft on both rows and cols
#pragma omp parallel sections
        {
#pragma omp section
            {
#pragma omp parallel for if (rows > 10) schedule(static)
                for (auto i = 0; i < rows; ++i) {
                    Eigen::Matrix<std::complex<typename Derived::Scalar>, Eigen::Dynamic, 1> tmp(cols);
                    fft.fwd(tmp, mat.row(i).eval());
                    res.row(i) = tmp;
                }
#pragma omp parallel for if (cols > 10) schedule(static)
                for (auto i = 0; i < cols; ++i) {
                    Eigen::Matrix<std::complex<typename Derived::Scalar>, 1, Eigen::Dynamic> tmp(rows);
                    fft.fwd(tmp, res.col(i).eval());
                    res.col(i) = tmp;
                }
            }
        }
        return res;
    }

    template <typename XprType, typename RowShift, typename ColShift>
    Eigen::CircShiftedView<XprType, RowShift, ColShift> circShift(Eigen::DenseBase<XprType>& x, RowShift r, ColShift c)
    {
        return Eigen::CircShiftedView<XprType, RowShift, ColShift>(x.derived(), r, c);
    }

    template <typename XprType>
    Eigen::CircShiftedView<XprType, Eigen::Index, Eigen::Index> fftshift(Eigen::DenseBase<XprType>& x)
    {
        Eigen::Index rs = x.rows() / 2;
        Eigen::Index cs = x.cols() / 2;
        return Eigen::CircShiftedView<XprType, Eigen::Index, Eigen::Index>(x.derived(), rs, cs);
    }

    template <typename XprType>
    Eigen::CircShiftedView<XprType, Eigen::Index, Eigen::Index> ifftshift(Eigen::DenseBase<XprType>& x)
    {
        Eigen::Index rs = (x.rows() + 1) / 2;
        Eigen::Index cs = (x.cols() + 1) / 2;
        return Eigen::CircShiftedView<XprType, Eigen::Index, Eigen::Index>(x.derived(), rs, cs);
    }
}

#endif //M_MATH_M_FFT_EIGEN_HPP


#pragma once
#include <Eigen/Core>

namespace mtao::linear_algebra {
    // the gram schmidt algorithm with a threshold for when 0-eigenvalues are found
    template <typename Derived>
        void gram_schmidt_in_place(Eigen::PlainObjectBase<Derived>& M) {
            using Scalar = typename Derived::Scalar;
            constexpr Scalar eps = std::numeric_limits<Scalar>::epsilon();
            for(int j = 0; j < M.cols(); ++j) {
                auto u = M.col(j);
                Scalar n = u.squaredNorm();
                for(int k = 0; k < j; ++k) {
                    auto v = M.col(k);
                    u -= v.dot(u) * v;
                }
                if(u.squaredNorm() < eps * n) {
                    u.setZero();
                } else {
                    u.normalize();
                }
            }
        }
    template <typename Derived>
        auto gram_schmidt(const Eigen::MatrixBase<Derived>& M) {
            typename Derived::PlainMatrix O = M;
            gram_schmidt_in_place(O);
            return O;
        }
}


#pragma once

#include <iostream>		// For console i/o
#include <vector>		// For vectors, not including linear algebra
#include <random>		// For random number generation
#include <algorithm>	// For min/max functions

// Additional libraries that you will have to download.
// First is Eigen, which we use for linear algebra: http://eigen.tuxfamily.org/index.php?title=Main_Page
#include <Eigen/Dense>
// Second is Boost, which we use for ibeta_inv: https://www.boost.org/
#include <boost/math/special_functions/beta.hpp>

// Typically these shouldn't be in a .hpp file.
using namespace std;
using namespace Eigen;
using namespace boost::math;

// This function returns the inverse of Student's t CDF using the degrees of
// freedom in nu for the corresponding probabilities in p. That is, it is
// a C++ implementation of Matlab's tinv function: https://www.mathworks.com/help/stats/tinv.html
// To see how this was created, see the "quantile" block here: https://www.boost.org/doc/libs/1_58_0/libs/math/doc/html/math_toolkit/dist_ref/dists/students_t_dist.html
double tinv(double p, unsigned int nu) {
	double x = ibeta_inv((double)nu / 2.0, 0.5, 2.0 * min(p, 1.0 - p));
	return sign(p - 0.5) * sqrt((double)nu * (1.0 - x) / x);
}

// Get the sample standard deviation of the vector v (an Eigen::VectorXd)
double stddev(const VectorXd& v) {
	double mu = v.mean(), result = 0;			// Get the sample mean
	for (unsigned int i = 0; i < v.size(); i++)
		result += (v[i] - mu) * (v[i] - mu);	// Compute the sum of the squared differences between samples and the sample mean
	result = sqrt(result / (v.size() - 1.0));	// Get the sample variance by dividing by the number of samples minus one, and then the sample standard deviation by taking the square root.
	return result;								// Return the value that we've computed.
}

// Assuming v holds i.i.d. samples of a random variable, compute
// a (1-delta)-confidence upper bound on the expected value of the random
// variable using Student's t-test. That is:
// sampleMean + sampleStandardDeviation/sqrt(n) * tinv(1-delta, n-1),
// where n is the number of points in v.
//
// If numPoints is provided, then ttestUpperBound predicts what its output would be if it were to
// be run using a new vector, v, containing numPoints values sampled from the same distribution as
// the points in v.
double ttestUpperBound(const VectorXd& v, const double& delta, const int numPoints = -1) {
	unsigned int n = (numPoints == -1 ? (unsigned int)v.size() : (unsigned int)numPoints);	// Set n to be numPoints if it was provided, and the number of points in v otherwise.
	return v.mean() + (numPoints != -1 ? 2.0 : 1.0) * stddev(v) / sqrt((double)n) * tinv(1.0 - delta, n - 1u);
}

double ttestLowerBound(const VectorXd& v, const double& delta, const int numPoints = -1) {
  unsigned int n = (numPoints == -1 ? (unsigned int)v.size() : (unsigned int)numPoints);	// Set n to be numPoints if it was provided, and the number of points in v otherwise.
  return v.mean() - (numPoints != -1 ? 2.0 : 1.0) * stddev(v) / sqrt((double)n) * tinv(1.0 - delta, n - 1u);
}

/*
This function implements CMA-ES (http://en.wikipedia.org/wiki/CMA-ES). Return
value is the minimizer / maximizer. This code is written for brevity, not clarity.
See the link above for a description of what this code is doing.
*/
VectorXd CMAES(
	const VectorXd& initialMean,											// Starting point of the search
	const double& initialSigma,												// Initial standard deviation of the search around initialMean
	const unsigned int& numIterations,										// Number of iterations to run before stopping
	// f, below, is the function to be optimized. Its first argument is the solution, the middle arguments are variables required by f (listed below), and the last is a random number generator.
	double(*f)(const VectorXd& theta, const void* params[], mt19937_64& generator),
	const void* params[],													// Parrameters of f other than theta
	const bool& minimize,													// If true, we will try to minimize f. Otherwise we will try to maximize f
	mt19937_64& generator)													// The random number generator to use
{
	// Define all of the terms that we will use in the iterations
	unsigned int N = (unsigned int)initialMean.size(), lambda = 4 + (unsigned int)floor(3.0 * log(N)), hsig;
	double sigma = initialSigma, mu = lambda / 2.0, eigeneval = 0, chiN = pow(N, 0.5) * (1.0 - 1.0 / (4.0 * N) + 1.0 / (21.0 * N * N));
	VectorXd xmean = initialMean, weights((unsigned int)mu);
	for (unsigned int i = 0; i < (unsigned int)mu; i++)
		weights[i] = i + 1;
	weights = log(mu + 1.0 / 2.0) - weights.array().log();
	mu = floor(mu);
	weights = weights / weights.sum();
	double mueff = weights.sum() * weights.sum() / weights.dot(weights), cc = (4.0 + mueff / N) / (N + 4.0 + 2.0 * mueff / N), cs = (mueff + 2.0) / (N + mueff + 5.0), c1 = 2.0 / ((N + 1.3) * (N + 1.3) + mueff), cmu = min(1.0 - c1, 2.0 * (mueff - 2.0 + 1.0 / mueff) / ((N + 2.0) * (N + 2.0) + mueff)), damps = 1.0 + 2.0 * max(0.0, sqrt((mueff - 1.0) / (N + 1.0)) - 1.0) + cs;
	VectorXd pc = VectorXd::Zero(N), ps = VectorXd::Zero(N), D = VectorXd::Ones(N), DSquared = D, DInv = 1.0 / D.array(), xold, oneOverD;
	for (unsigned int i = 0; i < DSquared.size(); i++)
		DSquared[i] *= DSquared[i];
	MatrixXd B = MatrixXd::Identity(N, N), C = B * DSquared.asDiagonal() * B.transpose(), invsqrtC = B * DInv.asDiagonal() * B.transpose(), arx(N, (int)lambda), repmat(xmean.size(), (int)(mu + .1)), artmp, arxSubMatrix(N, (int)(mu + .1));
	vector<double> arfitness(lambda);
	vector<unsigned int> arindex(lambda);
	// Perform the iterations
	for (unsigned int counteval = 0; counteval < numIterations;) {
	        cout << "Sampling Population" << endl;
		// Sample the population
		for (unsigned int k = 0; k < lambda; k++) {
			normal_distribution<double> distribution(0, 1);
			VectorXd randomVector(N);
			for (unsigned int i = 0; i < N; i++)
				randomVector[i] = D[i] * distribution(generator);
			arx.col(k) = xmean + sigma * B * randomVector;
 		}
		// Evaluate the population
		vector<VectorXd> fInputs(lambda);
		for (unsigned int i = 0; i < lambda; i++) {
		        cout << "Evaluating Candidate " << counteval+i << "/" << numIterations << endl;
			fInputs[i] = arx.col(i);
                        cout << "Theta: " << fInputs[i].transpose() << endl;
			arfitness[i] = (minimize ? 1 : -1) * f(fInputs[i], params, generator);
		}
		// Update the population distribution
		counteval += lambda;
		xold = xmean;
		for (unsigned int i = 0; i < lambda; ++i)
			arindex[i] = i;
		std::sort(arindex.begin(), arindex.end(), [&arfitness](unsigned int i1, unsigned int i2) {return arfitness[i1] < arfitness[i2]; });
		for (unsigned int col = 0; col < (unsigned int)mu; col++)
			arxSubMatrix.col(col) = arx.col(arindex[col]);
		xmean = arxSubMatrix * weights;
		ps = (1.0 - cs) * ps + sqrt(cs * (2.0 - cs) * mueff) * invsqrtC * (xmean - xold) / sigma;
		hsig = (ps.norm() / sqrt(1.0 - pow(1.0 - cs, 2.0 * counteval / lambda)) / (double)chiN < 1.4 + 2.0 / (N + 1.0) ? 1 : 0);
		pc = (1 - cc) * pc + hsig * sqrt(cc * (2 - cc) * mueff) * (xmean - xold) / sigma;
		for (unsigned int i = 0; i < repmat.cols(); i++)
			repmat.col(i) = xold;
		artmp = (1.0 / sigma) * (arxSubMatrix - repmat);
		C = (1 - c1 - cmu) * C + c1 * (pc * pc.transpose() + (1u - hsig) * cc * (2 - cc) * C) + cmu * artmp * weights.asDiagonal() * artmp.transpose();
		sigma = sigma * exp((cs / damps) * (ps.norm() / (double)chiN - 1.0));
		if ((double)counteval - eigeneval > (double)lambda / (c1 + cmu) / (double)N / 10.0) {
			eigeneval = counteval;
			for (unsigned int r = 0; r < C.rows(); r++)
				for (unsigned int c = r + 1; c < C.cols(); c++)
					C(r, c) = C(c, r);
			EigenSolver<MatrixXd> es(C);
			D = C.eigenvalues().real();
			B = es.eigenvectors().real();
			D = D.array().sqrt();
			for (unsigned int i = 0; i < B.cols(); i++)
				B.col(i) = B.col(i).normalized();
			oneOverD = 1.0 / D.array();
			invsqrtC = B * oneOverD.asDiagonal() * B.transpose();
		}
	} // End loop over iterations
	return arx.col(arindex[0]);
}


/* This sample is based on the Chapter 1 from book
 * "Building Machine Learning Systems with Python" by Willi Richert
 */

// third party includes
#include <csv.h>
#include <plot.h>
#include <Eigen/Dense>

// stl includes
#include <algorithm>
#include <experimental/filesystem>
#include <iostream>
#include <random>
#include <string>

// application includes
#include "../ioutils.h"
#include "../utils.h"

// Namespace and type aliases
namespace fs = std::experimental::filesystem;
typedef double DType;
using Matrix = Eigen::Matrix<DType, Eigen::Dynamic, Eigen::Dynamic>;

auto standardize(const Matrix& v) {
  assert(v.cols() == 1);
  auto m = v.colwise().mean();
  auto n = v.rows();
  DType sd = std::sqrt((v.rowwise() - m).array().pow(2).sum() /
                       static_cast<DType>(n - 1));
  Matrix sv = (v.rowwise() - m) / sd;
  return std::make_tuple(sv, m(0, 0), sd);
}

auto generate_polynomial(const Matrix& x, size_t degree) {
  assert(x.cols() == 1);
  auto rows = x.rows();

  Matrix poly_x = Matrix::Zero(rows, degree);
  // fill additional column for simpler vectorization
  {
    auto xv = poly_x.block(0, 0, rows, 1);
    xv.setOnes();
  }
  // copy initial data
  {
    auto xv = poly_x.block(0, 1, rows, 1);
    xv = x;
  }
  // generate additional terms
  for (size_t i = 2; i < degree; ++i) {
    auto xv = poly_x.block(0, i, rows, 1);
    xv = x.array().pow(static_cast<DType>(i));
  }
  return poly_x;
}

auto bgd(const Matrix& x, const Matrix& y) {
  size_t batch_size = 8;
  size_t n_epochs = 1000;
  DType lr = 0.0015;

  auto rows = x.rows();
  auto cols = x.cols();

  size_t batches = rows / batch_size;  // some samples will be skipped
  Matrix b = Matrix::Zero(cols, 1);

  DType prev_cost = std::numeric_limits<DType>::max();
  for (size_t i = 0; i < n_epochs; ++i) {
    for (size_t bi = 0; bi < batches; ++bi) {
      auto s = bi * batch_size;
      auto batch_x = x.block(s, 0, batch_size, cols);
      auto batch_y = y.block(s, 0, batch_size, 1);

      auto yhat = batch_x * b;
      auto error = yhat - batch_y;

      auto grad =
          (batch_x.transpose() * error) / static_cast<DType>(batch_size);

      b = b - lr * grad;
    }

    DType cost = (y - x * b).array().pow(2.f).sum() / static_cast<DType>(rows);

    std::cout << "BGD iteration : " << i << " Cost = " << cost << std::endl;
    if (cost <= prev_cost)
      prev_cost = cost;
    else
      break;  // early stopping
  }
  return b;
}

int main() {
  // Download the data
  const std::string data_path{"web_traffic.tsv"};
  if (!fs::exists(data_path)) {
    const std::string data_url{
        R"(https://raw.githubusercontent.com/luispedro/BuildingMachineLearningSystemsWithPython/master/ch01/data/web_traffic.tsv)"};
    if (!utils::DownloadFile(data_url, data_path)) {
      std::cerr << "Unable to download the file " << data_url << std::endl;
      return 1;
    }
  }

  // Read the data
  io::CSVReader<2, io::trim_chars<' '>, io::no_quote_escape<'\t'>> data_tsv(
      data_path);

  std::vector<DType> raw_data_x;
  std::vector<DType> raw_data_y;

  bool done = false;
  do {
    try {
      DType x = 0, y = 0;
      done = !data_tsv.read_row(x, y);
      if (!done) {
        raw_data_x.push_back(x);
        raw_data_y.push_back(y);
      }
    } catch (const io::error::no_digit& err) {
      // ignore bad formated samples
      std::cout << err.what() << std::endl;
    }
  } while (!done);

  // shuffle data
  size_t seed = 3465467546;
  std::shuffle(raw_data_x.begin(), raw_data_x.end(),
               std::default_random_engine(seed));
  std::shuffle(raw_data_y.begin(), raw_data_y.end(),
               std::default_random_engine(seed));

  // map data to the tensor
  size_t rows = raw_data_x.size();
  const auto data_x = Eigen::Map<Matrix>(raw_data_x.data(), rows, 1);
  std::cout << "X shape " << data_x.rows() << ":" << data_x.cols() << std::endl;
  Matrix x;
  std::tie(x, std::ignore, std::ignore) = standardize(data_x);

  const auto data_y = Eigen::Map<Matrix>(raw_data_y.data(), rows, 1);
  std::cout << "Y shape " << data_y.rows() << ":" << data_y.cols() << std::endl;
  // I'm not  using structured binding, because hard to debug such values
  Matrix y;
  DType ym{0};
  DType ysd{0};
  std::tie(y, ym, ysd) = standardize(data_y);

  // Scale data to prevent float overflow in BGD
  size_t p_degree = 64;
  const DType scale = 0.6;
  x *= scale;
  y *= scale;

  // solve normal equation
  Matrix poly_x = generate_polynomial(x, p_degree);
  Matrix b_eq =
      (poly_x.transpose() * poly_x).ldlt().solve(poly_x.transpose() * y);
  auto cost =
      (y - poly_x * b_eq).array().pow(2.f).sum() / static_cast<DType>(rows);
  std::cout << "cost for normal equation solution : " << cost << std::endl;

  // optimize with BGD
  Matrix b = bgd(poly_x, y);

  // generate new data
  const size_t new_x_size = 500;
  std::vector<DType> x_coord(new_x_size);
  auto new_x = Eigen::Map<Matrix>(x_coord.data(), new_x_size, 1);
  new_x = Eigen::Matrix<DType, Eigen::Dynamic, 1>::LinSpaced(
      new_x_size, data_x.minCoeff(), data_x.maxCoeff());
  Matrix new_x_std;
  std::tie(new_x_std, std::ignore, std::ignore) = standardize(new_x);
  new_x_std *= scale;
  Matrix new_poly_x = generate_polynomial(new_x_std, p_degree);

  // make predictions
  std::vector<DType> polyline_eq(new_x_size);
  auto new_y_eq = Eigen::Map<Matrix>(polyline_eq.data(), new_x_size, 1);
  new_y_eq = new_poly_x * b_eq;

  std::vector<DType> polyline(new_x_size);
  auto new_y = Eigen::Map<Matrix>(polyline.data(), new_x_size, 1);
  new_y = new_poly_x * b;

  // restore scalenew_y
  new_y_eq /= scale;
  new_y_eq = (new_y_eq * ysd).array() + ym;

  new_y /= scale;
  new_y = (new_y * ysd).array() + ym;

  // plot the data we read and approximate
  plotcpp::Plot plt(true);
  plt.SetTerminal("qt");
  plt.SetTitle("Web traffic over the last month");
  plt.SetXLabel("Time");
  plt.SetYLabel("Hits/hour");
  plt.SetAutoscale();
  plt.GnuplotCommand("set grid");

  auto time_range = data_x.maxCoeff() - data_x.minCoeff();
  auto tic_size = 7 * 24;
  auto time_tics = time_range / tic_size;
  plt.SetXRange(-tic_size / 2, data_x.maxCoeff() + tic_size / 2);

  plotcpp::Plot::Tics xtics;
  for (size_t t = 0; t < time_tics; ++t) {
    xtics.push_back({"week " + std::to_string(t), t * tic_size});
  }
  plt.SetXTics(xtics);

  plt.Draw2D(plotcpp::Points(raw_data_x.begin(), raw_data_x.end(),
                             raw_data_y.begin(), "data", "lc rgb 'black' pt 1"),
             plotcpp::Lines(x_coord.begin(), x_coord.end(), polyline_eq.begin(),
                            "neq approx", "lc rgb 'red' lw 2"),
             plotcpp::Lines(x_coord.begin(), x_coord.end(), polyline.begin(),
                            "bgd approx", "lc rgb 'cyan' lw 2"));
  plt.Flush();

  return 0;
}


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

// computes the center of a circle from 3 points on its boundary
Eigen::Vector2d circleCenter(const Eigen::Vector2d &P1, const Eigen::Vector2d &P2, const Eigen::Vector2d &P3)
{
	Eigen::Matrix<double,3,2> mat;
	mat.block<1,2>(0,0) = (P2 - P1).transpose();
	mat.block<1,2>(1,0) = (P3 - P2).transpose();
	mat.block<1,2>(2,0) = (P1 - P3).transpose();

	Eigen::Vector3d vec;
	vec.x() = 0.5*(P2 - P1).squaredNorm() + (P2 - P1).dot(P1);
	vec.y() = 0.5*(P3 - P2).squaredNorm() + (P3 - P2).dot(P2);
	vec.z() = 0.5*(P1 - P3).squaredNorm() + (P1 - P3).dot(P3);

	return mat.colPivHouseholderQr().solve(vec);
}

int main(int argc, char** argv)
{
	double t = 0.21111111111111111111111111111111111111111111111111111111111;
	Eigen::Vector2d P1( 0.0, 0.0),
					P2( 1.0, 0.0),
					P3( 1.0, 1.0),
					P4( 0.0, 1.0);
	Eigen::Matrix2d mat;
	mat <<  1.0, 0.0,
		    0.0, 1.0;
	Eigen::Vector2d tr(0.8,0.6);
	P1 = t*mat*P1 + tr;
	P2 = t*mat*P2 + tr;
	P3 = t*mat*P3 + tr;
	P4 = t*mat*P4 + tr;

	Eigen::Vector2d C = circleCenter(P1,P2,P3);

	double d1 = (C - P1).norm(),
		   d2 = (C - P2).norm(),
		   d3 = (C - P3).norm(),
		   d4 = (C - P4).norm();

	std::cout.precision(100);
	std::cout << "d1 " << d1 << std::endl;
	std::cout << "d2 " << d2 << std::endl;
	std::cout << "d3 " << d3 << std::endl;
	std::cout << "d4 " << d4 << std::endl;

	std::cout << std::endl;
	std::cout << "Test d4 == d1" << std::endl;
	if(d4 == d1)
		std::cout << "Distances égales" << std::endl;
	else
		std::cout << "Distances non égales" << std::endl;

	std::cout << std::endl;
	std::cout << "Test std::abs(d1 - d4) <= std::min(d1,d4)*std::numeric_limits<double>::epsilon()" << std::endl;
	if( std::abs(d1 - d4) <= std::min(d1,d4)*std::numeric_limits<double>::epsilon() )
		std::cout << "Distances égales" << std::endl;
	else
		std::cout << "Distances non égales" << std::endl;

	std::cout << std::endl;
	std::cout << "Test std::abs(d1 - d4) <= 6.0*std::min(d1,d4)*std::numeric_limits<double>::epsilon()" << std::endl;
	if( std::abs(d1 - d4) <= 6.0*std::min(d1,d4)*std::numeric_limits<double>::epsilon() )
		std::cout << "Distances égales" << std::endl;
	else
		std::cout << "Distances non égales" << std::endl;

	std::cout << std::endl;
	std::cout << "Test std::abs(d1 - d4) <= 7.0*std::min(d1,d4)*std::numeric_limits<double>::epsilon()" << std::endl;
	if( std::abs(d1 - d4) <= 7.0*std::min(d1,d4)*std::numeric_limits<double>::epsilon() )
		std::cout << "Distances égales" << std::endl;
	else
		std::cout << "Distances non égales" << std::endl;

	std::cout << std::endl;
	std::cout << "Test (float)d4 == (float)d1" << std::endl;
	if((float)d4 == (float)d1)
		std::cout << "Distances égales" << std::endl;
	else
		std::cout << "Distances non égales" << std::endl;

	std::cout << std::abs(d1+1.0) << " " << abs(d1+1.0) << std::endl;

	return 0;
}


#include <iostream>

#include <Eigen/Dense>

/* @brief
 * @param[in] z An $n$-dimensional vector containing one side of input data
 * @param[in] c An $n$-dimensional vector containing the other side of input data
 * @param[out] x The vector of parameters $(\alpha,\beta)$, intercept and slope of the line fitted
 */
Eigen::Vector2d lsqEst(const Eigen::VectorXd &z, const Eigen::VectorXd &c) {
	Eigen::Vector2d x;
	int n = z.size();

	Eigen::MatrixXd A(n, 2);
	A.col(0) = z;
	A(0, 1) = z(1);
	A(n - 1, 1) = z(n - 2);

	for(int i = 1; i < n - 1; i++) {
		A(i, 1) = z(i - 1) + z(i + 1);
	}

	x = (A.transpose() * A).llt().solve(A.transpose() * c);

	return x;
}

int main() {
    int n = 10;
    Eigen::VectorXd z(n), c(n);
    for(int i = 0; i < n; ++i) {
		z(i) = i + 1;
		c(i) = n - i;
	}

	Eigen::Vector2d x = lsqEst(z, c);

	std::cout << "alpha = " << x(0) << std::endl;
	std::cout << "beta = "  << x(1) << std::endl;

	return 0;
}


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

  @copyright 2016 NumScale SAS

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

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

 /*!

    @ingroup group-exponential
    Function object implementing cbrt capabilities

    Compute the cubic root: \f$\sqrt[3]{x}\f$

    @par Semantic:

    For every parameter of floating type T

    @code
    T r = cbrt(x);
    @endcode

    is similar to:

    @code
    T r = pow(x, T(1/3.0));
    @endcode

    but not equivalent because pow cannot raise a negative base to a fractional exponent.
    we have for all non Nan floating values cbrt(-x) = -cbrt(x).

    @par Decorators

    std_ for floating entries

    @see pow, sqrt

  **/
  Value cbrt(Value const & v0);
} }
#endif

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

#endif


/////////////
///
///    Constants.h
///         Constant miscellanea!!
///
////////

#ifndef ____CONSTANTS__
#define ____CONSTANTS__

#include <iostream>
#include <iomanip>
#include <math.h>
#include <string>
#include <map>
#include <fstream>
#include <vector>
#include <array>
#include <complex>
#include <chrono>
#include <ctime>
#define ARMA_DONT_USE_WRAPPER
// Take care with this macro: Use wisely!!!
#define ARMA_NO_DEBUG
#include <armadillo>

typedef std::complex<double> dcomplex;
/* typedef std::vector<std::vector<double>> matrix; */
/* typedef std::vector<std::vector<dcomplex>> cmatrix; */

using namespace std;
//using namespace arma;
//cout.precision(15);

/// Complex numbers
const dcomplex Im {0.0,1.0};
const dcomplex One {1.0,0.0};
const dcomplex Zero {0.0,0.0};

/// pi
const double pi = 4.0*atan(1.0);
const double twopi = 2.0 * pi;
const double one_over_pi = 1.0 / pi;

/// Square root two
const double sqrt_two = sqrt(2.0);
const double one_over_sqrt_two = 1.0 / sqrt_two;

/// Speed of light, in a.u. units
const double speed_light = 137.0;
const double one_over_speed_light = 1.0 / speed_light;
// Speed of light in meters over seconds
const double speed_light_is = 2.99792458e8;

/// electron charge
const double charge_electron_au = -1.;
/// Proton mass
const double mass_proton_au = 1836.;
/// Bohr radius (IS)
const double bohr_radius  = 5.29177211e-11;
const double aulength_nm  = 5.29177211e-2;
const double hatree_to_eV = 27.211;
// au time
const double autime_fs = 2.418884326505e-2;
const double autime_s  = 2.418884326505e-17;
// au intensity
const double auintensity = 3.5e16;

#endif  // ____constants__ //


#define DOCTEST_CONFIG_IMPLEMENT_WITH_MAIN
#include <Eigen/Dense>
#include <vector>

#include "copy.hpp"
#include "doctest.h"

struct TestData {
  TestData() {
    // Dimension of state space
    d = 2;

    // Variables for testing (iterations, final times)
    N = {128, 256, 512, 1024, 2048};
    T = {2.0, 5.0, 10.0, 20.0, 50.0};

    // Initial value for model
    y0.resize(d);
    y0 << 100, 5;

    // Definition of coefficients in Butcher scheme
    s = 2;

    A.resize(s, s);
    b.resize(s);
    A << 5. / 12., -1. / 12., 3. / 4., 1. / 4.;
    b << 3. / 4., 1. / 4.;

    // Coefficients for predator/prey model
    alpha1 = 3.;
    alpha2 = 2.;
    beta1 = 0.1;
    beta2 = 0.1;

    f = [this](const VectorXd &y) {
      auto temp = y;
      temp(0) *= alpha1 - beta1 * y(1);
      temp(1) *= -alpha2 + beta2 * y(0);
      return temp;
    };

    Jf = [this](const VectorXd &y) {
      MatrixXd temp(2, 2);
      temp << alpha1 - beta1 * y(1), -beta1 * y(0), beta2 * y(1),
          -alpha2 + beta2 * y(0);
      return temp;
    };
  }

  unsigned int s;
  unsigned int d;

  std::vector<unsigned int> N;
  std::vector<double> T;

  VectorXd y0;
  MatrixXd A;
  VectorXd b;

  double alpha1;
  double alpha2;
  double beta1;
  double beta2;

  std::function<MatrixXd(const VectorXd &)> Jf;
  std::function<VectorXd(const VectorXd &)> f;
};

TestData data;

TEST_SUITE("ImplRK Prey") {
  TEST_CASE("std::vector<VectorXd> solve" *
            doctest::description("RK solving")) {
    // Instantiate objects
    implicit_RKIntegrator RK_sol(data.A, data.b);
    implicit_RKIntegrator_TEST RK_stud(data.A, data.b);

    // Test with various values
    for (int i = 0; i < data.T.size(); i++) {
      for (int j = 0; j < data.N.size(); j++) {
        std::vector<VectorXd> sol_vec =
            RK_sol.solve(data.f, data.Jf, data.T[i], data.y0, data.N[j]);
        std::vector<VectorXd> stud_vec =
            RK_stud.solve_TEST(data.f, data.Jf, data.T[i], data.y0, data.N[j]);

        VectorXd sol = sol_vec.back();
        VectorXd stud = stud_vec.back();

        CHECK((sol - stud).norm() == doctest::Approx(0.).epsilon(1e-6));
      }
    }

    MESSAGE("Run the program to see a convergence study.");
  }

  TEST_CASE("MatrixXd kron" * doctest::description("skipped") *
            doctest::skip()) {}

  TEST_CASE("implicit_RKIntegrator" * doctest::description("skipped") *
            doctest::skip()) {}
}


/**
 * This file is part of https://github.com/adrelino/interpolation-methods
 *
 * Copyright (c) 2018 Adrian Haarbach <mail@adrian-haarbach.de>
 *
 * For the full copyright and license information, please view the LICENSE
 * file that was distributed with this source code.
 */
#ifndef INTERPOL_SU2_HPP
#define INTERPOL_SU2_HPP

#include <Eigen/Geometry>
#include <iomanip>

/**
 * Sinus cardinalis. Not normed version
 * @param t
 * @return sin(t)/t
 */
template<typename T>
T sinc(T theta) {
    return std::sin(theta) / theta; //si
    //return sin(M_PI*theta) / (M_PI*theta); normed //sinc
}//needed in QuaternionMapping.h

#include <hand_eye_calibration/QuaternionMapping.h>

//using namespace px; //to import logq, expq into global namespace!

//Added to namespace Eigen so that ADL works (otherwise won't compile on CLANG: https://clang.llvm.org/compatibility.html#dep_lookup)
namespace Eigen {
/**
 * Exp map applied to quaternions.
 *
 * The usual exp : so(3) -> SO(3) maps
 * from the Lie algebra (a tangent space) so(3) of skew-symmetric matrices of the from [wx,wy,wz]_x
 * to   the Lie group (a smooth manifold) SO(3) of orthonormal rotation matrices  : R e 3x3 : RR^T=I, |R|=1
 *
 * https://github.com/hengli/vmav-ros-pkg/blob/master/calibration/hand_eye_calibration/include/hand_eye_calibration/QuaternionMapping.h
 *
 * @param q' so(3) tangent space quaternion (if it was unit quaternion before, then its w is 0)
 * @return q = exp(q') SO(3) group quaternion
 */
template<typename T>
inline Eigen::Quaternion <T> expq(const Eigen::Quaternion <T> &q) {
    return px::expq(q);
    /*T a = q.vec().norm();
    T exp_w = std::exp(q.w());

    if (a == T(0)) {
        return Eigen::Quaternion<T>(exp_w, T(0), T(0), T(0));
    }

    Eigen::Quaternion <T> res;
    res.w() = exp_w * T(std::cos(a));
    res.vec() = exp_w * T(sinc(a)) * q.vec();

    return res;*/

//    assertCustom(q.w() == T(0));
//    Eigen::AngleAxis<T> angleAxis;
//    T angle = q.vec().norm();
//    angleAxis.axis() = q.vec() / angle;
//    angleAxis.angle() = angle * T(2.0);
//
//    Eigen::Quaternion<T> res(angleAxis);
//    return res;
}

/**
 * Log map applied to quaternions.
 *
 * If q was a unit quaternion, then w will be 0 in q' (since log(1)=0)
 *
 * The usual log : SO(3) -> so(3) maps
 * from the Lie group (a smooth manifold) SO(3) of orthonormal rotation matrices  : R e 3x3 : RR^T=I, |R|=1
 * to   the Lie algebra (a tangent space) so(3) of skew-symmetric matrices of the from [wx,wy,wz]_x
 *
 * https://github.com/hengli/vmav-ros-pkg/blob/master/calibration/hand_eye_calibration/include/hand_eye_calibration/QuaternionMapping.h
 *
 * @param q SO(3) group quaternion
 * @return q' = log(q) so(3) tangent space quaternion
 */
template<typename T>
inline Eigen::Quaternion <T> logq(Eigen::Quaternion <T> q) {
    return px::logq(q);
    /*

    //if(q.w()<T(0)) q.w() *=T(-1);

    T exp_w = q.norm();
    T w = std::log(exp_w);
    T a = std::acos(q.w() / exp_w);

    if (a == T(0)) {
        return Eigen::Quaternion<T>(w, T(0), T(0), T(0));
    }

    Eigen::Quaternion <T> res;
    res.w() = w;
    res.vec() = q.vec() / exp_w / (sinc(a));

    return res;//.normalized();
    */

//    assertCustom(abs(q.norm() - T(1)) < T(0.1));
//    Eigen::AngleAxis<T> angleAxis(q);
//    Eigen::Quaternion <T> res;
//    res.w() = T(0);
//    res.vec() = angleAxis.axis() * angleAxis.angle() * T(0.5);
//    return res;
}

//printing of Quaternion, Vector3 and Map<Vector3> works with this (even for Ceres Jet types)
template<typename T>
static std::ostream &operator<<(std::ostream &stream, const Eigen::QuaternionBase<T> &q) {
    return stream << std::setprecision(2) << std::fixed << q.w() << ", " << q.x() << ", " << q.y() << ", " << q.z();
}
template<typename T>
static std::ostream &operator<<(std::ostream &stream, const Eigen::Matrix<T,3,1> &q) {
    return stream << std::setprecision(2) << std::fixed << q.x() << ", " << q.y() << ", " << q.z();
}
template<typename T>
static std::ostream &operator<<(std::ostream &stream, const Eigen::Map<Eigen::Matrix<T, 3, 1>> &q) {
    return stream << std::setprecision(2) << std::fixed << q.x() << ", " << q.y() << ", " << q.z();
}

/**
 * Addition of quaternion coefficients.
 * \warning This operation has no direct geometric interpretation, and in the general case result in a non unit quaternion.
 * @return the addition of \c *this and \a other
 */
template<typename T>
static Eigen::Quaternion <T> operator+(const Eigen::Quaternion <T> &a, const Eigen::Quaternion <T> &b) {
    return Eigen::Quaternion<T>(a.coeffs() + b.coeffs());
}
/**
 * Substraction of quaternion coefficients.
 * \warning This operation has no direct geometric interpretation, and in the general case result in a non unit quaternion.
 * @return the substraction of \c *this and \a other
 */
template<typename T>
static Eigen::Quaternion <T> operator-(const Eigen::Quaternion <T> &a, const Eigen::Quaternion <T> &b) {
    return Eigen::Quaternion<T>(a.coeffs() - b.coeffs());
}
/**
 * Lhs multiplication of quaternion coefficients with a scalar.
 * Only useful in log space of unit quaternion (when w==0)
 */
template<typename T>
static Eigen::Quaternion <T> operator*(const T &b, const Eigen::Quaternion <T> &a) {
    return Eigen::Quaternion<T>(a.coeffs() * b);
}
/**
 * Rhs multiplication of quaternion coefficients with a scalar.
 * Only useful in log space of unit quaternion (when w==0)
 */
template<typename T>
static Eigen::Quaternion <T> operator*(const Eigen::Quaternion <T> &a, const T &b) {
    return Eigen::Quaternion<T>(a.coeffs() * b);
};

} // ns Eigen

#endif // INTERPOL_SU2_HPP


#include <iostream>
#include "problem_0002.h"

using namespace euler;

#define BOOST_TEST_MAIN
#define BOOST_TEST_MODULE Project_Euler_Problem_0002
#include <boost/test/included/unit_test.hpp>

BOOST_AUTO_TEST_CASE(sum_even_fibs_less_4million)
{
    constexpr int fiblessthan           = 4000000;
    const int fibn                      = fib_sequence_less_than( fiblessthan );
    const std::vector<int> fibs         = fib_sequence (fibn);
    const std::vector<int> fibs_even    = vector_even_values(fibs);
    const int sum_fibs_even             = std::accumulate(fibs_even.cbegin(), fibs_even.cend(), 0);

    BOOST_TEST( 4613732 == sum_fibs_even );
}


//
// This program is used to study how to calculate (SS|SS)^{m}
// for the recursive relation. Basically, it focus on the
// fmt function:
// f_{m}(t) = \int^{1}_{0} u^{2m} e^{-tu^{2}} du
// it needs the boost library to calcualte fmt
// fenglai liu
// Oct. 2013
//
// for reference, see Harris paper and the reference cited inside:
// Harris, Frank E
// Evaluation of GTO molecular integrals
// International Journal of Quantum Chemistry, 1983, Vol. 23, Page 1469--1478
//
// note:
// we found that the approximation of (2m-1)!!/R expression is
// not numerical stable. Combined with down recursive relation,
// it introduces greater error compared with other methods.
// Therefore we finally abadon it.
//
//
//

// common C head files used in the program
#include<cstdlib>
#include<cstdio>

// external math functions
#include<cmath>

// common C++ head files may be used in the program
#include<iostream>
#include<fstream>
#include<iomanip>
#include<string>
#include<vector>
#include<list>
#include<set>
#include<map>
#include<iterator>
#include<algorithm>

#include <boost/math/special_functions/binomial.hpp>  // special functions in math
#include <boost/math/special_functions/factorials.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/timer.hpp>
using namespace boost::math;
using namespace boost;
using namespace std;
#define PI              3.1415926535897932384626E0

double fm(const double& a, const int& m);
double doubleFac(const int& m, const int& n);

double fm(const double& a, const int& m) {
	double x = 1.0E0;
	if (fabs(a)<1.0E-14) return (pow(x,2*m+1)/(2*m+1));
	double f12 = 1.0E0/2.0E0;
	double p   = 1.0E0/(2.0E0*pow(a,m+f12));
	double upperLimit = a*pow(x,2.0E0);
	return tgamma_lower(m+f12,upperLimit)*p;
}

double doubleFac(const int& m, const int& n) {
	// calculate (2m-1)!!/(2m+2n+1)!!
	double x = 1.0E0;
	for(int i=0; i<=n; i++) {
		x *= 1.0E0/(2*m+2*i+1);
	}
	return x;
}

int main()
{
	///////////////////////////////////////////////////
	// global settings
	// step length and number of claculation for
	// time performance
	// global error is the error range
	// if the difference is less than the error, we
	// think the error could be omitted
	///////////////////////////////////////////////////
	int N = 10000000;
	double steplength  = 0.000001;
	double globalError = 1.0E-10;
	int M_limmit       = 10;
	int top_M_limmit   = 30;
	double T_max_limit = 36.0E0;
	double T_min_limit = 1.1E0;

	float steplength_f  = 0.00001;
	float globalError_f = 1.0E-6;

	/*
	///////////////////////////////////////////////////
	// this section testing the timing performance
	// between the different functions
	// so far, erf is 160 faster than the fmt function
	// in boost library
	///////////////////////////////////////////////////
	cout << "===========================================================" << endl;
	cout << "Time performance result: " << endl;
	cout << "Total sample number is " << N << endl;
	cout << "===========================================================" << endl;
	// testing the erf
	timer t1;
	for(int i=0; i<N; i++) {
		double a = i;
		double x = a/1000;
		double e = erf(x);
	}
	printf("erf time consuming: %-14.7f\n", t1.elapsed());

	// testing the sqrt
	timer t2;
	for(int i=0; i<N; i++) {
		double e = sqrt(i);
	}
	printf("sqrt time consuming: %-14.7f\n", t2.elapsed());

	// testing the exp
	timer t3;
	for(int i=0; i<N; i++) {
		double T = -(i+1)/N;
		double e = exp(T);
	}
	printf("exp(-T) time consuming: %-14.7f\n", t3.elapsed());

	// testing the fmt
	timer t4;
	for(int m=0; m<25; m++) {
		int n = N/25;
		for(int i=0; i<n; i++) {
			double T = 0.0001*(i+1);
			double e = fm(T,m);
		}
	}
	printf("fm(T,m) time consuming: %-14.7f\n", t4.elapsed());
	*/

	/*
	///////////////////////////////////////////////////
	// this section testing the up recursice relation
	// can we do it for m = 1 to 12?
	// 10 is perfect fine, however; 12 will have
	// error around 1.0E-10
	// we note, that up recursive relation can not
	// go with float
	///////////////////////////////////////////////////
	int numSteps = T_max_limit/steplength;
	cout << endl << endl;
	cout << "===========================================================" << endl;
	cout << "testing the up recursive relation for m = 1 to " << M_limmit << endl;
	cout << "error range is " << globalError << endl;
	cout << "step length for T is " << steplength << endl;
	cout << "T is ranging from " << T_min_limit << " to " <<T_max_limit << endl;
	cout << "===========================================================" << endl;
	for (int j = 0; j<numSteps; j++) {
		double T = T_min_limit+steplength*(j+1);
		double sqT = sqrt(T);
		double fmt= erf(sqT)*sqrt(PI)/2.0E0;
		double et = exp(-T);
		double e0 = fmt/sqT;
		double oneO2T = 0.5E0/T;
		for(int i=1; i<=M_limmit; i++) {
			double s = fm(T,i);
			double e = oneO2T*((2*i-1)*e0-et);
			double d = fabs(s-e);
			if (d>globalError) {
				printf("fmt is %-14.12f\n", s);
				printf("T   is %-14.12f\n", T);
				printf("error/fmt is %-14.12f\n", d/fmt);
				printf("for m=%d difference is %-14.12f\n", i, d);
			}
			e0 = e;
		}
	}
	*/

	/*
	///////////////////////////////////////////////////
	// testing the fmt from down recursive relation
	///////////////////////////////////////////////////
	cout << endl << endl;
	cout << "===========================================================" << endl;
	cout << "testing the down recursive relation for m = 1 to " << top_M_limmit << endl;
	cout << "this is for double type variable" << endl;
	cout << "error range is " << globalError << endl;
	cout << "step length for T is " << steplength << endl;
	cout << "T is ranging from 0 " << " to " <<T_max_limit << endl;
	cout << "===========================================================" << endl;
	int nSteps = T_max_limit/steplength;
	for (int n=1; n<=top_M_limmit; n++) {
		for (int j = 0; j<nSteps; j++) {
			double T = steplength*(j+1);
			double fmt= fm(T,n);
			double et = exp(-T);
			double e0 = fmt;
			double t2 = 2.0*T;
			for(int i=n-1; i>=0; i--) {
				double s = fm(T,i);
				double e = (1.0/(2*i+1.0))*(t2*e0+et);
				double d = fabs(s-e);
				if (d>globalError) {
					printf("T is %-14.12f\n", T);
					printf("for m=%d difference is %-14.12f\n", n, d);
				}
				e0 = e;
			}
		}
	}
	*/

	/*
	cout << endl << endl;
	cout << "===========================================================" << endl;
	cout << "testing the down recursive relation for m = 1 to " << top_M_limmit << endl;
	cout << "this is for float type variable" << endl;
	cout << "error range is " << globalError_f << endl;
	cout << "step length for T is " << steplength_f << endl;
	cout << "T is ranging from 0 " << " to " <<T_max_limit << endl;
	cout << "===========================================================" << endl;
	nSteps = T_max_limit/steplength_f;
	for (int n=1; n<=top_M_limmit; n++) {
		for (int j = 0; j<nSteps; j++) {
			float T = steplength_f*(j+1);
			float fmt= fm(T,n);
			float et = exp(-T);
			float e0 = fmt;
			float t2 = 2.0*T;
			for(int i=n-1; i>=0; i--) {
				float s = fm(T,i);
				float e = (1.0/(2*i+1.0))*(t2*e0+et);
				float d = fabs(s-e);
				if (d>globalError_f) {
					printf("T is %-10.7f\n", T);
					printf("for m=%d difference is %-10.7f\n", n, d);
				}
				e0 = e;
			}
		}
	}
	*/

	/*
	///////////////////////////////////////////////////
	// testing the fmt from down recursive relation
	// this is not so accurate
	// abandoned, too
	///////////////////////////////////////////////////
	cout << endl << endl;
	cout << "===========================================================" << endl;
	cout << "testing the down recursive relation for m = 1 to 10" << endl;
	cout << "the fm(t) is approximated by (2m-1)!!/(2t)^{m}*t^{1/2}" << endl;
	cout << "error range is 1.0E-10" << endl;
	cout << "step length for T is " << steplength << " from 30.0 to 50.0" << endl;
	cout << "===========================================================" << endl;
	int numSteps = (50-30)/steplength;
	double globalError = 1.0E-9;
	for (int n=1; n<=10; n++) {
		for (int j = 0; j<numSteps; j++) {

			// calculate fm(t)
			double T = 30.0E0+steplength*(j+1);
			double fmt= sqrt(PI)/(2*sqrt(T));
			for(int k=n; k>=1; k--) {
				fmt *= (2*k-1)/(2.0*T);
			}
			double s0 = fm(T,n);
			double d0 = fabs(s0-fmt);
			if (d0>globalError) {
				printf("T is %-14.12f\n", T);
				printf("error/fmt is %-14.12f\n", d0/fmt);
				printf("for m=%d difference is %-14.12f\n", n, d0);
			}

			// recursive relation
			double et = exp(-T);
			double e0 = fmt;
			double t2 = 2.0*T;
			for(int i=n-1; i>=0; i--) {
				double s = fm(T,i);
				double e = (1.0/(2*i+1.0))*(t2*e0+et);
				double d = fabs(s-e);
				if (d>globalError) {
					printf("T is %-14.12f\n", T);
					printf("error/fmt is %-14.12f\n", d/s);
					printf("for m=%d difference is %-14.12f\n", n, d);
				}
				e0 = e;
			}
		}
	}
	*/

	/*
	///////////////////////////////////////////////////
	// testing the power series
	// equation of 9 in Harris paper
	// We try to expand the power series up to 10 terms
	// here we test from m=1 to m=10
	// for T from 0 to 1.1
	///////////////////////////////////////////////////
	numSteps = T_min_limit/steplength;
	cout << endl << endl;
	cout << "===========================================================" << endl;
	cout << "testing the power series expansion with 13 terms" << endl;
	cout << "this is for double type variable" << endl;
	cout << "error range is " << globalError << endl;
	cout << "step length for T is " << steplength << endl;
	cout << "T is ranging from 0 to " << T_min_limit << endl;
	cout << "===========================================================" << endl;
	for (int m = 1; m<=M_limmit; m++) {
		for (int j = 0; j<numSteps; j++) {
			double T = steplength*(j+1);
			double et = exp(-T);
			double x  = 1.0;
			double t2 = 2.0*T;
			for(int i=12; i>=1; i--) {
				x = 1.0+t2/(2.0*(m+i)+1.0)*x;
			}
			x  = (1.0/(2*m+1))*x;
			x *= et;
			double result = fm(T,m);
			double d = fabs(result-x);
			if (d>globalError) {
				printf("fmt is %-16.14f\n", result);
				printf("for m=%d T=%f difference is %-16.14f\n", m, T, d);
			}
		}
	}
	*/

	/*
	cout << endl << endl;
	cout << "===========================================================" << endl;
	cout << "testing the power series expansion with 13 terms" << endl;
	cout << "this is for float type variable" << endl;
	cout << "error range is " << globalError_f << endl;
	cout << "step length for T is " << steplength_f << endl;
	cout << "T is ranging from 0 to " << T_min_limit << endl;
	cout << "===========================================================" << endl;
	numSteps = T_min_limit/steplength_f;
	for (int m = 1; m<=M_limmit; m++) {
		for (int j = 0; j<numSteps; j++) {
			float T  = steplength_f*(j+1);
			float et = exp(-T);
			float x  = 1.0;
			float t2 = 2.0*T;
			for(int i=12; i>=1; i--) {
				x = 1.0+t2/(2.0*(m+i)+1.0)*x;
			}
			x  = (1.0/(2*m+1))*x;
			x *= et;
			double result = fm(T,m);
			double d = fabs(result-x);
			if (d>globalError_f) {
				printf("fmt is %-16.14f\n", result);
				printf("for m=%d T=%f difference is %-16.14f\n", m, T, d);
			}
		}
	}
	*/

	////////////////////////////////////////////////////////
	// this is the continuous fraction implementation
	// of Harris equation, not correct yet, already abandoned
	// equation 24
	// testing the series
	// for m = 0, and T is from 0.01 to 25
	// test the convergence for the series
	////////////////////////////////////////////////////////
	/*
	int n = 20;
	for (int m=0; m<=30; m++) {
		for (int j = 0; j<2500; j++) {
			double T = 0.01*(j+1);
			double et = exp(-T);
			double minusn = 1.0E0;
			if ((n+1)%2 == 1) minusn = -1.0E0;
			double an = 2.0*(minusn*(m+n)-(m+1))/(4.0*(m+n)*(m+n)-1);
			double x  = 1+an*T;
			for(int i=n-1; i>1; i--) {
				double minusi = 1.0E0;
				if ((i+1)%2 == 1) minusi = -1.0E0;
				double ai = 2.0*(minusi*(m+i)-(m+1))/(4.0*(m+i)*(m+i)-1);
				x = (1+ai*T)/x;
			}
			double a1 = 2.0/(4.0*(m+1)*(m+1)-1);
			x = a1*T/x;
			x += 1/(2*m+1);
			x *= et;
			double result = fm(T,m);
			double d = fabs(result-x);
			if (d>0.00000000000001) {
				printf("for %d %f difference is %-16.14f\n", m, T, d);
			}
		}
	}
	*/

	return 0;
}


// Test101.cpp
//
// First program test the boost statistics library.
//
// 2008-6-27 DD initial code
// 2008-10-10 DD bernoulli distribution
//
// (C) Datasim Education BV 2009
//

#include <boost/math/distributions/uniform.hpp>
#include <boost/math/distributions/bernoulli.hpp>
#include <boost/math/distributions/gamma.hpp>
#include <boost/math/distributions/students_t.hpp>
#include <boost/math/distributions.hpp> // For non-member functions of distributions

#include <iostream>
using namespace std;

int main()
{
	// Don't forget to tell compiler which namespace
	using namespace boost::math;

	uniform_distribution<> myUniform(0.0, 1.0); // Default type ie 'double'
	cout << "Lower value: " << myUniform.lower() << ", upper value: " << myUniform.upper() << endl;

	// Choose another data type
	uniform_distribution<float> myUniform2(0.0, 1.0);
	cout << "Lower value: " << myUniform2.lower() << ", upper value: " << myUniform2.upper() << endl;

	// Distributional properties
	double x = 0.25;

	cout << "pdf of Uniform: " << pdf(myUniform, x) << endl;
	cout << "cdf of Uniform: " << cdf(myUniform, x) << endl;

	// Bernoulli distributions
	bernoulli_distribution<> myBernoulli(0.4);
	cout << "Probability of success: " << myBernoulli.success_fraction() << endl;

	int k = 0;
	cout << "pdf of Bernoulli: " << pdf(myBernoulli, k) << endl;
	cout << "cdf of Bernoulli : " << cdf(myBernoulli, k) << endl << endl;

	// Choose precision
	cout.precision(8); // Number of values behind the comma

	// Other properties
	cout << "\n***Uniform distribution: \n";
	cout << "mean: " << mean(myUniform) << endl;

	// ... more

	cout << "hazard: " << hazard(myUniform, x) << endl;

	// STUDENT
	students_t_distribution<float> myStudent(30);

	// Quantiles and conversions between significance levels (fractions, 0.05)
	// and cofidence levels (in percentages, e.g. 95%)
	double Alpha = 0.25;
	cout << "Confidence 1: " << quantile(myStudent, Alpha / 2) << endl;
	cout << "Confidence, in %: " << quantile(complement(myStudent, Alpha / 2)) << endl;

	// Required sample sizes for Students t-distribution
	double M = 1.2;		// True mean
	double Sm = 1.8;	// Sample mean
	double Sd = 2.4;	// Sample standard deviation

	try
	{
		double df = students_t::find_degrees_of_freedom (fabs(M-Sm), Alpha, Alpha, Sd);
		int dof = ceil(df) + 1; // ceil(x) == smallest integer not less than x
		cout << "One-sided degrees of freedom: " << dof << endl;
	}
	catch(const std::exception& e)
	{
      cout << e.what() << endl;
	}

	return 0;
}

windows/CsForFinancialMarketsPart2/Chapters20+21+22+23/Demos - CLI-CS Interop with Excel/CLI Interop Test (Chi-Squared)/CppVersion/Main.cpp


#include "EigenUtilities.h"

#include <Error.h>
#include <Eigen/Dense>

#include <iostream>

// Keep these utilities separate as using Eigen in an existing source
// file results in a 50% increase in compilation time.

// Compute the best-fitting projective transform that maps a set of 3D points
// to 2D points.
// A best-fit linear transform could be created by eliminating the denominators below.
void usgscsm::computeBestFitProjectiveTransform(std::vector<csm::ImageCoord> const& imagePts,
                                                std::vector<csm::EcefCoord>  const& groundPts,
                                                std::vector<double> & transformCoeffs) {

  if (imagePts.size() != groundPts.size())
    throw csm::Error(csm::Error::INVALID_USE,
                     "The number of inputs and outputs must agree.",
                     "computeBestFitProjectiveTransform");

  int numPts = imagePts.size();
  if (numPts < 8)
    throw csm::Error(csm::Error::INVALID_USE,
                     "At least 8 points are needed to fit a 3D-to-2D projective transform. "
                     "Ideally more are preferred.",
                     "computeBestFitProjectiveTransform");

  int numMatRows = 2 * numPts; // there exist x and y coords for each point
  int numMatCols = 14; // Number of variables in the projective transform

  Eigen::MatrixXd M = Eigen::MatrixXd::Zero(numMatRows, numMatCols);
  Eigen::VectorXd b = Eigen::VectorXd::Zero(numMatRows);

  for (int it = 0; it < numPts; it++) {

    double x = groundPts[it].x, y = groundPts[it].y, z = groundPts[it].z;
    double r = imagePts[it].line, c = imagePts[it].samp;

    // If the solution coefficients are u0, u1, ..., must have:

    // (u0 + u1 * x + u2 * y + u3  * z) / (1 + u4  * x + u5  * y + u6  * z) = r
    // (u7 + u8 * x + u9 * y + u10 * z) / (1 + u11 * x + u12 * y + u13 * z) = c

    // Multiply by the denominators. Get a linear regression in the coefficients.

    M.row(2 * it + 0) << 1, x, y, z, -x * r, -y * r, -z * r, 0, 0, 0, 0, 0, 0, 0;
    M.row(2 * it + 1) << 0, 0, 0, 0, 0, 0, 0, 1, x, y, z, -x * c, -y * c, -z * c;

    b[2 * it + 0] = r;
    b[2 * it + 1] = c;
  }

  // Solve the over-determined system, per:
  // https://eigen.tuxfamily.org/dox/group__LeastSquares.html
  Eigen::VectorXd u = M.colPivHouseholderQr().solve(b);

  // Copy back the result to a standard vector (to avoid using Eigen too much as
  // that is slow to compile).
  transformCoeffs.resize(numMatCols);
  for (int it = 0; it < numMatCols; it++)
    transformCoeffs[it] = u[it];

  return;
}


#include <armadillo>

using namespace std;
using namespace arma;

double f1(double x)
{
    return -x * sin(sqrt(abs(x)));
}

double f1_x(double x)
{
    return -sin(sqrt(abs(x))) - (x * x * cos(sqrt(abs(x)))) / (2 * pow(abs(x), 3.0 / 2));
}

double f2(double x)
{
    return x + 5 * sin(3 * x) + 8 * cos(5 * x);
}

double f2_x(double x)
{
    return -40 * sin(5 * x) + 15 * cos(3 * x) - 1;
}

double f3(double x, double y)
{
    return -(pow(x * x + y * y, 0.25) * (pow(sin(50 * pow(x * x + y * y, 0.1)), 2)) + 1);
}

double f3_x(double x, double y)
{
    return -(0.5 * x * (pow(sin(50 * pow(y * y + x * x, 0.1)), 2) + 1))
               / pow(y * y + x * x, 0.75)
           - (20.0
              * x
              * cos(50 * pow(y * y + x * x, 0.1))
              * sin(50 * pow(y * y + x * x, 0.1)))
                 / pow(y * y + x * x, 0.65);
}

double f3_y(double x, double y)
{
    return -(0.5
             * y
             * (pow(sin(50 * pow(y * y + x * x, 0.1)), 2) + 1))
               / pow(y * y + x * x, 0.75)
           - (20.0
              * y
              * cos(50 * pow(y * y + x * x, 0.1))
              * sin(50 * pow(y * y + x * x, 0.1)))
                 / pow(y * y + x * x, 0.65);
}

double gradienteDescendente(std::function<double(double)> f_prima,
                            pair<double, double> rangoBusqueda,
                            double alpha)
{
    const double rango = rangoBusqueda.second - rangoBusqueda.first;

    double x = randu(1).eval()(0) * rango + rangoBusqueda.first;

    //    cout << "x = " << x << endl
    //         << "f'(x) =" << f_prima(x) << endl
    //         << "|f'(x)| =" << abs(f_prima(x)) << endl;

    while (abs(f_prima(x)) > 1e-5) {
        x = x - alpha * f_prima(x);

        //        cout << "x = " << x << endl
        //             << "f'(x) =" << f_prima(x) << endl
        //             << "|f'(x)| =" << abs(f_prima(x)) << endl;
    }

    return x;
}

pair<double, double> gradienteDescendente(std::function<double(double, double)> f_x,
                                          std::function<double(double, double)> f_y,
                                          array<pair<double, double>, 2> rangosBusqueda,
                                          double alpha)
{
    const double rangoX = rangosBusqueda.at(0).second - rangosBusqueda.at(0).first;
    const double rangoY = rangosBusqueda.at(1).second - rangosBusqueda.at(1).first;

    double x = randu(1).eval()(0) * rangoX + rangosBusqueda.at(0).first;
    double y = randu(1).eval()(0) * rangoY + rangosBusqueda.at(1).first;

    while (abs(f_x(x, y)) > 1e-5 && abs(f_y(x, y)) > 1e-5) {
        x = x - alpha * f_x(x, y);
        y = y - alpha * f_y(x, y);
    }

    return {x, y};
}


/**
 * @file stabrk3.cc
 * @brief NPDE homework StabRK3 code
 * @author Unknown, Oliver Rietmann
 * @date 04.04.2021
 * @copyright Developed at ETH Zurich
 */

#include "stabrk3.h"

#include <Eigen/Core>
#include <vector>

#include "rkintegrator.h"

namespace StabRK3 {

/* SAM_LISTING_BEGIN_0 */
Eigen::Vector2d predPrey(Eigen::Vector2d y0, double T, unsigned int N) {
  double h = T / N;
  Eigen::Vector2d y = y0;

  auto f = [](Eigen::Vector2d y) {
    Eigen::Vector2d y_dot;
    y_dot(0) = (1 - y(1)) * y(0);
    y_dot(1) = (y(0) - 1) * y(1);
    return y_dot;
  };

  for (int j = 0; j < N; ++j) {
    Eigen::Vector2d k1 = f(y);
    Eigen::Vector2d k2 = f(y + h * k1);
    Eigen::Vector2d k3 = f(y + (h / 4.) * k1 + (h / 4.) * k2);
    y = y + (h / 6.) * k1 + (h / 6.) * k2 + (2. * h / 3.) * k3;
  }

  return y;
}
/* SAM_LISTING_END_0 */

/* SAM_LISTING_BEGIN_1 */
std::vector<Eigen::Vector2d> simulatePredPrey(
    const std::vector<unsigned int> &N_list) {
  int M = N_list.size();
  std::vector<Eigen::Vector2d> yT_list(M);

  // Initialize RK with Butcher table
  Eigen::Matrix3d A;
  A << 0, 0, 0, 1., 0, 0, 1. / 4., 1. / 4., 0;
  Eigen::Vector3d b(1.0 / 6.0, 1.0 / 6.0, 2.0 / 3.0);
  RKIntegrator<Eigen::Vector2d> integrator(A, b);

  // RHS function f for the prey/predator model
  double alpha1 = 1.0;
  double alpha2 = 1.0;
  double beta1 = 1.0;
  double beta2 = 1.0;
  auto f = [&alpha1, &alpha2, &beta1, &beta2](Eigen::Vector2d y) {
    Eigen::Vector2d temp = y;
    temp(0) *= alpha1 - beta1 * y(1);
    temp(1) *= -alpha2 + beta2 * y(0);
    return temp;
  };

  // Compute the solution(s) of the IVP(s)
  double T = 1.0;
  Eigen::Vector2d y0(100.0, 1.0);
  for (int m = 0; m < M; ++m) {
    yT_list[m] = integrator.solve(f, T, y0, N_list[m]).back();
  }

  return yT_list;
}
/* SAM_LISTING_END_1 */

}  // namespace StabRK3