//===------------------------------------------------------------*- C++ -*-===//
///
/// \brief 线性变换。
/// \details 设 U、V 为数域 K 上的两个线性空间，f 为 U 到 V 的一个映射，且满足如下条件：
///          (i)  对任意 α，β ∈ U 有 f(α+β) = f(α) + f(β)；
///          (ii) 对任意 α ∈ U，k ∈ K，有 f(kα) = kf(α)。
///          则称 f 为 U 到 V 的一个线性变换．
/// \sa <https://zh.wikipedia.org/wiki/线性变换>
///
/// \version 2021-11-08
/// \since 2021-11-08
/// \authors zhengrr
/// \copyright Unlicense
///
//===----------------------------------------------------------------------===//

#include <cmath>
#include <execution>
#include <numeric>

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

/// \brief 矩阵乘法。
TEST(LinearTransformation, MatrixMultiplication)
{
    using Scalar = float;
    using MatrixXs = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
    constexpr Eigen::Index m = 4;
    constexpr Eigen::Index n = 1;
    constexpr Eigen::Index p = 3;

    // m 行 n 列的矩阵 A
    const Eigen::Matrix<Scalar, m, n> a = MatrixXs::Random(m, n);
    // n 行 p 列的矩阵 B
    const Eigen::Matrix<Scalar, n, p> b = MatrixXs::Random(n, p);
    // m 行 p 列的矩阵 AB 是矩阵 A 和矩阵 B 的矩阵积（matrix product）
    const Eigen::Matrix<Scalar, m, p> ab = a * b;

    //                  [B(0,0)  B(0,1)  B(0,2) ]  [B(0,j) ]
    //                  [B(1,0)  B(1,1)  B(1,2) ]  [B(1,j) ]
    //
    // [A(0,0) A(0,1)]  [AB(0,0) AB(0,1) AB(0,2)]
    // [A(1,0) A(1,1)]  [AB(1,0) AB(1,1) AB(1,2)]
    // [A(2,0) A(2,1)]  [AB(2,0) AB(2,1) AB(2,2)]
    // [A(3,0) A(3,1)]  [AB(3,0) AB(3,1) AB(3,2)]
    //
    // [A(i,0) A(i,1)]                             [AB(i,j)] = A(i,0)B(0,j) + A(i,1)B(1,j) + ... + A(i,n)B(n,j)

    static const auto matrix_product_element = [&a, &b](Eigen::Index i, Eigen::Index j) -> Scalar {
#if 1
        return std::transform_reduce(std::execution::par_unseq, a.row(i).begin(), a.row(i).end(), b.col(j).begin(), Scalar {});
#else
        Scalar sum {};
        for (Eigen::Index t = 0; t < n; ++t)
            sum += a(i, t) * b(t, j);
        return sum;
#endif
    };

    for (Eigen::Index i = 0; i < m; ++i)
        for (Eigen::Index j = 0; j < p; ++j)
            EXPECT_EQ(ab(i, j), matrix_product_element(i, j));
}

namespace {

using Scalar = float;
constexpr Eigen::Index dimension = 2;
using Point = Eigen::Vector<Scalar, dimension>;
using LinearTransformationMatrix = Eigen::Matrix<Scalar, dimension, dimension>;

/// \brief 圆周率
const Scalar pi {std::acos(Scalar {-1})};

/// \brief 绕原点旋转。
/// \param theta 旋转角（逆时针，弧度制）。
/// \sa [坐标旋转变换公式的推导](https://blog.csdn.net/Tangyongkang/article/details/5484636)
LinearTransformationMatrix rotate_around_origin(Scalar theta)
{
    //                   [x ]
    //                   [y ]
    //
    // [cos(θ) -sin(θ)]  [x'] = cos(θ)x - sin(θ)y
    // [sin(θ)  cos(θ)]  [y'] = sin(θ)x + cos(θ)y

    return LinearTransformationMatrix {
        {std::cos(theta), -std::sin(theta)},
        {std::sin(theta), std::cos(theta)}
    };
}

/// \brief 通过过原点的反射轴反射。
/// \param theta 反射轴与 x 轴正方向的夹角（逆时针，弧度制）。
LinearTransformationMatrix reflect_through_axis_that_cross_origin(Scalar theta)
{
    //                     [x ]
    //                     [y ]
    //
    // [cos(2θ)  sin(2θ)]  [x'] = cos(2θ)x + sin(2θ)y
    // [sin(2θ) -cos(2θ)]  [y'] = sin(2θ)x - cos(2θ)y

    return LinearTransformationMatrix {
        {std::cos(2 * theta), std::sin(2 * theta)},
        {std::sin(2 * theta), -std::cos(2 * theta)}
    };
}

/// \brief 通过 x 轴反射。
LinearTransformationMatrix reflect_through_x_axis() { return reflect_through_axis_that_cross_origin(0); }

/// \brief 通过 y 轴反射。
LinearTransformationMatrix reflect_through_y_axis() { return reflect_through_axis_that_cross_origin(pi / 2); }

/// \breif 缩放。
/// \param xFactor x 轴向的缩放因子。
/// \param yFactor y 轴向的缩放因子。
LinearTransformationMatrix scale(Scalar xFactor, Scalar yFactor)
{
    //        [x ]
    //        [y ]
    //
    // [v 0]  [x'] = vx
    // [0 w]  [y'] = wy

    return LinearTransformationMatrix {
        {xFactor, 0},
        {0, yFactor}
    };
}

/// \brief 挤压。
/// \details 变换前后面积不变。
LinearTransformationMatrix squeeze(Scalar factor) { return scale(factor, Scalar {1} / factor); }

/// \brief 错切。
/// \param xFactor x 轴向的错切因子。
/// \param yFactor y 轴向的错切因子。
LinearTransformationMatrix shear(Scalar xFactor, Scalar yFactor)
{
    //        [x ]
    //        [y ]
    //
    // [1 v]  [x'] = x + vy
    // [w 1]  [y'] = wx + y

    return LinearTransformationMatrix {
        {1, xFactor},
        {yFactor, 1}
    };
}

/// \brief 水平错切。
LinearTransformationMatrix horizontal_shear(Scalar factor) { return shear(factor, 0); }

/// \brief 垂直错切。
LinearTransformationMatrix vertical_shear(Scalar factor) { return shear(0, factor); }

/// \brief 正交投影到 x 轴。
LinearTransformationMatrix projection_onto_x_axis()
{
    //        [x ]
    //        [y ]
    //
    // [0 0]  [x'] = 0
    // [0 1]  [y'] = y

    return LinearTransformationMatrix {
        {0, 0},
        {0, 1}
    };
}

/// \brief 正交投影到 y 轴。
LinearTransformationMatrix projection_onto_y_axis()
{
    //        [x ]
    //        [y ]
    //
    // [1 0]  [x'] = x
    // [0 0]  [y'] = 0

    return LinearTransformationMatrix {
        {0, 0},
        {0, 1}
    };
}

}

TEST(LinearTransformation, RotateAroundOrigin)
{
    // 点 A(x, y)
    const Point a {1, 0};
    // 旋转角 θ
    const Scalar theta {pi / 2};
    // 点 A'(x', y')
    const Point aPrime {0, 1};

    EXPECT_TRUE((rotate_around_origin(theta) * a).isApprox(aPrime));
}


#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <kv/autodif.hpp>

namespace ub = boost::numeric::ublas;

// f: R->R
kv::autodif<double> testfunc1(const kv::autodif<double>& x) {
	kv::autodif<double> tmp;

	tmp = x * x - 1.;
	return cos(tmp + sin(tmp));
}

// f: R->R (use template)
template <class T> T testfunc2(const T& x) {
	T tmp;

	tmp = x * x - 1.;
	return cos(tmp + sin(tmp));
}

// f: R^n->R
template <class T> T testfunc3(const ub::vector<T>& x) {
	return atan(x(0) * x(0) + 2. * x(1) * x(1) - 1.);
}

// f: R->R^n
template <class T> ub::vector<T> testfunc4(const T& x) {
	ub::vector<T> y(2);

	y(0) = sin(x);
	y(1) = cos(x);

	return y;
}

// f: R^n->R^n
template <class T> ub::vector<T> testfunc5(const ub::vector<T>& x) {
	ub::vector<T> y(2);

	y(0) = 2. * x(0) * x(0) * x(1) - 1.;
	y(1) = x(0) + 0.5 * x(1) * x(1) - 2.;

	return y;
}

int main()
{
	double d1, d2;
	kv::autodif<double> a1, a2, a3;
	ub::vector<double> v1, v2;
	ub::vector< kv::autodif<double> > va1, va2, va3, va4, va5;
	ub::matrix<double> m;
	int i;

	//
	// f: R->R (testfunc1)
	//

	// helper function to initialize autodif variable with single variable
	// same as " a1.v = 1.5; a1.d(0) = 1.; "
	a1 = kv::autodif<double>::init(1.5);

	a2 = testfunc1(a1);

	// helper function to separate autodif variable into value and gradient
	// same as " d1 = a2.v; d2 = a2.d(0); "
	kv::autodif<double>::split(a2, d1, d2);

	std::cout << "testfunc1\n";
	std::cout << d1 << "\n"; // f(1.5)
	std::cout << d2 << "\n"; // f'(1.5)

	//
	// f: R->R (testfunc2: use template)
	//

	a1 = kv::autodif<double>::init(1.5);
	a2 = testfunc2(a1);

	kv::autodif<double>::split(a2, d1, d2);

	std::cout << "testfunc2\n";
	std::cout << d1 << "\n"; // f(1.5)
	std::cout << d2 << "\n"; // f'(1.5)

	d1 = 1.5;
	d2 = testfunc2(d1); // double can be also passed
	std::cout << d2 << "\n"; // f(1.5)

	//
	// f: R^n->R
	//

	v1.resize(2);
	v1(0) = 5.; v1(1) = 6.;

	// helper function to initialize autodif vector with vector variable
	// same as " va1(0).v = 5.; va1(0).d(0) = 1.; va1(0).d(1) = 0,; "
	//         " va1(1).v = 6.; va1(1).d(0) = 0.; va1(1).d(1) = 1,; "
	va1 = kv::autodif<double>::init(v1);

	a1 = testfunc3(va1);

	// helper function to separate autodif variable into value and gradient
	// same as " y = a1.v; v2(0) = a1.d(0); v2(1) = a1.d(1) "
	kv::autodif<double>::split(a1, d1, v2);

	std::cout << "testfunc3\n";
	std::cout << d1 << "\n"; // f(5, 6)
	std::cout << v2 << "\n"; // gradient

	//
	// f: R->R^n
	//

	a1 = kv::autodif<double>::init(1.5);

	va1 = testfunc4(a1);

	// helper function to separate autodif variable into value and gradient
	// same as " v1(0) = va1(0).v; v1(1) = va1(1).v; "
	//         " v2(0) = va1(0).d(0); v2(1) = va1(1).d(0); "
	kv::autodif<double>::split(va1, v1, v2);

	std::cout << "testfunc4\n";
	std::cout << v1 << "\n"; // f(1.5)
	std::cout << v2 << "\n"; // f'(1.5)

	//
	// f: R^n->R^m
	//

	v1(0) = 5.; v1(1) = 6.;

	va1 = kv::autodif<double>::init(v1);

	va2 = testfunc5(va1);

	// helper function to separate autodif vector into vector value and jacobian
	// same as " v2(0) = va2(0).v; m(0)(0) = va2(0).d(0); m(0)(1) = va2(0).d(1); "
	//         " v2(1) = va2(1).v; m(1)(0) = va2(1).d(0); m(1)(1) = va2(1).d(1); "
	kv::autodif<double>::split(va2, v2, m);

	std::cout << "testfunc5\n";
	std::cout << v2 << "\n"; // f(5, 6)
	std::cout << m << "\n"; // Jacobian matrix

	//
	// operations for autodif
	//

	v1(0) = 0.7; v1(1) = 0.8;
	va1 = kv::autodif<double>::init(v1);
	a1 = va1(0);
	a2 = va1(1);

	std::cout << "autodif operations\n";

	std::cout << a1 << "\n";
	std::cout << a2 << "\n";

	a3 = a1 + a2; std::cout << a3 << "\n";
	a3 = a1 - a2; std::cout << a3 << "\n";
	a3 = a1 * a2; std::cout << a3 << "\n";
	a3 = a1 / a2; std::cout << a3 << "\n";
	a3 = pow(a1, 3); std::cout << a3 << "\n";
	a3 = pow(a1, a2); std::cout << a3 << "\n";
	a2 = sqrt(a1); std::cout << a2 << "\n";
	a2 = exp(a1); std::cout << a2 << "\n";
	a2 = log(a1); std::cout << a2 << "\n";
	a2 = sin(a1); std::cout << a2 << "\n";
	a2 = cos(a1); std::cout << a2 << "\n";
	a2 = tan(a1); std::cout << a2 << "\n";
	a2 = sinh(a1); std::cout << a2 << "\n";
	a2 = cosh(a1); std::cout << a2 << "\n";
	a2 = tanh(a1); std::cout << a2 << "\n";
	a2 = asin(a1); std::cout << a2 << "\n";
	a2 = acos(a1); std::cout << a2 << "\n";
	a2 = atan(a1); std::cout << a2 << "\n";
	// asinh, acosh, atanh may not compile for autodif<double>
	// because c++03 does not have asinh, acosh, atanh
	// a2 = asinh(a1); std::cout << a2 << "\n";
	// a2 = acosh(a1+1.); std::cout << a2 << "\n";
	// a2 = atanh(a1); std::cout << a2 << "\n";

	//
	// compress / expand
	//

	v1.resize(20);
	for (i=0; i<20; i++) v1(i) = i + 2.;
	va1 = kv::autodif<double>::init(v1);
	// use only a few of the many variables
	va2.resize(2);
	va2(0) = va1(10);
	va2(1) = va1(11);

	// usual way
	std::cout << "usual input\n";
	std::cout << va2 << "\n";
	va3 = testfunc5(va2);
	std::cout << "usual output\n";
	std::cout << va3 << "\n";

	// using compress/expand to save calculation time
	ub::matrix<double> save;
	va3 = kv::autodif<double>::compress(va2, save);
	std::cout << "compressed input\n";
	std::cout << va3 << "\n";
	va4 = testfunc5(va3);
	std::cout << "compressed output\n";
	std::cout << va4 << "\n";
	va5 = kv::autodif<double>::expand(va4, save);
	std::cout << "expanded output\n";
	std::cout << va5 << "\n";
}


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

Eigen::VectorXd poly(int const n, double & x){
	Eigen::VectorXd v(n);
	v(0)=1;
	if(n>1) v(1)=x;
	for (int i=2; i < n;i++){
		v(i)=2*x*v(i-1)-v(i-2);
	}
	return v;
}

Eigen::VectorXd zeroes(const int n){
	Eigen::VectorXd v(n);
	for (int i=0;i<n;i++){
		double temp=((2.*i-1.)/n*2)*M_PI;
		v(i)=cos(temp);
	}
	return v;
}

template <typename Function>
void bestpolchebnodes (const Function&f, Eigen::VectorXd &alpha){
		size_t n= alpha.size();
		Eigen::VectorXd v;
		Eigen::VectorXd fn(n+1);
		for (int i=0; i<n+1; i++){
			double temp=cos(M_PI*(2*k+1)/2/(n+1));
			fn(i)=f(temp);
		}
		Eigen::MatrixXd scal(n+1,n+1);
		for (int j=0; j<n+1; j++) {
			v=poly(n,cos(M_PI*(2*j+1)/2/(n+1)));
			for (int k=0; k<n+1; k++) scal(j,k)=V[k];
		}
		for (int l=0;l<n;l++){
			alpha(l)=0;
			for (int k=0;k<n;k++){
				alpha(l)+=2*(1./(n+1))*fn(k)*v(k);
			}
		}
		alpha(0)=alpha(0)/2;

}
using namespace std;
int main(){
    auto f = [] (double & x) {return 1/(pow(5*x,2)+1);};
    int n=20;
    Eigen::VectorXd alpha(n+1);
    bestpolchebnodes(f, alpha);

    //Compute the error
    Eigen::VectorXd X = Eigen::VectorXd::LinSpaced(1e6,-1,1);
    auto qn = [&alpha,&n] (double & x) {
        double temp;
        vector<double> V=poly(n,x);
        for (int k=0; k<n+1; k++) temp+=alpha(k)*V[k];
        return temp;
    };
    double err_max=abs(f(X(0))-qn(X(0)));
    for (int i=1; i<1e6; i++) err_max=std::max(err_max,abs(f(X(i))-qn(X(i))));
    cout<<"Error: "<< err_max <<endl;
}

Nummerical Methods for CSE/PS9/chebychev.cpp


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu>, Randi Cabezas <rcabezas@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */

#pragma once

#include <Eigen/Dense>

#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/distributions/inverse_gamma.hpp>

#include <boost/random/chi_squared_distribution.hpp>
#include <boost/random/normal_distribution.hpp>

#include <dpMM/distribution.hpp>
#include <dpMM/normal.hpp>

using namespace Eigen;
using std::endl;
using std::cout;

template<typename T>
class IW : public Distribution<T>
{
public:
  Matrix<T,Dynamic,Dynamic> Delta_;
  T nu_;
  uint32_t D_;

  IW(const Matrix<T,Dynamic,Dynamic>& Delta, T nu, boost::mt19937 *pRndGen);
  IW(const Matrix<T,Dynamic,Dynamic>& Delta, T nu, const Matrix<T,Dynamic,Dynamic>& scatter,
	 const Matrix<T,Dynamic,1>& mean, T counts, boost::mt19937 *pRndGen);
  IW(const IW& iw);
  ~IW();

  IW<T>* copy();

  IW<T> posterior(const Matrix<T,Dynamic,Dynamic>& x, const VectorXu& z,
      uint32_t k, uint32_t zDivider=1);
  IW<T> posterior(const Matrix<T,Dynamic,Dynamic>& outer, T count) const;
  IW<T> posterior() const ;
  void resetSufficientStatistics();

  Matrix<T,Dynamic,Dynamic> sample();
//  Normal<T> sample();
  T logPdf(const Matrix<T,Dynamic,Dynamic>& Sigma) const;
  T logPdf(const Normal<T>& normal) const;
  T logLikelihoodMarginalized() const; // log pdf of SS under NIW prior
  T logLikelihoodMarginalized(const Matrix<T,Dynamic,Dynamic>& Scatter,
    T count) const;

  Matrix<T,Dynamic,Dynamic> mode() const;

  const Matrix<T,Dynamic,Dynamic>& scatter() const {return scatter_;};
  Matrix<T,Dynamic,Dynamic>& scatter() {return scatter_;};
  const Matrix<T,Dynamic,1>& mean() const {return mean_;};
  Matrix<T,Dynamic,1>& mean() {return mean_;};
  T count() const {return count_;};
  T& count() {return count_;};

private:
  boost::random::normal_distribution<> gauss_;

  // sufficient statistics
  Matrix<T,Dynamic,Dynamic> scatter_;
  Matrix<T,Dynamic,1> mean_;
  T count_;
};

typedef IW<double> IWd;
typedef IW<float> IWf;

/* prior for spherical covariances (i.e. \Sigma = \sigma^2 I) */
template<typename T>
class IW_spherical : public Distribution<T>
{
public:
  T delta_;
  T nu_;
  uint32_t D_;

  IW_spherical(T delta, T nu, uint32_t D, boost::mt19937 *pRndGen);
  IW_spherical(const IW_spherical& iw);
  ~IW_spherical();

  IW_spherical<T>* copy();

  IW_spherical<T> posterior(const Matrix<T,Dynamic,Dynamic>& x,
      const VectorXu& z, uint32_t k, uint32_t zDivider=1);
//  T sample();
//  T logPdf(T sigma);
////  T logPdf(const Normal& normal);

private:
  boost::random::normal_distribution<> gauss_;
};

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


/*
 * Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * 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.
 *     * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL Christian Gehring, Hannes Sommer, Paul Furgale,
 * Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
 * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
*/
#pragma once

#include <Eigen/SVD>

namespace kindr {

/*!
 * \brief Gets a skew-symmetric matrix from a (column) vector
 * \param   vec 3x1-matrix (column vector)
 * \return skew   3x3-matrix
 */
template<typename PrimType_>
inline static Eigen::Matrix<PrimType_, 3, 3> getSkewMatrixFromVector(const Eigen::Matrix<PrimType_, 3, 1>& vec) {
  Eigen::Matrix<PrimType_, 3, 3> mat;
  mat << 0, -vec(2), vec(1), vec(2), 0, -vec(0), -vec(1), vec(0), 0;
  return mat;
}

/*!
 * \brief Gets a 3x1 vector from a skew-symmetric matrix
 * \param   matrix 3x3-matrix
 * \return  column vector (3x1-matrix)
 */
template<typename PrimType_>
inline static Eigen::Matrix<PrimType_, 3, 1> getVectorFromSkewMatrix(const Eigen::Matrix<PrimType_, 3, 3>& matrix) {
  return Eigen::Matrix<PrimType_, 3, 1> (matrix(2,1), matrix(0,2), matrix(1,0));
}

/*!
 * \brief Computes the Moore–Penrose pseudoinverse
 * info: http://eigen.tuxfamily.org/bz/show_bug.cgi?id=257
 * \param a: Matrix to invert
 * \param result: Result is written here
 * \param epsilon: Numerical precision (for example 1e-6)
 * \return true if successful
 */
template<typename _Matrix_TypeA_, typename _Matrix_TypeB_>
bool static pseudoInverse(const _Matrix_TypeA_ &a, _Matrix_TypeB_ &result,
                          double epsilon = std::numeric_limits<typename _Matrix_TypeA_::Scalar>::epsilon())
{
  // Shorthands
  constexpr auto rowsA = static_cast<int>(_Matrix_TypeA_::RowsAtCompileTime);
  constexpr auto colsA = static_cast<int>(_Matrix_TypeA_::ColsAtCompileTime);
  constexpr auto rowsB = static_cast<int>(_Matrix_TypeB_::RowsAtCompileTime);
  constexpr auto colsB = static_cast<int>(_Matrix_TypeB_::ColsAtCompileTime);

  // Assert wrong matrix types
  static_assert(std::is_same<typename _Matrix_TypeA_::Scalar, typename _Matrix_TypeB_::Scalar>::value,
                "[kindr::pseudoInverse] Matrices must be of the same Scalar type!");
  static_assert(rowsA == colsB && colsA == rowsB, "[kindr::pseudoInverse] Result type has wrong size!");

  // If one dimension is dynamic, compute everything as dynamic size
  constexpr auto m = Eigen::JacobiSVD< _Matrix_TypeA_ >::DiagSizeAtCompileTime;

  // JacobiSVD needs to be computed on dynamic size if we need ComputeThinU, ComputeThinV, see:
  // https://eigen.tuxfamily.org/dox/classEigen_1_1JacobiSVD.html
  Eigen::JacobiSVD< Eigen::Matrix<typename _Matrix_TypeA_::Scalar, Eigen::Dynamic, Eigen::Dynamic> > svd(a, Eigen::ComputeThinU | Eigen::ComputeThinV);

  typename _Matrix_TypeA_::Scalar tolerance =
    epsilon * std::max(a.cols(), a.rows()) * svd.singularValues().array().abs().maxCoeff();

  // Sigma for ThinU and ThinV
  Eigen::Matrix<typename _Matrix_TypeA_::Scalar, m, m> sigmaThin = Eigen::Matrix<typename _Matrix_TypeA_::Scalar, m, 1>(
    (svd.singularValues().array().abs() > tolerance).select(svd.singularValues().array().inverse(), 0)).asDiagonal();

  result = svd.matrixV() * sigmaThin * svd.matrixU().transpose();

  return true;
}

} // end namespace kindr


/*
 * Using Sundaram' sieve, print the first 2,000,000 numbers
 * darkturo 2014
 */
#include <iostream>
#include <fstream>
#include <bitset>
#include <ctime>
#include <limits>
#include <boost/spirit/include/karma.hpp>

#define TIMEDIFF(start, stop) 1000.0 * (stop - start)/CLOCKS_PER_SEC
#define MAX 32452843
#define MAXPRIMES 2000000
#define BUFFER_SIZE 8l * MAX

class SundaramSieve
{
   std::bitset<MAX> N;
   std::ofstream output;
   int counter;
   char * buffer;
   char * p_input;

   public:
   SundaramSieve() :
           output("primesEveryWhere.txt", std::ios::out | std::ios::trunc),
           counter(0)
   {
      buffer = new char[BUFFER_SIZE];
      p_input = buffer;
      N.set();
   }

   ~SundaramSieve()
   {
      if (p_input != buffer)
         output.write(buffer, p_input - buffer);  // flush
      output.close();
   }

   void sieve()
   {
      // Given i,j ∈ ℕ, 1 ≤ i ≤ j
      // Sieve all the x ∈ ℕ, x = i + j + 2ij ≤ MAX
      for (long i = 1; (2*i + 2*(i^2)) <= 2*sqrt(MAX); i++)
      {
         for (long j = i; (i + j + 2*i*j) <= (MAX - 2)/2; j++)
         {
            N.set((i + j + 2*i*j) - 1, false);
         }
      }

      // Generate the primes from the sieved list.
      print( 2 );
      for (int i = 1; counter < MAXPRIMES; i++)
      {
         if (N[i-1])
         {
            print( 2*i + 1 );
         }
      }
   }

   int printedPrimes()
   {
      return counter;
   }

   private:
   inline void print(int number)
   {
      doPrint(number);
      counter ++;
   }

   inline void doPrint(int number)
   {
      boost::spirit::karma::generate(p_input, boost::spirit::int_, number);
      *p_input++ = '\n';
   }
};

int main(int argc, char ** argv)
{
   clock_t t1, t2;

   t1 = std::clock();

   SundaramSieve siever;
   siever.sieve();

   t2 = std::clock();

   // Summary
   std::cout.precision(std::numeric_limits<double>::digits10);
   std::cerr << "Used " << std::fixed << TIMEDIFF(t1, t2)
             << " msecs to calculate " << siever.printedPrimes()
             << " primes." << std::endl;
}


#pragma once

#include "modprop/compo/Interfaces.h"
#include "modprop/compo/Parametric.hpp"
#include <Eigen/Dense>

namespace percepto
{

// TODO Support matrix inputs?
/*! \brief A simple linear regression class. Outputs Weights matrix * input.
 * The matrix and input are dynamically-sized. Assumes row-major ordering
 * for vectorizing the weights matrix. */
class LinearRegressor
: public Source<VectorType>
{
public:

	typedef VectorType InputType;
	typedef VectorType OutputType;
	typedef Source<VectorType> SourceType;

	LinearRegressor( unsigned int inputDim, unsigned int outputDim )
	: _inputDim( inputDim ), _outputDim( outputDim ), _W( nullptr, 0, 0 ),
	_inputPort( this ) {}

	LinearRegressor( const LinearRegressor& other )
	: _inputDim( other._inputDim ), _outputDim( other._outputDim ),
	_params( other._params ), _W( other._W ), _inputPort( this ) {}

	void SetSource( SourceType* b ) { b->RegisterConsumer( &_inputPort ); }

	unsigned int InputDim() const { return _inputDim; }
	unsigned int OutputDim() const { return _outputDim; }

	Parameters::Ptr CreateParameters()
	{
		Parameters::Ptr params = std::make_shared<Parameters>();
		params->Initialize( (_inputDim + 1) * _outputDim );
		SetParameters( params );
		return params;
	}

	void SetParameters( Parameters::Ptr params )
	{
		if( params->ParamDim() != (_inputDim+1) * _outputDim )
		{
			throw std::runtime_error( "LinearRegressor: Invalid parameter dimension." );
		}
		_params = params;
		new (&_W) Eigen::Map<const RowMatrixType>( params->GetParamsVec().data(),
		                                        _outputDim, _inputDim + 1 );
	}

	void SetOffsets( const VectorType& off )
	{
		RowMatrixType p( _W );
		if( off.size() != p.rows() )
		{
			throw std::runtime_error( "LinearRegressor: Offsets invalid dimension." );
		}
		p.col( p.cols() - 1 ) = off;
		Eigen::Map<VectorType> v( p.data(), p.size(), 1 );
		_params->SetParamsVec( v );
		new (&_W) Eigen::Map<const RowMatrixType>( _params->GetParamsVec().data(),
		                                              _outputDim, _inputDim + 1 );
	}

	virtual void Foreprop()
	{
		VectorType out = _W * _inputPort.GetInput().homogeneous();
		SourceType::SetOutput( out );
		SourceType::Foreprop();
	}

	virtual void BackpropImplementation( const MatrixType& nextDodx )
	{
		MatrixType nDodx;
		if( nextDodx.size() == 0 )
		{
			nDodx = MatrixType::Identity( OutputDim(), OutputDim() );
		}
		else
		{
			nDodx = nextDodx;
		}

		if( nDodx.cols() != OutputDim() )
		{
			throw std::runtime_error( "LinearRegressor: Backprop dim error!" );
		}

		const VectorType& input = _inputPort.GetInput();
		MatrixType thisDodw = MatrixType::Zero( nDodx.rows(), ParamDim() );
		for( unsigned int i = 0; i < nDodx.rows(); i++ )
		{
			for( unsigned int j = 0; j < OutputDim(); j++ )
			{
				thisDodw.block(i, j*(InputDim()+1), 1, InputDim()+1) =
					nDodx(i,j) * input.homogeneous().transpose();
			}
		}
		MatrixType thisDodx = nDodx * _W;

		if( !SourceType::modName.empty() )
		{
			std::cout << SourceType::modName << " dodx: " << nDodx << std::endl << "dodw: " << thisDodw << std::endl;
			std::cout << "acc dodw: " << _params->GetDerivs() << std::endl;
			std::cout << "input: " << input.transpose() << std::endl;
			std::cout << "param dim: " << ParamDim() << std::endl;
		}

		_params->AccumulateDerivs( thisDodw );
		_inputPort.Backprop( thisDodx );
	}

	virtual unsigned int ParamDim() const { return _W.size(); }

	const Eigen::Map<const RowMatrixType>& GetWeights() const { return _W; }

private:

	friend std::ostream& operator<<( std::ostream& os, const LinearRegressor& lreg );

	unsigned int _inputDim;
	unsigned int _outputDim;
	Parameters::Ptr _params;
	Eigen::Map<const RowMatrixType> _W; // The weight vector
	Sink<VectorType> _inputPort;

};

inline
std::ostream& operator<<( std::ostream& os, const LinearRegressor& lreg )
{
	os << lreg._W;
	return os;
}

}


// Boost.Geometry
// QuickBook Example

// Copyright (c) 2019 Barend Gehrels, Amsterdam, the Netherlands.

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

//[buffer_geographic_point_circle
//` Shows how the point_circle strategy, for the Earth, can be used as a PointStrategy to create circular buffers around points

#include <boost/geometry.hpp>
#include <iostream>

int main()
{
    namespace bg = boost::geometry;

    typedef bg::model::point<double, 2, bg::cs::geographic<bg::degree> > point;

    // Declare the geographic_point_circle strategy (with 36 points)
    // Default template arguments (taking Andoyer strategy)
    bg::strategy::buffer::geographic_point_circle<> point_strategy(36);

    // Declare the distance strategy (one kilometer, around the point, on Earth)
    bg::strategy::buffer::distance_symmetric<double> distance_strategy(1000.0);

    // Declare other necessary strategies, unused for point
    bg::strategy::buffer::join_round join_strategy;
    bg::strategy::buffer::end_round end_strategy;
    bg::strategy::buffer::side_straight side_strategy;

    // Declare/fill a point on Earth, near Amsterdam
    point p;
    bg::read_wkt("POINT(4.9 52.1)", p);

    // Create the buffer of a point on the Earth
    bg::model::multi_polygon<bg::model::polygon<point> > result;
    bg::buffer(p, result,
                distance_strategy, side_strategy,
                join_strategy, end_strategy, point_strategy);

    std::cout << "Area: " << bg::area(result) / (1000 * 1000) << " square kilometer" << std::endl;

    return 0;
}

//]

//[buffer_geographic_point_circle_output
/*`
Output:
[pre
Area: 3.12542 square kilometer
]
*/
//]


/**
 * @copyright   Copyright (c) 2021, Swift Engineering Inc.
 * @license     Licensed under the MIT license. See LICENSE for details.
 */
#include <gtest/gtest.h>

#include <string>
#include <sstream>
#include <boost/array.hpp>
#include <stdio.h>
#include <ignition/math/Pose3.hh>
#include <ignition/math/Vector3.hh>

#include "Math_util.hpp"

const unsigned int EQUALS = 1;
const unsigned int LESS_THAN = 2;
const unsigned int GREATER_THAN = 3;
const unsigned int LESS_THAN_EQ = 4;
const unsigned int GREATER_THAN_EQ = 5;

class MathUtilTest : public ::testing::Test {
  protected:
    avionics_sim::Math_util mathUtil;
    double tolerance = 0.00001;
};

TEST_F(MathUtilTest, TestCalculateMean) {
    // Given: Range of Values
    std::vector<double> samples = {2.0, -10, 99, -100000};
    double expectedMean = -24977.25;

    // When: Dynamic Pressure is Calculated
    double resultantMean = mathUtil.calculate_mean(samples.begin(), samples.end(),
                           samples.size());

    // Then: Dynamic Pressure should match expected
    ASSERT_NEAR(resultantMean, expectedMean, tolerance);
}

TEST_F(MathUtilTest, TestCalculateRMS) {
    // Given: Range of Values
    std::vector<double> samples = {2.0, -10, 99, -100000};
    double expectedRMS = 50000.02476;

    // When: Dynamic Pressure is Calculated
    double resultantRMS = mathUtil.calculate_rms(samples.begin(), samples.end(),
                          samples.size());

    // Then: Dynamic Pressure should match expected
    ASSERT_NEAR(resultantRMS, expectedRMS, tolerance);
}

// TEST_F(MathUtilTest, TestCalculateFPComparison) {
//   // Given: Planar Velocity and Air Density
//   double lhs;
//   double rhs;
//   unsigned int operation;
//   bool testValue;

//   MathUtilFPComparisonParams param = GetParam();
//   lhs = param.lhs;
//   rhs = param.rhs;
//   operation = param.operation;
//   testValue = param.testValue;

//   avionics_sim::Math_util mu;
//   double result;

//   switch (operation) {
//   case EQUALS:
//     result = mu.rough_eq(lhs, rhs);
//     break;

//   case LESS_THAN:
//     result = mu.rough_lt(lhs, rhs);
//     break;

//   case LESS_THAN_EQ:
//     result = mu.rough_lte(lhs, rhs);
//     break;

//   case GREATER_THAN:
//     result = mu.rough_gt(lhs, rhs);
//     break;

//   case GREATER_THAN_EQ:
//     result = mu.rough_gte(lhs, rhs);
//     break;
//   }

//   ASSERT_EQ(result, testValue);
// }

TEST_F(MathUtilTest, TestLinearMap) {
    // Given:
    double x = 4.651;
    double in_min = 4.0;
    double in_max = 5.0;
    double out_min = 0.44;
    double out_max = 0.55;
    double expectedValue = 0.511614;

    // When:
    double result = mathUtil.linear_map(x, in_min, in_max, out_min, out_max);

    // Then:
    ASSERT_NEAR(result, expectedValue, tolerance);
}

// TODO(Mike Lyons): Figure out why this has a hold up in the test
// TEST_F(MathUtilTest, TestPowInteger) {
//   // Given:
//   double base = 5;
//   size_t power = -2;
//   double expectedValue = 0.04;

//   // When:
//   double result = mathUtil.pow_integer(base, power);

//   // Then
//   ASSERT_NEAR(result, expectedValue, tolerance);
// }


/***********************************************************************
This file is part of the librjmcmc project source files.

Copyright : Institut Geographique National (2008-2012)
Contributors : Mathieu Brédif, Olivier Tournaire, Didier Boldo
email : librjmcmc@ign.fr

This software is a generic C++ library for stochastic optimization.

This software is governed by the CeCILL license under French law and
abiding by the rules of distribution of free software. You can use,
modify and/or redistribute the software under the terms of the CeCILL
license as circulated by CEA, CNRS and INRIA at the following URL
"http://www.cecill.info".

As a counterpart to the access to the source code and rights to copy,
modify and redistribute granted by the license, users are provided only
with a limited warranty and the software's author, the holder of the
economic rights, and the successive licensors have only limited liability.

In this respect, the user's attention is drawn to the risks associated
with loading, using, modifying and/or developing or reproducing the
software by the user in light of its specific status of free software,
that may mean that it is complicated to manipulate, and that also
therefore means that it is reserved for developers and experienced
professionals having in-depth computer knowledge. Users are therefore
encouraged to load and test the software's suitability as regards their
requirements in conditions enabling the security of their systems and/or
data to be ensured and, more generally, to use and operate it in the
same conditions as regards security.

The fact that you are presently reading this means that you have had
knowledge of the CeCILL license and that you accept its terms.

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

#ifndef EVEN_NUMBERED_ORDER_STATISTICS_UNIFORM_DISTRIBUTION_HPP
#define EVEN_NUMBERED_ORDER_STATISTICS_UNIFORM_DISTRIBUTION_HPP

#include <boost/random/uniform_real_distribution.hpp>
// boost::math::uniform is not used as it is linked to real values rather than discrete integral values

namespace rjmcmc {

    // 1_(n\in[a,b])/(b-a+1)
    class even_numbered_order_statistics_uniform_distribution {
    public:
        typedef double real_type;
        typedef int    int_type;
        typedef boost::random::uniform_real_distribution<real_type> rand_distribution_type;

        even_numbered_order_statistics_uniform_distribution(int_type k , real_type a, real_type b)
            : m_rand(a,b)
            , m_size(k) {}

        // new/old
        template<typename Iterator> real_type pdf_ratio(Iterator begin0, Iterator end0, Iterator begin1, Iterator end1) const
        {
            return pdf(begin1,end1)/pdf(begin0,end0);
        }

        template<typename Iterator> real_type pdf(Iterator begin, Iterator end) const
        {
            typedef typename std::iterator_traits<Iterator>::value_type T;
            const T a = m_rand.min();
            const T b = m_rand.max();
            const T s = b - a;
            T x = a;
            T res(1);
            int i = 2;
            for(Iterator it=begin; it != end; ++it, i+=2)
            {
                const T y = *it;
                if(y<x) return 0;
                res*=(y-x)*i*(i+1)/(s*s);
                x=y;
            }
            if(b<x) return 0;
            res *= (b - x) / s;
            return res;
        }

        template<typename Engine, typename Iterator>
        inline void operator()(Engine& e, Iterator out) const {
            std::vector<real_type> v(2*m_size+1);
            for(std::vector<real_type>::iterator it = v.begin(); it!=v.end(); ++it)
                *it = m_rand(e);
            std::sort(v.begin(),v.end());
            for(std::vector<real_type>::iterator it = ++v.begin(); it!=v.end(); it+=2)
                *out++ = *it;
        }

    private:
        mutable rand_distribution_type m_rand;
        int_type m_size;
    };

}; // namespace rjmcmc

#endif // EVEN_NUMBERED_ORDER_STATISTICS_UNIFORM_DISTRIBUTION_HPP


#ifndef FVMSCALAR1D_RECONSTRUCTION_HPP
#define FVMSCALAR1D_RECONSTRUCTION_HPP

#include <Eigen/Dense>
#include <cmath>
#include <memory>

#include <ancse/grid.hpp>
#include <ancse/model.hpp>
#include <ancse/rate_of_change.hpp>
#include <ancse/simulation_time.hpp>

inline double sign(double a) { return copysign(1.0, a); }

//// ANCSE_CUT_START_TEMPLATE
//----------------SlopeLimiterABegin----------------
inline double minmod(double a, double b) {
    return 0.5 * (sign(a) + sign(b)) * std::min(std::abs(a), std::abs(b));
}

inline double maxmod(double a, double b) {
    return 0.5 * (sign(a) + sign(b)) * std::max(std::abs(a), std::abs(b));
}

inline double minmod(double a, double b, double c) {
    return minmod(a, minmod(b, c));
}

inline double minabs(double a, double b) {
    double tolerance = 1e-10;
    if (std::abs( std::abs(a) - std::abs(b) ) < tolerance)
        return 0.5*(a+b);
    return std::abs(a) < std::abs(b) ? a : b;
}
//----------------SlopeLimiterAEnd----------------

//----------------SlopeLimiterBBegin----------------
struct MinMod {
    inline double operator()(double sL, double sR) const {
        return minmod(sL, sR);
    }
};
//----------------SlopeLimiterBEnd----------------

//----------------SlopeLimiterCBegin----------------
struct MinAbs {
    inline double operator()(double sL, double sR) const {
        return minabs(sL, sR);
    }
};

struct SuperBee {
    inline double operator()(double sL, double sR) const {
        double A = minmod(2.0 * sL, sR);
        double B = minmod(sL, 2.0 * sR);

        return maxmod(A, B);
    }
};

struct VanLeer {
    inline double operator()(double sL, double sR) const {
        double r = sL / (sR + eps);
        return (r + std::abs(r)) / (1 + std::abs(r)) * sR;
    }

  private:
    double eps = 1e-10;
};

struct MonotonizedCentral {
    inline double operator()(double sL, double sR) const {
        return minmod(2.0 * sL, 0.5 * (sL + sR), 2.0 * sR);
    }
};

struct Unlimited {
    inline double operator()(double a, double b) const { return 0.5 * (a + b); }
};
//----------------SlopeLimiterCEnd----------------
//// ANCSE_END_TEMPLATE

class PWConstantReconstruction {
  public:
    /// Compute the left and right trace at the interface i + 1/2.
    /** Note: This API is agnostic to the number of cell-averages required
     *        by the method. Therefore, reconstructions with different stencil
     *        sizes can implement this API; and this call can be used in parts
     *        of the code that do not need to know about the details of the
     *        reconstruction.
     */
    inline std::pair<double, double> operator()(const Eigen::VectorXd &u,
                                                int i) const {
        return (*this)(u[i], u[i + 1]);
    }

    /// Compute the left and right trace at the interface.
    /** Piecewise constant reconstruction of the left and right trace only
     *  requires the cell-average to the left and right of the interface.
     *
     *  Note: Compared to the other overload this reduces the assumption on
     *        how the cell-averages are stored. This is useful when testing and
     *        generally makes the function useful in more situations.
     */
    inline std::pair<double, double> operator()(double ua, double ub) const {
        return {ua, ub};
    }
};

//// ANCSE_CUT_START_TEMPLATE
//----------------LinearRCBegin----------------
template <class SlopeLimiter>
class PWLinearReconstruction {
  public:
    explicit PWLinearReconstruction(const SlopeLimiter &slope_limiter)
        : slope_limiter(slope_limiter) {}

    std::pair<double, double> operator()(const Eigen::VectorXd &u,
                                         int i) const {
        return (*this)(u[i - 1], u[i], u[i + 1], u[i + 2]);
    }

    std::pair<double, double>
    operator()(double ua, double ub, double uc, double ud) const {
        double sL = ub - ua;
        double sM = uc - ub;
        double sR = ud - uc;

        double uL = ub + 0.5 * slope_limiter(sL, sM);
        double uR = uc - 0.5 * slope_limiter(sM, sR);

        return {uL, uR};
    }

  private:
    SlopeLimiter slope_limiter;
};
//----------------LinearRCEnd----------------
//// ANCSE_END_TEMPLATE

#endif // FVMSCALAR1D_RATE_OF_CHANGE_HPP


#include<iostream>
#include<fstream>
#include<string>
#include<algorithm>
#include<list>
#include<stdexcept>
#include<functional>

#include <boost/program_options.hpp>
#include <boost/tokenizer.hpp>
#include <boost/algorithm/string/predicate.hpp>
#include <boost/lexical_cast.hpp>

#include <Interp.hpp>

#include "Integrate.hpp"

using namespace std;
namespace po = boost::program_options;

void print_version()
{
  cout<<"integrate-cli - linked against libIntegrate version "<< libIntegrate_VERSION_FULL << endl;
}

void print_usage(char prog_name[])
{

  cout<<"usage: "<<prog_name<<" [OPTIONS]"<<"\n";

}

void print_documentation( )
{
  cout<<"Reads function from a file an integrates it."
      <<"\n"
      <<"The data is contained in a gnuplot-style text file, with each x-y pair on a new line, separated by white space."
      <<"\n";
}

std::function<double(std::vector<double>&,std::vector<double>&)> create(std::string type)
{
  if( boost::starts_with("riemann",type) )
    return _1D::RiemannRule<double>();

  if( boost::starts_with("trapezoid",type) )
    return _1D::TrapezoidRule<double>();

  if( boost::starts_with("simpson",type) )
    return [](std::vector<double>& x, std::vector<double>& y){
      // create an interpolator to pass to the integrator
      _1D::CubicSplineInterpolator<double> interp(x,y);

      double min = *std::min_element( x.begin(), x.end() );
      double max = *std::max_element( x.begin(), x.end() );

      _1D::SimpsonRule<double> integ;

      return integ( interp, min, max, x.size() );
    };

  if( boost::starts_with("gauss-legendre",type) )
    return [](std::vector<double>& x, std::vector<double>& y){
      // create an interpolator to pass to the integrator
      _1D::CubicSplineInterpolator<double> interp(x,y);

      double min = *std::min_element( x.begin(), x.end() );
      double max = *std::max_element( x.begin(), x.end() );

      // we have quadratures of order
      // 8, 16, 32, and 64.
      if( x.size() <= 8 )
      {
        _1D::GQ::GaussLegendreQuadrature<double,8> integ;
        return integ( interp, min, max );
      }
      if( x.size() <= 16 )
      {
        _1D::GQ::GaussLegendreQuadrature<double,16> integ;
        return integ( interp, min, max );
      }
      if( x.size() <= 32 )
      {
        _1D::GQ::GaussLegendreQuadrature<double,32> integ;
        return integ( interp, min, max );
      }

      _1D::GQ::GaussLegendreQuadrature<double,64> integ;
      return integ( interp, min, max );
    };

  return nullptr;
}

void ReadFunction(std::istream &_in, double *&_x, double *&_y, int &_n, int _dimensions = 1, int _multiplicity = 1);

int main( int argc, char* argv[])
{
    po::options_description options("Allowed options");
    options.add_options()
      ("help,h"      , "print help message")
      ("batch,b"     , "output in 'batch' mode")
      ("method,m"    , po::value<string>()->default_value("riemann"),     "integration method.")
      ("list,l"      ,                                                   "list available integration methods.")
      ("integrate-data" ,                                                "file containing data to be integrated.")
      ;

    po::positional_options_description args;
    args.add("integrate-data", 1);

    po::variables_map vm;
    po::store(po::command_line_parser(argc, argv).options(options).positional(args).run(), vm);
    po::notify(vm);

    if (argc == 1 || vm.count("help"))
    {
      print_version();
      print_usage( argv[0] );
      cout<<"\n";
      cout << options<< "\n";
      cout<<"\n";
      print_documentation();
      cout<<"\n";
      return 1;
    }

    if (vm.count("list"))
    {
      print_version();
      cout<<"\t'riemann' : simple riemann sum\n";
      cout<<"\t'trapezoid' : trapezoid rule\n";
      cout<<"\t'simpson' : simposon's rule (uses interpolation)\n";
      cout<<"\t'gauss-legendre' : Gauss-Legendre interpolation (uses interpolation)\n";
      return 1;
    }

    ifstream in;
    double *x, *y, sum;
    int n;

    // load data
    in.open( vm["integrate-data"].as<string>().c_str() );
    if( !in.is_open() )
    {
      cerr << "ERROR: could not open file: "<<vm["integrate-data"].as<string>()<< endl;
      return 0;
    }

    ReadFunction( in, x, y, n );
    in.close();

    std::function<double(std::vector<double>&,std::vector<double>&)> interp;
    interp = create(vm["method"].as<string>());
    if( !interp )
    {
      delete[] x;
      delete[] y;
      cerr << "ERROR: Unrecognized interpolation method (" << vm["method"].as<string>()<< ")." << endl;
      return 0;
    }

    std::vector<double> X(x,x+n),Y(y,y+n);

    delete[] x;
    delete[] y;

    sum = interp(X,Y);

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

    return 0;
}

void ReadFunction(std::istream &_in, double  *&_x, double *&_y, int *&_n, int _dimensions, int _multiplicity)
{

  double *xbuffer1, *xbuffer2;
  double *ybuffer1, *ybuffer2;

  //////////////////////////////
  //  READ IN DATA FROM FILE  //
  //////////////////////////////

  const int chunck = 1000;
  int buffsize,buffsize_old;
  int i,j;
  int n;

  std::string line;

  buffsize = chunck;

  xbuffer1 = new double[buffsize*_dimensions  ];
  ybuffer1 = new double[buffsize*_multiplicity];

  i = 0;
  while( getline( _in, line ) )
  {
    // if line is blank, skip it
    if( line == "" )
      continue;
    std::vector<std::string> columns;
    boost::char_separator<char> sep(" \t");
    boost::tokenizer<boost::char_separator<char>> toks(line,sep);
    for( auto it = toks.begin(); it != toks.end(); ++it )
      columns.push_back( *it );

    // if first column starts with a #, this line is a comment
    // and we should skip it.
    if( columns.size() > 0 && columns[0].find('#') == 0 )
      continue;

    if(i == buffsize - 1)  // this "grows" the buffer
    {
      buffsize_old = buffsize;
      buffsize = buffsize + chunck;

      xbuffer2 = new double[buffsize*_dimensions  ];
      std::copy( xbuffer1, xbuffer1 + buffsize_old*_dimensions, xbuffer2);
      delete[] xbuffer1;

      ybuffer2 = new double[buffsize*_multiplicity];
      std::copy( ybuffer1, ybuffer1 + buffsize_old*_multiplicity, ybuffer2);
      delete[] ybuffer1;

      xbuffer1 = xbuffer2;
      ybuffer1 = ybuffer2;
    }

    for(j = 0; j < _dimensions; j++)
      xbuffer1[i*_dimensions + j]   = boost::lexical_cast<double>(columns[j]);
    for(j = 0; j < _multiplicity; j++)
      ybuffer1[i*_multiplicity + j] = boost::lexical_cast<double>(columns[j+_dimensions]);

    i++;
  }

  n = i;

  /** \todo add actual multi-coordinate support */

  _n = new int[_dimensions];
  _x = new double[n * _dimensions  ];
  _y = new double[n * _multiplicity];

  for(j = 0; j < n * _dimensions  ; j++)
    _x[j] = xbuffer1[j];
  for(j = 0; j < n * _multiplicity; j++)
    _y[j] = ybuffer1[j];
  for(j = 0; j < _dimensions; j++)
    _n[j] = n;

  delete[] xbuffer1;
  delete[] ybuffer1;

}

void ReadFunction(std::istream &_in, double *&_x, double *&_y, int &_n, int _dimensions, int _multiplicity)
{
  int *n;
  ReadFunction(_in, _x, _y, n, _dimensions, _multiplicity);
  _n = n[0];
  delete[] n;
}


#include <math.h>
#include <random>
#include <boost/random.hpp>
#include <boost/math/distributions.hpp>
boost::random::mt19937 gen(time(0));

//distributions
double runif(double lower, double higher)
{
	boost::random::uniform_real_distribution<> dist(lower, higher);
	return dist(gen);
}

double rnorm(double mean, double sd)
{
	boost::random::normal_distribution<> dist(mean, sd);
	return dist(gen);
}

double rbeta(double alpha, double beta)
{

	boost::math::beta_distribution<> dist(alpha, beta);
	double q = quantile(dist, runif(0,1));

	return(q);
}

double rinvchisq(double df, double scale)
{

	boost::math::inverse_chi_squared_distribution<> dist(df, scale);
	double q = quantile(dist, runif(0,1));

	return(q);
}
int rbernoulli(double p)
{
	std::bernoulli_distribution dist(p);
	return dist(gen);
}

int rbinom(int k, double p)
{

	std::binomial_distribution<> dist(k,p);
	return dist(gen);
}


//
// Created by yongqi on 17-12-4.
//

#ifndef VFORCE_MATHUTILS_HPP
#define VFORCE_MATHUTILS_HPP

#include <cmath>
#include <Eigen/Eigen>

namespace VForce {
namespace Utils {
/**
 * make angle between [-pi, pi)
 * @param x input angle in radius
 * @return normalized angle
 */
inline double normalize_angle(double x) {
  x = fmod(x + M_PI, 2 * M_PI);
  return x < 0 ? x += 2 * M_PI : x - M_PI;
}

inline float normalize_anlge(float x) {
  x = fmod(x + M_PI, 2 * M_PI);
  return x < 0 ? x += 2 * M_PI : x - M_PI;
}

/**
 * radian to degree
 * @tparam T
 * @param x
 * @return
 */
template<typename T>
inline T rad2deg(T x) {
  return x / M_PI * 180.;
}

/**
 * degree to radian
 * @tparam T
 * @param x
 * @return
 */
template<typename T>
inline T deg2rad(T x) {
  return x * M_PI / 180.;
}

/**
 * Convert pose(in Rz(yaw)Ry(pitch)Rx(roll) order) into transformation matrix
 * @tparam T
 * @param x
 * @param y
 * @param z
 * @param roll
 * @param pitch
 * @param yaw
 * @param tf
 */
template<typename T>
void pose2matrix(const T &x, const T &y, const T &z,
                 const T &roll, const T &pitch, const T &yaw,
                 Eigen::Matrix<T, 4, 4> &t) {
  T A = cos(yaw),  B = sin(yaw),  C = cos(pitch), D = sin (pitch),
      E = cos(roll), F = sin(roll), DE = D*E,       DF = D*F;

  t(0, 0) = A*C;  t(0, 1) = A*DF - B*E;  t(0, 2) = B*F + A*DE;  t(0, 3) = x;
  t(1, 0) = B*C;  t(1, 1) = A*E + B*DF;  t(1, 2) = B*DE - A*F;  t(1, 3) = y;
  t(2, 0) = -D;   t(2, 1) = C*F;         t(2, 2) = C*E;         t(2, 3) = z;
  t(3, 0) = 0;    t(3, 1) = 0;           t(3, 2) = 0;           t(3, 3) = 1;
}

/**
 * Convert transformation matrix into pose
 * @tparam T
 * @param matrix
 * @param x
 * @param y
 * @param z
 * @param roll
 * @param pitch
 * @param yaw
 */
template<typename T>
void matrix2pose(const Eigen::Matrix<T, 4, 4> &t,
                 T &x, T &y, T &z,
                 T &roll, T &pitch, T &yaw) {
  x = t(0, 3);
  y = t(1, 3);
  z = t(2, 3);
  roll = atan2(t(2, 1), t(2, 2));
  pitch = atan2(-t(2, 0), sqrt(t(2, 1) * t(2, 1) + t(2, 2) * t(2, 2)));
  yaw = atan2(t(1, 0), t(0, 0));
}

}
}

#endif //VFORCE_MATHUTILS_HPP


/**
 * gaussian.hpp v1.0.0
 *
 * Contains methods related to Gaussian distributions and smoothing.
 * Methods for curve fitting are in gaussianfit.cpp.
 */

#pragma once

#define SQRT2PI 2.506628275

#include <cmath>

#define  ARMA_DONT_USE_WRAPPER
#define  ARMA_USE_LAPACK
#define  ARMA_DONT_PRINT_ERRORS
#include <armadillo>

namespace bib {

  namespace gaussian {

    const double M_SQRT_2OVERPI = std::sqrt(2.0 / M_PI);
    const double M_SQRT_2OVERPI_CUBED = std::pow(M_SQRT_2OVERPI, 3.0);

    /**
     * Represents a Gaussian distribution. Comparison functions only
     * compare amplitude (used for sorting from most to least important
     * when curve fitting).
     *
     * amplitude: suqare root of the amplitude of the distribution.
     * mean: center point of the distribution.
     * stdev: standard deviation of the distribution.
     */
    struct Parameters {

      double amplitude;
      double mean;
      double stdev;

      bool operator >(const Parameters& a) const {
	return amplitude * amplitude / stdev > a.amplitude * a.amplitude / a.stdev;
      }

      bool operator <(const Parameters& a) const {
	return amplitude * amplitude / stdev < a.amplitude * a.amplitude / a.stdev;
      }

    };

    /**
     * Represents a Gaussian distribution with shape (adds skewness).
     * Comparison functions only compare amplitude (used for sorting
     * from most to least important when curve fitting).
     *
     * shape: shape parameter (alpha) capturing skewness.
     * amplitude: suqare root of the amplitude of the distribution.
     * mean: center point of the distribution.
     * stdev: standard deviation of the distribution.
     */
    struct SkewParameters {

      double amplitude;
      double mean;
      double stdev;
      double shape;

      bool operator >(const SkewParameters& a) const {
	return amplitude * amplitude / stdev > a.amplitude * a.amplitude / a.stdev;
      }

      bool operator <(const SkewParameters& a) const {
	return amplitude * amplitude / stdev < a.amplitude * a.amplitude / a.stdev;
      }

    };

    /**
     * Returns the value of a Gaussian distribution at a given point x.
     *
     * amplitude: suqare root of the amplitude of the distribution.
     * mean: center point of the distribution.
     * stdev: standard deviation of the distribution.
     * x: the point at which to sample the distribution.
     */
    inline double curveValue(double amplitude, double mean, double stdev, double x) {
      return amplitude * amplitude / stdev * std::exp( -(x - mean) * (x - mean) / stdev / stdev / 2.0 );
    }

    /**
     * Returns the value of a Gaussian distribution with shape at a given point x.
     *
     * amplitude: suqare root of the amplitude of the distribution.
     * mean: center point of the distribution.
     * stdev: standard deviation of the distribution.
     * shape: shape parameter (alpha) capturing skewness.
     * x: the point at which to sample the distribution.
     */
    inline double curveValue(double amplitude, double mean, double stdev, double shape, double x) {
      return amplitude * amplitude / stdev * std::exp( -(x - mean) * (x - mean) / stdev / stdev / 2.0 )
	* (1.0 + std::erf(shape * (x - mean) / (stdev * M_SQRT2)));
    }

    /**
     * Returns the value of a Gaussian distribution with shape at a given point x.
     *
     * distribution: the distribution to sample.
     * x: the point at which to sample the distribution.
     */
    inline double curveValue(Parameters distribution, double x) {
      return curveValue(distribution.amplitude, distribution.mean, distribution.stdev, x);
    }

    /**
     * Returns the value of a Gaussian distribution with shape at a given point x.
     *
     * distribution: the distribution to sample.
     * x: the point at which to sample the distribution.
     */
    inline double curveValue(SkewParameters distribution, double x) {
      return curveValue(distribution.amplitude, distribution.mean, distribution.stdev,
			distribution.shape, x);
    }

    /**
     * Smooths the input vector using a Gaussian distribution of the given width.
     *
     * input: the input vector to smooth.
     * kernelWidth: standard deviation of the distribution to use for smoothing.
     */
    arma::Col<double> smooth(const arma::Col<double>& input, double kernelWidth);

    /**
     * Smooths the input vector using the second derivative of a Gaussian distribution
     * of the given width.
     *
     * input: the input vector to smooth.
     * kernelWidth: standard deviation of the distribution to use for smoothing.
     */
    arma::Col<double> scaleSpaceSmooth(const arma::Col<double>& input, double kernelWidth);

    std::vector<double> getDistribution(double A, double x, double u, size_t length);

    inline int sgn(double val) {
      return (0.0 < val) - (val < 0.0);
    }

    inline double skewMode(SkewParameters gaussian) {
      double delta = gaussian.shape / std::sqrt(1.0 + gaussian.shape * gaussian.shape);
      double uz = M_SQRT_2OVERPI * delta;
      double oz = std::sqrt(1.0 - uz * uz);
      double skewness = ((4.0 - M_PI) / 2.0)
	* ((M_SQRT_2OVERPI_CUBED * delta * delta * delta) / std::pow(1.0 - 2.0 * delta * delta / M_PI, 1.5));
      return (uz - skewness * oz / 2.0 - sgn(gaussian.shape) / 2.0 * std::exp(-2.0 * M_PI / std::abs(gaussian.shape)))
	* gaussian.stdev + gaussian.mean;
    }

  }

} // namespace bib::gaussian


// Boost.Geometry (aka GGL, Generic Geometry Library)
// QuickBook Example

// Copyright (c) 2011 Barend Gehrels, Amsterdam, the Netherlands.

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

//[intersection_ls_ls_point
//` Calculate the intersection of two linestrings

#include <iostream>
#include <deque>

#include <boost/geometry.hpp>
#include <boost/geometry/domains/gis/io/wkt/wkt.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/register/linestring.hpp>

#include <boost/foreach.hpp>

BOOST_GEOMETRY_REGISTER_LINESTRING_TEMPLATED(std::vector)

int main()
{
    typedef boost::geometry::model::d2::point_xy<double> P;
    std::vector<P> line1, line2;
    boost::geometry::read_wkt("linestring(1 1,2 2,3 1)", line1);
    boost::geometry::read_wkt("linestring(1 2,2 1,3 2)", line2);

    std::deque<P> intersection_points;
    boost::geometry::intersection(line1, line2, intersection_points);

    BOOST_FOREACH(P const& p, intersection_points)
    {
        std::cout << " " << boost::geometry::wkt(p);
    }
    std::cout << std::endl;

    return 0;
}

//]

//[intersection_ls_ls_point_output
/*`
Output:
[pre
 POINT(1.5 1.5) POINT(2.5 1.5)
]
*/
//]


#include "Gaussian.h"
#include <iostream>
#include <stdexcept>
#include <cmath>
#include "../Utils.h"
//#include <boost/math/special_functions/erf.hpp>

namespace DNest4
{

Gaussian::Gaussian(double center, double width)
:center(center)
,width(width)
{
    if(width <= 0.0)
        throw std::domain_error("Gaussian distribution must have positive width.");
}

double Gaussian::cdf(double x) const
{
    return normal_cdf((x-center)/width);
}

double Gaussian::cdf_inverse(double x) const
{
    if(x < 0.0 || x > 1.0)
        throw std::domain_error("Input to cdf_inverse must be in [0, 1].");
    return center + width*normal_inverse_cdf(x);
    //return center + width * sqrt(2) * boost::math::erf_inv(2*x - 1);
}

double Gaussian::log_pdf(double x) const
{
	double r = (x - center)/width;
    return -0.5*r*r - _norm_pdf_logC;
}

TruncatedGaussian::TruncatedGaussian(double center, double width, double lower, double upper)
:center(center)
,width(width)
,lower(lower)
,upper(upper)
{
    if(width <= 0.0)
        throw std::domain_error("TruncatedGaussian distribution must have positive width.");
    if(lower >= upper)
        throw std::domain_error("TruncatedGaussian: lower bound should be less than upper bound.");
    // the original, untruncated, Gaussian distribution
    unG = Gaussian(center, width);
    c = unG.cdf(upper) - unG.cdf(lower);
}

double TruncatedGaussian::cdf(double x) const
{
    double up = std::max(std::min(x,upper), lower);
    return (unG.cdf(up) - unG.cdf(lower)) / c;
}

double TruncatedGaussian::cdf_inverse(double x) const
{
    if(x < 0.0 || x > 1.0)
        throw std::domain_error("Input to cdf_inverse must be in [0, 1].");
    double xx = unG.cdf(lower) + x * c;
    return unG.cdf_inverse(xx);
}

double TruncatedGaussian::log_pdf(double x) const
{
    if(x<lower or x>upper) return -std::numeric_limits<double>::infinity();
    return unG.log_pdf(x) - log(c);
}

} // namespace DNest4


/*
MIT License

Copyright (c) 2020 kyawakyawa

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
*/

#include <iostream>
#include <utility>

#include "macros.h"

IGNORE_STRICT_WARNING_PUSH

#include <Eigen/Core>

IGNORE_STRICT_WARNING_POP

// reference http://takashiijiri.com/study/miscs/QRfactorization.htm

template <typename Real, int N>
Eigen::Matrix<Real, N, N> HouseholderTransformation(
    const Eigen::Matrix<Real, N, 1>& x, const Eigen::Matrix<Real, N, 1>& y) {
  static_assert(N >= 1);
  const auto aux = x - y;
  const Eigen::Matrix<Real, N, 1> u = aux.normalized();

  return Eigen::Matrix<Real, N, N>::Identity() - 2 * u * u.transpose();
}

template <typename Real>
Eigen::Matrix<Real, Eigen::Dynamic, Eigen::Dynamic> HouseholderTransformation(
    const Eigen::Matrix<Real, Eigen::Dynamic, 1>& x,
    const Eigen::Matrix<Real, Eigen::Dynamic, 1>& y) {
  assert(x.rows() >= 1 && x.rows() == y.rows());
  const auto aux = x - y;
  const Eigen::Matrix<Real, Eigen::Dynamic, 1> u = aux.normalized();

  return Eigen::Matrix<Real, Eigen::Dynamic, Eigen::Dynamic>::Identity(
             x.rows(), x.rows()) -
         2 * u * u.transpose();
}

template <typename Real, int N, int M>
std::pair<Eigen::Matrix<Real, N, N>, Eigen::Matrix<Real, N, M>>
QRDecompositionWithHouseholderTransformation(
    const Eigen::Matrix<Real, N, M>& input) {
  static_assert(M >= 1 && N >= M);
  Eigen::Matrix<Real, N, M> R = input;
  Eigen::Matrix<Real, N, N> Q = Eigen::Matrix<Real, N, N>::Identity();

  for (int k = 0; k < M - 1; ++k) {
    const Real sign = (R(k, k) >= Real(0) ? Real(1) : Real(-1));
    const int Mmk = M - k;

    const Eigen::Matrix<Real, Eigen::Dynamic, 1> x = R.block(k, k, Mmk, 1);
    // グラフィック用途(ローテーションとスケールの分離)の場合、
    // y(0,0)の符号は+で固定したほうがいい(この値がRの対角要素になる)
    Eigen::Matrix<Real, Eigen::Dynamic, 1> y =
        Eigen::Matrix<Real, Eigen::Dynamic, 1>::Zero(Mmk, 1);
    const Real norm = x.norm();
    // uが0 vectorになる場合
    // この場合はそもそも計算する必要がない
    if (std::abs(norm - std::abs(x[0])) <
        std::numeric_limits<Real>::epsilon()) {
      continue;
    }
    y[0] = -sign * x.norm();

    const Eigen::Matrix<Real, Eigen::Dynamic, 1> u = (x - y).normalized();

    R.bottomRightCorner(Mmk, Mmk) -=
        Real(2) * u * (u.transpose() * R.bottomRightCorner(Mmk, Mmk));
    //////////////////// Rのみ欲しい場合はここまでの計算で良い
    ///////////////////////

    if (k > 0) {
      Q.topRightCorner(k, Mmk) -=
          Real(2) * (Q.topRightCorner(k, Mmk) * u) * u.transpose();
    }
    Q.bottomRightCorner(Mmk, Mmk) -=
        Real(2) * (Q.bottomRightCorner(Mmk, Mmk) * u) * u.transpose();
  }

  return std::make_pair(Q, R);
}

#ifdef USE_STACK_TRACE_LOGGER
#include <glog/logging.h>
#endif

int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv) {
#ifdef USE_STACK_TRACE_LOGGER
  (void)argc;
  google::InitGoogleLogging(argv[0]);
  google::InstallFailureSignalHandler();
#endif

  const Eigen::Vector3d x = {1, 2, 3};
  const Eigen::Vector3d y = {3, 2, 1};

  std::cout << "----------HouseholderTransformation----------" << std::endl;

  std::cout << " x = (" << x.transpose() << ")^T\n y = (" << y.transpose()
            << ")^T\n"
            << std::endl;

  const Eigen::Matrix3d H = HouseholderTransformation(x, y);
  std::cout << " H = \n" << H << "\n" << std::endl;

  const Eigen::Vector3d Hx = H * x;
  const Eigen::Vector3d Hy = H * y;

  std::cout << " (Hx)^T = (" << Hx.transpose() << ")\n" << std::endl;
  std::cout << " (Hy)^T = (" << Hy.transpose() << ")\n" << std::endl;

  std::cout << " H*H^T = \n" << H * H.transpose() << "\n" << std::endl;

  std::cout << "---------- ----------" << std::endl;

  std::cout
      << "----------QRDecompositionWithHouseholderTransformation----------"
      << std::endl;
  Eigen::Matrix3d A;

  A(0, 0) = 1.;
  A(0, 1) = 4.;
  A(0, 2) = 9.;

  A(1, 0) = 16.;
  A(1, 1) = 25.;
  A(1, 2) = 36.;

  A(2, 0) = 49.;
  A(2, 1) = 64.;
  A(2, 2) = 81.;

  const auto result = QRDecompositionWithHouseholderTransformation(A);

  std::cout << " Q = \n" << result.first << "\n" << std::endl;
  std::cout << " R = \n" << result.second << "\n" << std::endl;

  std::cout << " QR = \n" << result.first * result.second << "\n" << std::endl;

  std::cout << " QQ^T = \n"
            << result.first * result.first.transpose() << "\n"
            << std::endl;

  std::cout << "---------- ----------" << std::endl;
  return 0;
}


/**
 * @file Interpolation.hpp
 * @author bwu
 * @brief Interpolation method implementation
 * @version 0.1
 * @date 2022-02-22
 */
#ifndef GENERIC_MATH_INTERPOLATION_HPP
#define GENERIC_MATH_INTERPOLATION_HPP
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/banded.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include "generic/common/Traits.hpp"
#include "MathUtility.hpp"
#include <vector>
namespace generic {
namespace math    {
using generic::common::float_type;
template <typename num_type>
class Interpolation
{
    using float_t = float_type<num_type>;
    using vector_t = boost::numeric::ublas::vector<float_t>;
    using band_matrix_t = boost::numeric::ublas::banded_matrix<float_t>;
public:
    enum class Method { Linear = 1, Cubic = 2};
    enum class BCType { FirstDeriv = 1, SecondDeriv = 2, NotAKnot = 3 };

    ///@brief constructs an interpolation object with method
    Interpolation(Method method = Method::Linear);
    /**
     * @brief constructs an interpolation object with parameters
     * @param x input samples of x
     * @param y input samples of y
     * @param method interpolation method
     * @note if Cubic method failed with input x, y samples, it will decay to Linear method
     * @param monotonic set whether curve monotonic when interpolation
     * @param left left boundary condition type
     * @param lValue left boundary condition initial value
     * @param right right boundary condition type
     * @param rValue right boundary condition inital value
     */
    Interpolation(const std::vector<num_type> & x, const std::vector<num_type> & y,
                    Method method, bool monotonic = false,
                    BCType left  = BCType::SecondDeriv, float_t lValue  = 0,
                    BCType right = BCType::SecondDeriv, float_t rValue = 0);

    ///@brief sets boundary conditions and inital values
    void SetBoundary(BCType left, BCType right, float_t lVal, float_t rVal);
    ///@brief sets input samples of x and y
    void SetSamples(const std::vector<num_type> & x, const std::vector<num_type> & y);

    ///@brief gets interpolated value of input x
    num_type operator() (num_type x) const { return Interpolate(x); }

private:
    void UpdateCoefficients();
    bool makeMonotonic();
    size_t FindClosest(num_type x) const;
    num_type Interpolate(num_type x) const;
    num_type Interpolate(num_type x, const num_integer_tag) const;
    float_t Interpolate(num_type x, const num_floating_tag) const;

private:
    Method m_method;
    bool m_monotonic;
    BCType m_left, m_right;
    float_t m_lVal, m_rVal;
    std::vector<num_type> m_x, m_y;
    float_t m_coef0 = 0;
    std::vector<float_t> m_coef1, m_coef2, m_coef3;
};

template <typename num_type>
inline Interpolation<num_type>::Interpolation(Method method)
 : m_method(method), m_monotonic(false), m_left(BCType::SecondDeriv), m_right(BCType::SecondDeriv), m_lVal(0), m_rVal(0)
{
}

template <typename num_type>
inline Interpolation<num_type>::Interpolation(const std::vector<num_type> & x, const std::vector<num_type> & y,
                                                Method method, bool monotonic,
                                                BCType left, float_t lValue, BCType right, float_t rValue)
 : m_method(method), m_monotonic(monotonic), m_left(left), m_right(right), m_lVal(lValue), m_rVal(rValue)
{
    SetSamples(x, y);
}

template <typename num_type>
inline void Interpolation<num_type>::SetBoundary(BCType left, BCType right, float_t lVal, float_t rVal)
{
    assert(m_x.size() == 0 && "should set boundary type before setting samples");
    m_left = left; m_right = right;
    m_lVal = lVal; m_rVal = rVal;
}

template <typename num_type>
inline void Interpolation<num_type>::SetSamples(const std::vector<num_type> & x, const std::vector<num_type> & y)
{
    const size_t size = x.size();
    assert(size > 0 && size == y.size());
    m_x = x; m_y = y;
    m_coef1.assign(size, 0); m_coef2.assign(size, 0), m_coef3.assign(size, 0);
    if(size < 2) return;
    if(size < 3) m_method = Method::Linear;
    if(Method::Linear == m_method){
        for(size_t i = 0, j = 1; i < size - 1; ++i, ++j)
            m_coef1[i] = float_t(m_y[j] - m_y[i]) / float_t(m_x[j] - m_x[i]);
        m_coef1[size - 1] = m_coef1[size - 2];
    }
    else if(Method::Cubic == m_method){
        size_t lower = (m_right == BCType::NotAKnot) ? 2 : 1;
        size_t upper = (m_left  == BCType::NotAKnot) ? 2 : 1;
        band_matrix_t m(size, size, lower, upper);
        vector_t rhs(size);
        for(size_t i = 0, j = 1, k = 2; k < size; ++i, ++j, ++k){
            m(j, i) = 1.0 / 3.0 * (m_x[j] - m_x[i]);
            m(j, j) = 2.0 / 3.0 * (m_x[k] - m_x[i]);
            m(j, k) = 1.0 / 3.0 * (m_x[k] - m_x[j]);
            rhs[j] = (m_y[k] - m_y[j]) / (m_x[k] - m_x[j]) - (m_y[j] - m_y[i]) / (m_x[j] - m_x[i]);
        }
        //BC
        if(m_left == BCType::FirstDeriv){
            m(0, 0) = 2.0 * (m_x[1] - m_x[0]);
            m(0, 1) = 1.0 * (m_x[1] - m_x[0]);
            rhs[0] = 3.0 * ((m_y[1] - m_y[0]) / (m_x[1] - m_x[0]) - m_lVal);
        }
        else if(m_left == BCType::SecondDeriv){
            m(0, 0) = 2.0;
            m(0, 1) = 0.0;
            rhs[0] = m_lVal;
        }
        else if(m_left == BCType::NotAKnot){
            m(0, 0) = - m_x[2] + m_x[1];
            m(0, 1) =   m_x[2] - m_x[0];
            m(0, 2) = - m_x[1] + m_x[0];
            rhs[0] = 0.0;
        }
        else { assert(false); }

        if(m_right == BCType::FirstDeriv){
            m(size - 1, size - 1) = 2.0 * (m_x[size - 1] - m_x[size - 2]);
            m(size - 1, size - 2) = 1.0 * (m_x[size - 1] - m_x[size - 2]);
            rhs[size - 1] = 3.0 * (m_rVal - (m_y[size - 1] - m_y[size - 2]) / (m_x[size - 1] - m_x[size - 2]));
        }
        else if(m_right == BCType::SecondDeriv){
            m(size - 1, size - 1) = 2.0;
            m(size - 1, size - 2) = 0.0;
            rhs[size - 1] = m_rVal;
        }
        else if(m_right == BCType::NotAKnot){
            m(size - 1, size - 3) = - m_x[size - 1] + m_x[size - 2];
            m(size - 1, size - 2) =   m_x[size - 1] - m_x[size - 3];
            m(size - 1, size - 1) = - m_x[size - 2] + m_x[size - 3];
        }
        else { assert(false); }

        boost::numeric::ublas::permutation_matrix<size_t> pm(size);

        //decay to linear method if fail at LU factorization
        try {
            lu_factorize(m, pm);
            lu_substitute(m, pm, rhs);
        }
        catch (...) {
            m_method = Method::Linear;
            SetSamples(x, y);
            return;
        }

        for(size_t i = 0; i < size; ++i)
            m_coef2[i] = rhs[i];

        for(size_t i = 0, j = 1; j < size; ++i, ++j) {
            m_coef1[i] = (m_y[j] - m_y[i]) / (m_x[j] - m_x[i]) - 1.0 / 3.0 * (2.0 * m_coef2[i] + m_coef2[j]) * (m_x[j] - m_x[i]);
            m_coef3[i] = 1.0 / 3.0 * (m_coef2[j] - m_coef2[i]) / (m_x[j] - m_x[i]);
        }

        float_t h = x[size - 1] - x[size - 2];
        m_coef3[size - 1] = 0.0;
        m_coef1[size - 1] = 3.0 * m_coef3[size - 2] * h * h + 2.0 * m_coef2[size - 2] * h + m_coef1[size - 2];
        if(m_right == BCType::FirstDeriv) m_coef2[size - 1] = 0.0;
    }
    m_coef0 = (m_left == BCType::FirstDeriv) ? 0 : m_coef3[0];

    if(!m_monotonic && size > 2) makeMonotonic();
}

template <typename num_type>
inline void Interpolation<num_type>::UpdateCoefficients()
{
    size_t size = m_coef1.size();
    for(size_t i = 0; i < size - 1; ++i){
        const float_t h = m_x[i + 1] - m_x[i];
        m_coef2[i] = (3.0 * (m_y[i + 1] - m_y[i]) / h - (2.0 * m_coef1[i] + m_coef1[i + 1])) / h;
        m_coef3[i] = ((m_coef1[i + 1] - m_coef1[i]) / (3.0 * h) - 2.0 / 3.0 * m_coef2[i]) / h;
    }
    m_coef0 = (m_left == BCType::FirstDeriv) ? 0 : m_coef3[0];
}

template <typename num_type>
inline bool Interpolation<num_type>::makeMonotonic()
{
    assert(m_x.size() > 2);
    bool modified = false;
    for(size_t i = 0; i < m_x.size(); ++i){
        size_t im1 = std::max(i - 1, size_t(0));
        size_t ip1 = std::min(i + 1, m_x.size() - 1);
        if((m_y[im1] <= m_y[i] && m_y[i] <= m_y[ip1] && m_coef1[i] < 0) ||
           (m_y[im1] >= m_y[i] && m_y[i] >= m_y[ip1] && m_coef1[i] > 0)) {
            modified = true;
            m_coef1[i] = 0;
        }
    }

    for(size_t i = 0; i < m_x.size() - 1; ++i){
        float_t avg = float_t(m_y[i + 1] - m_y[i]) / float_t(m_x[i + 1] - m_x[i]);
        if(EQ(avg, 0.0) && (!EQ(m_coef1[i], 0.0) || !EQ(m_coef1[i], 0.0))){
            modified = true;
            m_coef1[i] = 0;
            m_coef1[i + 1] = 0;
        }
        else if((GE(m_coef1[i], 0.0) && GE(m_coef1[i + 1], 0.0) && GE(avg, 0.0)) ||
                (LE(m_coef1[i], 0.0) && LE(m_coef1[i + 1], 0.0) && LE(avg, 0.0))) {
            float_t r = std::sqrt(m_coef1[i] * m_coef1[i] + m_coef1[i + 1] * m_coef1[i + 1]) / std::fabs(avg);
            if(r > 3.0){
                modified = true;
                m_coef1[i] *= (3.0 / r);
                m_coef1[i + 1] *= (3.0 / r);
            }
        }
    }

    if(modified == true){
        UpdateCoefficients();
        m_monotonic = true;
    }
    return modified;
}

template <typename num_type>
inline size_t Interpolation<num_type>::FindClosest(num_type x) const
{
    typename std::vector<num_type>::const_iterator iter;
    iter = std::upper_bound(m_x.begin(), m_x.end(), x);
    size_t index = std::max(int(iter - m_x.begin() - 1), 0);
    return index;
}

template <typename num_type>
inline num_type Interpolation<num_type>::Interpolate(num_type x) const
{
    return Interpolate(x, typename num_traits_float_or_int<num_type>::tag());
}

template <typename num_type>
inline num_type Interpolation<num_type>::Interpolate(num_type x, const num_integer_tag) const
{
    return num_type(std::round(Interpolate(x, num_floating_tag())));
}

template <typename num_type>
inline float_type<num_type> Interpolation<num_type>::Interpolate(num_type x, const num_floating_tag) const
{
    size_t n = m_x.size();
    size_t idx = FindClosest(x);
    float_t h = x - m_x[idx];
    if(x < m_x[0]) return (m_coef0 * h + m_coef1[0]) * h + m_y[0];
    else if(x > m_x[n - 1]) return (m_coef2[n-1] * h + m_coef1[n - 1]) * h + m_y[n - 1];
    else return ((m_coef3[idx] * h + m_coef2[idx]) * h + m_coef1[idx]) * h + m_y[idx];
}

}//namespace math
}//namespace generic
#endif//GENERIC_MATH_INTERPOLATION_HPP


// http://www.alglib.net/specialfunctions/incompletegamma.php
// http://www.boost.org/doc/libs/1_35_0/libs/math/doc/sf_and_dist/html/math_toolkit/special/sf_gamma/igamma_inv.html
// http://www.cs.washington.edu/research/projects/uns/F9/src/boost-1.39.0/libs/math/doc/sf_and_dist/html/math_toolkit/special/sf_gamma/igamma.html
// http://en.wikipedia.org/wiki/Gamma_distribution
// http://en.wikipedia.org/wiki/Incomplete_gamma_function
#ifndef qlex_math_gamma_distribution_h
#define qlex_math_gamma_distribution_h

#include <ql/errors.hpp>
#include <ql/types.hpp>
#include <functional>
#include <boost/math/distributions/gamma.hpp>
#include <boost/math/special_functions/gamma.hpp>

using namespace QuantLib;

namespace QLExtension {
	class InverseCumulativeGamma
		: public std::unary_function < Real, Real > {
	public:
		InverseCumulativeGamma(Real shape = 1.0,
			Real scale = 1.0);

		// in boost p is lower incomplete gamma function, q is upper incomplete gamma function
		// gamma_p(a,z) = p = gamma(a,z)/Gamma(a)
		Real operator()(Real x) const;

		boost::math::gamma_distribution<> dist_;
	};
}

#endif


#include "Tetrahedron.h"
#include <Ziran/CS/Util/Debug.h>
#include <Eigen/Dense>

namespace ZIRAN {

template <class T>
void barycentricWeights(
    const Vector<T, 3>& p,
    const Vector<T, 3>& a,
    const Vector<T, 3>& b,
    const Vector<T, 3>& c,
    const Vector<T, 3>& d,
    Vector<T, 4>& weights)
{
    Matrix<T, 3, 3> Dm;
    Dm.col(0) = a - d;
    Dm.col(1) = b - d;
    Dm.col(2) = c - d;

    Vector<T, 3> weights_for_abc = Dm.partialPivLu().solve(p - d);
    weights.template head<3>() = weights_for_abc;
    weights(3) = (T)1 - weights_for_abc.array().sum();
    ZIRAN_ASSERT((weights.array() >= (T)0).all(), weights.transpose());
    ZIRAN_ASSERT((weights.array() <= (T)1).all(), weights.transpose());
    ZIRAN_ASSERT(std::abs(weights.array().sum() - (T)1) < 1e-7, weights.transpose());

    Vector<T, 3> test_p = Vector<T, 3>::Constant((T)0);
    test_p += weights(0) * a;
    test_p += weights(1) * b;
    test_p += weights(2) * c;
    test_p += weights(3) * d;
    ZIRAN_ASSERT((p - test_p).squaredNorm() < 1e-7);
}

template void barycentricWeights<double>(Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 3, 1, 0, 3, 1> const&, Eigen::Matrix<double, 4, 1, 0, 4, 1>&);
template void barycentricWeights<float>(Eigen::Matrix<float, 3, 1, 0, 3, 1> const&, Eigen::Matrix<float, 3, 1, 0, 3, 1> const&, Eigen::Matrix<float, 3, 1, 0, 3, 1> const&, Eigen::Matrix<float, 3, 1, 0, 3, 1> const&, Eigen::Matrix<float, 3, 1, 0, 3, 1> const&, Eigen::Matrix<float, 4, 1, 0, 4, 1>&);
} // namespace ZIRAN


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

#include <boost/math/bindings/e_float.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/multiprecision/number.hpp>

// cd C:\Users\User\Documents\Ks\PC_Software\NumericalPrograms\ExtendedNumberTypes\e_float\libs\e_float\build

// g++ -Wall -Wextra -m64 -O3 -std=gnu++11 -DE_FLOAT_TYPE_EFX -I../../../libs/e_float/src -IC:/boost/modular_boost/boost/libs/multiprecision/include -IC:/boost/modular_boost/boost/libs/math/include -IC:/boost/boost_1_75_0 ../src/e_float/efx/e_float_efx.cpp ../src/e_float/e_float.cpp ../src/e_float/e_float_base.cpp ../src/functions/constants/constants.cpp ../src/functions/elementary/elementary_complex.cpp ../src/functions/elementary/elementary_hyper_g.cpp ../src/functions/elementary/elementary_math.cpp ../src/functions/elementary/elementary_trans.cpp ../src/functions/elementary/elementary_trig.cpp ../test_boost/test_boost.cpp -o test_boost.exe

namespace local
{
  using big_float_type = boost::multiprecision::number<boost::math::ef::e_float, boost::multiprecision::et_off>;

  template<typename FloatingPointType>
  FloatingPointType hypergeometric_2f1(const FloatingPointType& a,
                                       const FloatingPointType& b,
                                       const FloatingPointType& c,
                                       const FloatingPointType& x)
  {
    // Compute the series representation of hypergeometric_2f1
    // taken from Abramowitz and Stegun 15.1.1.
    // There are no checks on input range or parameter boundaries.

    FloatingPointType x_pow_n_div_n_fact(x);
    FloatingPointType pochham_a         (a);
    FloatingPointType pochham_b         (b);
    FloatingPointType pochham_c         (c);
    FloatingPointType ap                (a);
    FloatingPointType bp                (b);
    FloatingPointType cp                (c);

    FloatingPointType h2f1 = 1 + (((pochham_a * pochham_b) / pochham_c) * x_pow_n_div_n_fact);

    const FloatingPointType tol = std::numeric_limits<FloatingPointType>::epsilon() * x;

    // Series expansion of hyperg_2f1(a, b; c; x).
    for(std::int32_t n = INT32_C(2); n < INT32_C(100000); ++n)
    {
      x_pow_n_div_n_fact *= x;
      x_pow_n_div_n_fact /= n;

      pochham_a *= ++ap;
      pochham_b *= ++bp;
      pochham_c *= ++cp;

      const FloatingPointType term = ((pochham_a * pochham_b) / pochham_c) * x_pow_n_div_n_fact;

      using std::fabs;

      if((n > 11) && (fabs(term) < tol))
      {
        break;
      }

      h2f1 += term;
    }

    return h2f1;
  }

  template<typename FloatingPointType>
  FloatingPointType hypergeometric_2f1_regularized(const FloatingPointType& a,
                                                   const FloatingPointType& b,
                                                   const FloatingPointType& c,
                                                   const FloatingPointType& x)
  {
    return hypergeometric_2f1(a, b, c, x) / boost::math::tgamma(c);
  }

  template<typename FloatingPointType>
  FloatingPointType pochhammer(const FloatingPointType& x,
                               const FloatingPointType& a)
  {
    return boost::math::tgamma(x + a) / boost::math::tgamma(x);
  }

  template<typename FloatingPointType>
  FloatingPointType legendre_pvu(const FloatingPointType& v,
                                 const FloatingPointType& u,
                                 const FloatingPointType& x)
  {
    using std::pow;

    // See also the third series representation provided in:
    // https://functions.wolfram.com/HypergeometricFunctions/LegendreP2General/06/01/04/

    const FloatingPointType u_half       = u / 2U;
    const FloatingPointType one_minus_x  = 1U - x;
    const FloatingPointType one_minus_mu = 1U - u;

    const FloatingPointType tgamma_term    = boost::math::tgamma(one_minus_mu);
    const FloatingPointType h2f1_reg_term  = hypergeometric_2f1_regularized(FloatingPointType(-v),
                                                                            FloatingPointType(1U + v),
                                                                            one_minus_mu,
                                                                            FloatingPointType(one_minus_x / 2U));

    return (pow(1U + x, u_half) * h2f1_reg_term) / pow(one_minus_x, u_half);
  }

  template<typename FloatingPointType>
  FloatingPointType legendre_qvu(const FloatingPointType& v,
                                 const FloatingPointType& u,
                                 const FloatingPointType& x)
  {
    using std::cos;
    using std::pow;
    using std::sin;

    // See also the third series representation provided in:
    // https://functions.wolfram.com/HypergeometricFunctions/LegendreQ2General/06/01/02/

    const FloatingPointType u_pi     = u * boost::math::constants::pi<FloatingPointType>();
    const FloatingPointType sin_u_pi = sin(u_pi);
    const FloatingPointType cos_u_pi = cos(u_pi);

    const FloatingPointType one_minus_x          = 1U - x;
    const FloatingPointType one_plus_x           = 1U + x;
    const FloatingPointType u_half               = u / 2U;
    const FloatingPointType one_minus_x_over_two = one_minus_x / 2U;

    const FloatingPointType one_plus_x_over_one_minus_x_pow_u_half = pow(one_plus_x / one_minus_x, u_half);

    const FloatingPointType v_plus_one =  v + 1U;

    const FloatingPointType h2f1_1 = hypergeometric_2f1_regularized(FloatingPointType(-v), v_plus_one, FloatingPointType(1U - u), one_minus_x_over_two);
    const FloatingPointType h2f1_2 = hypergeometric_2f1_regularized(FloatingPointType(-v), v_plus_one, FloatingPointType(1U + u), one_minus_x_over_two);

    const FloatingPointType term1 = (h2f1_1 * one_plus_x_over_one_minus_x_pow_u_half) * cos_u_pi;
    const FloatingPointType term2 = (h2f1_2 / one_plus_x_over_one_minus_x_pow_u_half) * pochhammer(FloatingPointType(v_plus_one - u), FloatingPointType(u * 2U));

    return (boost::math::constants::half_pi<FloatingPointType>() * (term1 - term2)) / sin_u_pi;
  }
}

bool test___tgamma()
{
  const local::big_float_type x(0.5F);

  const local::big_float_type g       = boost::math::tgamma(x);
  const local::big_float_type sqrt_pi = boost::math::constants::root_pi<local::big_float_type>();

  std::cout << std::setprecision(std::numeric_limits<local::big_float_type>::digits10) << g       << std::endl;
  std::cout << std::setprecision(std::numeric_limits<local::big_float_type>::digits10) << sqrt_pi << std::endl;

  const local::big_float_type ratio = g / sqrt_pi;
  const local::big_float_type delta = fabs(1 - ratio);

  const bool result_is_ok = delta < std::numeric_limits<local::big_float_type>::epsilon() * 10U;

  return result_is_ok;
}

bool test_legendre()
{
  const local::big_float_type x = local::big_float_type(UINT32_C(789)) / 1000U;

  // Compute some values of the Legendre function of the second kind
  // on the real axis within the unit circle.

  const local::big_float_type lpvu = local::legendre_pvu(local::big_float_type(local::big_float_type(1U) / 3),
                                                         local::big_float_type(local::big_float_type(1U) / 7),
                                                         x);

  const local::big_float_type lqvu = local::legendre_qvu(local::big_float_type(local::big_float_type(1U) / 3),
                                                         local::big_float_type(local::big_float_type(1U) / 7),
                                                         x);

  // N[LegendreP[1/3, 1/7, 2, 789/1000], 104]
  const local::big_float_type control_lpvu
  {
    "0.99315918549340645725680897933376572969241094126736434178747245976770375217673830111149222182128969080027"
  };

  // N[LegendreQ[1/3, 1/7, 2, 789/1000], 104]
  const local::big_float_type control_lqvu
  {
    "0.18027013586354735033576549475861160812128148962186378344662781978695122523958952227406954299821460356031"
  };

  std::cout << std::setprecision(std::numeric_limits<local::big_float_type>::digits10) << lpvu         << std::endl;
  std::cout << std::setprecision(std::numeric_limits<local::big_float_type>::digits10) << control_lpvu << std::endl;
  std::cout << std::setprecision(std::numeric_limits<local::big_float_type>::digits10) << lqvu         << std::endl;
  std::cout << std::setprecision(std::numeric_limits<local::big_float_type>::digits10) << control_lqvu << std::endl;

  using std::fabs;

  const local::big_float_type closeness_lpvu = fabs(1 - (lpvu / control_lpvu));
  const local::big_float_type closeness_lqvu = fabs(1 - (lqvu / control_lqvu));

  const bool result_lpvu_is_ok = (closeness_lpvu < (std::numeric_limits<local::big_float_type>::epsilon() * UINT32_C(10000000)));
  const bool result_lqvu_is_ok = (closeness_lqvu < (std::numeric_limits<local::big_float_type>::epsilon() * UINT32_C(10000000)));

  const bool result_is_ok = (result_lpvu_is_ok && result_lqvu_is_ok);

  return result_is_ok;
}

bool test_boost_sf()
{
  const bool test_tgamma___is_ok = test___tgamma();
  const bool test_legendre_is_ok = test_legendre();

  std::cout << "test_tgamma___is_ok: " << std::boolalpha << test_tgamma___is_ok << std::endl;
  std::cout << "test_legendre_is_ok: " << std::boolalpha << test_legendre_is_ok << std::endl;

  const bool result_is_ok = (test_tgamma___is_ok && test_legendre_is_ok);

  return result_is_ok;
}


#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>

int main()
{
  using namespace boost::numeric::ublas;

  matrix<double> m(3,3);

  for(int i=0; i!=m.size1(); ++i)
    for(int j=0; j!=m.size2(); ++j)
      m(i,j)=3*i+j;

  std::cout << trans(m) << std::endl;
}


//---------------------------------------------------------------------------//
// Copyright (c) 2013 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// 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
//
// See http://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE TestTransformReduce
#include <boost/test/unit_test.hpp>

#include <boost/compute/lambda.hpp>
#include <boost/compute/system.hpp>
#include <boost/compute/functional.hpp>
#include <boost/compute/algorithm/transform_reduce.hpp>
#include <boost/compute/container/vector.hpp>

#include "context_setup.hpp"

namespace compute = boost::compute;

BOOST_AUTO_TEST_CASE(sum_abs_int_doctest)
{
    using boost::compute::abs;
    using boost::compute::plus;

    int data[] = { 1, -2, -3, -4, 5 };
    compute::vector<int> vec(data, data + 5, queue);

//! [sum_abs_int]
int sum = 0;
boost::compute::transform_reduce(
    vec.begin(), vec.end(), &sum, abs<int>(), plus<int>(), queue
);
//! [sum_abs_int]

    BOOST_CHECK_EQUAL(sum, 15);
}

BOOST_AUTO_TEST_CASE(multiply_vector_length)
{
    float data[] = { 2.0f, 0.0f, 0.0f, 0.0f,
                     0.0f, 3.0f, 0.0f, 0.0f,
                     0.0f, 0.0f, 4.0f, 0.0f };
    compute::vector<compute::float4_> vector(
        reinterpret_cast<compute::float4_ *>(data),
        reinterpret_cast<compute::float4_ *>(data) + 3,
        queue
    );

    float product;
    compute::transform_reduce(
        vector.begin(),
        vector.end(),
        &product,
        compute::length<compute::float4_>(),
        compute::multiplies<float>(),
        queue
    );
    BOOST_CHECK_CLOSE(product, 24.0f, 1e-4f);
}

BOOST_AUTO_TEST_CASE(mean_and_std_dev)
{
    using compute::lambda::_1;
    using compute::lambda::pow;

    float data[] = { 1, 2, 3, 4, 5, 6, 7, 8, 9, 10 };
    compute::vector<float> vector(data, data + 10, queue);

    float sum;
    compute::reduce(
        vector.begin(),
        vector.end(),
        &sum,
        compute::plus<float>(),
        queue
    );

    float mean = sum / vector.size();
    BOOST_CHECK_CLOSE(mean, 5.5f, 1e-4);

    compute::transform_reduce(
        vector.begin(),
        vector.end(),
        &sum,
        pow(_1 - mean, 2),
        compute::plus<float>(),
        queue
    );

    float variance = sum / vector.size();
    BOOST_CHECK_CLOSE(variance, 8.25f, 1e-4);

    float std_dev = std::sqrt(variance);
    BOOST_CHECK_CLOSE(std_dev, 2.8722813232690143, 1e-4);
}

BOOST_AUTO_TEST_SUITE_END()


// In this quiz you'll implement the global kinematic model.
#include <iostream>
#include <Eigen/Core>
#include <kinematic.hpp>
#include <iomanip>

int main() {
  using Eigen::VectorXd;
  // [x, y, psi, v]
  VectorXd state(4);
  // [delta, v]
  VectorXd actuators(2);

  state << 0, 0, deg2rad(45), 1;
  actuators << deg2rad(5), 1;

  // should be [0.212132, 0.212132, 0.798488, 1.3]
  VectorXd next_state = globalKinematic(state, actuators, 0.3);

  std::cout << next_state << std::endl;
}


#ifndef NEURAL_NET_HPP
#define NEURAL_NET_HPP

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

//class Sigmoid_Neuron
//{
//Sigmoid_Neuron(int input_size,
//private:
//Eigen::VectorXd weights;
//double activation;
//double bias;
//};

class Neural_Net_Functions
{
  public:
    Neural_Net_Functions() {}
    ~Neural_Net_Functions() {}

    virtual double logistic(const double input) const;
    virtual double logisticPrime(const double input) const;
    virtual void vec_logistic(
        const Eigen::VectorXd &input,
        Eigen::VectorXd &output) const;
    virtual void vec_logisticPrime(
        const Eigen::VectorXd &input,
        Eigen::VectorXd &output) const;
    virtual double cost(
        const Eigen::VectorXd &output,
        const Eigen::VectorXd &desired_output) const;
    virtual Eigen::VectorXd costPrime(
        const Eigen::VectorXd &output,
        const Eigen::VectorXd &desired_output) const;
};

class Neural_Net
{
  public:
    Neural_Net(
        Eigen::VectorXd &weight_sizes,
        Neural_Net_Functions *functions);
    ~Neural_Net();

    inline void setInput(Eigen::VectorXd input)
    {
      _activations[0] = input;
    }

    void feedForward();

    double getCost(Eigen::VectorXd desired_output) const;
    void computeError(Eigen::VectorXd desired_output);
    //void gradientDescent(
    void gradientSum();
    void backPropagation(
        Eigen::MatrixXd input,
        Eigen::MatrixXd desired_output);

    void print_neural_net() const;
    void print_layer(unsigned int layer) const;

  private:
    unsigned int _layer_size;
    std::vector<Eigen::MatrixXd> _weights;
    std::vector<Eigen::VectorXd> _bias;
    std::vector<Eigen::VectorXd> _activations;
    std::vector<Eigen::VectorXd> _zs;
    std::vector<Eigen::VectorXd> _errors;
    double _gradientSum;
    Neural_Net_Functions *_functions;
};

#endif


/*
 * FileName: main.cpp
 * Description: Main program.
 * Copyright (C) 2014  K M Masum Habib <masum.habib@gmail.com>
 * Created: 02 Nov 2014.
 */

#include "main.h"
#include "config.h"

#include <iostream>
#include <string>
#include <stdexcept>

#include <armadillo>

using namespace std;
using namespace arma;

/*
 * The main function
 */
int main(int argc, char** argv) {

    cout << "Armadillo Minimum Project v" << ARMAMIN_VERSION << endl;

    mat A = randu<mat>(3,3); // construct a random 3x3 matrix.
    mat B;
    B << 1 << 2 << 3 << endr // B = | 1    2    3 |
      << 4 << 5 << 6 << endr //     | 4    5    6 |
      << 7 << 8 << 9 << endr;//     | 7    8    8 |

    // print A and B
    cout << "A = " << endl << A << endl;
    cout << "B = " << endl << B << endl;

    // compute A*B
    mat C = A*B;
    // show A*B
    cout << "A*B = " << endl << C << endl;

    // compute A+B
    C = A+B;
    // show A+B
    cout << "A+B = " << endl << C << endl;

    // compute inverse
    C = solve(A, B);
    cout << "A\\B = " << endl << C << endl;

    // Eigenvalue solver
    cx_vec E = eig_gen(A);
    cout << "Eigen values of (A): " << endl << E << endl;

    return MAIN_SUCCESS;
}