#ifndef CANNON_MATH_INTERP_H
#define CANNON_MATH_INTERP_H

/*!
 * \file cannon/math/interp.hpp
 * \brief File containing utilities for doing function interpolation.
 */

#include <vector>
#include <functional>
#include <memory>

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

using namespace Eigen;

namespace cannon {
  namespace math {

    /*!
     * \brief Class representing the cubic spline interpolation of a set of
     * points. Following https://timodenk.com/blog/cubic-spline-interpolation/.
     */
    class CubicSpline {
      public:

        enum class BoundaryType {
          FirstDeriv,
          SecondDeriv,
          NotAKnot
        };

        CubicSpline() = delete;

        /*!
         * \brief Constructor taking data to interpolate, boundary type for the
         * spline, and values of boundary conditions. Data should be sorted in
         * increasing order of xs.
         */
        CubicSpline(const std::vector<double>& xs, const std::vector<double>& ys,
                    BoundaryType bd = BoundaryType::SecondDeriv,
                    double bd_first = 0.0, double bd_last = 0.0);
        /*!
         * \brief Constructor taking data to interpolate, boundary type for the
         * spline, and values of boundary conditions. Data should be sorted in
         * increasing order of xs.
         */
        CubicSpline(const std::vector<Vector2d>& pts,
                    BoundaryType bd = BoundaryType::SecondDeriv,
                    double bd_first = 0.0, double bd_last = 0.0);

        /*!
         * \brief Functional operator.
         */
        double operator()(double x) const;

        /*!
         * \brief Get the derivative of this spline of input order at the input
         * point.
         *
         * \param x The point to evaluate curve derivative at.
         * \param order Derivative order to evaluate.
         *
         * \returns The derivative.
         */
        double deriv(double x, unsigned int order = 1) const;

        /*!
         * \brief Get the number of polynomials defining this spline.
         *
         * \returns Number of polynomials.
         */
        inline unsigned int num_polys() const;

      private:
        /*!
         * \brief Compute coefficients for spline from data points and boundary
         * conditions.
         */
        void compute_coeffs_();

        /*!
         * \brief Compute A matrix for system of equations defining cubic
         * spline.
         *
         * \returns A matrix which will be solved to compute spline
         * coefficients.
         */
        SparseMatrix<double> construct_A_mat_() const;

        /*!
         * \brief Compute b vector for system of equations defining cubic
         * spline.
         *
         * \returns b vector which will be used to solve system of equations
         * for spline coefficients.
         */
        VectorXd construct_b_vec_() const;

        /*!
         * \brief Find the closest idx such that xs_[idx] <= x
         *
         * \param x Input value to locate
         *
         * \returns Index of closest x datum
         */
        unsigned int find_closest_x_(double x) const;

        std::vector<double> xs_; //!< X-values of data
        std::vector<double> ys_; //!< Y-values of data

        const BoundaryType bd_; //!< Type of boundary condition for this spline
        const double bd_first_; //!< First boundary condition value
        const double bd_last_; //!< Last boundary condition value

        std::vector<double> a_coeffs_; //!< Coefficients of x^3 for each polynomial
        std::vector<double> b_coeffs_; //!< Coefficients of x^2 for each polynomial
        std::vector<double> c_coeffs_; //!< Coefficients of x for each polynomial
        std::vector<double> d_coeffs_; //!< Constant coefficients for each polynomial

    };

    /*!
     * \brief Class representing a multivariate spline, defined by its
     * component coordinate splines.
     */
    class MultiSpline {
      public:

        MultiSpline() = delete;

        /*!
         * \brief Constructor taking trajectory times and coordinate points, as
         * well as boundary conditions that will be set uniformly for each
         * spline.
         */
        MultiSpline(
            const std::vector<double> &times, const std::vector<VectorXd> &coords,
            CubicSpline::BoundaryType bd = CubicSpline::BoundaryType::SecondDeriv,
            double bd_first = 0.0, double bd_last = 0.0);

        /*!
         * \brief Functor operator taking a time and returning the coordinates
         * of the multispline at that time.
         *
         * \param t Time to evaluate spline at.
         *
         * \returns Value of spline at that time.
         */
        VectorXd operator()(double t) const;

        /*!
         * \brief Compute derivative with respect to time parameterization of
         * this cubic spline.
         *
         * \param t The time to get derivative at.
         * \param order The order of derivative to compute.
         *
         * \returns The derivative.
         */
        VectorXd deriv(double t, unsigned int order = 1) const;

        /*!
         * \brief Compute a normal vector for this spline at the input time.
         *
         * \param t The time to compute a normal for.
         *
         * \returns The computed normal.
         */
        VectorXd normal(double t) const;

      private:
        std::vector<std::shared_ptr<CubicSpline>> coord_splines_; //!< Coordinate spline functions
        unsigned int dim_; //!< Dimension of spline
    };

    /*!
     * \brief Compute the Chebyshev points between the input bounds. See
     * https://en.wikipedia.org/wiki/Chebyshev_nodes
     *
     * \param num Number of points to generate
     * \param low Lower limit for points
     * \param high Upper limit for points
     *
     * \returns A vector containing the Chebyshev points.
     */
    std::vector<double> cheb_points(unsigned int num, double low=-1.0, double high=1.0);

    /*!
     * \brief Compute the Lagrange interpolant for the input function on the
     * input points.
     *
     * \param f The function to interpolate.
     * \param pts The points to interpolate on.
     *
     * \returns The Lagrange interpolating polynomial on the input points.
     */
    std::function<double(double)>
    lagrange_interp(std::function<double(double)> f, const std::vector<double>& pts);

    /*!
     * \brief Interpolate between the two input vectors.
     *
     * \param a First vector
     * \param b Second vector
     * \param t Interpolation amount. Should be between 0 and 1
     *
     * \returns Interpolated vector.
     */
    VectorXd lerp(const Ref<const VectorXd> &a, const Ref<const VectorXd> &b,
                  double t);

    /*!
     * \brief Compute the projection of the first vector onto the second.
     *
     * \param a The vector to be projected
     * \param b The vector that projection is computed with respect to.
     *
     * \returns The projected vector.
     */
    VectorXd projection(const Ref<const VectorXd> &a,
                        const Ref<const VectorXd> &b);

    /*!
     * \brief Compute the complementary projection of the first vector with
     * respect to the second.
     *
     * \param a The vector to be projected
     * \param b The vector that projection is computed with respect to.
     *
     * \returns The complementary projection.
     */
    VectorXd complementary_projection(const Ref<const VectorXd> &a,
                                      const Ref<const VectorXd> &b);
  }
}

#endif /* ifndef CANNON_MATH_INTERP_H */


// Boost.Geometry
// QuickBook Example

// Copyright (c) 2018, 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)

//[densify_strategy
//` Shows how to densify a linestring in geographic coordinate system

#include <iostream>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/linestring.hpp>

int main()
{
    namespace bg = boost::geometry;
    typedef bg::model::point<double, 2, bg::cs::geographic<bg::degree> > point_type;
    typedef bg::model::linestring<point_type> linestring_type;

    linestring_type ls;
    bg::read_wkt("LINESTRING(0 0,1 1)", ls);

    linestring_type res;

    bg::srs::spheroid<double> spheroid(6378137.0, 6356752.3142451793);
    bg::strategy::densify::geographic<> strategy(spheroid);

    boost::geometry::densify(ls, res, 50000.0, strategy);

    std::cout << "densified: " << boost::geometry::wkt(res) << std::endl;

    return 0;
}

//]

//[densify_strategy_output
/*`
Output:
[pre
densified: LINESTRING(0 0,0.249972 0.250011,0.499954 0.500017,0.749954 0.750013,1 1)
]
*/
//]


#include "ReferenceModel.hpp"
#include <Eigen/Dense>
#include <stdexcept>
#include <ros/ros.h>
#include <ros/console.h>

using namespace Eigen;
using namespace std;

ReferenceModel::ReferenceModel(void)
{
    this->dt = 0.0;
    this->nStates = 0;
    this->nInputs = 0;
    this->nOutputs = 0;
}
ReferenceModel::~ReferenceModel(void)
{

}

void ReferenceModel::initialize(const ReferenceModel_Config_t _config)
{
    // Safety checks
    if (_config.A.cols() != _config.A.rows())
        throw invalid_argument("A is not a squre matrix!");
    if (_config.A.rows() != _config.B.rows())
        throw invalid_argument("A and B have different # of rows!");

    this->dt = _config.dt;
    this->Aref = _config.A;
    this->Bref = _config.B;
    this->nStates = this->Aref.cols();
    this->nInputs = this->Bref.cols();
    this->nOutputs = this->nStates; // full states output
    this->Cref = MatrixXd::Identity(this->nOutputs, this->nOutputs);

    // Safety checks
    if(_config.states0.size() != this->nStates)
        throw invalid_argument("Unexpected initial states size!");

    this->states0 = VectorXd::Zero(this->nStates);
    for(int i=0; i<(this->nStates); i++) {
        //ROS_INFO("initialize: %.4f\n", _config.states0(i));
        this->states0(i) = _config.states0(i);
    }
    reset();
}

void ReferenceModel::reset()
{
    this->states = VectorXd::Zero(this->nStates);
    for(int i=0; i<(this->nStates); i++) {

        this->states(i) = this->states0(i);
        //ROS_INFO("Reset: %.4f, ", this->states(i));
    }
    //ROS_INFO("\n");
}

void ReferenceModel::update(const VectorXd _inputs)
{
    // Safety check
    if(_inputs.size() != this->nInputs)
        throw invalid_argument("Unexpected inputs size!");
    //ROS_INFO("update: %.4f", _inputs(0));

    // Euler integration
    VectorXd states_dot = this->Aref * this->states + this->Bref * _inputs;
    this->states += states_dot * this->dt;
}

void ReferenceModel::get_outputs(VectorXd &_outputs)
{
    _outputs = this->Cref * this->states;
}

void ReferenceModel::get_states(VectorXd &_states)
{
    _states = VectorXd::Zero(this->nStates);
    for (int i=0; i<(this->nStates); i++) {
        _states(i) = this->states(i);
    }
    //ROS_INFO("get_states A: %.4f, %.4f", _states(0), _states(1));
    //ROS_INFO("get_states B: %.4f, %.4f", this->states(0), this->states(1));
}


/**
 * @author Eric Cousineau <eacousineau@gmail.com>, member of Dr. Aaron
 * Ames's AMBER Lab
 */
#ifndef EIGEN_UTILITIES_INTERP_UTILITIES_H
    #define EIGEN_UTILITIES_INTERP_UTILITIES_H

#include <Eigen/Dense>

#include <vector>
#include <eigen_utilities/assert_size.hpp>

namespace eigen_utilities
{

template<typename T, typename Container>
T find_lower_index(const Container &data, const T &value, bool do_saturate = false)
{
    // Assume sorted
    if (data.size() == 0)
        return -1;
    else
    {
        int index = 0;
        int last = data.size() - 1;
        if (do_saturate)
        {
            const T &min = data[0];
            const T &max = data[last];
            if (value < min)
                return -2;
            if (value > max)
                return -3; // Return this as upper
        }
        while (index + 1 < data.size())
        {
            const T &lower = data[index];
            const T &upper = data[index + 1];
            if (value >= lower && value <= upper)
                return index;
            else
                index += 1;
        }
        return -1;
    }
}

template<typename ZType>
class BilinearInterp
{
public:
    Eigen::VectorXd xs;
    Eigen::VectorXd ys;
    std::vector<std::vector<ZType> > zs;
private:
    ZType q11;
    ZType q21;
    ZType q12;
    ZType q22;
    ZType r1;
    ZType r2;
    ZType z;
public:
    void check()
    {
        common_assert(xs.size() > 0);
        common_assert(ys.size() > 0);
        common_assert_msg(zs.size() == xs.size(), "Bad tensor x-size: " << zs.size() << " != " << xs.size());
        for (int i = 0; i < zs.size(); ++i)
            common_assert_msg(zs[i].size() == ys.size(), "Bad tensor y-size at [" << i << "]: " << zs[i].size() << " != " << ys.size());
    }

    inline const ZType& eval(const Eigen::VectorXd &v, bool do_saturate = false)
    {
        assert_size_vector(v, 2);
        return eval(v(0), v(1), do_saturate);
    }

    inline const ZType& eval(double x, double y, bool do_saturate = false)
    {
//        common_assert(xs.size() > 0);
//        common_assert(ys.size() > 0);
//        assert_size_matrix(zs, xs.size(), ys.size());
        // http://en.wikipedia.org/wiki/Bilinear_interpolation
        if (do_saturate)
        {
            double xmin = xs(0);
            double xmax = xs(xs.size() - 1);
            x = std::min(std::max(x, xmin), xmax);
            double ymin = ys(0);
            double ymax = ys(ys.size() - 1);
            y = std::min(std::max(y, ymin), ymax);
        }
        int x_index = find_lower_index(xs, x);
        int y_index = find_lower_index(ys, y);
        if (x_index == -1 || y_index == -1)
            throw std::runtime_error("Bilinear interpolation values out of range");
        double xl = xs(x_index);
        double xu = xs(x_index + 1);
        double yl = ys(y_index);
        double yu = ys(y_index + 1);
        q11 = zs[x_index][y_index];
        q21 = zs[x_index + 1][y_index];
        q12 = zs[x_index][y_index + 1];
        q22 = zs[x_index + 1][y_index + 1];
        r1 = (xu - x) / (xu - xl) * q11 + (x - xl) / (xu - xl) * q21;
        r2 = (xu - x) / (xu - xl) * q12 + (x - xl) / (xu - xl) * q22;
        z = (yu - y) / (yu - yl) * r1 + (y - yl) / (yu - yl) * r2;
        return z;
    }
};

//class MatrixBilinearInterp
//{
//public:
//    Eigen::VectorXd xs;
//    Eigen::VectorXd ys;
//    std::vector<std::vector<Eigen::MatrixXd> > zs;
//private:
//    Eigen::MatrixXd Z;
//    std::vector<std::vector<BilinearInterp> > interps;
//public:
//    void checkAndInit()
//    {
//        common_assert(xs.size() > 0);
//        common_assert(ys.size() > 0);
//        common_assert_msg(zs.size() == xs.size(), "Bad tensor x-size: " << zs.size() << " != " << xs.size());
//        int rows = -1;
//        int cols = -1;
//        for (int i = 0; i < zs.size(); ++i)
//        {
//            common_assert_msg(zs[i].size() == ys.size(), "Bad tensor y-size at [" << i << "]: " << zs[i].size() << " != " << ys.size());
//            for (int j = 0; j < zs[i].size(); ++j)
//            {
//                if (rows == -1 && cols == -1)
//                    assert_size_matrix(zs[i][j], rows, cols);
//                else
//                {
//                    rows = zs[i][j].rows();
//                    cols = zs[i][j].cols();
//                    Z.resize(rows, cols);
//                }
//            }
//        }
//        std::vector<std::vector<BilinearInterp> > interp_row;
//        Eigen::MatrixXd blank(xs.size(), ys.size());
//        interp_row.assign(cols, BilinearInterp(xs, ys, blank));
//        interps.assign(rows, interp_row);
//        for (int ix = 0; ix < xs.size(); ++ix)
//        {
//            for (int iy = 0; iy < ys.size(); ++iy)
//            {
//                for (int ir = 0; ir < rows; ++ ir)
//                {
//                    for (int ij = 0; ij < cols; ++ ij)
//                    {
//                        interps[ir][ij].zs(ix, iy) = zs[ix][iy](ir, ij);
//                    }
//                }
//            }
//        }
//    }
//};

}

#endif // EIGEN_UTILITIES_INTERP_UTILITIES_H


#include <iostream>
#include <fstream>
#include <random>
#include <Eigen/Dense>
#include <functional>

using std::cout;
using std::endl;
using std::ofstream;

using std::default_random_engine;
using std::normal_distribution;
using std::bind;

// start random number engine with fixed seed
default_random_engine re{12345};

normal_distribution<double> norm(5,2); // mean and standard deviation
auto rnorm = bind(norm, re);

int main()
{
    using namespace Eigen;

    VectorXd x1(6);
    x1 << 1, 2, 3, 4, 5, 6;
    VectorXd x2 = VectorXd::LinSpaced(6, 1, 2);
    VectorXd x3 = VectorXd::Zero(6);
    VectorXd x4 = VectorXd::Ones(6);
    VectorXd x5 = VectorXd::Constant(6, 3);
    VectorXd x6 = VectorXd::Random(6);

    double data[] = {6,5,4,3,2,1};
    Map<VectorXd> x7(data, 6);

    VectorXd x8 = x6 + x7;

    MatrixXd A1(3,3);
    A1 << 1 ,2, 3,
          4, 5, 6,
          7, 8, 9;
    MatrixXd A2 = MatrixXd::Constant(3, 4, 1);
    MatrixXd A3 = MatrixXd::Identity(3, 3);

    Map<MatrixXd> A4(data, 3, 2);

    MatrixXd A5 = A4.transpose() * A4;
    MatrixXd A6 = x7 * x7.transpose();
    MatrixXd A7 = A4.array() * A4.array();
    MatrixXd A8 = A7.array().log();
    MatrixXd A9 = A8.unaryExpr([](double x) { return exp(x); });
    MatrixXd A10 = MatrixXd::Zero(3,4).unaryExpr([](double x) { return rnorm(); });

    VectorXd x9 = A1.colwise().norm();
    VectorXd x10 = A1.rowwise().sum();

    MatrixXd A11(x1.size(), 3);
    A11 << x1, x2, x3;

    MatrixXd A12(3, x1.size());
    A12 << x1.transpose(),
          x2.transpose(),
          x3.transpose();

    JacobiSVD<MatrixXd> svd(A10, ComputeThinU | ComputeThinV);

    cout << "x1: comman initializer\n" << x1.transpose() << "\n\n";
    cout << "x2: linspace\n" << x2.transpose() << "\n\n";
    cout << "x3: zeors\n" << x3.transpose() << "\n\n";
    cout << "x4: ones\n" << x4.transpose() << "\n\n";
    cout << "x5: constant\n" << x5.transpose() << "\n\n";
    cout << "x6: rand\n" << x6.transpose() << "\n\n";
    cout << "x7: mapping\n" << x7.transpose() << "\n\n";
    cout << "x8: element-wise addition\n" << x8.transpose() << "\n\n";

    cout << "max of A1\n";
    cout << A1.maxCoeff() << "\n\n";
    cout << "x9: norm of columns of A1\n" << x9.transpose() << "\n\n";
    cout << "x10: sum of rows of A1\n" << x10.transpose() << "\n\n";

    cout << "head\n";
    cout << x1.head(3).transpose() << "\n\n";
    cout << "tail\n";
    cout << x1.tail(3).transpose() << "\n\n";
    cout << "slice\n";
    cout << x1.segment(2, 3).transpose() << "\n\n";

    cout << "Reverse\n";
    cout << x1.reverse().transpose() << "\n\n";

    cout << "Indexing vector\n";
    cout << x1(0);
    cout << "\n\n";

    cout << "A1: comma initilizer\n";
    cout << A1 << "\n\n";
    cout << "A2: constant\n";
    cout << A2 << "\n\n";
    cout << "A3: eye\n";
    cout << A3 << "\n\n";
    cout << "A4: mapping\n";
    cout << A4 << "\n\n";
    cout << "A5: matrix multiplication\n";
    cout << A5 << "\n\n";
    cout << "A6: outer product\n";
    cout << A6 << "\n\n";
    cout << "A7: element-wise multiplication\n";
    cout << A7 << "\n\n";
    cout << "A8: ufunc log\n";
    cout << A8 << "\n\n";
    cout << "A9: custom ufucn\n";
    cout << A9 << "\n\n";
    cout << "A10: custom ufunc for normal deviates\n";
    cout << A10 << "\n\n";
    cout << "A11: np.c_\n";
    cout << A11 << "\n\n";
    cout << "A12: np.r_\n";
    cout << A12 << "\n\n";

    cout << "2x2 block startign at (0,1)\n";
    cout << A1.block(0,1,2,2) << "\n\n";
    cout << "top 2 rows of A1\n";
    cout << A1.topRows(2) << "\n\n";
    cout << "bottom 2 rows of A1";
    cout << A1.bottomRows(2) << "\n\n";
    cout << "leftmost 2 cols of A1";
    cout << A1.leftCols(2) << "\n\n";
    cout << "rightmost 2 cols of A1";
    cout << A1.rightCols(2) << "\n\n";

    cout << "Diagonal elements of A1\n";
    cout << A1.diagonal() << "\n\n";
    A1.diagonal() = A1.diagonal().array().square();
    cout << "Transforming diagonal eelemtns of A1\n";
    cout << A1 << "\n\n";

    cout << "Indexing matrix\n";
    cout << A1(0,0) << "\n\n";

    cout << "singular values\n";
    cout << svd.singularValues() << "\n\n";

    cout << "U\n";
    cout << svd.matrixU() << "\n\n";

    cout << "V\n";
    cout << svd.matrixV() << "\n\n";
}


/***************************************
*
* LanXin TECH, All Rights Reserverd.
* Created at Thu May 16 10:27:58 2019
* Contributor: Ling Shi, Ph.D
* Email: lshi@robvision.cn
*
***************************************/

#include "common.h"

#include <Eigen/SVD>
#include <iostream>
#include <fstream>

using namespace std;

namespace lanXin {

RotMat skew(Geo3d v)
{
	RotMat rot;
	rot.setZero();

	rot(0, 1) = -v(2);
	rot(0, 2) = v(1);
	rot(1, 2) = -v(0);

	rot(1, 0) = -rot(0, 1);
	rot(2, 0) = -rot(0, 2);
	rot(2, 1) = -rot(1, 2);

	return rot;
}

Geo3d rodrigues2(const RotMat& matrix)
{
	Eigen::JacobiSVD<Eigen::Matrix3f> svd(matrix, Eigen::ComputeFullV | Eigen::ComputeFullU);
	RotMat R = svd.matrixU() * svd.matrixV().transpose();

	double rx = R(2, 1) - R(1, 2);
	double ry = R(0, 2) - R(2, 0);
	double rz = R(1, 0) - R(0, 1);

	double s = sqrt((rx*rx + ry*ry + rz*rz)*0.25);
	double c = (R.trace() - 1) * 0.5;
	c = c > 1. ? 1. : c < -1. ? -1. : c;

	double theta = acos(c);

	if (s < XEPS)
	{
		double t;

		if (c > 0)
			rx = ry = rz = 0;
		else
		{
			t = (R(0, 0) + 1)*0.5;
			rx = sqrt(std::max(t, 0.0));
			t = (R(1, 1) + 1)*0.5;
			ry = sqrt(std::max(t, 0.0)) * (R(0, 1) < 0 ? -1.0 : 1.0);
			t = (R(2, 2) + 1)*0.5;
			rz = sqrt(std::max(t, 0.0)) * (R(0, 2) < 0 ? -1.0 : 1.0);

			if (fabs(rx) < fabs(ry) && fabs(rx) < fabs(rz) && (R(1, 2) > 0) != (ry*rz > 0))
				rz = -rz;
			theta /= sqrt(rx*rx + ry*ry + rz*rz);
			rx *= theta;
			ry *= theta;
			rz *= theta;
		}
	}
	else
	{
		double vth = 1 / (2 * s);
		vth *= theta;
		rx *= vth; ry *= vth; rz *= vth;
	}
	return Eigen::Vector3d(rx, ry, rz).cast<float>();
}

GeoTransform calibrateHandEye(std::vector<GeoTransform>& vH_robot, std::vector<GeoTransform>& vH_mark, HandEyeType t)
{
	//Eigen::Matrix4f rt;
	const int n = lxmin(vH_robot.size(), vH_mark.size());
	if(n <3)
	{
		printf("At lease 3 point-pairs.\n");
		return GeoTransform();
	}

	printf("Start to calibrate with %d point-pairs.\n", n);

	std::vector<GeoTransform> vA, vB;

	for (int i = 0; i < n; i++)
	{
		for (int j = i + 1; j < n; ++j)
		{
			//if(i == 0 && j==i+1) continue;
			if (t == EyeToHand)
			{
				GeoTransform A = vH_robot[j] * vH_robot[i].inverse();
				GeoTransform B = vH_mark[j] * vH_mark[i].inverse();

				vA.push_back(A);
				vB.push_back(B);
			}
			else if (t == EyeInHand)
			{

				GeoTransform A = vH_robot[j].inverse() * vH_robot[i];
				GeoTransform B = vH_mark[j] * vH_mark[i].inverse();

				vA.push_back(A);
				vB.push_back(B);
			}
		}
	}

	GeoTransform H = sovleAXequalXB(vA, vB);

	return H;
}

Eigen::MatrixXf svdInverse(Eigen::MatrixXf  A)
{
	Eigen::JacobiSVD<Eigen::MatrixXf> svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV);//M=USV*
	float  pinvtoler = 1.e-6; //tolerance
	int row = A.rows();
	int col = A.cols();
	int k = min(row, col);
	Eigen::MatrixXf X = Eigen::MatrixXf::Zero(col, row);
	Eigen::MatrixXf singularValues_inv = svd.singularValues();//奇异值
	Eigen::MatrixXf singularValues_inv_mat = Eigen::MatrixXf::Zero(col, row);
	for (long i = 0; i < k; ++i) {
		if (singularValues_inv(i) > pinvtoler)
			singularValues_inv(i) = 1.0 / singularValues_inv(i);
		else singularValues_inv(i) = 0;
	}
	for (long i = 0; i < k; ++i)
	{
		singularValues_inv_mat(i, i) = singularValues_inv(i);
	}
	X = (svd.matrixV())*(singularValues_inv_mat)*(svd.matrixU().transpose());//X=VS+U*

	return X;
}

GeoTransform sovleAXequalXB(std::vector<GeoTransform>& vA, std::vector<GeoTransform>& vB)
{
	GeoTransform H;
	H.setIdentity();
	if (vA.size() != vB.size())
	{
		printf("A and B must be same size.\n");
		return H;
	}

	const int n = vA.size();

	RotMat R_a, R_b;
	Geo3d r_a, r_b;

	Eigen::MatrixXf A(n*3, 3);
	Eigen::MatrixXf b(n*3, 1);
	A.setZero();
	b.setZero();

	for (int i = 0; i < n; ++i)
	{
		R_a = vA[i].linear();
		R_b = vB[i].linear();

		Geo3d rod_a = rodrigues2(R_a);
		Geo3d rod_b = rodrigues2(R_b);

		float theta_a = rod_a.norm();
		float theta_b = rod_b.norm();

		rod_a /= theta_a;
		rod_b /= theta_b;

		Geo3d P_a = 2*sin(theta_a/2)*rod_a;
		Geo3d P_b = 2*sin(theta_b/2)*rod_b;

		Eigen::Matrix3f rot = skew(Geo3d(P_b+P_a));
		Geo3d v = P_b - P_a;
		//for (int row = 3 * i; row < 3 * i + 3; ++row)
		//{
		//	for (int col = 0; col < 3; ++col)
		//	{
		//		A(row, col) = rot(row - 3 * i, col);
		//	}

		//	b(row) = v(row - 3 * i);
		//}

		//cout << A.middleRows(3 * i, 3 * i + 3) << endl;
		//cout << rot << endl;
		A.middleRows(3 * i, 3) = rot;
		b.middleRows(3*i, 3) = v;
	}
	//Eigen::JacobiSVD<Eigen::MatrixXf> svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV);

	// 3 by 3*n
	Eigen::MatrixXf pinA = svdInverse(A);

	// 3 by 1 = 3 by 3*n multi 3*n by 1
	Geo3d H_ba_prime = pinA * b;

	Geo3d H_ba = 2 * H_ba_prime / sqrt(1 + lxsq(H_ba_prime.norm()));

	// 1 by 3
	Eigen::MatrixXf H_ba_Trs = H_ba.transpose();

	RotMat R_ba = (1 - lxsq(H_ba.norm()) / 2) * RotMat::Identity()
		+ 0.5 * (H_ba * H_ba_Trs + sqrt(4 - lxsq(H_ba.norm()))*skew(H_ba));

	A.setZero();
	b.setZero();
	for (int i = 0; i < n; ++i)
	{
		RotMat AA = vA[i].linear() - RotMat::Identity();
		Geo3d bb = R_ba * vB[i].translation() - vA[i].translation();
		//for (int row = 3 * i; row < 3 * i + 3; ++row)
		//{
		//	for (int col = 0; col < 3; ++col)
		//	{
		//		A(row, col) = AA(row - 3 * i, col);
		//	}

		//	b(row) = bb(row - 3 * i);
		//}
		A.middleRows(3 * i, 3) = AA;
		b.middleRows(3 * i, 3) = bb;
	}
	pinA = svdInverse(A);
	Geo3d t_ba = pinA * b;
	H.linear() = R_ba;
	H.translation() = t_ba;

	// check
	for(int i = 0; i<n; ++i)
	{
		GeoTransform AX = vA[i] * H;
		GeoTransform XB = H * vB[i];

		Geo3d angles1 = AX.linear().eulerAngles(0, 1, 2);
		Geo3d angles2 = XB.linear().eulerAngles(0, 1, 2);
		// cout << i << " Dist Error: " << (AX.translation() - XB.translation()).norm()
		// 	<< ". Rotation Error: " << (angles1 - angles2).transpose() << endl;
	}

	std::string fname("H.txt");
	printf("Calibration Finished. Saved at %s.\n", fname.c_str());

	cout << "H: \n" << H.matrix() << endl;
	ofstream out(fname);
	out <<"# Calibrated At " << fname  << endl;
	out << H.matrix() << endl;
	out.close();

	return H;
}

} /* End of namespace lanXin */


// TestNormal.cpp
//
// First program test the Boost statistics library.
//
// Look at the Normal distribution because it is important.
// And gamma distribution
//
// 2008-6-27 DD initial code
// 2011-11-9 DD for QN course
//
// (C) Datasim Education BV 2009-2011
//

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

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

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

	normal_distribution<> myNormal(1.0, 10.0); // Default type is 'double'
	cout << "Mean: " << mean(myNormal) << ", standard deviation: " << standard_deviation(myNormal) << endl;

	// Distributional properties
	double x = 10.25;

	cout << "pdf: " << pdf(myNormal, x) << endl;
	cout << "cdf: " << cdf(myNormal, x) << endl;

	// Choose another data type and now a N(0,1) variate
	normal_distribution<float> myNormal2;
	cout << "Mean: " << mean(myNormal2) << ", standard deviation: " << standard_deviation(myNormal2) << endl;

	cout << "pdf: " << pdf(myNormal2, x) << endl;
	cout << "cdf: " << cdf(myNormal2, x) << endl;

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

	// Other properties
	cout << "\n***normal distribution: \n";
	cout << "mean: " << mean(myNormal) << endl;
	cout << "variance: " << variance(myNormal) << endl;
	cout << "median: " << median(myNormal) << endl;
	cout << "mode: " << mode(myNormal) << endl;
	cout << "kurtosis excess: " << kurtosis_excess(myNormal) << endl;
	cout << "kurtosis: " << kurtosis(myNormal) << endl;
	cout << "characteristic function: " << chf(myNormal, x) << endl;
	cout << "hazard: " << hazard(myNormal, x) << endl;

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

	double val = 13.0;
	cout << endl <<  "pdf: " << pdf(myGamma, val) << endl;
	cout << "cdf: " << cdf(myGamma, val) << endl;

	vector<double> pdfList;
	vector<double> cdfList;

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

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

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

		val += h;
	}

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

	}

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

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

	}

	return 0;
}

Level_8/V. An Introduction to the Boost C++ Libraries/05b - Normal Distribution/TestNormalDistribution.cpp


/**
 *  @file    diffusion.hpp
 *  @brief   Collections of diffusion problem, definitions.
 *  @author  Francois Roy
 *  @date    12/01/2019
 */
#ifndef DIFFUSION_H
#define DIFFUSION_H

#include <string>
#include <Eigen/Core>
#include "spdlog/spdlog.h"
#include "numerical/fdm/fdm.hpp"

namespace bench{
namespace diffusion{

typedef Eigen::VectorXd Vec;
typedef Eigen::ArrayXd Arr;

/**
* Reference solution for the 1D diffusion problem a, defined as:
*
* \f[
*    \frac{\partial u}{\partial t} = -\alpha \frac{\partial^2 u}{\partial x^2}
*                                    + f(x,t)\quad x \in (0, L),~t \in (0, T]
* \f]
*
* with Dirichlet boundary condition \f$u(0, t) = 0\f$ and  \f$ u(L, t) = 0\f$
* for \f$t>0\f$.
*
* The diffusion coefficient, \f$ \alpha(L, t) = 1\f$ is constant and uniform.
*
* We define the source term and initial condition with a reference
* (manufactured) solution that respects the boundary conditions at the
* extremities of the interval:
*
* \f[
*    u(x, t) = 5tx*\left(L-x\right)
* \f]
*
* We get the source term substituting the manufactured solution in the
* diffusion equation, i.e.:
*
* \f[
*    f(x, t) = 5x\left(L-x\right)+10\alpha t
* \f]
*
* The initial condition is simply set to the manufactured solution at
* \f$t=0\f$:
*
* \f[
*    u(x, 0) = u_0 = 0
* \f]
*
* @param x The spatial mesh node.
* @param t The temporal mesh node.
* @param L The length of the interval.
* @return The reference solution.
*/
template<typename T>
T reference_diffusion_a(T x, T t, T L){
	return 5.0 * t * x * (L - x);
}

/**
* Computes the 1D diffusion problem defined in @ref reference_diffusion_a()
* using the finite difference method. Here we solve the problem using the
* \f$\Theta\f$-scheme in time and a centered difference scheme in space, i.e.
*
* \f[
*    u_i^{n+1} - \Theta\left(\alpha\left(
*        \frac{u_{i-1}^{n+1}-2u_{i}^{n+1}+u_{i+1}^{n+1}}{\Delta x^2}
*        \right)\right) = \left(1-\Theta\right)\left(\alpha\left(
*        \frac{u_{i-1}^{n}-2u_{i}^{n}+u_{i+1}^{n}}{\Delta x^2}
*        \right)\right) + \Theta\Delta t f_i^{n+1}+\left(1-\Theta\right)f_i^n
*        + u_i^n
* \f]
*
* For \f$\Theta=0\f$ we have the forward Euler scheme in time, for
* \f$\Theta=1\f$ we have the backward Euler scheme in time and for
* \f$\Theta=1/2\f$ we have the Crank-Nicolson scheme in time.
*
* In matrix form we have:
*
* \f[
*    \mathbf{A}\mathbf{u} = \mathbf{b}
* \f]
*
* Using the Fourrier coefficient \f$F=\alpha\Delta t/\Delta x^2\f$, the matrix
* entries are:
*
* \f[
*    A_{i,i-1}=-F\Theta,~A{i, i}=1+2F\Theta,~A_{i, i+1}=-F\Theta
* \f].
*
* The vector \f$\mathbf{u}\f$ represents the unknown at the discrete time
* \f$t=t^{n+1}\f$.
*
* The RHS can be written as:
*
* \f[
*    b_i = u_i^n + F\left(1-\Theta\right)u_{i+1}^n-2u_i^n+u_{i-1}^n +
*        \Delta t \Theta f_i^{n+1} + \Delta t \left(1-\Theta\right)f_i^n
* \f]
*
* For more information see pp. 181-183 of:
*
* R. J. Leveque, Finite Difference Methods for Ordinary and Partial
* Differential Equations: Steady-State and Time-Dependent Problems, SIAM, 2007.
*
* @return The solution.
* @see numerical::fdm::Parameters
* @see numerical::fdm::Mesh
* @see numerical::fdm::SparseSolver
*/
bool fdm_diffusion_a();

class FDMDiffusionA : public numerical::fdm::Problem<double>{
public:
    FDMDiffusionA(numerical::fdm::Parameters<double>* p):
        numerical::fdm::Problem<double>(p){
        }
    // double alpha(double x, double y, double z, double t);

};

}  // namespace diffusion
}  // namespace bench

#endif  // DIFFUSION_H


#include "RegressionEigen.hpp"
#include "RegressionGSL.hpp"
#include <Math/GeneralMath.hpp>
#include <Util/VecD.hpp>

//#define EIGEN_USE_MKL_ALL
//#define EIGEN_USE_BLAS // Enables openblas64
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;
using namespace EnjoLib;

RegressionEigen::RegressionEigen(){}
RegressionEigen::~RegressionEigen(){}

/*
VecD RegressionEigen::polynomialfit(int degree, const VecD & dx, const VecD & dy) const
{
    return RegressionGSL().polynomialfit(degree, dx, dy);
}
*/

VecD RegressionEigen::polynomialfit(int degree, const VecD & dx, const VecD & dy) const
{
    VecD coeffs(degree);
    const int obs = dx.size();
    //VecD weights(obs);
    MatrixXd A(obs, degree);
   //cout << "Here is the matrix A:\n" << A << endl;
    VectorXd b(obs);
    //Map<VectorXd> b(dy.data(), obs, 1);
    //VecD dycopy = dy;
    //Map<VectorXd> b(dycopy.data(), obs);
   const GeneralMath gmat;

    for(int i=0; i < obs; ++i)
    {
        b(i) = dy[i];

        //gsl_vector_set(y.vec, i, dy[i]);
        //gsl_vector_set(w.vec, i, weights[i]);
        //gsl_vector_set(w.vec, i, i);        // More weight to latest
        //gsl_vector_set(w.vec, i, obs - i);  // More weight to initial
        //gsl_vector_set(w.vec, i, 1);      // Disabled different weights
        for(int j=0; j < degree; ++j)
        {
            //gsl_matrix_set(X.mat, i, j, pow(dx[i], j));
            A(i, j) = gmat.PowInt(dx[i], j);
            //A(i, j) = dx[i];
        }
    }

    /*
    const auto & eigRes = A.bdcSvd(ComputeThinU | ComputeThinV).solve(b);
       cout << "The least-squares solution is:\n"
        << eigRes << endl;
      */
    //const MatrixXd & aTrans = A.transpose();
	//const auto & f = (aTrans*A).inverse()*aTrans*b;
	const auto & f = (A.transpose()*A).inverse()*A.transpose()*b; // Faster

	// Faster!
	//const auto & f = (A.transpose()*A).inverse()*A.transpose()*b;
	//const auto & ff = A.colPivHouseholderQr().solve(b);
	//const auto & ff = A.bdcSvd(ComputeThinU | ComputeThinV).solve(b);

	//cout << "Matrix f" << endl;
	//cout << ff << endl;

    /* store result ... */
    for(int i=0; i < degree; ++i)
    {
        const double coeff = f(i, 0);
        //cout << "Coeff " << i << " = " << coeff << endl;;
        coeffs[i] = coeff;
    }
    return coeffs; /* we do not "analyse" the result (cov matrix mainly)
          to know if the fit is "good" */
    // http://stats.stackexchange.com/questions/151018/how-does-the-covariance-matrix-of-a-fit-get-computed
}


/* -*- 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)";
}


/**
 * @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;
        }
      })
    ;
}


#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));
    }

  }

}


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


#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
}


#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;

};


#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;
}


// 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;
}