/* -*- Mode: C++; tab-width: 4; indent-tabs-mode: t; c-basic-offset: 4 -*- vim:set ts=4 sw=4 sts=4 noet: */
#include "runtime/matrix.h"

#include <cassert>

#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/LU>

using Eigen::Map;
using Eigen::Matrix3d;
using Eigen::MatrixXd;
using Eigen::Vector3d;
using Eigen::VectorXd;

namespace flint {
namespace runtime {

void Transpose(double *d0, const double *d1, int kr, int kc)
{
	Map<MatrixXd> m0(d0, kr, kc);
	Map<const MatrixXd> m1(d1, kc, kr);
	m0 = m1.transpose();
}

void Outerproduct(double *d0,
				  int k1, const double *d1,
				  int k2, const double *d2)
{
	Map<const VectorXd> v1(d1, k1);
	Map<const VectorXd> v2(d2, k2);
	Map<MatrixXd> m0(d0, k2, k1);
	m0.noalias() = v1 * v2.transpose();
}

double Scalarproduct(int k, const double *d1, const double *d2)
{
	Map<const VectorXd> v1(d1, k);
	Map<const VectorXd> v2(d2, k);
	return v1.dot(v2);
}

void Vectorproduct(double *d0, const double *d1, const double *d2)
{
	Map<Vector3d> v0(d0);
	Map<const Vector3d> v1(d1);
	Map<const Vector3d> v2(d2);
	v0 = v1.cross(v2);
}

double Determinant(int k, const double *d1)
{
	Map<const MatrixXd> m1(d1, k, k);
	return m1.determinant();
}

double Select2(const double *d1, int n2)
{
	assert(n2 > 0); // 1-based
	return d1[n2 - 1];
}

double Select3(int kr, int kc, const double *d1, int n2, int n3)
{
	assert(n2 > 0);
	assert(n3 > 0);
	Map<const MatrixXd> m1(d1, kr, kc);
	return m1(n2 - 1, n3 - 1);
}

void Selrow(double *d0, int kr, int kc, const double *d1, int n2)
{
	assert(n2 > 0); // 1-based
	Map<VectorXd> v0(d0, kr);
	Map<const MatrixXd> m1(d1, kr, kc);
	v0 = m1.row(n2 - 1);
}

void Mult(double *d0, int k, double a1, const double *d2)
{
	Map<VectorXd> v0(d0, k);
	Map<const VectorXd> v2(d2, k);
	v0.noalias() = a1 * v2;
}

void Mmul(double *d0, int kr, int kx, int kc,
		  const double *d1, const double *d2)
{
	Map<MatrixXd> m0(d0, kr, kc);
	Map<const MatrixXd> m1(d1, kr, kx);
	Map<const MatrixXd> m2(d2, kx, kc);
	m0.noalias() = m1 * m2;
}

}
}


#include <gtest/gtest.h>
#include <Eigen/Core>
#include <Eigen/Dense>
#include "stiffness_matrix.hpp"

TEST(TestStiffnessMatrix, TestReferenceElement) {
    Eigen::MatrixXd vertices(3, 3);
    vertices << 0, 0, 0,
        1, 0, 0,
        0, 1, 0;

    Eigen::Matrix3d stiffnessMatrix;

    computeStiffnessMatrix(stiffnessMatrix, vertices.row(0), vertices.row(1), vertices.row(2));

    // This should be equal to
    //
    //   int_K grad lambda(0, x) * grad lambda(0, x) dx = 1
    //
    // where K is the triangle spanned by (0,0), (1,0), (1,0)
    const double expected00 = 1;
    ASSERT_NEAR(expected00, stiffnessMatrix(0, 0), 1e-8) << "stiffness matrix vector not correct at (0,0)";

    // This should be equal to
    //
    //   int_K grad lambda(0, x) * grad lambda(1, x) dx = -0.5
    //
    // where K is the triangle spanned by (0,0), (1,0), (1,0)
    const double expected01 = -0.5;
    ASSERT_NEAR(expected01, stiffnessMatrix(0, 1), 1e-8) << "stiffness matrix vector not correct at (0,1)";

    // This should be equal to
    //
    //   int_K grad lambda(0, x) * grad lambda(2, x) dx = -0.5
    //
    // where K is the triangle spanned by (0,0), (1,0), (1,0)
    const double expected02 = -0.5;
    ASSERT_NEAR(expected02, stiffnessMatrix(0, 2), 1e-8) << "stiffness matrix vector not correct at (0,2)";

    // This should be equal to
    //
    //   int_K grad lambda(1, x) * grad lambda(0, x) dx = -0.5
    //
    // where K is the triangle spanned by (0,0), (1,0), (1,0)
    const double expected10 = -0.5;
    ASSERT_NEAR(expected10, stiffnessMatrix(1, 0), 1e-8) << "stiffness matrix vector not correct at (1,0)";

    // This should be equal to
    //
    //   int_K grad lambda(1, x) * grad lambda(1, x) dx = 0.5
    //
    // where K is the triangle spanned by (0,0), (1,0), (1,0)
    const double expected11 = 0.5;
    ASSERT_NEAR(expected11, stiffnessMatrix(1, 1), 1e-8) << "stiffness matrix vector not correct at (1,1)";

    // This should be equal to
    //
    //   int_K grad lambda(1, x) * grad lambda(2, x) dx = 0
    //
    // where K is the triangle spanned by (0,0), (1,0), (1,0)
    const double expected12 = 0;
    ASSERT_NEAR(expected12, stiffnessMatrix(1, 2), 1e-8) << "stiffness matrix vector not correct at (1,2)";

    // This should be equal to
    //
    //   int_K grad lambda(2, x) * grad lambda(0, x) dx = -0.5
    //
    // where K is the triangle spanned by (0,0), (1,0), (1,0)
    const double expected20 = -0.5;
    ASSERT_NEAR(expected20, stiffnessMatrix(2, 0), 1e-8) << "stiffness matrix vector not correct at (2, 0)";

    // This should be equal to
    //
    //   int_K grad lambda(2, x) * grad lambda(1, x) dx = 0
    //
    // where K is the triangle spanned by (0,0), (1,0), (1,0)
    const double expected21 = 0;
    ASSERT_NEAR(expected21, stiffnessMatrix(2, 1), 1e-8) << "stiffness matrix vector not correct at (2,1)";

    // This should be equal to
    //
    //   int_K grad lambda(1, x) * grad lambda(2, x) dx = 0.5
    //
    // where K is the triangle spanned by (0,0), (1,0), (1,0)
    const double expected22 = 0.5;
    ASSERT_NEAR(expected22, stiffnessMatrix(2, 2), 1e-8) << "stiffness matrix vector not correct at (2,2)";
}

TEST(TestStiffnessMatrix, TestSkewTriagnle) {
    Eigen::MatrixXd vertices(3, 3);
    vertices << 0, 0, 0,
        1, 0.4, 0,
        0, 1, 0;

    Eigen::Matrix3d stiffnessMatrix;

    computeStiffnessMatrix(stiffnessMatrix, vertices.row(0), vertices.row(1), vertices.row(2));

    // This should be equal to
    //
    //   int_K grad lambda(0, x) * grad lambda(0, x) dx = 0.68
    //
    // where K is the triangle spanned by (0,0), (1,0.4), (1,0)
    const double expected00 = 0.68;
    ASSERT_NEAR(expected00, stiffnessMatrix(0, 0), 1e-8) << "stiffness matrix vector not correct at (0,0)";

    // This should be equal to
    //
    //   int_K grad lambda(0, x) * grad lambda(1, x) dx = -0.3
    //
    // where K is the triangle spanned by (0,0), (1,0.4), (1,0)
    const double expected01 = -0.3;
    ASSERT_NEAR(expected01, stiffnessMatrix(0, 1), 1e-8) << "stiffness matrix vector not correct at (0,1)";

    // This should be equal to
    //
    //   int_K grad lambda(0, x) * grad lambda(2, x) dx = -0.38
    //
    // where K is the triangle spanned by (0,0), (1,0.4), (1,0)
    const double expected02 = -0.38;
    ASSERT_NEAR(expected02, stiffnessMatrix(0, 2), 1e-8) << "stiffness matrix vector not correct at (0,2)";

    // This should be equal to
    //
    //   int_K grad lambda(1, x) * grad lambda(0, x) dx = -0.3
    //
    // where K is the triangle spanned by (0,0), (1,0.4), (1,0)
    const double expected10 = -0.3;
    ASSERT_NEAR(expected10, stiffnessMatrix(1, 0), 1e-8) << "stiffness matrix vector not correct at (1,0)";

    // This should be equal to
    //
    //   int_K grad lambda(1, x) * grad lambda(1, x) dx = 0.5
    //
    // where K is the triangle spanned by (0,0), (1,0.4), (1,0)
    const double expected11 = 0.5;
    ASSERT_NEAR(expected11, stiffnessMatrix(1, 1), 1e-8) << "stiffness matrix vector not correct at (1,1)";

    // This should be equal to
    //
    //   int_K grad lambda(1, x) * grad lambda(2, x) dx = 0.2
    //
    // where K is the triangle spanned by (0,0), (1,0.4), (1,0)
    const double expected12 = -0.2;
    ASSERT_NEAR(expected12, stiffnessMatrix(1, 2), 1e-8) << "stiffness matrix vector not correct at (1,2)";

    // This should be equal to
    //
    //   int_K grad lambda(2, x) * grad lambda(0, x) dx = -0.38
    //
    // where K is the triangle spanned by (0,0), (1,0.4), (1,0)
    const double expected20 = -0.38;
    ASSERT_NEAR(expected20, stiffnessMatrix(2, 0), 1e-8) << "stiffness matrix vector not correct at (2, 0)";

    // This should be equal to
    //
    //   int_K grad lambda(2, x) * grad lambda(1, x) dx = -0.2
    //
    // where K is the triangle spanned by (0,0), (1,0.4), (1,0)
    const double expected21 = -0.2;
    ASSERT_NEAR(expected21, stiffnessMatrix(2, 1), 1e-8) << "stiffness matrix vector not correct at (2,1)";

    // This should be equal to
    //
    //   int_K grad lambda(1, x) * grad lambda(2, x) dx = 0.58
    //
    // where K is the triangle spanned by (0,0), (1,0.4), (1,0)
    const double expected22 = 0.58;
    ASSERT_NEAR(expected22, stiffnessMatrix(2, 2), 1e-8) << "stiffness matrix vector not correct at (2,2)";
}


#define CATCH_CONFIG_MAIN
#include <catch2/catch.hpp>

#include <iostream>
#include <Eigen/LU>
#include "mrob/matrix_base.hpp"
#include "mrob/SE3.hpp"
#include "mrob/SO3.hpp"

using namespace std;

TEST_CASE("SO3 tests")
{
    SECTION("Testing the Identity element")
    {
        mrob::SO3 R;
        R.print();
        std::cout << "Identity element error= " << R.ln_vee().norm() << std::endl;
        REQUIRE(R.ln_vee().norm() == Approx(0.0));
    }

    SECTION("Testing the Identity element plus epsilon")
    {
        mrob::Mat31 w;
        w << 1e-15, 0, 0;
        mrob::SO3 R = mrob::SO3(w);
        std::cout << "Identity element error epsilon = " << R.ln_vee().norm() << std::endl;
        REQUIRE(R.ln_vee().norm() == Approx(0.0).margin(1e-12));
    }

    SECTION("Testing Regular")
    {
        mrob::Mat31 w;
        w << 1.2, -0.3, 0.2;
        mrob::SO3 R = mrob::SO3(w);
        std::cout << "Exponent and Log test " << (R.ln_vee() - w).norm() << std::endl;
        REQUIRE((R.ln_vee() - w).norm() == Approx(0.0).margin(1e-12));
    }

    SECTION("testing bool function isSO3")
    {
        mrob::Mat31 w;
        w << 1.2, -0.3, 0.2;
        mrob::SO3 R(w);
        std::cout << "Matris is SO3: " << mrob::isSO3(R.R()) << std::endl;
        REQUIRE(mrob::isSO3(R.R()) == true);
        mrob::Mat3 notR = mrob::Mat3::Random();
        std::cout << "Matris is SO3: " << mrob::isSO3(notR) << std::endl;
        REQUIRE(mrob::isSO3(notR) == false);
    }

    SECTION("Testing Pi. 1 component")
    {
        mrob::Mat31 w;
        w << M_PI, 0.0, 0.0;
        mrob::SO3 R = mrob::SO3(w);
        //R.print();
        std::cout << "Pi rotation 1 component = " << (R.ln_vee() - w).norm() << std::endl;
        REQUIRE((R.ln_vee() - w).norm() == Approx(0.0));
    }

    SECTION("Testing Pi. 3 components")
    {
        mrob::Mat31 w;
        w << M_PI * std::sqrt(1.0 / 3), M_PI * std::sqrt(1.0 / 3), M_PI * std::sqrt(1.0 / 3);
        mrob::SO3 R = mrob::SO3(w);
        std::cout << "Pi rotation 3 components= " << (R.ln_vee() - w).norm() << std::endl;
        REQUIRE((R.ln_vee() - w).norm() == Approx(0.0).margin(1e-12));
    }

    SECTION("Testing operators")
    {
        std::cout << "\ntesting operators\n";
        mrob::Mat31 w;
        w << M_PI, 0.0, 0.0;
        mrob::SO3 R = mrob::SO3(w);
        R.print();
        mrob::Mat3 w_hat = R.ln();
        std::cout << w_hat << std::endl;
        REQUIRE((w - mrob::vee3(w_hat)).norm() == Approx(0.0));
    }

    SECTION("Testing inverse")
    {
        mrob::Mat31 w;
        w << M_PI, 0.0, 0.0;
        mrob::SO3 R = mrob::SO3(w);

        std::cout << "testing inverse" << std::endl;
        mrob::SO3 Rt = R.inv();
        std::cout << "invers = " << Rt.R() << std::endl;
        REQUIRE((Rt.mul(R).R() - mrob::Mat3::Identity()).norm() == Approx(0.0));
    }
}

TEST_CASE("SE3 tests")
{
    SECTION("Testing the constructor. Default")
    std::cout << "\n\nSE3 tests" << std::endl;
    {
        mrob::SE3 T;
        //T.print();
        std::cout << "Identity element error= " << T.ln_vee().norm() << std::endl;
        REQUIRE(T.ln_vee().norm() == Approx(0.0));
    }

    SECTION("Testing the constructor. Xi argument. 1 case")
    {
        mrob::Mat61 xi;
        xi << 1e-9, 0, 0, 20, 100, 4;
        mrob::SE3 T(xi);
        T.print();
        std::cout << "random element plus epsilon= " << (T.ln_vee() - xi).norm() << std::endl;
        REQUIRE((T.ln_vee() - xi).norm() == Approx(0.0).margin(1e-12));
    }

    SECTION("Testing the constructor. Xi argument. 2 case")
    {
        mrob::Mat61 xi;
        xi << 1, 0, -0.2, 5, 10, 2;
        mrob::SE3 T(xi);
        std::cout << "Some normal element error = " << (T.ln_vee() - xi).norm() << std::endl;
        REQUIRE((T.ln_vee() - xi).norm() == Approx(0.0).margin(1e-12));
    }

    SECTION("Testing bool function isSE3")
    {
        mrob::Mat61 xi;
        xi << 1, 0, -0.2, 5, 10, 2;
        mrob::SE3 T(xi);
        std::cout << "input SE3 and Matris is SE3?: " << mrob::isSE3(T.T()) << std::endl;
        REQUIRE(mrob::isSE3(T.T()) == true);
        mrob::Mat4 notT = mrob::Mat4::Random();
        std::cout << "Random Matris is SE3: " << mrob::isSE3(notT) << std::endl;
        REQUIRE(mrob::isSE3(notT) == false);
    }

    SECTION("Testing Pi")
    {
        mrob::Mat61 xi;
        xi << M_PI, 0, 0, 5, 100, 2;
        mrob::SE3 T(xi);
        T.print_lie();
        std::cout << "Pi error plus trans= " << (T.ln_vee() - xi).norm() << std::endl;
        REQUIRE((T.ln_vee() - xi).norm() == Approx(0.0).margin(1e-12));
    }

    SECTION("Testin Pi. 3 components")
    {
        mrob::Mat61 xi;
        xi << M_PI * std::sqrt(1.0 / 3) - 1e-4, M_PI * std::sqrt(1.0 / 3), M_PI * std::sqrt(1.0 / 3), 5, 1000, 200;
        mrob::SE3 T(xi);
        T.print_lie();
        std::cout << "Pi error 3 comp plus trans= " << (T.ln_vee() - xi).norm() << std::endl;
        REQUIRE((T.ln_vee() - xi).norm() == Approx(0.0).margin(1e-10));
    }

    SECTION("Testing update")
    {
        mrob::SE3 T1;
        T1.print();
        T1.print_lie();
        mrob::Mat61 xi;
        xi << 1, 0, -0.2, 5, 10, 2;
        mrob::SE3 T(xi);
        T.print();
        xi << T.ln_vee();
        mrob::SE3 T2(xi);
        T2.print();
        std::cout << "Matrix distance = " << (T.T() - T2.T()).norm() << std::endl;
        REQUIRE((T.T() - T2.T()).norm() == Approx(0.0).margin(1e-12));

        std::cout << "testing update\n";
        T2.update_lhs(xi);
        T2.print();
        mrob::Mat41 v;
        v << 1.0, 3.2, -1.2, 1.0;
        std::cout << T2.T() * v << std::endl;
        mrob::Mat41 tmp = T2.T() * v;
        v = T2.T() * v;
        std::cout << v << std::endl;
        REQUIRE((v - tmp).norm() == Approx(0.0).margin(1e-12));
    }

    SECTION("Access inner matrix")
    {
        mrob::SE3 T;
        mrob::Mat4 gt;
        gt << 1, 0, 0, 1,
            0, 1, 0, 2,
            0, 0, 1, 3,
            0, 0, 0, 1;

        T.ref2T() << gt;

        REQUIRE((T.T() - gt).norm() == Approx(0.0).margin(1e-12));
        T.print();
    }

    SECTION("Testing inverse()")
    {
        mrob::Mat61 xi;
        xi << 1, 0, -0.2, 5, 10, 2;
        mrob::SE3 T(xi);
        T.print();
        xi << T.ln_vee();
        mrob::SE3 T2(xi);
        std::cout << "testing inverse" << std::endl;
        mrob::SE3 Tt = T2.inv();
        std::cout << "invers = " << Tt.T() << std::endl;
        std::cout << "Matrix distance = " << (Tt.T() - T2.T().inverse()).norm() << std::endl;
        REQUIRE((Tt.T() - T2.T().inverse()).norm() == Approx(0.0).margin(1e-12));

        mrob::Mat61 xi2 = -T2.ln_vee();
        mrob::SE3 T22(xi2);
        std::cout << "Matrix distance by negating= " << (T22.T() - T2.T().inverse()).norm() << std::endl;
        REQUIRE((T22.T() - T2.T().inverse()).norm() == Approx(0.0).margin(1e-12));
    }

    SECTION("Testing adj()")
    {
        mrob::Mat61 xi;
        xi << 0, 0, 0, 1, 2, 3;
        mrob::SE3 T(xi);

        mrob::Mat6 gt;
        gt << 1, 0, 0, 0, 0, 0,
            0, 1, 0, 0, 0, 0,
            0, 0, 1, 0, 0, 0,
            0, -3, 2, 1, 0, 0,
            3, 0, -1, 0, 1, 0,
            -2, 1, 0, 0, 0, 1;

        std::cout << "testing adjoint" << std::endl;
        std::cout << "Adjoint= " << T.adj() << std::endl;
        REQUIRE((T.adj() - gt).norm() == Approx(0.0).margin(1e-12));
    }

    SECTION("Testing adjoint property: T*exp(xi) = exp(Adj_t(xi))*Ts")
    {
        // checking equality for Adj(): T*exp(xi) = exp(Adj_t(xi))*T
        //initial pose
        mrob::Mat61 xi;
        xi << 0.1, 0.2, 0.3, 4, 5, 6;
        mrob::SE3 T(xi);

        //small disturbance
        mrob::Mat61 eta;
        eta << 0.05, 0.05, 0.05, 0.1, 0.1, 0.1;

        // left part of equality
        mrob::SE3 T_exp(T);
        T_exp.update_rhs(eta);

        // right part of the equality
        mrob::SE3 tmp_pose(mrob::Mat4(T.T()));
        tmp_pose.update_lhs(T.adj()*eta);

        // checking that both parts of equality are equal
        REQUIRE((T_exp.T() - tmp_pose.T()).norm() == Approx(0.0).margin(1e-12));
    }

    SECTION("Testing subblock metrixes")
    {
        mrob::Mat61 xi;
        xi << 0, 0, 0, 1, 2, 3;
        mrob::SE3 T(xi);

        mrob::Mat4 gt;
        gt << 1, 0, 0, 1,
            0, 1, 0, 2,
            0, 0, 1, 3,
            0, 0, 0, 1;

        std::cout << "Subblock methods an matrix:" << std::endl;
        T.print();
        std::cout << "\nT = " << T.T() << std::endl;
        REQUIRE((T.T() - gt).norm() == Approx(0.0).margin(1e-12));

        std::cout << "\nR = " << T.R() << std::endl;
        REQUIRE((T.R() - mrob::Mat3::Identity()).norm() == Approx(0.0).margin(1e-12));

        std::cout << "\nt = " << T.t() << std::endl;
        REQUIRE((T.t() - gt.topRightCorner<3, 1>()).norm() == Approx(0.0).margin(1e-12));
    }

    SECTION("Testing transformation")
    {
        mrob::Mat61 xi;
        xi << 1, 0, -0.2, 5, 10, 2;
        mrob::SE3 T(xi);
        mrob::Mat31 p, p2;
        p << 2, 3, -1;
        p2 = p;
        p = T.transform(p);
        p = T.inv().transform(p);
        std::cout << "Testing transformations = " << (p - p2).norm() << std::endl;
        REQUIRE((p - p2).norm() == Approx(0.0).margin(1e-12));
    }

    SECTION("Multiplication")
    {
        mrob::SE3 T1;
        T1.ref2T() <<
        1,0,0,0,
        0,1,0,0,
        0,0,1,0,
        0,0,0,1;

        mrob::SE3 T2;
        T2.ref2T() <<
        1,0,0,1,
        0,1,0,2,
        0,0,1,3,
        0,0,0,1;

        mrob::Mat4 gt;
        gt <<
        1,0,0,1,
        0,1,0,2,
        0,0,1,3,
        0,0,0,1;

        REQUIRE((T1.mul(T2).T() - gt).norm() == Approx(0.0).margin(1e-12));
    }
}


/**
 * @file    useGeometry.cpp
 * @date    2017/9/8
 * @version V1.0
 * @author  Jacob.lin
 */

#include <iostream>
#include <cmath>
using namespace std;

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

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

    Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
    Eigen::AngleAxisd rotation_vector(M_PI/4, Eigen::Vector3d(0, 0, 1));

    cout.precision(3);
    cout << "rotation_matrix = \r\n" << rotation_vector.matrix() << endl;

    rotation_matrix = rotation_vector.toRotationMatrix();

    Eigen::Vector3d v(1, 0, 0);
    Eigen::Vector3d v_rotation = rotation_vector * v;
    cout << "(1, 0, 0) after rotation = " << v_rotation.transpose() << endl;

    v_rotation = rotation_matrix * v;
    cout << "(1, 0, 0) after rotation = " << v_rotation.transpose() << endl;

    Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0);    /* 2,1,0 is ZYX axis, yaw, pitch, roll */
    cout << "yaw pitch roll = " << euler_angles.transpose();

    Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
    T.rotate(rotation_vector);
    T.pretranslate(Eigen::Vector3d(1, 3, 4));
    cout << "Transform matrix = \r\n" << T.matrix() << endl;

    Eigen::Vector3d v_transformed = T * v;
    cout << "v tranformed = " << v_transformed.transpose() << endl;

    Eigen::Quaterniond q = Eigen::Quaterniond(rotation_vector);
    cout << "quaternion = \r\n" << q.coeffs() << endl;
    q = Eigen::Quaterniond(rotation_matrix);
    cout << "quaternion = \r\n" << q.coeffs() << endl;

    v_rotation = q * v; /* qvq^(-1) */
    cout << "(1, 0, 0) after rotation = " << v_rotation.transpose() << endl;

    return 0;
}


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

class exp_func
{
    public:
        exp_func(Eigen::Vector2d init) : x_0{init[0]}, y_0{init[1]} {}
        exp_func() {}

        double operator() (const Eigen::Vector2d x_) const
        {
            double x{x_[0]}, y{x_[1]};
            return std::exp(- std::pow(x - x_0, 2) - std::pow(y - y_0, 2));
        }

        double d_dx(const Eigen::Vector2d x_) const
        {
            double x{x_[0]}, y{x_[1]};
            return -2 * (x - this->x_0) *  this->operator()(x_);
        }

        double d2_dx2(const Eigen::Vector2d x_) const
        {
            double x{x_[0]}, y{x_[1]};
            return (4 * (x - this->x_0) * (x - this->x_0) - 2) * this->operator()(x_);
        }
        double d_dy(const Eigen::Vector2d x_) const
        {
            double x{x_[0]}, y{x_[1]};
            return -2 * (y - this->y_0) *  this->operator()(x_);
        }

        double d2_dy2(const Eigen::Vector2d x_) const
        {
            double x{x_[0]}, y{x_[1]};
            return (4 * (y - this->y_0) * (y - this->y_0) - 2) * this->operator()(x_);
        }

        double d2_dxdy(const Eigen::Vector2d x_) const
        {
            double x{x_[0]}, y{x_[1]};
            return 4 * (x - this->x_0) * (y - this->y_0) * this->operator()(x_);
        }

        double d2_dydx(const Eigen::Vector2d x_) const
        {
            return this->d2_dxdy(x_);
        }

    private:
        double x_0{0.0}, y_0{0.0};
};

class F_func
{
    public:
        F_func(Eigen::Vector2d init) : Fx(init), Fy(init) {}

        const Eigen::Vector2d operator() (const Eigen::Vector2d x_) const
        {
            return Eigen::Vector2d(Fx.d_dx(x_), Fy.d_dy(x_));
        }

        const Eigen::Matrix2d J(const Eigen::Vector2d x_) const
        {
            Eigen::Matrix2d J;
            J << Fx.d2_dx2 (x_),   Fx.d2_dxdy(x_),
                 Fx.d2_dydx(x_),   Fx.d2_dy2 (x_);
            return J;
        }

        const Eigen::Matrix2d J_inv(const Eigen::Vector2d x_) const
        {
            return this->J(x_).inverse();
        }

        const Eigen::Matrix2d J_sec(const Eigen::Vector2d x_) const
        {
            const double h_x{x_[0] * std::sqrt(std::numeric_limits<double>::epsilon())};
            const double h_y{x_[1] * std::sqrt(std::numeric_limits<double>::epsilon())};
            const Eigen::Vector2d e_x{1.0, 0.0};
            const Eigen::Vector2d e_y{0.0, 1.0};

            Eigen::Matrix2d J_sec;
            J_sec << (this->operator()(x_ + h_x * e_x)[0] - this->operator()(x_)[0]) / h_x,
                     (this->operator()(x_ + h_y * e_y)[0] - this->operator()(x_)[0]) / h_y,
                     (this->operator()(x_ + h_x * e_x)[1] - this->operator()(x_)[1]) / h_x,
                     (this->operator()(x_ + h_y * e_y)[1] - this->operator()(x_)[1]) / h_y;
            return J_sec;
        }

        const Eigen::Matrix2d J_sec_inv(const Eigen::Vector2d x_) const
        {
            return this->J_sec(x_).inverse();
        }

    private:
        exp_func Fx, Fy;
};

inline double dist(const Eigen::Vector2d a,
                   const Eigen::Vector2d b = Eigen::Vector2d::Zero(2))
{
    return std::sqrt((a[0] - b[0]) * (a[0] - b[0]) + (a[1] - b[1]) * (a[1] - b[1]));
}

int main(int argc, char* argv[])
{
    if (argc < 5 || argc > 5 || argv[1] == "-h") // check cl args and give some help
    {
        std::cerr << "Usage: " << argv[0] << "\n\t"
                  << " x_0(double): parameter x_0" << "\n\t"
                  << " y_0(double): parameter y_0" << "\n\t"
                  << " x_init(double): initial x" << "\n\t"
                  << " y_init(double): initial y" << "\n\t"
                  << std::endl << std::endl;
        return 1;
    }
    const double x_0{atof(argv[1])};
    const double y_0{atof(argv[2])};
    const Eigen::Vector2d init(x_0, y_0);

    const double x_start{atof(argv[3])};
    const double y_start{atof(argv[4])};
    const Eigen::Vector2d start(x_start, y_start);

    exp_func f(init);
    F_func F(init);

    // Newton-Raphson method
    Eigen::Vector2d x_n;
    Eigen::Vector2d x_np1{start - F.J_inv(start) * F(start)};

    std::cout << start[0] << "\t" << start[1] << "\t" << f(start) << std::endl;

    while (dist(F(x_np1)) > std::numeric_limits<double>::epsilon())
    {
        x_n= x_np1;
        std::cout << x_n[0] << "\t" << x_n[1] << "\t" << f(x_n) << std::endl;
        x_np1= x_n - F.J_inv(x_n) * F(x_n);
    }

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

    // Secant method
    x_np1= start - F.J_sec_inv(start) * F(start);

    std::cout << start[0] << "\t" << start[1] << "\t" << f(start) << std::endl;

    while (dist(F(x_np1)) > std::numeric_limits<double>::epsilon())
    {
        x_n= x_np1;
        std::cout << x_n[0] << "\t" << x_n[1] << "\t" << f(x_n) << std::endl;
        x_np1= x_n - F.J_sec_inv(x_n) * F(x_n);
    }

    return 0;
}


//
//  Trilateration.cpp
//
//  Created by waynewang on 19/4/14.
//  Copyright (c) 2014 waynewang. All rights reserved.
//
#pragma warning(push)
#pragma warning(disable : 4244) // Disable warnings from external toolkit.
#include "Trilateration.h"
#include <Eigen/Dense>
#include <math.h>
#pragma warning(pop)

bool Trilateration::CalculateLocation2d(const PosAndDistance2dVec& beacons, Pos2d& location)
{
    // To locate position on a 2d plan, have to get at least 3 becaons,
    // otherwise return false.
    if (beacons.size() < 3)
        return false;

    // Define the matrix that we are going to use
    size_t count = beacons.size();
    size_t rows = count * (count - 1) / 2;
    Eigen::MatrixXd m(rows, 2);
    Eigen::VectorXd b(rows);

    // Fill in matrix according to the equations
    size_t row = 0;
    double x1, x2, y1, y2, r1, r2;
    PosAndDistance2d beacon1, beacon2;
    for (size_t i=0; i<count; ++i) {
        beacon1 = beacons[i];
        for (size_t j=i+1; j<count; ++j) {
            beacon2 = beacons[j];

            x1 = beacon1.m_pos(0), y1 = beacon1.m_pos(1);
            x2 = beacon2.m_pos(0), y2 = beacon2.m_pos(1);
            r1 = beacon1.m_distance;
            r2 = beacon2.m_distance;
            m(row, 0) = x1 - x2;
            m(row, 1) = y1 - y2;
            b(row) = ((pow(x1, 2)-pow(x2, 2)) +
                      (pow(y1, 2)-pow(y2, 2)) -
                      (pow(r1, 2) - pow(r2, 2))) / 2;
            row++;
        }
    }

    // Then calculate to solve the equations, using the least square solution
    location = m.jacobiSvd(Eigen::ComputeThinU|Eigen::ComputeThinV).solve(b);

    return true;
}

bool Trilateration::CalculateLocation3d(const PosAndDistance3dVec &beacons, Pos3d &location)
{
    // To locate position in a 3D space, have to get at least 4 becaons,
    // otherwise return false.
    if (beacons.size() < 3)
        return false;

    // Define the matrix that we are going to use
    size_t count = beacons.size();
    size_t rows = count * (count - 1) / 2;
    Eigen::MatrixXd m(rows, 3);
    Eigen::VectorXd b(rows);

    // Fill in matrix according to the equations
    size_t row = 0;
    double x1, x2, y1, y2, z1, z2, r1, r2;
    PosAndDistance3d beacon1, beacon2;
    for (size_t i=0; i<count; ++i) {
        beacon1 = beacons[i];
        for (size_t j=i+1; j<count; ++j) {
            beacon2 = beacons[j];

            x1 = beacon1.m_pos(0), y1 = beacon1.m_pos(1), z1 = beacon1.m_pos(2);
            x2 = beacon2.m_pos(0), y2 = beacon2.m_pos(1), z2 = beacon2.m_pos(2);
            r1 = beacon1.m_distance;
            r2 = beacon2.m_distance;
            m(row, 0) = x1 - x2;
            m(row, 1) = y1 - y2;
            m(row, 2) = z1 - z2;
            b(row) = ((pow(x1, 2)-pow(x2, 2)) +
                      (pow(y1, 2)-pow(y2, 2)) +
                      (pow(z1, 2)-pow(z2, 2)) -
                      (pow(r1, 2) - pow(r2, 2))) / 2;
            row++;
        }
    }

    // Then calculate to solve the equations, using the least square solution
    location = m.jacobiSvd(Eigen::ComputeThinU|Eigen::ComputeThinV).solve(b);

    return true;
}


/* ----------------------------------------------------------------------------
 * Copyright 2022, Jeferson Lima
 * All Rights Reserved
 * See LICENSE for the license information
 * -------------------------------------------------------------------------- */

/**
 *  @file   examples/1/hTransformation_ex.cpp
 *  @author Jeferson Lima
 *  @brief  Homogeneous Transformation Matrix Example
 *  @date   Mar 11, 2022
 **/

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

void hTrans3dVec(const Eigen::Matrix<float, 4, 1>& v,
                Eigen::Matrix<float, 4, 1>& r,
		const Eigen::Matrix<float, 3, 1>& q,
		float theta)
{

  Eigen::Matrix<float, 4, 4> htMatrix;
  htMatrix << std::cos(theta), -std::sin(theta), 0.0, q[0],
              std::sin(theta),  std::cos(theta), 0.0, q[1],
	      0.0,              0.0,             1.0, q[2],
	      0.0,              0.0,             0.0, 1.0;

  std::cout << "v values:\n" << v << std::endl;
  std::cout << "homogeneous transformation values:\n" << htMatrix << std::endl;

  r = htMatrix * v;

}

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

  Eigen::Matrix<float, 4, 1> v;
  Eigen::Matrix<float, 4, 1> r;
  Eigen::Matrix<float, 3, 1> Q;
  float theta;

  // init matrix
  v << 0.5, 0.5, 0.0, 1.0;
  Q << 1.0, 1.0, 0.0;
  theta = 00.523599; // 30 degree -> radian

  hTrans3dVec(v, r, Q, theta);

  std::cout << "r:\n" << r << std::endl;

  return 0;
}


#include <gsl/gsl_integration.h>

#include <algorithm>

#include <Eigen/Dense>

#include "GaussLegendre.hh"
#include "TypesFunctions.hh"

using namespace Eigen;

void GaussLegendre::init() {
  size_t npoints = 0;
  for (size_t n: m_orders) {
    npoints += n;
  }
  m_points.resize(npoints);
  m_weights.resize(npoints);
  //k stands for a bin number
  size_t k = 0;

  //simply allocates the weights and abscissae table of Gauss-Legendre n-point
  //integration rule
  //https://en.wikipedia.org/wiki/Gaussian_quadrature
  for (size_t i = 0; i < m_orders.size(); ++i) {
    size_t n = m_orders[i];
    auto *t = gsl_integration_glfixed_table_alloc(n);

    double a = m_edges[i];
    double b = m_edges[i+1];

    //actually provides the absc and weight. for interval [a,b] and store it
    //to m_points and m_weights,
    for (size_t j = 0; j < t->n; ++j) {
      gsl_integration_glfixed_point(a, b, j, &m_points[k], &m_weights[k], t);
      k++;
    }
    gsl_integration_glfixed_table_free(t);
  }
  //return only points, WTF?
  transformation_("points")
    .output("x")
    .output("xedges")
    .output("xhist")
    .types([](GaussLegendre *obj, TypesFunctionArgs& fargs) {
        auto& rets=fargs.rets;
        rets[0] = DataType().points().shape(obj->m_points.size());
        rets[1] = DataType().points().shape(obj->m_edges.size());
        rets[2] = DataType().hist().edges(obj->m_edges);
      })
    .func([](GaussLegendre *obj, FunctionArgs& fargs) {
        auto& rets=fargs.rets;
        rets[0].x = obj->m_points;
        rets[1].x = Eigen::Map<const Eigen::ArrayXd>(&obj->m_edges[0], obj->m_edges.size());
      })
    .finalize()
    ;
}

GaussLegendreHist::GaussLegendreHist(const GaussLegendre *base)
  : m_base(base)
{
  transformation_("hist")
    .input("f")
    .output("hist")
    .types(TypesFunctions::ifSame, [](GaussLegendreHist *obj, TypesFunctionArgs& fargs) {
        fargs.rets[0] = DataType().hist().bins(obj->m_base->m_orders.size()).edges(obj->m_base->m_edges);
      })
    .func([](GaussLegendreHist *obj, FunctionArgs& fargs) {
        auto& ret=fargs.rets[0].x;
        ArrayXd prod = fargs.args[0].x*obj->m_base->m_weights;
        auto *data = prod.data();
        const auto &orders = obj->m_base->m_orders;
        for (size_t i = 0; i < orders.size(); ++i) {
          size_t n = orders[i];
          ret(i) = std::accumulate(data, data+n, 0.0);
          data += n;
        }
      })
    ;
}


// Boost.Geometry
// QuickBook Example

// Copyright (c) 2021, Oracle and/or its affiliates
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

//[azimuth
//` Shows how to calculate azimuth

#include <iostream>

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

int main()
{
    typedef boost::geometry::model::d2::point_xy<double> point_type;

    point_type p1(0, 0);
    point_type p2(1, 1);

    auto azimuth = boost::geometry::azimuth(p1, p2);

    std::cout << "azimuth: " << azimuth << std::endl;

    return 0;
}

//]

//[azimuth_output
/*`
Output:
[pre
azimuth: 0.785398
]
*/
//]


#pragma once
#include <Eigen/Dense>

namespace Geometry {

  using Mat3 = Eigen::Matrix3f;
  using Vec3 = Eigen::Vector3f;

  namespace SO3 {
    // TODO: Exp, Log, boxplus, boxminus, interpolation

    inline
      Mat3 Exp(const Vec3& w) {
      Mat3 w_hat;
      w_hat <<
        0, -w.z(), w.y(),
        w.z(), 0, -w.x(),
        -w.y(), w.x(), 0;
      // Note: e.g. w.cross(some_other_vector) == w_hat * some_other_vector;

      /*const double phi = w.norm(); // L2 norm

      return Mat3::Identity() +
        w_hat / phi * sin(phi) +
        w_hat * w_hat / (phi * phi) * (1 - cos(phi));*/

      const double phi2 = w.squaredNorm();

      if (phi2 > std::numeric_limits<float>::epsilon()) {
        const double phi = std::sqrt(phi2);
        return Mat3::Identity() +
          w_hat / phi * sin(phi) +
          w_hat * w_hat / (phi * phi) * (1 - cos(phi));
      }
      else {
        return Mat3::Identity() + w_hat;
      }
    }

    inline
      Vec3 Log(const Mat3& R) {

      // question: how to improve? (homework) +1.0p
      const double phi = std::acos((R.trace() - 1) / 2);

      if (phi > std::numeric_limits<float>::epsilon()) {
        Vec3 w_normalized = 1 / (2 * sin(phi)) * Vec3(
          R(2, 1) - R(1, 2),
          R(0, 2) - R(2, 0),
          R(1, 0) - R(0, 1)
        );

        return phi * w_normalized;
      }
      else {
        return (1.0 / 2.0) * Vec3(
          R(2, 1) - R(1, 2),
          R(0, 2) - R(2, 0),
          R(1, 0) - R(0, 1)
        );
      }
    }

    inline
      Vec3 boxminus(const Mat3& X, const Mat3& Y) {
      return Log(X * Y.transpose());
    }

    inline
      Mat3 boxplus(const Mat3& X, const Vec3& u) {
      return Exp(u) * X;
    }

    inline
      Mat3 interpolate(const Mat3& X, const Mat3& Y, const double& t) {
      // t == 0 -> Y
      // t == 1 -> X
      // t \in (0,1) -> .....
      return boxplus(X, t * boxminus(Y, X));
    }

  }

}


/*******************************************************
 * This file is part of PISCES.
 * Author: Leo
 *******************************************************/

#include <catch2/catch.hpp>
#include <spdlog/spdlog.h>
#include <spdlog/fmt/ostr.h>  // NOTE Include this header for log Eigen matrix

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

TEST_CASE("Matrix", "[Core][.]") {
  Eigen::Matrix<float, 3, 3> default_matrix;

  SECTION("Matrix Constructor") {
    default_matrix << 1, 2, 3, 4, 5, 6, 7, 8, 9;
    SPDLOG_INFO("Default matrix of Eigen(ColumnMajor)_{}x{}:\n{}", default_matrix.rows(), default_matrix.cols(),
                default_matrix);
  }

  SECTION("Matrix SelfadjointView") {
    default_matrix = default_matrix.selfadjointView<Eigen::Upper>();
    SPDLOG_INFO("SelfadjointView matrix_{}x{}:\n{}", default_matrix.rows(), default_matrix.cols(), default_matrix);
  }
}

TEST_CASE("Matrix multi", "[Core][.]") {
  Eigen::Matrix<double, 3, 3> default_matrix;
  default_matrix << 1, 2, 3, 4, 5, 6, 7, 8, 9;
  auto x = Eigen::Matrix3d::Random();
  auto y = Eigen::Matrix3d::Random();
  auto z = x * y;
  Eigen::Matrix3d z_1 = x * y;

  SPDLOG_INFO("x:\n{}, \ny:\n{}, \nz:\n{}, \nz_1:\n{}", x, y, z, z_1);
}

TEST_CASE("Quaternion Slerp", "[Geometry][.]") {
  Eigen::Quaterniond q1 = Eigen::Quaterniond::UnitRandom();
  Eigen::Quaterniond q2;
  q2.coeffs() = -q1.coeffs();
}

TEST_CASE("SelfAdjointEigenSolver", "[Matrix][.]") {
  // 1. Create a random self-adjoint matrix
  Eigen::Matrix<double, 6, 6> random_6x6 = Eigen::Matrix<double, 6, 6>::Random();
  SPDLOG_INFO("Random_6x6:\n {}", random_6x6);
  Eigen::Matrix<double, 6, 1> eigen_vals = random_6x6.selfadjointView<Eigen::Lower>().eigenvalues();
  SPDLOG_INFO("Eigen values:\n {}", eigen_vals);
}

TEST_CASE("Alias", "[Matrix]") {
  Eigen::Matrix3d a;
  a << 1, 2, 3, 4, 5, 6, 7, 8, 9;
  SPDLOG_INFO("Here is the matrix a:\n{}", a);

  a = 0.5 * (a + a.transpose().eval());  // !!! do NOT do this !!!
  a = a.selfadjointView<Eigen::Upper>();
  SPDLOG_INFO("and the result of the aliasing effect:\n{}", a);
}

TEST_CASE("conservativeResizeLike", "[Matrix][.]") {
  Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic> a;
  Eigen::Matrix3i tmp;
  tmp << 1, 2, 3, 4, 5, 6, 7, 8, 9;
  a = tmp;
  SPDLOG_INFO("Here is the matrix a:\n{}", a);

  a.conservativeResizeLike(Eigen::MatrixXi::Zero(4, 4));

  SPDLOG_INFO("The matrix a after conservativeResizeLike:\n{}", a);
}

TEST_CASE("Givens Rotation", "[Jacobi][.]") {
  Eigen::JacobiRotation<double> jacobi_rotation;
  Eigen::Vector2d vec_2d = {1, 2};
  jacobi_rotation.makeGivens(vec_2d(0), vec_2d(1));

  SPDLOG_INFO("Givens rotation: {}, {}", jacobi_rotation.c(), jacobi_rotation.s());
  Eigen::Matrix2d givens_mat;
  givens_mat << jacobi_rotation.c(), jacobi_rotation.s(), -jacobi_rotation.s(), jacobi_rotation.c();
  SPDLOG_INFO("Givens matrix: \n{}", givens_mat);
  SPDLOG_INFO("Adjoint of Givens matrix: \n{}", givens_mat.adjoint());
  Eigen::Vector2d vec_2d_givens = givens_mat.adjoint() * vec_2d;
  SPDLOG_INFO("vec_2d_givens: {}", vec_2d_givens);
  vec_2d.applyOnTheLeft(0, 1, jacobi_rotation.adjoint());
  SPDLOG_INFO("vec_2d: {}", vec_2d);
}


// Copyright Paul A. Bristow 2014, 2015.

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

// Note that this file contains Quickbook mark-up as well as code
// and comments, don't change any of the special comment mark-ups!

// Example of finding nth root using 1st and 2nd derivatives of x^n.

#include <boost/math/tools/roots.hpp>
//using boost::math::policies::policy;
//using boost::math::tools::newton_raphson_iterate;
//using boost::math::tools::halley_iterate;
//using boost::math::tools::eps_tolerance; // Binary functor for specified number of bits.
//using boost::math::tools::bracket_and_solve_root;
//using boost::math::tools::toms748_solve;

#include <boost/math/special_functions/next.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>
#include <boost/math/special_functions/pow.hpp>
#include <boost/math/constants/constants.hpp>

#include <boost/multiprecision/cpp_dec_float.hpp> // For cpp_dec_float_50.
#include <boost/multiprecision/cpp_bin_float.hpp> // using boost::multiprecision::cpp_bin_float_50;
#ifndef _MSC_VER  // float128 is not yet supported by Microsoft compiler at 2013.
# include <boost/multiprecision/float128.hpp>
#endif

#include <iostream>
// using std::cout; using std::endl;
#include <iomanip>
// using std::setw; using std::setprecision;
#include <limits>
using std::numeric_limits;
#include <tuple>
#include <utility> // pair, make_pair

//[root_finding_nth_functor_2deriv
template <int N, class T = double>
struct nth_functor_2deriv
{ // Functor returning both 1st and 2nd derivatives.
  BOOST_STATIC_ASSERT_MSG(boost::is_integral<T>::value == false, "Only floating-point type types can be used!");
  BOOST_STATIC_ASSERT_MSG((N > 0) == true, "root N must be > 0!");

  nth_functor_2deriv(T const& to_find_root_of) : a(to_find_root_of)
  { /* Constructor stores value a to find root of, for example: */ }

  // using boost::math::tuple; // to return three values.
  std::tuple<T, T, T> operator()(T const& x)
  {
    // Return f(x), f'(x) and f''(x).
    using boost::math::pow;
    T fx = pow<N>(x) - a;                  // Difference (estimate x^n - a).
    T dx = N * pow<N - 1>(x);              // 1st derivative f'(x).
    T d2x = N * (N - 1) * pow<N - 2 >(x);  // 2nd derivative f''(x).

    return std::make_tuple(fx, dx, d2x);   // 'return' fx, dx and d2x.
  }
private:
  T a;                                     // to be 'nth_rooted'.
};

//] [/root_finding_nth_functor_2deriv]

/*
To show the progress, one might use this before the return statement above?
#ifdef BOOST_MATH_ROOT_DIAGNOSTIC
std::cout << " x = " << x << ", fx = " << fx << ", dx = " << dx << ", dx2 = " << d2x << std::endl;
#endif
*/

// If T is a floating-point type, might be quicker to compute the guess using a built-in type,
// probably quickest using double, but perhaps with float or long double, T.

// If T is a type for which frexp and ldexp are not defined,
// then it is necessary to compute the guess using a built-in type,
// probably quickest (but limited range) using double,
// but perhaps with float or long double, or a multiprecision T for the full range of T.
// typedef double guess_type; is used to specify the this.

//[root_finding_nth_function_2deriv

template <int N, class T = double>
T nth_2deriv(T x)
{ // return nth root of x using 1st and 2nd derivatives and Halley.

  using namespace std;  // Help ADL of std functions.
  using namespace boost::math::tools; // For halley_iterate.

  BOOST_STATIC_ASSERT_MSG(boost::is_integral<T>::value == false, "Only floating-point type types can be used!");
  BOOST_STATIC_ASSERT_MSG((N > 0) == true, "root N must be > 0!");
  BOOST_STATIC_ASSERT_MSG((N > 1000) == false, "root N is too big!");

  typedef double guess_type; // double may restrict (exponent) range for a multiprecision T?

  int exponent;
  frexp(static_cast<guess_type>(x), &exponent);                 // Get exponent of z (ignore mantissa).
  T guess = ldexp(static_cast<guess_type>(1.), exponent / N);   // Rough guess is to divide the exponent by n.
  T min = ldexp(static_cast<guess_type>(1.) / 2, exponent / N); // Minimum possible value is half our guess.
  T max = ldexp(static_cast<guess_type>(2.), exponent / N);     // Maximum possible value is twice our guess.

  int digits = std::numeric_limits<T>::digits * 0.4;            // Accuracy triples with each step, so stop when
                                                                // slightly more than one third of the digits are correct.
  const boost::uintmax_t maxit = 20;
  boost::uintmax_t it = maxit;
  T result = halley_iterate(nth_functor_2deriv<N, T>(x), guess, min, max, digits, it);
  return result;
}

//] [/root_finding_nth_function_2deriv]

template <int N, typename T = double>
T show_nth_root(T value)
{ // Demonstrate by printing the nth root using all possibly significant digits.
  //std::cout.precision(std::numeric_limits<T>::max_digits10);
  // or use   cout.precision(max_digits10 = 2 + std::numeric_limits<double>::digits * 3010/10000);
  // Or guaranteed significant digits:
   std::cout.precision(std::numeric_limits<T>::digits10);

  T r = nth_2deriv<N>(value);
  std::cout << "Type " << typeid(T).name() << " value = " << value << ", " << N << "th root = " << r << std::endl;
  return r;
} // print_nth_root

int main()
{
  std::cout << "nth Root finding Example." << std::endl;
  using boost::multiprecision::cpp_dec_float_50; // decimal.
  using boost::multiprecision::cpp_bin_float_50; // binary.
#ifndef _MSC_VER  // Not supported by Microsoft compiler.
  using boost::multiprecision::float128; // Requires libquadmath
#endif
  try
  { // Always use try'n'catch blocks with Boost.Math to get any error messages.

//[root_finding_n_example_1
    double r1 = nth_2deriv<5, double>(2); // Integral value converted to double.

    // double r2 = nth_2deriv<5>(2); // Only floating-point type types can be used!

//] [/root_finding_n_example_1

    //show_nth_root<5, float>(2); // Integral value converted to float.
    //show_nth_root<5, float>(2.F); // 'initializing' : conversion from 'double' to 'float', possible loss of data

//[root_finding_n_example_2

    show_nth_root<5, double>(2.);
    show_nth_root<5, long double>(2.);
#ifndef _MSC_VER  // float128 is not supported by Microsoft compiler 2013.
    show_nth_root<5, float128>(2);
#endif
    show_nth_root<5, cpp_dec_float_50>(2); // dec
    show_nth_root<5, cpp_bin_float_50>(2); // bin
//] [/root_finding_n_example_2

    // show_nth_root<1000000>(2.); // Type double value = 2, 555th root = 1.00124969405651
    // Type double value = 2, 1000th root = 1.00069338746258
    // Type double value = 2, 1000000th root = 1.00000069314783
  }
  catch (const std::exception& e)
  { // Always useful to include try & catch blocks because default policies
    // are to throw exceptions on arguments that cause errors like underflow, overflow.
    // Lacking try & catch blocks, the program will abort without a message below,
    // which may give some helpful clues as to the cause of the exception.
    std::cout <<
      "\n""Message from thrown exception was:\n   " << e.what() << std::endl;
  }
  return 0;
} // int main()

/*
//[root_finding_example_output_1
 Using MSVC 2013

nth Root finding Example.
Type double value = 2, 5th root = 1.14869835499704
Type long double value = 2, 5th root = 1.14869835499704
Type class boost::multiprecision::number<class boost::multiprecision::backends::cpp_dec_float<50,int,void>,1> value = 2,
  5th root = 1.1486983549970350067986269467779275894438508890978
Type class boost::multiprecision::number<class boost::multiprecision::backends::cpp_bin_float<50,10,void,int,0,0>,0> value = 2,
  5th root = 1.1486983549970350067986269467779275894438508890978

//] [/root_finding_example_output_1]

//[root_finding_example_output_2

 Using GCC 4.91  (includes float_128 type)

 nth Root finding Example.
Type d value = 2, 5th root = 1.14869835499704
Type e value = 2, 5th root = 1.14869835499703501
Type N5boost14multiprecision6numberINS0_8backends16float128_backendELNS0_26expression_template_optionE0EEE value = 2, 5th root = 1.148698354997035006798626946777928
Type N5boost14multiprecision6numberINS0_8backends13cpp_dec_floatILj50EivEELNS0_26expression_template_optionE1EEE value = 2, 5th root = 1.1486983549970350067986269467779275894438508890978
Type N5boost14multiprecision6numberINS0_8backends13cpp_bin_floatILj50ELNS2_15digit_base_typeE10EviLi0ELi0EEELNS0_26expression_template_optionE0EEE value = 2, 5th root = 1.1486983549970350067986269467779275894438508890978

RUN SUCCESSFUL (total time: 63ms)

//] [/root_finding_example_output_2]
*/

/*
Throw out of range using GCC release mode :-(

 */


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/

#pragma once

#include "../util/FluidEigenMappings.hpp"
#include "../../data/TensorTypes.hpp"
#include <Eigen/Core>
#include <Eigen/SVD>
#include <cassert>
#include <cmath>

namespace fluid {
namespace algorithm {

class PCA
{
public:
  using MatrixXd = Eigen::MatrixXd;
  using VectorXd = Eigen::VectorXd;

  void init(RealMatrixView in)
  {
    using namespace Eigen;
    using namespace _impl;
    MatrixXd input = asEigen<Matrix>(in);
    mMean = input.colwise().mean();
    MatrixXd         X = (input.rowwise() - mMean.transpose());
    BDCSVD<MatrixXd> svd(X.matrix(), ComputeThinV | ComputeThinU);
    mBases = svd.matrixV();
    mValues = svd.singularValues();
    mInitialized = true;
  }

  void init(RealMatrixView bases, RealVectorView values, RealVectorView mean)
  {
    mBases = _impl::asEigen<Eigen::Matrix>(bases);
    mValues = _impl::asEigen<Eigen::Matrix>(values);
    mMean = _impl::asEigen<Eigen::Matrix>(mean);
    mInitialized = true;
  }

  void processFrame(const RealVectorView in, RealVectorView out, index k) const
  {
    using namespace Eigen;
    using namespace _impl;
    if (k > mBases.cols()) return;
    VectorXd input = asEigen<Matrix>(in);
    input = input - mMean;
    VectorXd result = input.transpose() * mBases.block(0, 0, mBases.rows(), k);
    out = _impl::asFluid(result);
  }

  double process(const RealMatrixView in, RealMatrixView out, index k) const
  {
    using namespace Eigen;
    using namespace _impl;
    if (k > mBases.cols()) return 0;
    MatrixXd input = asEigen<Matrix>(in);
    MatrixXd result = (input.rowwise() - mMean.transpose()) *
                      mBases.block(0, 0, mBases.rows(), k);
    double variance = 0;
    double total = mValues.sum();
    for (index i = 0; i < k; i++) variance += mValues[i];
    out = _impl::asFluid(result);
    return variance / total;
  }

  bool  initialized() const { return mInitialized; }
  void  getBases(RealMatrixView out) const { out = _impl::asFluid(mBases); }
  void  getValues(RealVectorView out) const { out = _impl::asFluid(mValues); }
  void  getMean(RealVectorView out) const { out = _impl::asFluid(mMean); }
  index dims() const { return mBases.rows(); }
  index size() const { return mBases.cols(); }
  void  clear()
  {
    mBases.setZero();
    mMean.setZero();
    mInitialized = false;
  }

  MatrixXd mBases;
  VectorXd mValues;
  VectorXd mMean;
  bool     mInitialized{false};
};
}; // namespace algorithm
}; // namespace fluid


//####### Test module for quadrature functions ####################################

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

//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 <array>

#include "math_constants.h"
#include "quadrature.hpp"

using namespace picsar::multi_physics::math;

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

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

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

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

// ***Test quadrature algorithms in a finite interval

template<typename RealType>
constexpr void test_quadrature_finite_interval()
{
    const auto sin2 = [](RealType x)
        {return sin(x)*sin(x);};

    const auto a = static_cast<RealType>(0.0);
    const auto b = static_cast<RealType>(2.0)*pi<RealType>;

    const auto exp_res = pi<RealType>;

    const auto res_a_b = quad_a_b<RealType>(sin2, a, b);
    const auto res_a_b_s = quad_a_b_s<RealType>(sin2, a, b);
    const auto res_tanh_sinh =
        generic_quad_a_b<RealType, quadrature_algorithm::tanh_sinh>(
            sin2, a, b);
    const auto res_gauss_kronrod15 =
        generic_quad_a_b<RealType, quadrature_algorithm::gauss_kronrod15>(
            sin2, a, b);
    const auto res_gauss_kronrod31 =
        generic_quad_a_b<RealType, quadrature_algorithm::gauss_kronrod31>(
            sin2, a, b);
    const auto res_gauss_kronrod41 =
        generic_quad_a_b<RealType, quadrature_algorithm::gauss_kronrod41>(
            sin2, a, b);
    const auto res_gauss_kronrod51 =
        generic_quad_a_b<RealType, quadrature_algorithm::gauss_kronrod51>(
            sin2, a, b);
    const auto res_gauss_kronrod61 =
        generic_quad_a_b<RealType, quadrature_algorithm::gauss_kronrod61>(
            sin2, a, b);

    BOOST_CHECK_SMALL((res_a_b-exp_res)/exp_res, tolerance<RealType>());
    BOOST_CHECK_SMALL((res_a_b_s-exp_res)/exp_res, tolerance<RealType>());
    BOOST_CHECK_SMALL((res_tanh_sinh-exp_res)/exp_res, tolerance<RealType>());
    BOOST_CHECK_SMALL((res_gauss_kronrod15-exp_res)/exp_res, tolerance<RealType>());
    BOOST_CHECK_SMALL((res_gauss_kronrod31-exp_res)/exp_res, tolerance<RealType>());
    BOOST_CHECK_SMALL((res_gauss_kronrod41-exp_res)/exp_res, tolerance<RealType>());
    BOOST_CHECK_SMALL((res_gauss_kronrod51-exp_res)/exp_res, tolerance<RealType>());
    BOOST_CHECK_SMALL((res_gauss_kronrod61-exp_res)/exp_res, tolerance<RealType>());
}

BOOST_AUTO_TEST_CASE( picsar_quadrature_finite_interval )
{
    test_quadrature_finite_interval <double>();
    test_quadrature_finite_interval <float>();
}

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

// ***Test quadrature algorithms in a semi-infinite interval

template<typename RealType>
constexpr void test_quadrature_infinite_interval_1()
{
    const auto expx2 = [](RealType x)
        {return exp(-x*x);};

    const auto a = static_cast<RealType>(0.0);

    const auto exp_res = static_cast<RealType>(sqrt(pi<double>)/2.0);

    const auto res_expx2 = quad_a_inf<RealType>(expx2, a);

    BOOST_CHECK_SMALL((res_expx2-exp_res)/exp_res, tolerance<RealType>());
}

BOOST_AUTO_TEST_CASE( picsar_quadrature_infinite_interval_1 )
{
    test_quadrature_infinite_interval_1<double>();
    test_quadrature_infinite_interval_1<float>();
}

template<typename RealType>
constexpr void test_quadrature_infinite_interval_2()
{
    const auto datan = [](RealType x)
        {return 1.0/(1.0+x*x);};

    const auto a = static_cast<RealType>(0.0);

    const auto exp_res = static_cast<RealType>(0.5)*pi<RealType>;

    const auto res_datan = quad_a_inf<RealType>(datan, a);

    BOOST_CHECK_SMALL((res_datan-exp_res)/exp_res, tolerance<RealType>());
}

BOOST_AUTO_TEST_CASE( picsar_quadrature_infinite_interval_2 )
{
    test_quadrature_infinite_interval_2<double>();
    test_quadrature_infinite_interval_2<float>();
}

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


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

#include "math_unit_test.hpp"
#include <numeric>
#include <utility>
#include <random>
#include <cmath>
#include <boost/math/tools/test_value.hpp>
#include <boost/core/demangle.hpp>
#include <boost/math/tools/agm.hpp>
#ifdef BOOST_HAS_FLOAT128
#include <boost/multiprecision/float128.hpp>
using boost::multiprecision::float128;
#endif

using boost::math::tools::agm;
using std::sqrt;

template<class Real>
void test_gauss_constant()
{
    // http://oeis.org/A014549/constant
    Real G_expected = BOOST_MATH_TEST_VALUE(Real, 0.83462684167407318628142973279904680899399301349034700244982737010368199270952641186969116035127532412906785);

    Real G_computed = 1/agm(sqrt(Real(2)), Real(1));
    if(!CHECK_ULP_CLOSE(G_expected, G_computed, 2)) {
        std::cerr << "  Gauss constant not computed correctly.\n";
    }
}

template<typename Real>
void test_scaling()
{
    Real a = 2;
    Real g = 1;
    Real scale = 7;
    Real expected = agm(scale*a, scale*g);
    Real computed = scale*agm(a, g);
    if(!CHECK_ULP_CLOSE(expected, computed, 2)) {
        std::cerr << "  Scaling property agm(kx,ky) = k*agm(x, y) is violated.\n";
    }

    expected = 0;
    computed = agm(a, Real(0));
    if(!CHECK_ULP_CLOSE(expected, computed, 0)) {
        std::cerr << "  agm(a, 0) != 0.\n";
    }

    computed = agm(Real(0), Real(0));
    if(!CHECK_ULP_CLOSE(expected, computed, 0)) {
        std::cerr << "  agm(0, 0) != 0.\n";
    }

    expected = 1;
    computed = agm(Real(1), Real(1));
    if(!CHECK_ULP_CLOSE(expected, computed, 0)) {
        std::cerr << "  agm(1, 1) != 1.\n";
    }

    expected = 7;
    computed = agm(Real(7), Real(7));
    if(!CHECK_ULP_CLOSE(expected, computed, 0)) {
        std::cerr << "  agm(7, 7) != 1.\n";
    }

    // Properties I found at: https://mathworld.wolfram.com/Arithmetic-GeometricMean.html
    // agm(x,y) = agm((x+y)/2, sqrt(xy))
    expected = agm(Real(3), Real(1));
    computed = agm(Real(2), sqrt(Real(3)));
    if(!CHECK_ULP_CLOSE(expected, computed, 0)) {
        std::cerr << "  agm(x, y) != agm((x+y)/2, sqrt(xy)).\n";
    }

    //computed = agm(std::numeric_limits<Real>::infinity(), Real(7));
    //std::cout << "Computed at infinity = " << computed << "\n";

    for (Real x = 0; x < 1; x += Real(1)/128) {
        expected = agm(Real(1), sqrt(1-x*x));
        computed = agm(1+x, 1-x);
        if(!CHECK_ULP_CLOSE(expected, computed, 0)) {
            std::cerr << "  agm(1, sqrt(1-x^2) != agm(1+x,1-x).\n";
        }
    }
}

int main()
{
    test_gauss_constant<float>();
    test_gauss_constant<double>();
    test_gauss_constant<long double>();

    test_scaling<float>();
    test_scaling<double>();
    test_scaling<long double>();

    #ifdef BOOST_HAS_FLOAT128
    test_gauss_constant<float128>();
    test_scaling<float128>();
    #endif
    return boost::math::test::report_errors();
}


#pragma once

#include <skynet/config.hpp>

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

namespace skynet{ namespace statistics{

	struct pca_result{
		Eigen::MatrixXd    eigen_vectors;
		Eigen::VectorXd    eigen_values;
		Eigen::VectorXd    mean;
	};

	//mxn  matrix  warning:  m is the num of samples
	pca_result pca(const Eigen::MatrixXd &mat){
		Eigen::VectorXd mean(mat.rows());
		for (int row = 0; row < mat.rows(); ++row){
			double temp = 0;
			for (int col = 0; col < mat.cols(); ++col){
				temp += mat(row, col);
			}
			temp /= mat.cols();
			mean(row) = temp;
		}

		Eigen::MatrixXd data(mat.rows(), mat.cols());
		for (int row = 0; row < mat.rows(); ++row){
			for (int col = 0; col < mat.cols(); ++col){
				data(row, col) = mat(row, col) - mean(row);
			}
		}

		//auto  data_t = data.transpose() / sqrt(mat.rows());
		Eigen::JacobiSVD<Eigen::MatrixXd>  svd(data, Eigen::ComputeFullU | Eigen::ComputeFullV);
		pca_result pca;
		pca.eigen_values = move(svd.singularValues().cwiseAbs2());
		pca.eigen_vectors = move(svd.matrixU());
		pca.mean = mean;

		return pca;
	//	auto  sample_size = mat.row
	}

}}


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*

    This is an example illustrating the use the general purpose non-linear
    least squares optimization routines from the dlib C++ Library.

    This example program will demonstrate how these routines can be used for data fitting.
    In particular, we will generate a set of data and then use the least squares
    routines to infer the parameters of the model which generated the data.
*/

#include <dlib/optimization.h>
#include <iostream>
#include <vector>

using namespace std;
using namespace dlib;

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

typedef matrix<double,2,1> input_vector;
typedef matrix<double,3,1> parameter_vector;

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

// We will use this function to generate data.  It represents a function of 2 variables
// and 3 parameters.   The least squares procedure will be used to infer the values of
// the 3 parameters based on a set of input/output pairs.
double model (
    const input_vector& input,
    const parameter_vector& params
)
{
    const double p0 = params(0);
    const double p1 = params(1);
    const double p2 = params(2);

    const double i0 = input(0);
    const double i1 = input(1);

    const double temp = p0*i0 + p1*i1 + p2;

    return temp*temp;
}

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

// This function is the "residual" for a least squares problem.   It takes an input/output
// pair and compares it to the output of our model and returns the amount of error.  The idea
// is to find the set of parameters which makes the residual small on all the data pairs.
double residual (
    const std::pair<input_vector, double>& data,
    const parameter_vector& params
)
{
    return model(data.first, params) - data.second;
}

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

// This function is the derivative of the residual() function with respect to the parameters.
parameter_vector residual_derivative (
    const std::pair<input_vector, double>& data,
    const parameter_vector& params
)
{
    parameter_vector der;

    const double p0 = params(0);
    const double p1 = params(1);
    const double p2 = params(2);

    const double i0 = data.first(0);
    const double i1 = data.first(1);

    const double temp = p0*i0 + p1*i1 + p2;

    der(0) = i0*2*temp;
    der(1) = i1*2*temp;
    der(2) = 2*temp;

    return der;
}

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

#if defined(BUILD_MONOLITHIC)
#define main(cnt, arr)      dlib_least_squares_ex_main(cnt, arr)
#endif

int main(int argc, const char** argv)
{
    try
    {
        // randomly pick a set of parameters to use in this example
        const parameter_vector params = 10*randm(3,1);
        cout << "params: " << trans(params) << endl;

        // Now let's generate a bunch of input/output pairs according to our model.
        std::vector<std::pair<input_vector, double> > data_samples;
        input_vector input;
        for (int i = 0; i < 1000; ++i)
        {
            input = 10*randm(2,1);
            const double output = model(input, params);

            // save the pair
            data_samples.push_back(make_pair(input, output));
        }

        // Before we do anything, let's make sure that our derivative function defined above matches
        // the approximate derivative computed using central differences (via derivative()).
        // If this value is big then it means we probably typed the derivative function incorrectly.
        cout << "derivative error: " << length(residual_derivative(data_samples[0], params) -
                                               derivative(residual)(data_samples[0], params) ) << endl;

        // Now let's use the solve_least_squares_lm() routine to figure out what the
        // parameters are based on just the data_samples.
        parameter_vector x;
        x = 1;

        cout << "Use Levenberg-Marquardt" << endl;
        // Use the Levenberg-Marquardt method to determine the parameters which
        // minimize the sum of all squared residuals.
        solve_least_squares_lm(objective_delta_stop_strategy(1e-7).be_verbose(),
                               residual,
                               residual_derivative,
                               data_samples,
                               x);

        // Now x contains the solution.  If everything worked it will be equal to params.
        cout << "inferred parameters: "<< trans(x) << endl;
        cout << "solution error:      "<< length(x - params) << endl;
        cout << endl;

        x = 1;
        cout << "Use Levenberg-Marquardt, approximate derivatives" << endl;
        // If we didn't create the residual_derivative function then we could
        // have used this method which numerically approximates the derivatives for you.
        solve_least_squares_lm(objective_delta_stop_strategy(1e-7).be_verbose(),
                               residual,
                               derivative(residual),
                               data_samples,
                               x);

        // Now x contains the solution.  If everything worked it will be equal to params.
        cout << "inferred parameters: "<< trans(x) << endl;
        cout << "solution error:      "<< length(x - params) << endl;
        cout << endl;

        x = 1;
        cout << "Use Levenberg-Marquardt/quasi-newton hybrid" << endl;
        // This version of the solver uses a method which is appropriate for problems
        // where the residuals don't go to zero at the solution.  So in these cases
        // it may provide a better answer.
        solve_least_squares(objective_delta_stop_strategy(1e-7).be_verbose(),
                            residual,
                            residual_derivative,
                            data_samples,
                            x);

        // Now x contains the solution.  If everything worked it will be equal to params.
        cout << "inferred parameters: "<< trans(x) << endl;
        cout << "solution error:      "<< length(x - params) << endl;

    }
    catch (std::exception& e)
    {
        cout << e.what() << endl;
    }
}

// Example output:
/*
params: 8.40188 3.94383 7.83099

derivative error: 9.78267e-06
Use Levenberg-Marquardt
iteration: 0   objective: 2.14455e+10
iteration: 1   objective: 1.96248e+10
iteration: 2   objective: 1.39172e+10
iteration: 3   objective: 1.57036e+09
iteration: 4   objective: 2.66917e+07
iteration: 5   objective: 4741.9
iteration: 6   objective: 0.000238674
iteration: 7   objective: 7.8815e-19
iteration: 8   objective: 0
inferred parameters: 8.40188 3.94383 7.83099

solution error:      0

Use Levenberg-Marquardt, approximate derivatives
iteration: 0   objective: 2.14455e+10
iteration: 1   objective: 1.96248e+10
iteration: 2   objective: 1.39172e+10
iteration: 3   objective: 1.57036e+09
iteration: 4   objective: 2.66917e+07
iteration: 5   objective: 4741.87
iteration: 6   objective: 0.000238701
iteration: 7   objective: 1.0571e-18
iteration: 8   objective: 4.12469e-22
inferred parameters: 8.40188 3.94383 7.83099

solution error:      5.34754e-15

Use Levenberg-Marquardt/quasi-newton hybrid
iteration: 0   objective: 2.14455e+10
iteration: 1   objective: 1.96248e+10
iteration: 2   objective: 1.3917e+10
iteration: 3   objective: 1.5572e+09
iteration: 4   objective: 2.74139e+07
iteration: 5   objective: 5135.98
iteration: 6   objective: 0.000285539
iteration: 7   objective: 1.15441e-18
iteration: 8   objective: 3.38834e-23
inferred parameters: 8.40188 3.94383 7.83099

solution error:      1.77636e-15
*/


/******************************************************************************
  Prácticas de cálculo matricial de estructuras
  https://github.com/ingmec-ual/practicas-calculo-matricial-estructuras

  Copyright 2017 - Jose Luis Blanco Claraco <jlblanco@ual.es>
  University of Almeria
  Redistribution and use in source and binary forms, with or without
  modification, are permitted provided that the following conditions
  are met:
  1. Redistributions of source code must retain the above copyright
  notice, this list of conditions and the following disclaimer.
  2. Redistributions in binary form must reproduce the above copyright
  notice, this list of conditions and the following disclaimer in the
  documentation and/or other materials provided with the distribution.
  3. Neither the name of the copyright holder nor the names of its
  contributors may be used to endorse or promote products derived from
  this software without specific prior written permission.
  Complete BSD-3-clause License: https://opensource.org/licenses/BSD-3-Clause
******************************************************************************/

#include <Eigen/Dense>       // Libreria Eigen (matrices)
#include "eigen_indexing.h"  // Funcion indexing(MAT, idxs_rows, idxs_cols)
#include <iostream>          // Para std::cout

int main()
{
	Eigen::Matrix3d M = Eigen::Matrix3d::Identity();
	std::cout << "Matriz identidad I_3:" << std::endl << M << std::endl;

	// Modificamos la matriz:
	for (int i = 0; i < 3; i++) {
		for (int j = 0; j < 3; j++) {
			M(i, j) += i+j;
		}
	}

	std::cout << "Matriz M:" << std::endl << M << std::endl;
	std::cout << "Inversa de la matriz M:" << std::endl << M.inverse() << std::endl;

	return 0; // el programa finaliza sin errores
}


#ifndef BASEMODEL_HPP
#define BASEMODEL_HPP

#include <math.h> /* exp */
#include <Eigen/Dense>
#include <fstream>
#include <vector>

class BaseModel {
  public:
    BaseModel(int weights_count, bool is_classification) : weights_count(weights_count), is_classification(is_classification) {}
    virtual ~BaseModel() { delete[] weights; }

    inline double* getWeigths() const { return weights; }

    virtual void train(const Eigen::MatrixXd& train_inputs, const Eigen::MatrixXd& train_outputs, int epochs, double learning_rate) = 0;
    virtual void predict(const Eigen::MatrixXd& inputs, Eigen::MatrixXd& outputs) = 0;

    // npl is an array of number of weight by layer (for example: npl = [4, 3, 3,
    // 2], the first layer have 4 weigth, the second have 3 weight ...)
    virtual void save(const char* path = "./model.csv") const;
    virtual void load(const char* path = "./model.csv");

  protected:
    double* weights;
    int weights_count;
    bool is_classification;

    // The Sigmoid function, which describes an S shaped curve.
    // We pass the weighted sum of the inputs through this function to
    // normalise them between 0 and 1.
    inline double _sigmoid(double x) const { return 1 / (1 + exp(-x)); }

    // The derivative of the Sigmoid function.
    // This is the gradient of the Sigmoid curve.
    // It indicates how confident we are about the existing weight
    inline double _sigmoid_derivative(double x) const { return x * (1 - x); }

    void debuglog(std::string msg);
};

#endif


//------------------------------------------------------------------------------
/// \file Recursion_tests.cpp
///
/// \ref cf. https://www.bogotobogo.com/cplusplus/functors.php
//------------------------------------------------------------------------------

#include <boost/test/unit_test.hpp>
#include <algorithm> // std::for_each
#include <iostream>
#include <vector>

BOOST_AUTO_TEST_SUITE(Cpp)
BOOST_AUTO_TEST_SUITE(Functions_tests)

// https://gitlab.com/manning-fpcpp-book/code-examples/blob/master/chapter-06/fibonacci/main.cpp

// O(2^n) complexity
unsigned int fibonacci_recursion(unsigned int n)
{
  return n == 0 ? 0 :
    n == 1 ? 1 :
      fibonacci_recursion(n - 1) + fibonacci_recursion(n - 2);
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(FibonacciOrdinaryRecursion)
{
  BOOST_TEST(fibonacci_recursion(0) == 0);
  BOOST_TEST(fibonacci_recursion(1) == 1);
  BOOST_TEST(fibonacci_recursion(2) == 1);
  BOOST_TEST(fibonacci_recursion(3) == 2);
  BOOST_TEST(fibonacci_recursion(4) == 3);
  BOOST_TEST(fibonacci_recursion(5) == 5);
  BOOST_TEST(fibonacci_recursion(6) == 8);
  BOOST_TEST(fibonacci_recursion(7) == 13);
}

// Cached Fibonacci
std::vector<unsigned int> cache {0, 1};

// Implementation of the Fibonacci function which caches all previously
// calculated results.
unsigned int fibonacci_cached(unsigned int n)
{
  if (cache.size() > n)
  {
    std::cerr << "using cache " << n << std::endl;
    return cache[n];
  }
  else
  {
    const auto result = fibonacci_cached(n - 1) + fibonacci_cached(n - 2);
    cache.push_back(result);
    return result;
  }
}

// USE functors.
// cf. https://www.bogotobogo.com/cplusplus/functors.php
template <typename T>
class PrintElements
{
  public:
    void operator()(const T& element)
    {
      std::cout << element << ' ';
    }
};

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(FibonacciCachedRecursion)
{
  std::cout << "\n FibonacciCachedRecursion begins \n";

  std::for_each(cache.begin(), cache.end(), PrintElements<unsigned int>{});

  BOOST_TEST(fibonacci_cached(0) == 0);
  BOOST_TEST(fibonacci_cached(1) == 1);
  BOOST_TEST(fibonacci_cached(2) == 1);
  BOOST_TEST(fibonacci_cached(3) == 2);
  BOOST_TEST(fibonacci_cached(4) == 3);
  BOOST_TEST(fibonacci_cached(5) == 5);
  BOOST_TEST(fibonacci_cached(6) == 8);

  std::cout << "\n FibonacciCachedRecursion ends \n";
}

BOOST_AUTO_TEST_SUITE_END() // Functions_tests
BOOST_AUTO_TEST_SUITE_END() // Cpp


#pragma once

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

class KalmanFilter {

public:

    // Generate the Kalman Filter with specified matrices
    // A : System dynamics matrix
    // B : Input matrix
    // C : Output matrix
    // Q : Process noise covariance
    // R : Measurement noise covariance
    // P0 : Initial error covariance
    KalmanFilter(
    const Eigen::MatrixXd& A,
    const Eigen::MatrixXd& B,
    const Eigen::MatrixXd& C,
    const Eigen::MatrixXd& Q,
    const Eigen::MatrixXd& R,
    const Eigen::MatrixXd& P0);

    // Initialize the filter with a guess for the initial states
    void init(const Eigen::VectorXd& x0);

    // Update the a-priori estimates based on the system model and control input
    void prediction_update(const Eigen::VectorXd& u);

    // Update the a-posteriori estimates based on the measurements
    void innovation_update(const Eigen::VectorXd& y);

    // Return the current state
    Eigen::VectorXd get_state() {return x_hat_post;};

    // Return the current state error covariance
    Eigen::MatrixXd get_P() {return P_post;};

private:
    // Matrices used in predict and correction steps
    Eigen::MatrixXd A, B, C, Q, R, P_prio, P_post, K;

    // system dimensions
    // n : # state variables to be estimated
    // m : # measured variables
    // c : # control input variables
    int32_t m, n, c;

    // n-sized identity
    Eigen::MatrixXd I;

    // is the filter initialized?
    bool initialized = false;

    // Estimated states
    Eigen::VectorXd x_hat_prio, x_hat_post;

};


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

#include "math_unit_test.hpp"
#include <vector>
#include <random>
#include <utility>
#include <tuple>
#include <algorithm>
#include <boost/math/statistics/linear_regression.hpp>

using boost::math::statistics::simple_ordinary_least_squares;
using boost::math::statistics::simple_ordinary_least_squares_with_R_squared;

template<typename Real>
void test_line()
{
    std::vector<Real> x(128);
    std::vector<Real> y(128);
    Real expected_c0 = 7;
    Real expected_c1 = 12;
    for (size_t i = 0; i < x.size(); ++i) {
        x[i] = i;
        y[i] = expected_c0 + expected_c1*x[i];
    }

    std::pair<Real, Real> temp = simple_ordinary_least_squares(x, y);
    Real computed_c0 = std::get<0>(temp);
    Real computed_c1 = std::get<1>(temp);

    CHECK_ULP_CLOSE(expected_c0, computed_c0, 0);
    CHECK_ULP_CLOSE(expected_c1, computed_c1, 0);

    std::tuple<Real, Real, Real> temp_with_R = simple_ordinary_least_squares_with_R_squared(x, y);
    Real computed_c0_R = std::get<0>(temp_with_R);
    Real computed_c1_R = std::get<1>(temp_with_R);
    Real Rsquared = std::get<2>(temp_with_R);

    Real expected_Rsquared = 1;
    CHECK_ULP_CLOSE(expected_c0, computed_c0_R, 0);
    CHECK_ULP_CLOSE(expected_c1, computed_c1_R, 0);
    CHECK_ULP_CLOSE(expected_Rsquared, Rsquared, 0);

}

template<typename Z>
void test_integer_line()
{
    std::vector<Z> x(128);
    std::vector<Z> y(128);
    double expected_c0 = 7;
    double expected_c1 = 12;
    for (size_t i = 0; i < x.size(); ++i) {
        x[i] = i;
        y[i] = expected_c0 + expected_c1*x[i];
    }

    std::pair<double, double> temp = simple_ordinary_least_squares(x, y);
    double computed_c0 = std::get<0>(temp);
    double computed_c1 = std::get<1>(temp);

    CHECK_ULP_CLOSE(expected_c0, computed_c0, 0);
    CHECK_ULP_CLOSE(expected_c1, computed_c1, 0);

    std::tuple<double, double, double> temp_with_R = simple_ordinary_least_squares_with_R_squared(x, y);
    double computed_c0_R = std::get<0>(temp_with_R);
    double computed_c1_R = std::get<1>(temp_with_R);
    double Rsquared = std::get<2>(temp_with_R);

    double expected_Rsquared = 1;
    CHECK_ULP_CLOSE(expected_c0, computed_c0_R, 0);
    CHECK_ULP_CLOSE(expected_c1, computed_c1_R, 0);
    CHECK_ULP_CLOSE(expected_Rsquared, Rsquared, 0);

}

template<typename Real>
void test_constant()
{
    std::vector<Real> x(128);
    std::vector<Real> y(128);
    Real expected_c0 = 7;
    Real expected_c1 = 0;
    for (size_t i = 0; i < x.size(); ++i) {
        x[i] = i;
        y[i] = expected_c0 + expected_c1*x[i];
    }

    std::pair<Real, Real> temp = simple_ordinary_least_squares(x, y);
    Real computed_c0 = std::get<0>(temp);
    Real computed_c1 = std::get<1>(temp);

    CHECK_ULP_CLOSE(expected_c0, computed_c0, 0);
    CHECK_ULP_CLOSE(expected_c1, computed_c1, 0);

    std::tuple<Real, Real, Real> temp_with_R = simple_ordinary_least_squares_with_R_squared(x, y);
    Real computed_c0_R = std::get<0>(temp_with_R);
    Real computed_c1_R = std::get<1>(temp_with_R);
    Real Rsquared = std::get<2>(temp_with_R);

    Real expected_Rsquared = 1;
    CHECK_ULP_CLOSE(expected_c0, computed_c0_R, 0);
    CHECK_ULP_CLOSE(expected_c1, computed_c1_R, 0);
    CHECK_ULP_CLOSE(expected_Rsquared, Rsquared, 0);

}

template<typename Z>
void test_integer_constant()
{
    std::vector<Z> x(128);
    std::vector<Z> y(128);
    double expected_c0 = 7;
    double expected_c1 = 0;
    for (size_t i = 0; i < x.size(); ++i) {
        x[i] = i;
        y[i] = expected_c0 + expected_c1*x[i];
    }

    std::pair<double, double> temp = simple_ordinary_least_squares(x, y);
    double computed_c0 = std::get<0>(temp);
    double computed_c1 = std::get<1>(temp);

    CHECK_ULP_CLOSE(expected_c0, computed_c0, 0);
    CHECK_ULP_CLOSE(expected_c1, computed_c1, 0);

    std::tuple<double, double, double> temp_with_R = simple_ordinary_least_squares_with_R_squared(x, y);
    double computed_c0_R = std::get<0>(temp_with_R);
    double computed_c1_R = std::get<1>(temp_with_R);
    double Rsquared = std::get<2>(temp_with_R);

    double expected_Rsquared = 1;
    CHECK_ULP_CLOSE(expected_c0, computed_c0_R, 0);
    CHECK_ULP_CLOSE(expected_c1, computed_c1_R, 0);
    CHECK_ULP_CLOSE(expected_Rsquared, Rsquared, 0);

}

template<typename Real>
void test_permutation_invariance()
{
    std::vector<Real> x(256);
    std::vector<Real> y(256);
    std::mt19937_64 gen{123456};
    std::normal_distribution<Real> dis(0, 0.1);

    Real expected_c0 = -7.2;
    Real expected_c1 = -13.5;

    x[0] = 0;
    y[0] = expected_c0 + dis(gen);
    for(size_t i = 1; i < x.size(); ++i) {
        Real t = dis(gen);
        x[i] = x[i-1] + t*t;
        y[i] = expected_c0 + expected_c1*x[i] + dis(gen);
    }

    std::tuple<Real, Real, Real> temp = simple_ordinary_least_squares_with_R_squared(x, y);
    Real c0 = std::get<0>(temp);
    Real c1 = std::get<1>(temp);
    Real Rsquared = std::get<2>(temp);
    CHECK_MOLLIFIED_CLOSE(expected_c0, c0, 0.002);
    CHECK_MOLLIFIED_CLOSE(expected_c1, c1, 0.002);

    int j = 0;
    std::mt19937_64 gen1{12345};
    std::mt19937_64 gen2{12345};
    while(j++ < 10) {
        std::shuffle(x.begin(), x.end(), gen1);
        std::shuffle(y.begin(), y.end(), gen2);

        std::tuple<Real, Real, Real> temp_ = simple_ordinary_least_squares_with_R_squared(x, y);
        Real c0_ = std::get<0>(temp_);
        Real c1_ = std::get<1>(temp_);
        Real Rsquared_ = std::get<2>(temp_);

        CHECK_ULP_CLOSE(c0, c0_, 100);
        CHECK_ULP_CLOSE(c1, c1_, 100);
        CHECK_ULP_CLOSE(Rsquared, Rsquared_, 65);
    }
}

template<typename Real>
void test_scaling_relations()
{
    std::vector<Real> x(256);
    std::vector<Real> y(256);
    std::mt19937_64 gen{123456};
    std::normal_distribution<Real> dis(0, 0.1);

    Real expected_c0 = 3.2;
    Real expected_c1 = -13.5;

    x[0] = 0;
    y[0] = expected_c0 + dis(gen);
    for(size_t i = 1; i < x.size(); ++i) {
        Real t = dis(gen);
        x[i] = x[i-1] + t*t;
        y[i] = expected_c0 + expected_c1*x[i] + dis(gen);
    }

    std::tuple<Real, Real, Real> temp = simple_ordinary_least_squares_with_R_squared(x, y);
    Real c0 = std::get<0>(temp);
    Real c1 = std::get<1>(temp);
    Real Rsquared = std::get<2>(temp);
    CHECK_MOLLIFIED_CLOSE(expected_c0, c0, 0.005);
    CHECK_MOLLIFIED_CLOSE(expected_c1, c1, 0.005);

    // If y -> lambda y, then c0 -> lambda c0 and c1 -> lambda c1.
    Real lambda = 6;

    for (auto& s : y) {
        s *= lambda;
    }

    temp = simple_ordinary_least_squares_with_R_squared(x, y);
    Real c0_lambda = std::get<0>(temp);
    Real c1_lambda = std::get<1>(temp);
    Real Rsquared_lambda = std::get<2>(temp);

    CHECK_ULP_CLOSE(lambda*c0, c0_lambda, 30);
    CHECK_ULP_CLOSE(lambda*c1, c1_lambda, 30);
    CHECK_ULP_CLOSE(Rsquared, Rsquared_lambda, 3);

    // If x -> lambda x, then c0 -> c0 and c1 -> c1/lambda
    for (auto& s : x) {
        s *= lambda;
    }
    // Put y back into it's original state:
    for (auto& s : y) {
        s /= lambda;
    }

    temp = simple_ordinary_least_squares_with_R_squared(x, y);
    Real c0_ = std::get<0>(temp);
    Real c1_ = std::get<1>(temp);
    Real Rsquared_ = std::get<2>(temp);

    CHECK_ULP_CLOSE(c0, c0_, 70);
    CHECK_ULP_CLOSE(c1, c1_*lambda, 50);
    CHECK_ULP_CLOSE(Rsquared, Rsquared_, 50);

}

int main()
{
    test_line<float>();
    test_line<double>();
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
    test_line<long double>();
#endif
    test_integer_line<int>();
    test_integer_line<int32_t>();
    test_integer_line<int64_t>();
    test_integer_line<uint32_t>();

    test_constant<float>();
    test_constant<double>();
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
    test_constant<long double>();
#endif
    test_integer_constant<int>();
    test_integer_constant<int32_t>();
    test_integer_constant<int64_t>();
    test_integer_constant<uint32_t>();

    test_permutation_invariance<float>();
    test_permutation_invariance<double>();
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
    test_permutation_invariance<long double>();
#endif

    test_scaling_relations<float>();
    test_scaling_relations<double>();
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
    test_scaling_relations<long double>();
#endif
    return boost::math::test::report_errors();
}


#include <boost/math/distributions/binomial.hpp>
  using boost::math::binomial;
#include <boost/math/distributions/students_t.hpp>
#include <boost/math/distributions/normal.hpp> // for normal_distribution
  using boost::math::normal; // typedef provides default type is double.

#include <boost/math/distributions/hypergeometric.hpp>
using boost::math::hypergeometric;

#include <array>
#include <vector>
#include "container.h"
#include "stat.h"

  using namespace std;

double p_value_for_z_score(double z)
	{
		normal s;
		return cdf(s, -fabs(z));
	}

// note both enrichment and depletion
double binom_test(int trials,int success,double success_fraction)
{
    binomial binom(trials, success_fraction);
    double p = 1.0 - cdf(binom,success);
    if (p > 0.5) p = 1.0 - p; //depletion
    return p;
}

double hypergeometric_test(unsigned k, unsigned r, unsigned n, unsigned N)
{	// k: positive samples
	// n: total samples
	// r: total positive
	// N: total
	hypergeometric hg(r,n,N);
	double p = 1.0 - cdf(hg,k);
	return p;
}

// normal approximation is ok for n1 and n2 > 8
// see paper: Carine A. Bellera et al, Normal Approximations to the Distributions of the Wilcoxon
// Statistics: Accurate to What N? Graphical Insights, Journal of Statistics Education, Volume 18, Number 2, (2010)
// http://www.amstat.org/publications/jse/v18n2/bellera.pdf
array<double,2> Mann_Whitney_U_test(vector<double> ranks, int N)
{
	double n1 = ranks.size();
	double n2 = N - n1;
	double rank_sum = ranks[0];
	for (int i=1;i<n1;i++) rank_sum += ranks[i];
	double n1n2 = n1*n2;
	double U = n1n2+n1*(n1+1)/2-rank_sum;
	double mU = n1n2/2.0;
	double sigmaU = sqrt(n1n2*(N+1)/12.0);
	double z = (U-mU)/sigmaU;
	normal s;
	//cout << n1n2 << "\t" << n1n2*(N+1)/12.0 << "\t" << sigmaU << endl;
	double p = cdf(s, -fabs(z));
	array<double,2> res = {{z,p}};
	return res;
}

array<double,2> two_samples_t_test_equal_sd(double Sm1,double Sd1, unsigned Sn1, double Sm2, double Sd2,unsigned Sn2)
{
   //
   // Sm1 = Sample Mean 1.
   // Sd1 = Sample Standard Deviation 1.
   // Sn1 = Sample Size 1.
   // Sm2 = Sample Mean 2.
   // Sd2 = Sample Standard Deviation 2.
   // Sn2 = Sample Size 2.
   //
   // A Students t test applied to two sets of data.
   // We are testing the null hypothesis that the two
   // samples have the same mean and that any difference
   // if due to chance.
   // See http://www.itl.nist.gov/div898/handbook/eda/section3/eda353.htm
   //
   using namespace std;
   using namespace boost::math;

   // Now we can calculate and output some stats:
   //
   // Degrees of freedom:
   double v = Sn1 + Sn2 - 2;
   // Pooled variance:
   double sp = sqrt(((Sn1-1) * Sd1 * Sd1 + (Sn2-1) * Sd2 * Sd2) / v);
   // t-statistic:
   double t_stat = (Sm1 - Sm2) / (sp * sqrt(1.0 / Sn1 + 1.0 / Sn2));
   //
   // Define our distribution, and get the probability:
   //
   students_t dist(v);
   double q = cdf(complement(dist, fabs(t_stat)));

	array<double,2> res = {{t_stat,q}};

	return res;
}

array<double,2> two_samples_t_test_unequal_sd(double Sm1,double Sd1, unsigned Sn1,double Sm2, double Sd2,unsigned Sn2)
{
   //
   // Sm1 = Sample Mean 1.
   // Sd1 = Sample Standard Deviation 1.
   // Sn1 = Sample Size 1.
   // Sm2 = Sample Mean 2.
   // Sd2 = Sample Standard Deviation 2.
   // Sn2 = Sample Size 2.
   // alpha = Significance Level.
   //
   // A Students t test applied to two sets of data.
   // We are testing the null hypothesis that the two
   // samples have the same mean and that any difference
   // if due to chance.
   // See http://www.itl.nist.gov/div898/handbook/eda/section3/eda353.htm
   //
   using namespace std;
   using namespace boost::math;

   //
   // Degrees of freedom:
   double v = Sd1 * Sd1 / Sn1 + Sd2 * Sd2 / Sn2;
   v *= v;
   double t1 = Sd1 * Sd1 / Sn1;
   t1 *= t1;
   t1 /=  (Sn1 - 1);
   double t2 = Sd2 * Sd2 / Sn2;
   t2 *= t2;
   t2 /= (Sn2 - 1);
   v /= (t1 + t2);
   // t-statistic:
   double t_stat = (Sm1 - Sm2) / sqrt(Sd1 * Sd1 / Sn1 + Sd2 * Sd2 / Sn2);
   //
   // Define our distribution, and get the probability:
   //
   students_t dist(v);
   double q = cdf(complement(dist, fabs(t_stat)));

	array<double,2> res = {{t_stat,q}};
	return res;
}

// return 6 double
// t.statistics, p-value, Sm1, Sd1,  Sm2, Sd2
array<double,6> t_test(vector<double> x, vector<double> y, bool equal_var)
{
	unsigned Sn1 = x.size();
	double Sm1 = mean(x);
	double Sd1 = sqrt(var(x));

    unsigned Sn2 = y.size();
    double Sm2 = mean(y);
    double Sd2 = sqrt(var(y));

	//debug
	//cout << Sn1 << "," << Sm1 << "," << Sd1 << endl;
	//cout << Sn2 << "," << Sm2 << "," << Sd2 << endl;

	array<double,6> res = {{-1,-1,Sm1, Sd1,  Sm2, Sd2}};

	array<double,2> ttest;
	if (equal_var) ttest = two_samples_t_test_equal_sd(Sm1,Sd1,Sn1,Sm2,Sd2,Sn2);
	else ttest = two_samples_t_test_unequal_sd(Sm1,Sd1,Sn1,Sm2,Sd2,Sn2);

	res[0] = ttest[0];
	res[1] = ttest[1];

	return res;
}


#include <cmath>
//#include <tuple>
//#include <armadillo>
#include <gsl/gsl_sf_bessel.h>  // Bessel functions
#include <gsl/gsl_sf_lambert.h> // Lambert W functions
#include "nufftw.h"

namespace nufftw
{
    typedef arma::Mat<double>   Mat;
    typedef arma::Mat<complex> CMat;
    typedef arma::Col<double>   Vec;
    typedef arma::Col<complex> CVec;

    /** Evaluate Chebyshev polynomials of degree 0,...,n at x. */
    Mat chebT(int n, const Vec& x)
    {
        int N = x.n_rows;
        Mat T = arma::zeros(N,n+1);
        T.col(0).ones();
        if (n>0) {
            T.col(1) = x;
            for (int k=1; k<n; ++k) {
                T.col(k+1) = 2*x%T.col(k)-T.col(k-1);
            }
        }
        return T;
    }

    /** The bivarate Chebyshev coefficients for the function
     *  f(x,y) = exp(-i*x.*y) on the domain [-gam, gam]x[0,2*pi], as given by
     *  Lemma A.2 of Townsend's DPhil thesis. */
    void bessel_coeffs(int K, double gamma, complex* cfs)
    {
        const complex I(0,1);
        //CMat cfs(K,K,arma::fill::zeros);
        double arg = -0.5*gamma*M_PI;
        for (int p=0; p<K; ++p) {
            for (int q=p%2; q<K; q+=2) {
                cfs(p,q) = 4. * std::pow(I,q) * gsl_sf_bessel_Jn((p+q)*0.5, arg) * gsl_sf_bessel_Jn((q-p)*0.5, arg);
            }
        }
        cfs.row(0)*=0.5;
        cfs.col(0)*=0.5;
        return cfs;
    }

    // arg = -gam*pi/2;
    // [pp, qq] = meshgrid(0:K-1);
    // cfs = 4*(1i).^qq.*besselj((pp+qq)/2,arg).*besselj((qq-pp)/2, arg);
    // cfs(2:2:end,1:2:end) = 0;
    // cfs(1:2:end,2:2:end) = 0;
    // cfs(1,:) = cfs(1,:)/2;
    // cfs(:,1) = cfs(:,1)/2;

    // std::tuple<Vec, Vec, double, int> parameters(const Vec& x, double tol)
    // {
    //     int N = x.n_rows;
    //     Vec s = arma::round(N*x);
    //     Vec t = s - arma::floor(s/N)*N;
    //     double gamma = arma::norm(N*x - s, "inf");
    //     double xi = std::log(std::log(10/tol)/(7*gamma));
    //     double lw = gsl_sf_lambert_W0(xi);
    //     int K = std::ceil(5*gamma*std::exp(lw));
    //     // double log_xi = std::log(xi);
    //     // double inv_xi = 1.0/xi;
    //     // double lw = xi - log_xi*(1.0 + inv_xi*(1.0 + inv_xi*(0.5*log_xi - 1.0)));
    //     return {s, t, gamma, K};
    // }

    Vec closestGridpoint(const Vec& x)
    {
        int N = x.n_rows;
        return arma::round(N*x);
    }

    Vec closestFFTSample(const Vec& s)
    {
        int N = s.n_rows;
        return s - arma::floor(s/N)*N;
    }

    double perturbationParameter(const Vec& x, const Vec& s)
    {
        int N = x.n_rows;
        return arma::norm(N*x-s, "inf");
    }

    int rankParameter(double gamma, double tol)
    {
        double xi = std::log(std::log(10./tol)/(7.*gamma));
        double lw = gsl_sf_lambert_W0(xi);
        return std::ceil(5.*gamma*std::exp(lw));
    }

    /** Construct a low rank approximation to A_{jk} = exp(-2*pi*1i*(x_j-s_j/N)*k),
     *  0<=j,k<=N-1, where |x_j-j/N|<=gamma<=1/2. */
    // std::tuple<Mat<complex>, Mat<complex>> constructUV(const Vec& x, const Vec& omega, int K, double tol)
    // {
    //     int N = omega.n_rows;
    //     auto [s, t, gamma, K] = parameters(x, tol);
    //     Vec er = N*x - s;
    //     CVec scl = arma::exp(-I*M_PI*er);
    //     CMat u = (chebT(K-1, er/gamma) * besselCoeffs(K, gamma)).each_col() % scl;
    //     CMat v = chebT(K-1, 2*omega/N-1);
    // }

    CMat constructU(const Vec& x, const Vec& s, int gamma,  int K)
    {
        const complex I(0,1);
        int N = x.n_rows;
        Vec er = N*x - s;
        CVec scl = arma::exp(-I*M_PI*er);
        CMat u = (chebT(K-1, er/gamma) * besselCoeffs(K, gamma)).each_col() % scl;
        return u;
    }

    CMat constructV(int N, int K)
    {
        CMat v = chebT(K-1, 2*arma::linspace(0,N-1)/N-1);
        return v;
    }

    CMat constructV(const Vec& omega, int K)
    {
        int N = omega.n_rows;
        CMat v = chebT(K-1, 2*omega/N-1);
        return v;
    }

    plan_nufft2_1d()
    nufftw::execute();
    nufftw::destroy_plan()

    nufftw::plan_nudft2_1d

    nufftw_plan nufft2(int N, const complex* c, const double* x, complex* f, double tol)
    {
        // Compute parameters
        Vec s = closestGridpoint(x);
        Vec t = closestFFTSample(s);
        double gamma = perturbationParameter(x, s);
        int K = rankParameter(gamma, tol);

        CMat u = constructU(x, s, gamma, K);
        CMat v = constructV(N, K);

        // nufftw_plan p = plan_nufft2(N, x);
        nufftw_plan p = [&]() {

        };
        p(c,f);
        return p;
    }
}


// intersections.cpp
//
// Copyright (c) 2018
// Justinas V. Daugmaudis
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

//[intersections
/*`
    For the source of this example see
    [@boost://libs/random/example/intersections.cpp intersections.cpp].

    This example demonstrates generating quasi-randomly distributed chord
    entry and exit points on an S[sup 2] sphere.

    First we include the headers we need for __niederreiter_base2
    and __uniform_01 distribution.
 */

#include <boost/random/niederreiter_base2.hpp>
#include <boost/random/uniform_01.hpp>

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

#include <boost/tuple/tuple.hpp>

/*`
  We use 4-dimensional __niederreiter_base2 as a source of randomness.
 */
boost::random::niederreiter_base2 gen(4);

int main()
{
  typedef boost::tuple<double, double, double> point_t;

  const std::size_t n_points = 100; // we will generate 100 points

  std::vector<point_t> points;
  points.reserve(n_points);

  /*<< __niederreiter_base2 produces integers in the range [0, 2[sup 64]-1].
  However, we want numbers in the range [0, 1). The distribution
  __uniform_01 performs this transformation.
  >>*/
  boost::random::uniform_01<double> dist;

  for (std::size_t i = 0; i != n_points; ++i)
  {
    /*`
      Using formula from J. Rovira et al., "Point sampling with uniformly distributed lines", 2005
      to compute uniformly distributed chord entry and exit points on the surface of a sphere.
    */
    double cos_theta = 1 - 2 * dist(gen);
    double sin_theta = std::sqrt(1 - cos_theta * cos_theta);
    double phi = boost::math::constants::two_pi<double>() * dist(gen);
    double sin_phi = std::sin(phi), cos_phi = std::cos(phi);

    point_t point_on_sphere(sin_theta*sin_phi, cos_theta, sin_theta*cos_phi);

    /*`
      Here we assume that our sphere is a unit sphere at origin. If your sphere was
      different then now would be the time to scale and translate the `point_on_sphere`.
    */

    points.push_back(point_on_sphere);
  }

  /*`
    Vector `points` now holds generated 3D points on a sphere.
  */

  return 0;
}

//]


/*
 * oper_matrix.cpp
 *
 *  Created on: 2017. 6. 6.
 *      Author: cho
 */

#include <random>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/io.hpp>

using namespace std;
namespace ublas = boost::numeric::ublas;

/// number of data
const unsigned int N = 15;
const int order = 3; // 3 means second order polynomial

// return polynomial value
// ex, return y = a0 + a1*x + a2*x^2
double polyval( const ublas::vector<double>& a, const double x ) {
	ublas::vector<double> tmp(a);

	for ( unsigned i = 1; i < a.size(); ++i ) {
		for ( unsigned j = i; j < a.size(); ++j )
			tmp(j) *= x;
	}

	return ublas::sum( tmp );
}

int main() {
	cout << "==== START " << endl;
	random_device rd;     // seed, rd()
	mt19937_64 gen(rd());    // random number generator

	ublas::vector<double> t (N); // time vector, sec
	for (unsigned i = 0; i < t.size(); ++i) t(i) = (i+1);
	cout << "t("<<N<<") = " << t << endl;

	uniform_int_distribution<int> dis_y( -10, 10 );
	ublas::vector<double> y (N); // measured value
	for (unsigned i = 0; i < y.size(); ++i) y(i) = dis_y(gen);
	cout << "y("<<N<<") = " << y << endl;

	cout << "==== compute polynomial fitting from data t(time) and y(value)" << endl;
	cout << "==== find coefficients of 2nd oder polynomial, y = a0 + a1*x + a2*x^2" << endl;

	cout << "-- make Vandermonde matrix X " << endl;
	ublas::matrix<double> X(N, order);                 // X = Vandermonde matrix
	ublas::column(X, 0) = ublas::vector<double>(N, 1); // 0 column = t^0, all one
	ublas::column(X, 1) = t;                           // 1 column = t^1, t itself
//	ublas::column(X, 2) = ublas::element_prod(t, t);   // 2 column = t^2
	for ( unsigned j = 2; j<order; ++ j ) {
		ublas::column(X, j) = element_prod(t, ublas::column(X, j-1));
	}
	cout << "X         = " << X << endl;
	cout << "trans(X)  = " << trans(X) << endl;

	ublas::matrix<double> M = prod( trans(X), X );
	cout << "M = trans(X) x X = " << M << endl;

	ublas::vector<double> Y = prod( trans(X), y );
	cout << "Y = trans(X) x y = " << Y << endl;

	cout << endl;
	cout << "==== solve the equation : M x a = Y by LU factorization" << endl;
	cout << "====      LU=PM and PM x a = PY" << endl;
	cout << "====      so,       LU x a = PY" << endl;
	ublas::permutation_matrix<double> pm(M.size1());
	ublas::vector<double> a( Y );                           //  a <- Y
	ublas::matrix<double> LU( M );                          // LU <- M
	bool singular = ublas::lu_factorize( LU, pm);           // LU <- LU
	if ( singular ) {
		cout << "ERROR!!! " << endl;
	} else {
		cout << "LU = " << LU << endl;
		cout << "Y  = " << Y << endl;
		cout << "pm = " << pm << endl;
		ublas::swap_rows( pm, a);                              // a <- PY
		ublas::inplace_solve( LU, a, ublas::unit_lower_tag()); // a <- z : Lz = PY
		ublas::inplace_solve( LU, a, ublas::upper_tag());      // a <- x : Ux = z
		cout << "polynomial coefficient = " << a << endl;
		cout << "prod( M, a ) = " << prod( M, a ) << endl;
	}

	cout << endl;
	cout << "==== y = "<< a(0) << " + (" << a(1) << ")*x + ("<< a(2) << ")*x^2" << endl;
	cout << "==== y(1) = " << a(0) + a(1) + a(2);
	cout << ",    y(2) = " << a(0) + a(1)*2 + a(2)*2*2;
	cout << ",    y(3) = " << a(0) + a(1)*3 + a(2)*3*3 << endl;

	cout << "==== run 10 times" << endl;
	for ( int cnt = 0; cnt < 10; ++ cnt  ) {
		cout << endl;
		cout << "[" << cnt << "] t = " << t << endl;

		for (unsigned i = 0; i < y.size(); ++i) y(i) = dis_y(gen);
		cout << "[" << cnt << "] y = " << y << endl;

		ublas::vector<double> a( prod( trans(X), y ) );  // a <- trans(X) x y
		ublas::swap_rows( pm, a); // a <- PY
		ublas::inplace_solve( LU, a, ublas::unit_lower_tag()); // a <- z : Lz = PY
		ublas::inplace_solve( LU, a, ublas::upper_tag());      // a <- x : Ux = z
		cout << "polynomial coefficient = " << a << endl;
		cout << "[" << cnt << "] ";
		for ( double x : t ) {
			cout << "y(" << x << ") = " << polyval(a, x) << ", ";
		}
		cout << endl;
	}

	// generate data for plot
	cout << "============== plot data ===========" << endl;
	cout << "t, y, polyval_y" << endl;
	for ( unsigned i = 0; i < 20; ++i ) {
		y(0) = dis_y(gen);
		for (unsigned i = 1; i < y.size(); ++i) y(i) = y(i-1) + dis_y(gen);
		ublas::vector<double> a( prod( trans(X), y ) );
		ublas::lu_substitute( LU, pm, a );
		for ( unsigned j = 0; j < N; ++ j ) {
			cout << t(j) << "," << y(j) << "," << polyval(a, t(j)) << endl;
		}
	}

	cout << "==== END " << endl;
}