#include <stdio.h>
#include <iostream>

#ifdef NDEBUG
#define EIGEN_NO_DEBUG
#endif

#include "main.h"
#include <unsupported/Eigen/LevenbergMarquardt>
#include <unsupported/Eigen/SparseExtra>

#include <Eigen/Eigen>
#include "unsupported/Eigen/src/SparseExtra/BlockSparseQR.h"
#include "unsupported/Eigen/src/SparseExtra/BlockDiagonalSparseQR.h"

// This disables some useless Warnings on MSVC.
// It is intended to be done for this test only.
#include <Eigen/src/Core/util/DisableStupidWarnings.h>

const size_t NUM_SAMPLE_POINTS =
#ifdef NDEBUG
500000;
#else
50000;
#endif

template <typename _Scalar>
struct EllipseFitting : SparseFunctor<_Scalar, int>
{
    // Class data: 2xN matrix with each column a 2D point
    Matrix2Xd ellipsePoints;

    // Number of parameters in the model, to which will be added
    // one latent variable per point.
    static const int nParamsModel = 5;

    // Constructor initializes points, and tells the base class how many parameters there are in total
    EllipseFitting(const Matrix2Xd& points ):
      SparseFunctor<_Scalar, int>(nParamsModel + points.cols(), points.cols()*2),
      ellipsePoints(points)
    {
    }

    // Functor functions
    int operator()(const InputType& uv, ValueType& fvec) const {
      // Ellipse parameters are the last 5 entries
      auto params = uv.tail(nParamsModel);
      double a = params[0];
      double b = params[1];
      double x0 = params[2];
      double y0 = params[3];
      double r = params[4];

      // Correspondences (t values) are the first N
      for (int i = 0; i < ellipsePoints.cols(); i++) {
        double t = uv(i);
        double x = a*cos(t)*cos(r) - b*sin(t)*sin(r) + x0;
        double y = a*cos(t)*sin(r) + b*sin(t)*cos(r) + y0;
        fvec(2 * i + 0) = ellipsePoints(0, i) - x;
        fvec(2 * i + 1) = ellipsePoints(1, i) - y;
      }

      return 0;
    }

    // Functor jacobian
    int df(const InputType& uv, JacobianType& fjac) {
        // X_i - (a*cos(t_i) + x0)
        // Y_i - (b*sin(t_i) + y0)
        int npoints = ellipsePoints.cols();
        auto params = uv.tail(nParamsModel);
        double a = params[0];
        double b = params[1];
        double r = params[4];

        TripletArray<JacobianType::Scalar> triplets(npoints * 2 * 5); // npoints * rows_per_point * nonzeros_per_row
        for(int i=0; i<npoints; i++) {
            double t = uv(i);
            triplets.add(2 * i, i,             +a*cos(r)*sin(t) + b*sin(r)*cos(t));
            triplets.add(2 * i, npoints + 0,   -cos(t)*cos(r));
            triplets.add(2 * i, npoints + 1,   +sin(t)*sin(r));
            triplets.add(2 * i, npoints + 2,   -1);
            triplets.add(2 * i, npoints + 4,   +a*cos(t)*sin(r) + b*sin(t)*cos(r));

            triplets.add(2 * i + 1, i,             +a*sin(r)*sin(t) - b*cos(r)*cos(t));
            triplets.add(2 * i + 1, npoints + 0,   -cos(t)*sin(r));
            triplets.add(2 * i + 1, npoints + 1,   -sin(t)*cos(r));
            triplets.add(2 * i + 1, npoints + 3,   -1);
            triplets.add(2 * i + 1, npoints + 4,   -a*cos(t)*cos(r) + b*sin(t)*sin(r));
        }

        fjac.setFromTriplets(triplets.begin(), triplets.end());
        return 0;
    }

    // For generic Jacobian, one might use this Dense QR solver.
    typedef SparseQR<JacobianType, COLAMDOrdering<int> > GeneralQRSolver;

    // But for optimal performance, declare QRSolver that understands the sparsity structure.
    // Here it's block-diagonal LHS with dense RHS
    //
    // J1 = [J11   0   0 ... 0
    //         0 J12   0 ... 0
    //                   ...
    //         0   0   0 ... J1N];
    // And
    // J = [J1 J2];

    // QR for J1 subblocks is 2x1
    typedef ColPivHouseholderQR<Matrix<Scalar, 2, 1> > DenseQRSolver2x1;

    // QR for J1 is block diagonal
    typedef BlockDiagonalSparseQR<JacobianType, DenseQRSolver2x1> LeftSuperBlockSolver;

    // QR for J1'J2 is general dense (faster than general sparse by about 1.5x for n=500K)
    typedef ColPivHouseholderQR<Matrix<Scalar, Dynamic, Dynamic> > RightSuperBlockSolver;

    // QR for J is concatenation of the above.
    typedef BlockSparseQR<JacobianType, LeftSuperBlockSolver, RightSuperBlockSolver> SchurlikeQRSolver;

    typedef SchurlikeQRSolver QRSolver;

    // And tell the algorithm how to set the QR parameters.
    void initQRSolver(GeneralQRSolver &qr) {}

    void initQRSolver(SchurlikeQRSolver &qr) {
        // set block size
        qr.getLeftSolver().setSparseBlockParams(2, 1);
        qr.setBlockParams(ellipsePoints.cols());
    }
};

void ellipseFitting()
{

  //eigen_assert(false);

  // _CrtSetDbgFlag(_CRTDBG_CHECK_ALWAYS_DF);

  if (1) {
    // Check fast QR
    Matrix<double, 5, 2> A;
    A << 12, 3, -5, 17, -7, 132, 1.0, 1.1, -3.1, 4.7;
    ColPivHouseholderQR<Matrix<double, 5, 2> > qr(A);

    MatrixXd R = qr.matrixR().template triangularView<Upper>();

    Matrix<double, 5, 5> Q = qr.matrixQ();
    std::cout << "A=\n" << A << std::endl;
    std::cout << "AP=\n" << A * qr.colsPermutation() << std::endl;
    //std::cout << "QR=\n" << qr.matrixQ() * qr.matrixR().template triangularView<Upper>() << std::endl;
    std::cout << "Q=\n" << Q << std::endl;
    std::cout << "R=\n" << R << std::endl;
    std::cout << "QR=\n" << Q * R << std::endl;
    VERIFY_IS_APPROX(Q * R, A * qr.colsPermutation());
  }

    // ELLIPSE PARAMETERS
    double a, b, x0, y0, r;
    a = 7.5;
    b = 2;
    x0 = 17.;
    y0 = 23.;
    r = 0.23;

    std::cout << "GROUND TRUTH   " << " ";
    std::cout << "a=" << a << "\t";
    std::cout << "b=" << b << "\t";
    std::cout << "x0=" << x0 << "\t";
    std::cout << "y0=" << y0 << "\t";
    std::cout << "r=" << r*180./EIGEN_PI << "\t";
    std::cout << std::endl;

    // CREATE DATA SAMPLES

    int nDataPoints = NUM_SAMPLE_POINTS;
    Matrix2Xd ellipsePoints;
    ellipsePoints.resize(2, nDataPoints);
    double incr = 1.3*EIGEN_PI / double(nDataPoints);
    for(int i=0; i<nDataPoints; i++) {
        double t = double(i)*incr;
        ellipsePoints(0, i) = x0 + a*cos(t)*cos(r) - b*sin(t)*sin(r);
        ellipsePoints(1, i) = y0 + a*cos(t)*sin(r) + b*sin(t)*cos(r);
    }

    // INITIAL PARAMS
    EllipseFitting<double>::InputType params;
    params.resize(EllipseFitting<double>::nParamsModel+nDataPoints);
    double minX, minY, maxX, maxY;
    minX = maxX = ellipsePoints(0,0);
    minY = maxY = ellipsePoints(1,0);
    for(int i=0; i<ellipsePoints.cols(); i++) {
        minX = (std::min)(minX, ellipsePoints(0,i));
        maxX = (std::max)(maxX, ellipsePoints(0,i));
        minY = (std::min)(minY, ellipsePoints(1,i));
        maxY = (std::max)(maxY, ellipsePoints(1,i));
    }
    params(ellipsePoints.cols()) = 0.5*(maxX - minX);
    params(ellipsePoints.cols()+1) = 0.5*(maxY - minY);
    params(ellipsePoints.cols()+2) = 0.5*(maxX + minX);
    params(ellipsePoints.cols()+3) = 0.5*(maxY + minY);
    params(ellipsePoints.cols()+4) = 0;
    for(int i=0; i<ellipsePoints.cols(); i++) {
        params(i) = double(i)*incr;
    }

    std::cout << "INITIALIZATION" << " ";
    std::cout << "a=" << params(ellipsePoints.cols()) << "\t";
    std::cout << "b=" << params(ellipsePoints.cols() + 1) << "\t";
    std::cout << "x0=" << params(ellipsePoints.cols() + 2) << "\t";
    std::cout << "y0=" << params(ellipsePoints.cols() + 3) << "\t";
    std::cout << "r=" << params(ellipsePoints.cols() + 4)*180. / EIGEN_PI << "\t";
    std::cout << std::endl << std::endl;

    typedef EllipseFitting<double> Functor;
    Functor functor(ellipsePoints);
    Eigen::LevenbergMarquardt< Functor > lm(functor);
    lm.setVerbose(true);

    Eigen::LevenbergMarquardtSpace::Status info = lm.minimize(params);

    std::cout << "END[" << info << "]";
    std::cout << "a=" << params(ellipsePoints.cols()) << "\t";
    std::cout << "b=" << params(ellipsePoints.cols()+1) << "\t";
    std::cout << "x0=" << params(ellipsePoints.cols()+2) << "\t";
    std::cout << "y0=" << params(ellipsePoints.cols()+3) << "\t";
    std::cout << "r=" << params(ellipsePoints.cols()+4)*180./EIGEN_PI << "\t";
    std::cout << std::endl << std::endl;

    // check parameters ambiguity before test result
    // a should be bigger than b
    if( fabs(params(ellipsePoints.cols()+1)) > fabs(params(ellipsePoints.cols())) ) {
       std::swap( params(ellipsePoints.cols()), params(ellipsePoints.cols()+1) );
       params(ellipsePoints.cols()+4) -= 0.5*EIGEN_PI;
    }
    // a and b should be positive
    if(params(ellipsePoints.cols())<0 ) {
       params(ellipsePoints.cols()) *= -1.;
       params(ellipsePoints.cols()+1) *= -1.;
       params(ellipsePoints.cols()+4) += EIGEN_PI;
    }
    // fix rotation angle range
    while( params(ellipsePoints.cols()+4) < 0 ) params(ellipsePoints.cols()+4) += 2.*EIGEN_PI;
    while( params(ellipsePoints.cols()+4) > EIGEN_PI ) params(ellipsePoints.cols()+4) -= EIGEN_PI;

    eigen_assert( fabs(a - params(ellipsePoints.cols())) < 0.00001 );
    eigen_assert( fabs(b - params(ellipsePoints.cols()+1)) < 0.00001 );
    eigen_assert( fabs(x0 - params(ellipsePoints.cols()+2)) < 0.00001 );
    eigen_assert( fabs(y0 - params(ellipsePoints.cols()+3)) < 0.00001 );
    eigen_assert( fabs(r - params(ellipsePoints.cols()+4)) < 0.00001 );

}

void test_ellipse_fitting()
{
    CALL_SUBTEST(ellipseFitting());
}


/*
	kstatboost
	Ver. k09.01

	Written by Koji Yamamoto
	Copyright (C) 2020-2021 Koji Yamamoto
	In using this, please read the document which states terms of use.

	Statistical Computations using Boost
	Plus, Random Number from Distrbution using Boost

*/

/* ********** Preprocessor Directives ********** */

#ifndef kstatboost_cpp_include_guard
#define kstatboost_cpp_include_guard

#include <vector>
#include <functional>

#include <k09/krand01.cpp>

#include <boost/math/statistics/univariate_statistics.hpp>
#include <boost/math/statistics/bivariate_statistics.hpp>
#include <boost/math/distributions/beta.hpp>

/* ********** Using Directives ********** */

//using namespace std;

/* ********** Type Declarations: enum, class, etc. ********** */

/* ********** Function Declarations ********** */

double unbiasedVarBoost( const std::vector <double> &);
double corrBoost( const std::vector <double> &, const std::vector <double> &);

std::function <double(double)>
getBetaQ(
	double, double
);

void
getBetaRandomVec(
	std::vector <double> &, RandomNumberEngine &, int, double, double
);

/* ********** Type Definitions: enum, class, etc. ********** */

/* ********** Global Variables ********** */

/* ********** Definitions of Static Member Variables ********** */

/* ********** Function Definitions ********** */

double unbiasedVarBoost( const std::vector <double> &vec0)
{

	// sample_variance() returns unbiased variance in estimation from sample
	double sigma_sq = boost::math::statistics::sample_variance( vec0);
	return sigma_sq;

}

// corrBoost()について、コンパイル時に警告が出てくるので、これを抑制しておく。
// 警告の趣旨は型変換によって精度が落ちる可能性を指摘したものらしい。
// 実際には、boost\math\statistics\bivariate_statictics.hppの
// correlation_coefficient_seq_impl( )の定義で、整数を浮動小数点にキャストしていないから。
// →Githubでプルリクを投げた。

#if defined(_MSC_VER) && _MSC_VER >= 1400
#pragma warning(push)
#pragma warning(disable:4244)
#endif

double corrBoost( const std::vector <double> &xvec, const std::vector <double> &yvec)
{

	double ret = boost::math::statistics::correlation_coefficient( xvec, yvec);
	return ret;

}

#if defined(_MSC_VER) && _MSC_VER >= 1400
#pragma warning(pop)
#endif

// returns quatile function (inverse distribution function)
// of targeted Beta dist. with specific mean and variance
std::function <double(double)> getBetaQ( double mean0, double var0)
{

	using boost::math::beta_distribution;

	double alpha = beta_distribution<>::find_alpha( mean0, var0);
	double beta = beta_distribution<>::find_beta( mean0, var0);

	beta_distribution<> mybeta( alpha, beta);

	auto ret =
		[=]( double p) -> double
		{
			return quantile( mybeta, p);
		};

	return ret;

}

// returns vector of random numbers from targeted Beta distribution
void
getBetaRandomVec(
	std::vector <double> &ret,
	RandomNumberEngine &rne,
	int len,
	double mean0,
	double var0
)
{

	ret.clear();

	auto qfunc = getBetaQ( mean0, var0);

	ret = rne.getDistRandomVec( len, qfunc);

}

/* ********** Definitions of Member Functions ********** */

#endif /* kstatboost_cpp_include_guard */


#include<NTL/ZZ_p.h>
#include <NTL/ZZ.h>
#include<NTL/ZZ_pXFactoring.h>
#include<NTL/vector.h>
using namespace std;
using namespace NTL;

int main()
{

   ZZ n1,n2,p1,p2,q,f,n_cpy;

   long L = 512*4;

   GenGermainPrime(p1,L,800);
   GenGermainPrime(p2,L,800);
   GenGermainPrime(q1,L,800);
   GenGermainPrime(q2,L,800);
   n1 = p1*q1;
   n2 = p2*q2;

   cout <<"P1="<< p1 << "\n";
   cout <<"P2="<< p2 << "\n";
   cout <<"Q1="<< q1 << "\n";
   cout <<"Q2="<< q2 << "\n";
   cout <<"N1="<< n1 << "\n";
   cout <<"N2="<< n2 << "\n";
   //~ ZZ d;
   //~ GCD(d, n1,n2);
   //~ cout <<"\n\nGCD="<< d << "\n";
   //~ if(d == 1){
	   //~ cout <<"GCD is correct"<< "\n";
	   //~ }
   return 0;
}


#pragma once
#include <cmath>
#include <iostream>
#include <exception>

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

using Eigen::ArrayXd;
using Eigen::MatrixXcd;
using Eigen::MatrixXd;
using Eigen::Vector3d;

namespace GPU
{
    class Matrix : public MatrixXd
    {
    public:
        void fromGpu(double *gpu_rep, unsigned row, unsigned col, size_t pitch);
        double *toGpu(size_t *pitch) const;
        int toGpu(double** p, size_t *pitch, size_t offset, size_t batch_size,
                  bool iscol) const;
    };

    struct gpu_closest_matrix_params
    {
        const Matrix &p;
        const Matrix &m;
        Matrix &y;
    };

    struct gpu_err_compute_params
    {
        const Matrix &Y;
        Matrix &p;
        bool s;
        const Matrix sr;
        const Matrix &t;
    };

    class ICP
    {
    public:
        ICP(Matrix m_, Matrix p_, int max_iter_)
            : m{m_},
              p{p_},
              new_p{p_},
              np{(unsigned int)p_.cols()},
              nm{(unsigned int)m_.cols()},
              dim{3},
              max_iter{max_iter_}
        {
            this->s = 1.;
            this->r = {Matrix::Identity(m_.rows(), m_.rows())};
            this->t = {Matrix::Zero(m_.rows(), 1)};
        }

        struct gpu_closest_matrix_params get_closest_matrix_params(Matrix &Y)
        {
            struct gpu_closest_matrix_params cmp
                {
                    new_p, m, Y
                };
            return cmp;
        }
        struct gpu_err_compute_params get_err_compute_params(Matrix &Y, bool s)
        {
            struct gpu_err_compute_params cmp
                {
                    Y, new_p, s, s*r, t
                };
            return cmp;
        }

        double getDim() {return dim;}
        double getNp() {return np;}

        ~ICP()
        {
        }

        void find_corresponding_naive();
        void find_corresponding_opti();
        Matrix compute_y_naive();
        double find_alignment(Matrix y);

    public:
        Matrix new_p;

    private:
        double s;
        Matrix t;
        Matrix r;

        Matrix m;
        Matrix p;

        unsigned int np;
        unsigned int nm;
        unsigned int dim;

        int max_iter;
        const double threshold = 1e-5;
    };
} // namespace GPU

// Compute wrapper functions

void compute_Y_w_opti(const GPU::Matrix &m, const GPU::Matrix &pi, GPU::Matrix &Y);
int compute_distance_w_naive(const GPU::Matrix &m, const GPU::Matrix &pi);
double compute_err_w(const GPU::Matrix &Y, GPU::Matrix &p, bool in_place,
                     const GPU::Matrix &sr, const GPU::Matrix &t);

GPU::Matrix substract_col_w(const GPU::Matrix &M, const GPU::Matrix &m);
void y_p_norm_w(const GPU::Matrix &y, const GPU::Matrix &p, size_t size_arr, double &d_caps, double &sp);


/**
 *  @file eigen_vector_operations.cpp
 *  @author Maximilian Harr <maximilian.harr@daimler.com>
 *  @date 13.07.2017
 *
 *  @brief Eigen vector computations
 *
 *
 *
 *          Coding Standard:
 *          wiki.ros.org/CppStyleGuide
 *          https://google.github.io/styleguide/cppguide.html
 *
 *
 *  @bug
 *
 *
 *  @todo
 *
 *
 */

// PRAGMA

// SYSTEM INCLUDES
#include <iostream>  /* Header that defines the standard input/output stream objects */
#include <cstdlib>  /* Header that defines several general purpose functions */
#include <string>
#include <Eigen/Dense>  /* template library for linear algebra http://eigen.tuxfamily.org */
#include <math.h> /* Defines M_PI for pi value */
#include <Eigen/Eigenvalues>
#include <Eigen/Geometry>

// PROJECT INCLUDES

// LOCAL INCLUDES

// FORWARD REFERENCES

// FUNCTION PROTOTYPES
void ProjectVector(Eigen::VectorXd vec_a, Eigen::VectorXd vec_b,
  Eigen::VectorXd& vec_parallel, Eigen::VectorXd& vec_perpendicular );

void ProjectVectorAbsolute(Eigen::VectorXd vec_a, Eigen::VectorXd vec_b,
  double& parallel, double& perpendicular );

void ProjectVectorAbsolute(Eigen::Vector2d vec_a, Eigen::Vector2d vec_b,
  double& parallel, double& perpendicular );

// GLOBAL VARIABLES

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

  Eigen::VectorXd vec_a(3), vec_b(3);

  Eigen::VectorXd vec_parallel(3), vec_perpendicular(3);
  double parallel, perpendicular;

  /* Project b on a */
  vec_a << 2, 0, 0;
  vec_b << 1, 1, 0;
  ProjectVector(vec_a, vec_b, vec_parallel, vec_perpendicular);
  vec_b << -1, 1, 0;
  ProjectVector(vec_a, vec_b, vec_parallel, vec_perpendicular);
  vec_b << -1, -1, 0;
  ProjectVector(vec_a, vec_b, vec_parallel, vec_perpendicular);
  vec_b << 1, -1, 0;
  ProjectVector(vec_a, vec_b, vec_parallel, vec_perpendicular);
  vec_b << 1, 0, 0;
  ProjectVector(vec_a, vec_b, vec_parallel, vec_perpendicular);
  vec_b << 0, 0, 1;
  ProjectVector(vec_a, vec_b, vec_parallel, vec_perpendicular);

  std::cout << "---------- 3d vector rotation in x,y ----------" << std::endl;
  Eigen::Vector2d vec_a3, vec_b3;
  vec_a3 << 2, 0;
  vec_b3 << 1, 1;
  ProjectVectorAbsolute(vec_a3, vec_b3, parallel, perpendicular);
  vec_b3 << -1, 1;
  ProjectVectorAbsolute(vec_a3, vec_b3, parallel, perpendicular);
  vec_b3 << -1, -1;
  ProjectVectorAbsolute(vec_a3, vec_b3, parallel, perpendicular);
  vec_b3 << 1, -1;
  ProjectVectorAbsolute(vec_a3, vec_b3, parallel, perpendicular);

  return 0;

}

//// FUNCTION DEFINITIONS //////////////////////////////////////////////////////////////////////////

void ProjectVector(Eigen::VectorXd vec_a, Eigen::VectorXd vec_b,
  Eigen::VectorXd& vec_parallel, Eigen::VectorXd& vec_perpendicular ){

  double parallel, perpendicular;
  ProjectVectorAbsolute(vec_a, vec_b, parallel, perpendicular);

  vec_parallel = vec_a;
  vec_parallel.normalize();
  vec_parallel = parallel*vec_parallel;

  vec_perpendicular = vec_b-vec_parallel;
  vec_perpendicular.normalize();
  vec_perpendicular = perpendicular*vec_perpendicular;

  std::cout << "vec_b            : \n" << vec_b << std::endl;
  std::cout << "vec_parallel     : \n" << vec_parallel << std::endl;
  std::cout << "vec_perpendicular: \n" << vec_perpendicular << std::endl;

}

void ProjectVectorAbsolute(Eigen::VectorXd vec_a, Eigen::VectorXd vec_b,
  double& parallel, double& perpendicular ){

  // Warning: there is no check whether perpendicular portion is left/right use Vector2d function

  /* rotation from a to b (Kosinussatz) */
  double phi = acos( vec_a.dot(vec_b)/ (vec_a.norm()*vec_b.norm()) );

  parallel = cos(phi)*vec_b.norm();
  perpendicular = sin(phi)*vec_b.norm();
  std::cout << std::endl << "---------------" << std::endl;
  std::cout << "phi          : " << phi << std::endl;
  std::cout << "parallel     : " << parallel << std::endl;
  std::cout << "perpendicular: " << perpendicular << std::endl;

}

void ProjectVectorAbsolute(Eigen::Vector2d vec_a2, Eigen::Vector2d vec_b2,
  double& parallel, double& perpendicular ){

  Eigen::Vector3d vec_a, vec_b;
  vec_a << vec_a2[0], vec_a2[1], 0;
  vec_b << vec_b2[0], vec_b2[1], 0;

  /* rotation from a to b (Kosinussatz) */
  double phi = acos( vec_a.dot(vec_b) / (vec_a.norm()*vec_b.norm()) );

  Eigen::Vector3d cross_product = vec_a.cross(vec_b);
  double sign = 1;
  if( cross_product[2] < 0) {sign = -1;}

  parallel = cos(phi)*vec_b.norm();
  perpendicular = sign*sin(phi)*vec_b.norm();
  std::cout << std::endl << "---------------" << std::endl;
  std::cout << "phi          : " << phi << std::endl;
  std::cout << "parallel     : " << parallel << std::endl;
  std::cout << "perpendicular: " << perpendicular << std::endl;

}


/*
 * Copyright Nick Thompson, 2017
 * Use, modification and distribution are subject to the
 * Boost Software License, Version 1.0. (See accompanying file
 * LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
 *
 * This example shows to to numerically integrate a periodic function using the adaptive_trapezoidal routine provided by boost.
 */

#include <iostream>
#include <cmath>
#include <limits>
#include <boost/math/quadrature/trapezoidal.hpp>

int main()
{
    using boost::math::constants::two_pi;
    using boost::math::constants::third;
    using boost::math::quadrature::trapezoidal;
    // This function has an analytic form for its integral over a period: 2pi/3.
    auto f = [](double x) { return 1/(5 - 4*cos(x)); };

    double Q = trapezoidal(f, (double) 0, two_pi<double>());

    std::cout << std::setprecision(std::numeric_limits<double>::digits10);
    std::cout << "The adaptive trapezoidal rule gives the integral of our function as " << Q << "\n";
    std::cout << "The exact result is                                                 " << two_pi<double>()*third<double>() << "\n";

}


/*
 * MIT License
 *
 * Copyright (c) 2018 Forrest
 *
 * 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.
*/
#ifndef CUBIC_SPLINE_H
#define CUBIC_SPLINE_H

#include <Eigen/Eigen>
#include <algorithm>
#include <array>
#include <iostream>
#include <stdexcept>
#include <string>
#include <vector>

static std::vector<double> vec_diff(const std::vector<double> & input)
{
  std::vector<double> output;
  for (unsigned int i = 1; i < input.size(); i++) {
    output.push_back(input[i] - input[i - 1]);
  }
  return output;
}

static std::vector<double> cum_sum(const std::vector<double> & input)
{
  std::vector<double> output;
  double temp = 0;
  for (unsigned int i = 0; i < input.size(); i++) {
    temp += input[i];
    output.push_back(temp);
  }
  return output;
}

class Spline
{
public:
  std::vector<double> x;
  std::vector<double> y;
  int nx;
  std::vector<double> h;
  std::vector<double> a;
  std::vector<double> b;
  std::vector<double> c;
  // Eigen::VectorXf c;
  std::vector<double> d;

  Spline(){};
  // d_i * (x-x_i)^3 + c_i * (x-x_i)^2 + b_i * (x-x_i) + a_i
  Spline(const std::vector<double> & x_, const std::vector<double> & y_)
  : x(x_), y(y_), nx(x_.size()), h(vec_diff(x_)), a(y_)
  {
    Eigen::MatrixXd A = calc_A();
    Eigen::VectorXd B = calc_B();
    Eigen::VectorXd c_eigen = A.colPivHouseholderQr().solve(B);
    double * c_pointer = c_eigen.data();
    c.assign(c_pointer, c_pointer + c_eigen.rows());

    for (int i = 0; i < nx - 1; i++) {
      d.push_back((c[i + 1] - c[i]) / (3.0 * h[i]));
      b.push_back((a[i + 1] - a[i]) / h[i] - h[i] * (c[i + 1] + 2 * c[i]) / 3.0);
    }
  };

  double calc(double t)
  {
    if (t < x.front() || t > x.back()) {
      std::cout << "Dangerous" << std::endl;
      std::cout << t << std::endl;
      throw std::invalid_argument("received value out of the pre-defined range");
    }
    int seg_id = bisect(t, 0, nx);
    double dx = t - x[seg_id];
    return a[seg_id] + b[seg_id] * dx + c[seg_id] * dx * dx + d[seg_id] * dx * dx * dx;
  }

  double calc(double t, double s)
  {
    if (t < 0 || t > s) {
      std::cout << "Dangerous" << std::endl;
      std::cout << t << std::endl;
      throw std::invalid_argument("received value out of the pre-defined range");
    }
    int seg_id = bisect(t, 0, nx);
    double dx = t - x[seg_id];
    return a[seg_id] + b[seg_id] * dx + c[seg_id] * dx * dx + d[seg_id] * dx * dx * dx;
  }

  double calc_d(double t)
  {
    if (t < x.front() || t > x.back()) {
      std::cout << "Dangerous" << std::endl;
      std::cout << t << std::endl;
      throw std::invalid_argument("received value out of the pre-defined range");
    }
    int seg_id = bisect(t, 0, nx - 1);
    double dx = t - x[seg_id];
    return b[seg_id] + 2 * c[seg_id] * dx + 3 * d[seg_id] * dx * dx;
  }

  double calc_d(double t, double s)
  {
    if (t < 0 || t > s) {
      std::cout << "Dangerous" << std::endl;
      std::cout << t << std::endl;
      throw std::invalid_argument("received value out of the pre-defined range");
    }
    int seg_id = bisect(t, 0, nx - 1);
    double dx = t - x[seg_id];
    return b[seg_id] + 2 * c[seg_id] * dx + 3 * d[seg_id] * dx * dx;
  }

  double calc_dd(double t)
  {
    if (t < x.front() || t > x.back()) {
      std::cout << "Dangerous" << std::endl;
      std::cout << t << std::endl;
      throw std::invalid_argument("received value out of the pre-defined range");
    }
    int seg_id = bisect(t, 0, nx);
    double dx = t - x[seg_id];
    return 2 * c[seg_id] + 6 * d[seg_id] * dx;
  }

  double calc_dd(double t, double s)
  {
    if (t < 0.0 || t > s) {
      std::cout << "Dangerous" << std::endl;
      std::cout << t << std::endl;
      throw std::invalid_argument("received value out of the pre-defined range");
    }
    int seg_id = bisect(t, 0, nx);
    double dx = t - x[seg_id];
    return 2 * c[seg_id] + 6 * d[seg_id] * dx;
  }

private:
  Eigen::MatrixXd calc_A()
  {
    Eigen::MatrixXd A = Eigen::MatrixXd::Zero(nx, nx);
    A(0, 0) = 1;
    for (int i = 0; i < nx - 1; i++) {
      if (i != nx - 2) {
        A(i + 1, i + 1) = 2 * (h[i] + h[i + 1]);
      }
      A(i + 1, i) = h[i];
      A(i, i + 1) = h[i];
    }
    A(0, 1) = 0.0;
    A(nx - 1, nx - 2) = 0.0;
    A(nx - 1, nx - 1) = 1.0;
    return A;
  };
  Eigen::VectorXd calc_B()
  {
    Eigen::VectorXd B = Eigen::VectorXd::Zero(nx);
    for (int i = 0; i < nx - 2; i++) {
      B(i + 1) = 3.0 * (a[i + 2] - a[i + 1]) / h[i + 1] - 3.0 * (a[i + 1] - a[i]) / h[i];
    }
    return B;
  };

  int bisect(double t, int start, int end)
  {
    int mid = (start + end) / 2;
    if (t == x[mid] || end - start <= 1) {
      return mid;
    } else if (t > x[mid]) {
      return bisect(t, mid, end);
    } else {
      return bisect(t, start, mid);
    }
  }
};

class Spline2D
{
public:
  Spline sx;
  Spline sy;
  std::vector<double> s;

  Spline2D(const std::vector<double> & x, const std::vector<double> & y)
  {
    s = calc_s(x, y);
    sx = Spline(s, x);
    sy = Spline(s, y);
    max_s_value_ = *std::max_element(s.begin(), s.end());
  };

  std::array<double, 2> calc_position(double s_t)
  {
    double x = sx.calc(s_t, max_s_value_);
    double y = sy.calc(s_t, max_s_value_);
    return {{x, y}};
  };

  double calc_curvature(double s_t)
  {
    double dx = sx.calc_d(s_t, max_s_value_);
    double ddx = sx.calc_dd(s_t, max_s_value_);
    double dy = sy.calc_d(s_t, max_s_value_);
    double ddy = sy.calc_dd(s_t, max_s_value_);
    return (ddy * dx - ddx * dy) / (dx * dx + dy * dy);
  };

  double calc_yaw(double s_t)
  {
    double dx = sx.calc_d(s_t, max_s_value_);
    double dy = sy.calc_d(s_t, max_s_value_);
    return std::atan2(dy, dx);
  };

private:
  std::vector<double> calc_s(const std::vector<double> & x, const std::vector<double> & y)
  {
    std::vector<double> ds;
    std::vector<double> out_s{0};
    std::vector<double> dx = vec_diff(x);
    std::vector<double> dy = vec_diff(y);

    for (unsigned int i = 0; i < dx.size(); i++) {
      ds.push_back(std::sqrt(dx[i] * dx[i] + dy[i] * dy[i]));
    }

    std::vector<double> cum_ds = cum_sum(ds);
    out_s.insert(out_s.end(), cum_ds.begin(), cum_ds.end());
    return out_s;
  };
  double max_s_value_;
};

class Spline3D
{
public:
  Spline sx;
  Spline sy;
  Spline sv;
  std::vector<double> s;

  Spline3D(
    const std::vector<double> & x, const std::vector<double> & y, const std::vector<double> & v)
  {
    s = calc_s(x, y);
    sx = Spline(s, x);
    sy = Spline(s, y);
    sv = Spline(s, v);
    max_s_value_ = *std::max_element(s.begin(), s.end());
  };

  std::array<double, 3> calc_trajectory_point(double s_t)
  {
    double x = sx.calc(s_t, max_s_value_);
    double y = sy.calc(s_t, max_s_value_);
    double v = sv.calc(s_t, max_s_value_);
    return {{x, y, v}};
  };

  double calc_curvature(double s_t)
  {
    double dx = sx.calc_d(s_t, max_s_value_);
    double ddx = sx.calc_dd(s_t, max_s_value_);
    double dy = sy.calc_d(s_t, max_s_value_);
    double ddy = sy.calc_dd(s_t, max_s_value_);
    return (ddy * dx - ddx * dy) / (dx * dx + dy * dy);
  };

  double calc_yaw(double s_t)
  {
    double dx = sx.calc_d(s_t, max_s_value_);
    double dy = sy.calc_d(s_t, max_s_value_);
    return std::atan2(dy, dx);
  };

private:
  std::vector<double> calc_s(const std::vector<double> & x, const std::vector<double> & y)
  {
    std::vector<double> ds;
    std::vector<double> out_s{0};
    std::vector<double> dx = vec_diff(x);
    std::vector<double> dy = vec_diff(y);

    for (unsigned int i = 0; i < dx.size(); i++) {
      ds.push_back(std::sqrt(dx[i] * dx[i] + dy[i] * dy[i]));
    }

    std::vector<double> cum_ds = cum_sum(ds);
    out_s.insert(out_s.end(), cum_ds.begin(), cum_ds.end());
    return out_s;
  };
  double max_s_value_;
};

class Spline4D
{
public:
  Spline sx;
  Spline sy;
  Spline sz;
  Spline sv;
  std::vector<double> s;

  Spline4D(
    const std::vector<double> & x, const std::vector<double> & y, const std::vector<double> & z,
    const std::vector<double> & v)
  {
    s = calc_s(x, y);
    sx = Spline(s, x);
    sy = Spline(s, y);
    sz = Spline(s, z);
    sv = Spline(s, v);
    max_s_value_ = *std::max_element(s.begin(), s.end());
  };

  std::array<double, 4> calc_trajectory_point(double s_t)
  {
    double x = sx.calc(s_t, max_s_value_);
    double y = sy.calc(s_t, max_s_value_);
    double z = sz.calc(s_t, max_s_value_);
    double v = sv.calc(s_t, max_s_value_);
    return {{x, y, z, v}};
  };

  double calc_curvature(double s_t)
  {
    double dx = sx.calc_d(s_t, max_s_value_);
    double ddx = sx.calc_dd(s_t, max_s_value_);
    double dy = sy.calc_d(s_t, max_s_value_);
    double ddy = sy.calc_dd(s_t, max_s_value_);
    return (ddy * dx - ddx * dy) / (dx * dx + dy * dy);
  };

  double calc_yaw(double s_t)
  {
    double dx = sx.calc_d(s_t, max_s_value_);
    double dy = sy.calc_d(s_t, max_s_value_);
    return std::atan2(dy, dx);
  };

private:
  std::vector<double> calc_s(const std::vector<double> & x, const std::vector<double> & y)
  {
    std::vector<double> ds;
    std::vector<double> out_s{0};
    std::vector<double> dx = vec_diff(x);
    std::vector<double> dy = vec_diff(y);

    for (unsigned int i = 0; i < dx.size(); i++) {
      ds.push_back(std::sqrt(dx[i] * dx[i] + dy[i] * dy[i]));
    }

    std::vector<double> cum_ds = cum_sum(ds);
    out_s.insert(out_s.end(), cum_ds.begin(), cum_ds.end());
    return out_s;
  };
  double max_s_value_;
};

#endif  // CUBIC_SPLINE_H


// Copyright 2019, Collabora, Ltd.
// Copyright 2016, Sensics, Inc.
// SPDX-License-Identifier: Apache-2.0
/*!
 * @file
 * @brief  Base implementations for math library.
 * @author Ryan Pavlik <ryan.pavlik@collabora.com>
 * @ingroup aux_math
 *
 * Based in part on inc/osvr/Util/EigenQuatExponentialMap.h in OSVR-Core
 */

#include "math/m_api.h"
#include "math/m_eigen_interop.hpp"

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

#include <assert.h>

// anonymous namespace for internal types
namespace {
template <typename Scalar> struct FourthRootMachineEps;
template <> struct FourthRootMachineEps<double>
{
	/// machine epsilon is 1e-53, so fourth root is roughly 1e-13
	static double
	get()
	{
		return 1.e-13;
	}
};
template <> struct FourthRootMachineEps<float>
{
	/// machine epsilon is 1e-24, so fourth root is 1e-6
	static float
	get()
	{
		return 1.e-6f;
	}
};
/// Computes the "historical" (un-normalized) sinc(Theta)
/// (sine(theta)/theta for theta != 0, defined as the limit value of 0
/// at theta = 0)
template <typename Scalar>
inline Scalar
sinc(Scalar theta)
{
	/// fourth root of machine epsilon is recommended cutoff for taylor
	/// series expansion vs. direct computation per
	/// Grassia, F. S. (1998). Practical Parameterization of Rotations
	/// Using the Exponential Map. Journal of Graphics Tools, 3(3),
	/// 29-48. http://doi.org/10.1080/10867651.1998.10487493
	Scalar ret;
	if (theta < FourthRootMachineEps<Scalar>::get()) {
		// taylor series expansion.
		ret = Scalar(1.f) - theta * theta / Scalar(6.f);
		return ret;
	}
	// direct computation.
	ret = std::sin(theta) / theta;
	return ret;
}

/// fully-templated free function for quaternion expontiation
template <typename Derived>
inline Eigen::Quaternion<typename Derived::Scalar>
quat_exp(Eigen::MatrixBase<Derived> const &vec)
{
	EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 3);
	using Scalar = typename Derived::Scalar;
	/// Implementation inspired by
	/// Grassia, F. S. (1998). Practical Parameterization of Rotations
	/// Using the Exponential Map. Journal of Graphics Tools, 3(3),
	/// 29–48. http://doi.org/10.1080/10867651.1998.10487493
	///
	/// However, that work introduced a factor of 1/2 which I could not
	/// derive from the definition of quaternion exponentiation and
	/// whose absence thus distinguishes this implementation. Without
	/// that factor of 1/2, the exp and ln functions successfully
	/// round-trip and match other implementations.
	Scalar theta = vec.norm();
	Scalar vecscale = sinc(theta);
	Eigen::Quaternion<Scalar> ret;
	ret.vec() = vecscale * vec;
	ret.w() = std::cos(theta);
	return ret.normalized();
}

/// Taylor series expansion of theta over sin(theta), aka cosecant, for
/// use near 0 when you want continuity and validity at 0.
template <typename Scalar>
inline Scalar
cscTaylorExpansion(Scalar theta)
{
	return Scalar(1) +
	       // theta ^ 2 / 6
	       (theta * theta) / Scalar(6) +
	       // 7 theta^4 / 360
	       (Scalar(7) * theta * theta * theta * theta) / Scalar(360) +
	       // 31 theta^6/15120
	       (Scalar(31) * theta * theta * theta * theta * theta * theta) /
	           Scalar(15120);
}

/// fully-templated free function for quaternion log map.
///
/// Assumes a unit quaternion.
template <typename Scalar>
inline Eigen::Matrix<Scalar, 3, 1>
quat_ln(Eigen::Quaternion<Scalar> const &quat)
{
	// ln q = ( (phi)/(norm of vec) vec, ln(norm of quat))
	// When we assume a unit quaternion, ln(norm of quat) = 0
	// so then we just scale the vector part by phi/sin(phi) to get the
	// result (i.e., ln(qv, qw) = (phi/sin(phi)) * qv )
	Scalar vecnorm = quat.vec().norm();

	// "best for numerical stability" vs asin or acos
	Scalar phi = std::atan2(vecnorm, quat.w());

	// Here is where we compute the coefficient to scale the vector part
	// by, which is nominally phi / std::sin(phi).
	// When the angle approaches zero, we compute the coefficient
	// differently, since it gets a bit like sinc in that we want it
	// continuous but 0 is undefined.
	Scalar phiOverSin = vecnorm < 1e-4 ? cscTaylorExpansion<Scalar>(phi)
	                                   : (phi / std::sin(phi));
	return quat.vec() * phiOverSin;
}

} // namespace

extern "C" void
math_quat_integrate_velocity(const struct xrt_quat *quat,
                             const struct xrt_vec3 *ang_vel,
                             const float dt,
                             struct xrt_quat *result)
{
	assert(quat != NULL);
	assert(ang_vel != NULL);
	assert(result != NULL);
	assert(dt != 0);

	Eigen::Quaternionf q = map_quat(*quat);
	Eigen::Quaternionf incremental_rotation =
	    quat_exp(map_vec3(*ang_vel) * dt * 0.5f).normalized();
	map_quat(*result) = q * incremental_rotation;
}

extern "C" void
math_quat_finite_difference(const struct xrt_quat *quat0,
                            const struct xrt_quat *quat1,
                            const float dt,
                            struct xrt_vec3 *out_ang_vel)
{
	assert(quat0 != NULL);
	assert(quat1 != NULL);
	assert(out_ang_vel != NULL);
	assert(dt != 0);

	Eigen::Quaternionf inc_quat =
	    map_quat(*quat1) * map_quat(*quat0).conjugate();
	map_vec3(*out_ang_vel) = 2.f * quat_ln(inc_quat);
}


#include <iostream>
#include <cassert>
#include <cmath>

#include <Eigen/Dense>

#include <cannon/log/registry.hpp>

using namespace Eigen;

using namespace cannon::log;

/*!
 * There are exactly ten ways of selecting three from five, 12345:
 *
 *   123, 124, 125, 134, 135, 145, 234, 245, 345
 *
 * In combinatorics, we use the notation, 5c3 = 10.
 *
 * In generall, ncr = n! / r!*(n - r)!, where r <= n.
 *
 * It is not until n = 23 that a value exceeds one-million: 23c10 = 1144066.
 *
 * How many, not necessarily distinct, values of ncr for 1 <= n <= 100, are
 * greater than one-million.
 */

unsigned int compute_num_combinations_over_million() {
  unsigned int num_greater = 0;
  MatrixXd pascal = MatrixXd::Zero(101, 101);
  pascal.col(0) = VectorXd::Ones(101);

  for (unsigned int n = 1; n <= 100; ++n) {
    for (unsigned int r = 1; r <= n; ++r) {
      pascal(n, r) = pascal(n - 1, r) + pascal(n - 1, r -1);

      if (pascal(n, r) > 1000000) {
        pascal(n, r) = 1000000;
        ++num_greater;
      }
    }
  }

  return num_greater;
}

int main(int argc, char **argv) {
  std::cout << compute_num_combinations_over_million() << std::endl;
}


// combination_iterator.hpp
//
// Produces all k-combinations of the universe {0,...,n-1} in
// lexicographical order. The algorithm is a tuned-up version
// of a simple generation method ("Algorithm T") described in:
//
//   Knuth, Donald E.
//   The Art of Computer Programming, Volume 4A: Combinatorial Algorithms,
//   Part 1. Pearson Education Inc., 2011.

#ifndef COMBINATION_ITERATOR_HPP
#define COMBINATION_ITERATOR_HPP

// This code was produced by Juho Lauri (euler314).

#include <cassert>
#include <cstdint>
#include <numeric>
#include <vector>

#include <boost/iterator/iterator_facade.hpp>

template <typename T>
class combination_iterator
    : public boost::iterator_facade<combination_iterator<T>, const std::vector<T>&, boost::forward_traversal_tag>
{
public:
    combination_iterator() : comb_() {}

    explicit combination_iterator(T n, T k)
        : end_(false), n_(n), k_(k), comb_(k)
    {
        assert(k != 0 && n_ > k);
        std::iota(comb_.begin(), comb_.end(), 0);
        assert(!end_);
    }

private:
    friend class boost::iterator_core_access;

    void increment()
    {
        std::int64_t j = k_ - 1;

        for (const T end = n_ - k_; j >= 0 && comb_[j] >= end + j; --j) {}

        if (j < 0)
        {
            assert(comb_.front() == n_ - k_);
            end_ = true;
            return;
        }

        ++comb_[j];

        for (const std::int64_t end = k_ - 1; j < end; ++j)
        {
            comb_[j + 1] = comb_[j] + 1;
        }
    }

    bool equal(const combination_iterator& other) const
    {
        return end_ == other.end_;
    }

    const std::vector<T>& dereference() const { return comb_; }

    bool end_{true};
    const int n_{0};
    const int k_{0};
    std::vector<T> comb_;
};

template <typename T>
class combination_iterator_minimax_order
    : public boost::iterator_facade<combination_iterator_minimax_order<T>,
                                    const std::vector<T>&,
                                    boost::forward_traversal_tag>
{
public:
    combination_iterator_minimax_order() : comb_() {}

    explicit combination_iterator_minimax_order(T n, T k)
        : end_(false), n_(n), k_(k), hint_(k), comb_(k)
    {
        assert(k != 0 && n_ > k);
        std::iota(comb_.begin(), comb_.end(), 0);
        assert(!end_);
    }

private:
    friend class boost::iterator_core_access;

    void increment()
    {
        // The following code was copied from the discreture library:
        // http://github.com/mraggi/discreture
        if (k_ == 0)
            return;

        if (hint_ > 0)
        {
            --hint_;
            ++comb_[hint_];
            return;
        }

        T i = 0;
        for (const T last = comb_.size() - 1;
             (i < last) && (comb_[i] + 1 == comb_[i + 1]);
             ++i)
        {
            comb_[i] = i;
        }

        ++comb_[i];

        if (comb_[i] == n_)
            end_ = true;
        else
            hint_ = i;
    }

    bool equal(const combination_iterator_minimax_order& other) const
    {
        return end_ == other.end_;
    }

    const std::vector<T>& dereference() const { return comb_; }

    bool end_{true};
    const int n_{0};
    const int k_{0};
    int hint_{};
    std::vector<T> comb_;
};

#endif


#include <Eigen/Core>
#include <cmath>
#include <fstream>
#include <iostream>
#include <mathtoolbox/gaussian-process-regression.hpp>
#include <random>
#include <string>
#include <timer.hpp>
#include <vector>

using Eigen::MatrixXd;
using Eigen::VectorXd;

namespace
{
    std::random_device                     seed;
    std::default_random_engine             engine(seed());
    std::uniform_real_distribution<double> uniform_dist(0.0, 1.0);
    std::normal_distribution<double>       normal_dist(0.0, 1.0);

    double CalculateFunction(double x) { return x * std::sin(10.0 * x); }
} // namespace

int main(int argc, char** argv)
{
    // Set a output directory path
    const std::string output_directory_path = (argc < 2) ? "." : argv[1];

    // Define the scene setting
    constexpr int    number_of_samples = 20;
    constexpr double noise_intensity   = 0.010;

    // Generate (and export) scattered data
    std::ofstream scattered_data_stream(output_directory_path + "/scattered_data.csv");
    scattered_data_stream << "x,y" << std::endl;
    MatrixXd X(1, number_of_samples);
    VectorXd y(number_of_samples);
    for (int i = 0; i < number_of_samples; ++i)
    {
        X(0, i) = uniform_dist(engine);
        y(i)    = CalculateFunction(X(0, i)) + noise_intensity * normal_dist(engine);

        scattered_data_stream << X(0, i) << "," << y(i) << std::endl;
    }
    scattered_data_stream.close();

    // Define the kernel type
    const auto kernel_type = mathtoolbox::GaussianProcessRegressor::KernelType::ArdMatern52;

    // Instantiate the interpolation object
    mathtoolbox::GaussianProcessRegressor regressor(X, y, kernel_type);

    // Perform hyperparameter estimation
    const Eigen::Vector2d default_kernel_hyperparams{0.50, 0.50};
    {
        timer::Timer t("maximum likelihood estimation");
        regressor.PerformMaximumLikelihood(default_kernel_hyperparams, 0.010);
    }

    // Define constants for export
    constexpr int    resolution       = 200;
    constexpr double percentile_point = 1.95996398454005423552;

    // Calculate (and export) predictive distribution
    std::ofstream estimated_data_stream(output_directory_path + "/estimated_data.csv");
    estimated_data_stream << "x,mean,standard deviation,95-percent upper,95-percent lower" << std::endl;
    for (int i = 0; i <= resolution; ++i)
    {
        const double x = (1.0 / static_cast<double>(resolution)) * i;
        const double y = regressor.PredictMean(VectorXd::Constant(1, x));
        const double s = regressor.PredictStdev(VectorXd::Constant(1, x));

        estimated_data_stream << x << "," << y << "," << s << "," << y + percentile_point * s << ","
                              << y - percentile_point * s << std::endl;
    }
    estimated_data_stream.close();

    return 0;
}


///////////////////////////////////////////////////////////////////////////////
//  Copyright 2014 Anton Bikineev
//  Copyright 2014 Christopher Kormanyos
//  Copyright 2014 John Maddock
//  Copyright 2014 Paul Bristow
//  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)

#ifndef BOOST_HYPERGEOMETRIC_1F1_RECURRENCE_HPP_
#define BOOST_HYPERGEOMETRIC_1F1_RECURRENCE_HPP_

#include <boost/math/special_functions/modf.hpp>
#include <boost/math/special_functions/next.hpp>

#include <boost/math/tools/recurrence.hpp>
#include <boost/math/special_functions/detail/hypergeometric_pFq_checked_series.hpp>

  namespace boost { namespace math { namespace detail {

  // forward declaration for initial values
  template <class T, class Policy>
  inline T hypergeometric_1F1_imp(const T& a, const T& b, const T& z, const Policy& pol);

  template <class T, class Policy>
  inline T hypergeometric_1F1_imp(const T& a, const T& b, const T& z, const Policy& pol, int& log_scaling);

  template <class T>
  struct hypergeometric_1F1_recurrence_a_coefficients
  {
    typedef boost::math::tuple<T, T, T> result_type;

    hypergeometric_1F1_recurrence_a_coefficients(const T& a, const T& b, const T& z):
    a(a), b(b), z(z)
    {
    }

    result_type operator()(boost::intmax_t i) const
    {
      const T ai = a + i;

      const T an = b - ai;
      const T bn = (2 * ai - b + z);
      const T cn = -ai;

      return boost::math::make_tuple(an, bn, cn);
    }

  private:
    const T a, b, z;
    hypergeometric_1F1_recurrence_a_coefficients operator=(const hypergeometric_1F1_recurrence_a_coefficients&);
  };

  template <class T>
  struct hypergeometric_1F1_recurrence_b_coefficients
  {
    typedef boost::math::tuple<T, T, T> result_type;

    hypergeometric_1F1_recurrence_b_coefficients(const T& a, const T& b, const T& z):
    a(a), b(b), z(z)
    {
    }

    result_type operator()(boost::intmax_t i) const
    {
      const T bi = b + i;

      const T an = bi * (bi - 1);
      const T bn = bi * (1 - bi - z);
      const T cn = z * (bi - a);

      return boost::math::make_tuple(an, bn, cn);
    }

  private:
    const T a, b, z;
    hypergeometric_1F1_recurrence_b_coefficients& operator=(const hypergeometric_1F1_recurrence_b_coefficients&);
  };
  //
  // for use when we're recursing to a small b:
  //
  template <class T>
  struct hypergeometric_1F1_recurrence_small_b_coefficients
  {
     typedef boost::math::tuple<T, T, T> result_type;

     hypergeometric_1F1_recurrence_small_b_coefficients(const T& a, const T& b, const T& z, int N) :
        a(a), b(b), z(z), N(N)
     {
     }

     result_type operator()(boost::intmax_t i) const
     {
        const T bi = b + (i + N);
        const T bi_minus_1 = b + (i + N - 1);

        const T an = bi * bi_minus_1;
        const T bn = bi * (-bi_minus_1 - z);
        const T cn = z * (bi - a);

        return boost::math::make_tuple(an, bn, cn);
     }

  private:
     hypergeometric_1F1_recurrence_small_b_coefficients operator=(const hypergeometric_1F1_recurrence_small_b_coefficients&);
     const T a, b, z;
     int N;
  };

  template <class T>
  struct hypergeometric_1F1_recurrence_a_and_b_coefficients
  {
    typedef boost::math::tuple<T, T, T> result_type;

    hypergeometric_1F1_recurrence_a_and_b_coefficients(const T& a, const T& b, const T& z, int offset = 0):
    a(a), b(b), z(z), offset(offset)
    {
    }

    result_type operator()(boost::intmax_t i) const
    {
      const T ai = a + (offset + i);
      const T bi = b + (offset + i);

      const T an = bi * (b + (offset + i - 1));
      const T bn = bi * (z - (b + (offset + i - 1)));
      const T cn = -ai * z;

      return boost::math::make_tuple(an, bn, cn);
    }

  private:
    const T a, b, z;
    int offset;
    hypergeometric_1F1_recurrence_a_and_b_coefficients operator=(const hypergeometric_1F1_recurrence_a_and_b_coefficients&);
  };
#if 0
  //
  // These next few recurrence relations are archived for future reference, some of them are novel, though all
  // are trivially derived from the existing well known relations:
  //
  // Recurrence relation for double-stepping on both a and b:
  // - b(b-1)(b-2) / (2-b+z) M(a-2,b-2,z) + [b(a-1)z / (2-b+z) + b(1-b+z) + abz(b+1) /(b+1)(z-b)] M(a,b,z) - a(a+1)z^2 / (b+1)(z-b) M(a+2,b+2,z)
  //
  template <class T>
  struct hypergeometric_1F1_recurrence_2a_and_2b_coefficients
  {
     typedef boost::math::tuple<T, T, T> result_type;

     hypergeometric_1F1_recurrence_2a_and_2b_coefficients(const T& a, const T& b, const T& z, int offset = 0) :
        a(a), b(b), z(z), offset(offset)
     {
     }

     result_type operator()(boost::intmax_t i) const
     {
        i *= 2;
        const T ai = a + (offset + i);
        const T bi = b + (offset + i);

        const T an = -bi * (b + (offset + i - 1)) * (b + (offset + i - 2)) / (-(b + (offset + i - 2)) + z);
        const T bn = bi * (a + (offset + i - 1)) * z / (z - (b + (offset + i - 2)))
           + bi * (z - (b + (offset + i - 1)))
           + ai * bi * z * (b + (offset + i + 1)) / ((b + (offset + i + 1)) * (z - bi));
        const T cn = -ai * (a + (offset + i + 1)) * z * z / ((b + (offset + i + 1)) * (z - bi));

        return boost::math::make_tuple(an, bn, cn);
     }

  private:
     const T a, b, z;
     int offset;
     hypergeometric_1F1_recurrence_2a_and_2b_coefficients operator=(const hypergeometric_1F1_recurrence_2a_and_2b_coefficients&);
  };

  //
  // Recurrence relation for double-stepping on a:
  // -(b-a)(1 + b - a)/(2a-2-b+z)M(a-2,b,z)  + [(b-a)(a-1)/(2a-2-b+z) + (2a-b+z) + a(b-a-1)/(2a+2-b+z)]M(a,b,z)   -a(a+1)/(2a+2-b+z)M(a+2,b,z)
  //
  template <class T>
  struct hypergeometric_1F1_recurrence_2a_coefficients
  {
     typedef boost::math::tuple<T, T, T> result_type;

     hypergeometric_1F1_recurrence_2a_coefficients(const T& a, const T& b, const T& z, int offset = 0) :
        a(a), b(b), z(z), offset(offset)
     {
     }

     result_type operator()(boost::intmax_t i) const
     {
        i *= 2;
        const T ai = a + (offset + i);
        // -(b-a)(1 + b - a)/(2a-2-b+z)
        const T an = -(b - ai) * (b - (a + (offset + i - 1))) / (2 * (a + (offset + i - 1)) - b + z);
        const T bn = (b - ai) * (a + (offset + i - 1)) / (2 * (a + (offset + i - 1)) - b + z) + (2 * ai - b + z) + ai * (b - (a + (offset + i + 1))) / (2 * (a + (offset + i + 1)) - b + z);
        const T cn = -ai * (a + (offset + i + 1)) / (2 * (a + (offset + i + 1)) - b + z);

        return boost::math::make_tuple(an, bn, cn);
     }

  private:
     const T a, b, z;
     int offset;
     hypergeometric_1F1_recurrence_2a_coefficients operator=(const hypergeometric_1F1_recurrence_2a_coefficients&);
  };

  //
  // Recurrence relation for double-stepping on b:
  // b(b-1)^2(b-2)/((1-b)(2-b-z)) M(a,b-2,z)  + [zb(b-1)(b-1-a)/((1-b)(2-b-z)) + b(1-b-z) + z(b-a)(b+1)b/((b+1)(b+z)) ] M(a,b,z) + z^2(b-a)(b+1-a)/((b+1)(b+z)) M(a,b+2,z)
  //
  template <class T>
  struct hypergeometric_1F1_recurrence_2b_coefficients
  {
     typedef boost::math::tuple<T, T, T> result_type;

     hypergeometric_1F1_recurrence_2b_coefficients(const T& a, const T& b, const T& z, int offset = 0) :
        a(a), b(b), z(z), offset(offset)
     {
     }

     result_type operator()(boost::intmax_t i) const
     {
        i *= 2;
        const T bi = b + (offset + i);
        const T bi_m1 = b + (offset + i - 1);
        const T bi_p1 = b + (offset + i + 1);
        const T bi_m2 = b + (offset + i - 2);

        const T an = bi * (bi_m1) * (bi_m1) * (bi_m2) / (-bi_m1 * (-bi_m2 - z));
        const T bn = z * bi * bi_m1 * (bi_m1 - a) / (-bi_m1 * (-bi_m2 - z)) + bi * (-bi_m1 - z) + z * (bi - a) * bi_p1 * bi / (bi_p1 * (bi + z));
        const T cn = z * z * (bi - a) * (bi_p1 - a) / (bi_p1 * (bi + z));

        return boost::math::make_tuple(an, bn, cn);
     }

  private:
     const T a, b, z;
     int offset;
     hypergeometric_1F1_recurrence_2b_coefficients operator=(const hypergeometric_1F1_recurrence_2b_coefficients&);
  };

  //
  // Recurrence relation for a+ b-:
  // -z(b-a)(a-1-b)/(b(a-1+z)) M(a-1,b+1,z) + [(b-a)(a-1)b/(b(a-1+z)) + (2a-b+z) + a(b-a-1)/(a+z)] M(a,b,z) + a(1-b)/(a+z) M(a+1,b-1,z)
  //
  // This is potentially the most useful of these novel recurrences.
  //              -                                      -                  +        -                           +
  template <class T>
  struct hypergeometric_1F1_recurrence_a_plus_b_minus_coefficients
  {
     typedef boost::math::tuple<T, T, T> result_type;

     hypergeometric_1F1_recurrence_a_plus_b_minus_coefficients(const T& a, const T& b, const T& z, int offset = 0) :
        a(a), b(b), z(z), offset(offset)
     {
     }

     result_type operator()(boost::intmax_t i) const
     {
        const T ai = a + (offset + i);
        const T bi = b - (offset + i);

        const T an = -z * (bi - ai) * (ai - 1 - bi) / (bi * (ai - 1 + z));
        const T bn = z * ((-1 / (ai + z) - 1 / (ai + z - 1)) * (bi + z - 1) + 3) + bi - 1;
        const T cn = ai * (1 - bi) / (ai + z);

        return boost::math::make_tuple(an, bn, cn);
     }

  private:
     const T a, b, z;
     int offset;
     hypergeometric_1F1_recurrence_a_plus_b_minus_coefficients operator=(const hypergeometric_1F1_recurrence_a_plus_b_minus_coefficients&);
  };
#endif

  template <class T, class Policy>
  inline T hypergeometric_1F1_backward_recurrence_for_negative_a(const T& a, const T& b, const T& z, const Policy& pol, const char* function, int& log_scaling)
  {
    BOOST_MATH_STD_USING // modf, frexp, fabs, pow

    boost::intmax_t integer_part = 0;
    T ak = modf(a, &integer_part);
    //
    // We need ak-1 positive to avoid infinite recursion below:
    //
    if (0 != ak)
    {
       ak += 2;
       integer_part -= 2;
    }

    if (-integer_part > static_cast<boost::intmax_t>(policies::get_max_series_iterations<Policy>()))
       return policies::raise_evaluation_error<T>(function, "1F1 arguments sit in a range with a so negative that we have no evaluation method, got a = %1%", std::numeric_limits<T>::quiet_NaN(), pol);

    T first, second;
    if(ak == 0)
    {
       first = 1;
       ak -= 1;
       second = 1 - z / b;
    }
    else
    {
       int scaling1(0), scaling2(0);
       first = detail::hypergeometric_1F1_imp(ak, b, z, pol, scaling1);
       ak -= 1;
       second = detail::hypergeometric_1F1_imp(ak, b, z, pol, scaling2);
       if (scaling1 != scaling2)
       {
          second *= exp(T(scaling2 - scaling1));
       }
       log_scaling += scaling1;
    }
    ++integer_part;

    detail::hypergeometric_1F1_recurrence_a_coefficients<T> s(ak, b, z);

    return tools::apply_recurrence_relation_backward(s,
                                                     static_cast<unsigned int>(std::abs(integer_part)),
                                                     first,
                                                     second, &log_scaling);
  }

  template <class T, class Policy>
  T hypergeometric_1F1_backwards_recursion_on_b_for_negative_a(const T& a, const T& b, const T& z, const Policy& pol, const char*, int& log_scaling)
  {
     using std::swap;
     BOOST_MATH_STD_USING // modf, frexp, fabs, pow
     //
     // We compute
     //
     // M[a + a_shift, b + b_shift; z]
     //
     // and recurse backwards on a and b down to
     //
     // M[a, b, z]
     //
     // With a + a_shift > 1 and b + b_shift > z
     //
     // There are 3 distinct regions to ensure stability during the recursions:
     //
     // a > 0         :  stable for backwards on a
     // a < 0, b > 0  :  stable for backwards on a and b
     // a < 0, b < 0  :  stable for backwards on b (as long as |b| is small).
     //
     // We could simplify things by ignoring the middle region, but it's more efficient
     // to recurse on a and b together when we can.
     //

     BOOST_ASSERT(a < -1); // Not tested nor taken for -1 < a < 0

     int b_shift = itrunc(z - b) + 2;

     int a_shift = itrunc(-a);
     if (a + a_shift != 0)
     {
        a_shift += 2;
     }
     //
     // If the shifts are so large that we would throw an evaluation_error, try the series instead,
     // even though this will almost certainly throw as well:
     //
     if (b_shift > static_cast<boost::intmax_t>(boost::math::policies::get_max_series_iterations<Policy>()))
        return hypergeometric_1F1_checked_series_impl(a, b, z, pol, log_scaling);

     if (a_shift > static_cast<boost::intmax_t>(boost::math::policies::get_max_series_iterations<Policy>()))
        return hypergeometric_1F1_checked_series_impl(a, b, z, pol, log_scaling);

     int a_b_shift = b < 0 ? itrunc(b + b_shift) : b_shift;   // The max we can shift on a and b together
     int leading_a_shift = (std::min)(3, a_shift);        // Just enough to make a negative
     if (a_b_shift > a_shift - 3)
     {
        a_b_shift = a_shift < 3 ? 0 : a_shift - 3;
     }
     else
     {
        // Need to ensure that leading_a_shift is large enough that a will reach it's target
        // after the first 2 phases (-,0) and (-,-) are over:
        leading_a_shift = a_shift - a_b_shift;
     }
     int trailing_b_shift = b_shift - a_b_shift;
     if (a_b_shift < 5)
     {
        // Might as well do things in two steps rather than 3:
        if (a_b_shift > 0)
        {
           leading_a_shift += a_b_shift;
           trailing_b_shift += a_b_shift;
        }
        a_b_shift = 0;
        --leading_a_shift;
     }

     BOOST_ASSERT(leading_a_shift > 1);
     BOOST_ASSERT(a_b_shift + leading_a_shift + (a_b_shift == 0 ? 1 : 0) == a_shift);
     BOOST_ASSERT(a_b_shift + trailing_b_shift == b_shift);

     if ((trailing_b_shift == 0) && (fabs(b) < 0.5) && a_b_shift)
     {
        // Better to have the final recursion on b alone, otherwise we lose precision when b is very small:
        int diff = (std::min)(a_b_shift, 3);
        a_b_shift -= diff;
        leading_a_shift += diff;
        trailing_b_shift += diff;
     }

     T first, second;
     int scale1(0), scale2(0);
     first = boost::math::detail::hypergeometric_1F1_imp(T(a + a_shift), T(b + b_shift), z, pol, scale1);
     //
     // It would be good to compute "second" from first and the ratio - unfortunately we are right on the cusp
     // recursion on a switching from stable backwards to stable forwards behaviour and so this is not possible here.
     //
     second = boost::math::detail::hypergeometric_1F1_imp(T(a + a_shift - 1), T(b + b_shift), z, pol, scale2);
     if (scale1 != scale2)
        second *= exp(T(scale2 - scale1));
     log_scaling += scale1;

     //
     // Now we have [a + a_shift, b + b_shift, z] and [a + a_shift - 1, b + b_shift, z]
     // and want to recurse until [a + a_shift - leading_a_shift, b + b_shift, z] and [a + a_shift - leadng_a_shift - 1, b + b_shift, z]
     // which is leading_a_shift -1 steps.
     //
     second = boost::math::tools::apply_recurrence_relation_backward(
        hypergeometric_1F1_recurrence_a_coefficients<T>(a + a_shift - 1, b + b_shift, z),
        leading_a_shift, first, second, &log_scaling, &first);

     if (a_b_shift)
     {
        //
        // Now we need to switch to an a+b shift so that we have:
        // [a + a_shift - leading_a_shift, b + b_shift, z] and [a + a_shift - leadng_a_shift - 1, b + b_shift - 1, z]
        // A&S 13.4.3 gives us what we need:
        //
        {
           // local a's and b's:
           T la = a + a_shift - leading_a_shift - 1;
           T lb = b + b_shift;
           second = ((1 + la - lb) * second - la * first) / (1 - lb);
        }
        //
        // Now apply a_b_shift - 1 recursions to get down to
        // [a + 1, b + trailing_b_shift + 1, z] and [a, b + trailing_b_shift, z]
        //
        second = boost::math::tools::apply_recurrence_relation_backward(
           hypergeometric_1F1_recurrence_a_and_b_coefficients<T>(a, b + b_shift - a_b_shift, z, a_b_shift - 1),
           a_b_shift - 1, first, second, &log_scaling, &first);
        //
        // Now we need to switch to a b shift, a different application of A&S 13.4.3
        // will get us there, we leave "second" where it is, and move "first" sideways:
        //
        {
           T lb = b + trailing_b_shift + 1;
           first = (second * (lb - 1) - a * first) / -(1 + a - lb);
        }
     }
     else
     {
        //
        // We have M[a+1, b+b_shift, z] and M[a, b+b_shift, z] and need M[a, b+b_shift-1, z] for
        // recursion on b: A&S 13.4.3 gives us what we need.
        //
        T third = -(second * (1 + a - b - b_shift) - first * a) / (b + b_shift - 1);
        swap(first, second);
        swap(second, third);
        --trailing_b_shift;
     }
     //
     // Finish off by applying trailing_b_shift recursions:
     //
     if (trailing_b_shift)
     {
        second = boost::math::tools::apply_recurrence_relation_backward(
           hypergeometric_1F1_recurrence_small_b_coefficients<T>(a, b, z, trailing_b_shift),
           trailing_b_shift, first, second, &log_scaling);
     }
     return second;
  }

  } } } // namespaces

#endif // BOOST_HYPERGEOMETRIC_1F1_RECURRENCE_HPP_


#include <armadillo>
#include "GEFE_utility.h"

// Demonstration.
int main() {
    const arma::mat H("-0.2414 0.3160; 0.3160 -0.8649");
    const arma::vec ii("0 1");
    const arma::vec gefe_eigvecs = getEigenvectorFromEigenvalues(H, ii, /*column j=*/0);
    printf("\ngefe:\n");
    gefe_eigvecs.print(); //    0.1488
                          //    0.8512

    arma::vec arma_eigvals;
    arma::mat arma_eigvecs;
    arma::eig_sym(arma_eigvals, arma_eigvecs, H);

    printf("\narma::eig_sym:\n");
    arma_eigvecs = arma::square(arma_eigvecs);
    arma_eigvecs.col(0).print(); //    0.1488
                                         //    0.8512
}


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>

#include <CGAL/Delaunay_triangulation_on_sphere_2.h>
#include <CGAL/Projection_on_sphere_traits_3.h>

#include <boost/iterator/transform_iterator.hpp>

typedef CGAL::Exact_predicates_inexact_constructions_kernel          K;

typedef CGAL::Projection_on_sphere_traits_3<K>                       Traits;
typedef CGAL::Delaunay_triangulation_on_sphere_2<Traits>             DToS2;

typedef Traits::Point_3                                              Point_3;

int main(int, char**)
{
  std::vector<Point_3> points;
  points.emplace_back( 3,  1,  1);
  points.emplace_back(-8,  1,  1);
  points.emplace_back( 1,  2,  1);
  points.emplace_back( 1, -2,  1);
  points.emplace_back( 1,  1, 10);

  Traits traits(Point_3(1,1,1)); // radius is 1 by default
  DToS2 dtos(traits);

  Traits::Construct_point_on_sphere_2 cst = traits.construct_point_on_sphere_2_object();

  for(const auto& pt : points)
  {
    std::cout << "----- Inserting (" << pt
              << ") at squared distance " << CGAL::squared_distance(pt, traits.center())
              << " from the center of the sphere" << std::endl;
    dtos.insert(cst(pt));

    std::cout << "The triangulation now has dimension: " << dtos.dimension() << " and\n";
    std::cout << dtos.number_of_vertices() << " vertices" << std::endl;
    std::cout << dtos.number_of_edges() << " edges" << std::endl;
    std::cout << dtos.number_of_faces() << " solid faces" << std::endl;
    std::cout << dtos.number_of_ghost_faces() << " ghost faces" << std::endl;
  }

  CGAL::IO::write_OFF("result.off", dtos, CGAL::parameters::stream_precision(17));

  return EXIT_SUCCESS;
}


#include <pa/stages/ftle_map_generator.hpp>

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

namespace pa
{
ftle_map_generator::ftle_map_generator(partitioner* partitioner, mode mode) : partitioner_(partitioner), mode_(mode)
{

}

void                          ftle_map_generator::set_flow_map(vector_field* flow_map   )
{
  flow_map_ = flow_map;
}
void                          ftle_map_generator::set_time    (scalar        time       )
{
  time_ = time;
}

std::unique_ptr<scalar_field> ftle_map_generator::generate    ()
{
  auto ftle_map = std::make_unique<scalar_field>();
  ftle_map->data.resize(boost::extents
   [flow_map_->data.shape()[0]]
   [flow_map_->data.shape()[1]]
   [flow_map_->data.shape()[2]]);
  ftle_map->offset  = flow_map_->offset ;
  ftle_map->size    = flow_map_->size   ;
  ftle_map->spacing = flow_map_->spacing;

  auto gradient = flow_map_->gradient();
  tbb::parallel_for(tbb::blocked_range3d<std::size_t>(0, gradient->data.shape()[0], 0, gradient->data.shape()[1], 0, gradient->data.shape()[2]),
    [&] (const tbb::blocked_range3d<std::size_t>& index) {
    for (auto x = index.pages().begin(), x_end = index.pages().end(); x < x_end; ++x) {
    for (auto y = index.rows ().begin(), y_end = index.rows ().end(); y < y_end; ++y) {
    for (auto z = index.cols ().begin(), z_end = index.cols ().end(); z < z_end; ++z) {
      // Compute the spectral norm (Left Cauchy-Green tensor).
      matrix3 spectral_norm = gradient->data[x][y][z].transpose().eval() * gradient->data[x][y][z];

      // Compute eigenvalues and eigenvectors.
      Eigen::SelfAdjointEigenSolver<matrix3> solver(spectral_norm);
      auto eigenvalues  = solver.eigenvalues ();
      auto eigenvectors = solver.eigenvectors();

      // Compute FTLE.
      pa::scalar value;
      if (mode_ == mode::regular)
        value = std::log(std::sqrt(eigenvalues.maxCoeff())) / std::abs(time_);
      else if (mode_ == mode::fractional_anisotropy)
        value = std::sqrt(0.5) *
                std::sqrt(std::pow(eigenvalues[0] - eigenvalues[1], 2) + std::pow(eigenvalues[1] - eigenvalues[2], 2) + std::pow(eigenvalues[2] - eigenvalues[0], 2)) /
                std::sqrt(std::pow(eigenvalues[0], 2) + std::pow(eigenvalues[1], 2) + std::pow(eigenvalues[2], 2));

      ftle_map->data[x][y][z] = value;
  }}}});

  return ftle_map;
}
}


/*
    mvee.cpp
    Desc: minimum volume ellipsoid method
    @author Chris Larson, Cornell University
    @date (2017)
    @version 1.0
*/

#include <iostream>
#include <stdio.h>
#include <stdlib.h>
#include <fstream>
#include <algorithm>
#include <exception>
#include <string>
#include <map>
#include <cmath>
#include <vector>
#include <Eigen/Dense>
#include "boost/chrono.hpp"
#include "mvee.hpp"
#include "utils.hpp"

using namespace std;
using namespace Eigen;
using namespace mvee;

Mvee::Mvee() {}

void Mvee::decompose(MatrixXd& A, VectorXd& s, MatrixXd& U, MatrixXd& V)
{
    JacobiSVD<MatrixXd> SVD(A, ComputeThinU | ComputeThinV);
    s = SVD.singularValues();
    V = SVD.matrixV();
    U = SVD.matrixU();
}

/*
    Khachiyan's ellipsoid method
    @param X: matrix of data (n x d)
    @param eps: approximation error (default: 0.001)
    @param lmcoeff: Levenberg-Marquardt coefficient
                    used for inverting ill-condditioned
                    matrices. (default: 1e-8)
    Ref: Khachiyan (1979)
*/
void Mvee::khachiyan(MatrixXd& data, double eps, double lmcoeff)
{
    // Khachiyan's algorithm
    auto t0 = chrono::system_clock::now();
    iters = 0;
    long double err = INFINITY;
    double alpha;
    int n = data.rows();
    int d = data.cols();
    MatrixXd X = data.transpose();
    MatrixXd Q(X.rows() + 1, X.cols());
    Q.row(0) = X.row(0);
    Q.row(1) = X.row(1);
    Q.row(2).setOnes();
    VectorXd u = (1 / (double) n) * VectorXd::Ones(n);
    VectorXd uhat = u;
    MatrixXd G(d + 1, d + 1);
    MatrixXd noiseye = MatrixXd::Identity(G.rows(), G.cols()) * lmcoeff;
    VectorXd g(n);
    double m; int i;
    while (err > eps)
    {
        G = Q * u.asDiagonal() * Q.transpose() + noiseye;
        g = (Q.transpose() * G.inverse() * Q).diagonal();
        m = g.maxCoeff();
        i = findIdx(g, m);
        alpha = (m - d - 1) / ((d + 1) * (m - 1));
        uhat = (1 - alpha) * u;
        uhat(i) += alpha;
        err = (uhat - u).norm();
        u = uhat;
        iters++;
    }
    time = chrono::duration<double>(
        chrono::system_clock::now() - t0
    ).count();

    // Decompose US^2U^T
    MatrixXd E = (1 / (double) d) * (
        X * u.asDiagonal() * X.transpose()
        - (X * u) * (X * u).transpose()
    ).inverse();
    VectorXd x = X * u;
    VectorXd s(d);
    MatrixXd V(d, d);
    MatrixXd U(d, d);
    decompose(E, s, U, V);

    // Cache result
    _centroid = toStdVec(x, [](double z) { return z; });
    _radii    = toStdVec(s, [](double z) { return 1 / sqrt(z); });
    _pose     = toStdMat(V, [](double z) { return z; });
}

/*
    Computes minimum volume ellipse enclosing data in file (path/to/file.csv)
    @param file:string path to .csv file containing data
    @param delim:char delimiter character
    @param eps:double approximation error (default: 0.001)
*/
void Mvee::compute(string file, char delim, double eps, double lmcoeff)
{
    MatrixXd D = readCSV(file, delim);
    khachiyan(D, eps, lmcoeff);
}

/*
    Computes minimum volume ellipse enclosing data
    @param data:vector<vector<double>> data (n x d)
    @param delim:char delimiter character
    @param eps:double approximation error (default: 0.001)
*/
void Mvee::compute(vector<vector<double>>& data, double eps, double lmcoeff)
{
    int rows = data.size();
    int cols = data[0].size();
    MatrixXd D(rows, cols);
    for (int i=0; i<rows; i++)
    {
        for (int j=0; j<cols; j++)
        {
            D(i, j) = data[i][j];
        }
    }
    khachiyan(D, eps, lmcoeff);
}

/*
    Computes minimum volume ellipse enclosing data in file
    @param file:Eigen::MatrixXd data (n x d)
    @param delim:char delimiter character
    @param eps:double approximation error (default: 0.001)
*/
void Mvee::compute(MatrixXd& data, double eps, double lmcoeff)
{
    khachiyan(data, eps, lmcoeff);
}

/*
    Ellipse centroid
    @return centroid:vector<double> (d x 1)
    Note: Invoke mveeInstance.compute() prior to retrieving
            the ellipse parameters.
*/
vector<double> Mvee::centroid()
{
    return _centroid;
}

/*
    Ellipse radii
    @return radii:vector<double> (d x 1)
    Note: Invoke mveeInstance.compute() prior to retrieving
            the ellipse parameters.
*/
vector<double> Mvee::radii()
{
    return _radii;
}

/*
    Ellipse pose (rotation)
    @return pose:vector<vector<double>> (d x d)
    Note: Invoke mveeInstance.compute() prior to retrieving
            the ellipse parameters.
*/
vector<vector<double>> Mvee::pose()
{
    return _pose;
}

Mvee::~Mvee() {}


/**
 * \file boost/numeric/ublasx/operation/logspace.hpp
 *
 * \brief Logarithmically spaced vector
 *
 * Inspired by MATLAB's logspace function.
 *
 * <hr/>
 *
 * Copyright (c) 2015, Marco Guazzone
 *
 * 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)
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 */

#ifndef BOOST_NUMERIC_UBLASX_OPERATION_LOGSPACE_HPP
#define BOOST_NUMERIC_UBLASX_OPERATION_LOGSPACE_HPP

#include <boost/numeric/ublas/exception.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublasx/operation/element_pow.hpp>
#include <boost/numeric/ublasx/operation/linspace.hpp>
#include <cstddef>

namespace boost { namespace numeric { namespace ublasx {

using namespace ::boost::numeric::ublas;

/**
 * \brief Generates a logarithmically spaced vector.
 *
 * Generates \a n values logarithmically equally spaced between `pow(base,a)`
 * and `pow(base,b)`.
 * Note, in case \f$a<b\f$, generates a decreasing sequence.
 *
 * Inspired by MATLAB's logspace function.
 *
 * \param a The starting value of the logarithmically spaced sequence.
 * \param b The final value of the logarithmically spaced sequence.
 * \param n The number of values to generate
 * \param base The base of the logarithm
 * \return A vector of logarithmically spaced values in
 *  \f$[\mathrm{base}^a,\mathrm{base}^b]\f$; if `n=1`, returns `pow(base,b)`.
 *
 * The call `logspace(a,b,n)` is equivalent to the call:
 * ```
 *    base .^ linspace(a,b,n)
 * ```
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 */
template <typename ValueT>
BOOST_UBLAS_INLINE
vector<ValueT> logspace(ValueT a, ValueT b, std::size_t n = 100, ValueT base = 10)
{
	// pre: n > 0
	BOOST_UBLAS_CHECK( n > 0,
					   bad_argument() );
	// pre: base > 0
	BOOST_UBLAS_CHECK( base > 0,
					   bad_argument() );

	return element_pow(base, linspace(a, b, n));
}

}}} // Namespace boost::numeric::ublasx

#endif // BOOST_NUMERIC_UBLASX_OPERATION_LOGSPACE_HPP


//
//  main.cpp
//  NE - Normal Equations (for regression problem)
//
//  Created by Zhalgas Baibatyr on 1/15/16.
//  Copyright © 2016 Zhalgas Baibatyr. All rights reserved.
//

/*

    DEGREE ----- the degree of a polynomial in regression function
    N_SAMPLES -- number of training samples
    N_FEATURES - number of given input features
    features --- input features, including intercept term (xₒ = 1)
    target ----- target (output) values
    params ----- parameters (weights) with initial values (zeros)
    hypothesis - hypothesis function;

*/

#include <armadillo>

using namespace arma;

int main()
{
    /* Loading initial data: */
    mat initData;
    initData.load("../data/regression1", arma_ascii);

    /* Initializing constants: */
    const uword DEGREE = 1;
    const uword N_SAMPLES  = initData.n_rows;
    const uword N_FEATURES = initData.n_cols - 1;

    /* Transforming input data using polynomial formula: */
    mat features(N_SAMPLES, DEGREE * N_FEATURES + 1);
    features.col(0) = ones<vec>(N_SAMPLES);
    for (int i = 0; i < N_FEATURES; ++i)
    {
        for (int j = 1; j <= DEGREE; ++j)
        {
            features.col(i * DEGREE + j) = pow(initData.col(i), j);
        }
    }

    /* Preparing other data: */
    vec target = initData.col(initData.n_cols - 1);
    vec params = zeros<vec>(features.n_cols);
    vec hypothesis;

    wall_clock timer;
    double elapsedTime;
    timer.tic();

    /* Normal Equations is solved here: */
    params = (features.t() * features).i() * features.t() * target;
    hypothesis = features * params;

    /* Measuring the performance of the algorithm: */
    elapsedTime = timer.toc();
    printf("Elapsed time: %f sec.\n\n", elapsedTime);

    mat outputData = join_rows(initData.cols(0, initData.n_cols - 2), hypothesis);
    outputData.save("outputData", arma_ascii);
    hypothesis.save("hypothesis", arma_ascii);
    params.save("params", arma_ascii);

    return EXIT_SUCCESS;
}


//////////////////////////////////////////////////////////////////////////
// Author       :  Ngo Tien Dat
// Email        :  dat.ngo@epfl.ch
// Organization :  EPFL
// Purpose      :  Linear algebra library
// Date         :  15 March 2012
//////////////////////////////////////////////////////////////////////////

#include "LinearAlgebraUtils.h"
#include <armadillo>

using namespace arma;

mat LinearAlgebraUtils::SolveWithConstraints (const mat& A, const mat& B, const mat& Ac, const mat& Bc)
{
  mat P, Q;
  makeLinearParam(Ac, Bc, P, Q); // Given Ac * X = Bc, we represent X = PY + Q

  // Solution of the least square problem
  return P * solve(A*P, B-A*Q) + Q;
}

void LinearAlgebraUtils::makeLinearParam(const mat& A, const mat& B, mat& P, mat& Q)
{
  int m = A.n_rows;
  int n = A.n_cols;

  Q = solve(A, B);

  // P is an orthonormal basis for the null space of A
  mat U, V;
  vec s;
  svd(U, s, V, A);

  double tolerance = std::max(m, n) * max(s) * math::eps();

  uvec c = s > tolerance;
  int nR = sum(c);

  if (nR <= n-1)
    P = V.cols(nR, n-1);
  else
    P.set_size(V.n_rows, 0);
}

mat LinearAlgebraUtils::PseudoInverse(const mat& A, double lambda)
{
  int m = A.n_rows;
  int n = A.n_cols;

  if (m > n)
  {
    if (lambda > 0)
    {
      return solve(A.t()*A + lambda*eye(n,n), A.t());    // solve(A, B) = A \ B
    }
    else
    {
      return solve(A.t()*A, A.t());
    }
  }
  else
  {
    if (lambda > 0)
    {
      // We want to compute this: A' / ( A*A' + lambda*eye(n,n) )
      // Using relation: A / B = (B' \ A')'
      mat B = A*A.t() + lambda*eye(m,m);    // B' = B, since B is symmetric
      mat C = solve(B, A);

      return C.t();
    }
    else
    {
      // We want to compute: A' / (A*A')
      return solve(A*A.t(), A);
    }
  }
}

vec LinearAlgebraUtils::LeastSquareSolve(const mat& A, const vec& b, double lambda)
{
  if (lambda > 0)
  {
    int m = A.n_rows;
    int n = A.n_cols;

    if (m > n)      // Over-constrained system
    {
      mat AtA = A.t() * A + lambda * eye(n, n);
      mat Atb = A.t() * b;

      return solve(AtA, Atb);
    }
    else if (m < n)    // Under-constrained system
    {
      mat AAt = A * A.t() + lambda * eye(m, m);
      vec y  = solve(AAt, b);

      return A.t() * y;
    }
    else        // As many unknowns as equations
    {
      mat AlamdaI = A + lambda * eye(m, m);
      return solve(AlamdaI, b);
    }
  }
  else
  {
    return solve(A, b);
  }
}


#include <Eigen/Dense>

#include <ceres/ceres.h>
#include <ceres/rotation.h>
#include <ceres/loss_function.h>
#include <ceres/autodiff_cost_function.h>

#include <sphericalsfm/so3.h>

#include <sphericalsfm/rotation_averaging.h>

namespace sphericalsfm {

    struct RotationError
    {
        RotationError( const Eigen::Matrix3d &_meas ) : meas(_meas) { }

        template <typename T>
        bool operator()(const T* const r0,
                        const T* const r1,
                        T* residuals) const
        {
            Eigen::Matrix<T,3,3> R, R0, R1;
            for ( int i = 0; i < 3; i++ )
                for ( int j = 0; j < 3; j++ )
                    R(i,j) = T(meas(i,j));
            ceres::AngleAxisToRotationMatrix( r0, R0.data() );
            ceres::AngleAxisToRotationMatrix( r1, R1.data() );

            Eigen::Matrix<T,3,3> cycle = (R1 * R0.transpose()) * R.transpose();
            ceres::RotationMatrixToAngleAxis( cycle.data(), residuals );

            return true;
        }

        Eigen::Matrix3d meas;
    };

    void optimize_rotations( std::vector<Eigen::Matrix3d> &rotations, const std::vector<RelativeRotation> &relative_rotations )
    {
        std::vector<Eigen::Vector3d> data(rotations.size());
        for ( int i = 0; i < rotations.size(); i++ ) data[i] = so3ln(rotations[i]);

        ceres::Problem problem;
        ceres::LossFunction* loss_function = new ceres::SoftLOneLoss(0.03);
        for ( int i = 0; i < relative_rotations.size(); i++ )
        {
            std::cout << relative_rotations[i].index0 << "\n";
            std::cout << relative_rotations[i].index1 << "\n";
            std::cout << relative_rotations[i].R << "\n";
            RotationError *error = new RotationError(relative_rotations[i].R);
            ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction<RotationError, 3, 3, 3>(error);
            problem.AddResidualBlock(cost_function,
                loss_function,
                data[relative_rotations[i].index0].data(),
                data[relative_rotations[i].index1].data()
            );
        }

        ceres::Solver::Options options;
        options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
        options.minimizer_progress_to_stdout = true;
        ceres::Solver::Summary summary;
        ceres::Solve(options, &problem, &summary);
        std::cout << summary.FullReport() << "\n";
        if ( summary.termination_type == ceres::FAILURE )
        {
            std::cout << "error: ceres failed.\n";
            exit(1);
        }

        for ( int i = 0; i < rotations.size(); i++ ) rotations[i] = so3exp(data[i]);
    }

}


#include <boost/random.hpp>
#include <cmath>
#include <iostream>

#define mean 0.0
#define sigma 1.0
#define EPS 0.01
#define SAMPLES 10000000

using namespace boost::random;
using namespace std;

int main()
{
	mt19937 rng(static_cast<unsigned int>(time(0)));
	normal_distribution<double> dist(mean, sigma);

	cout << "Generating numbers..." << endl;
	int meanpoints = 0;
	int epoints = 0;
	for (int i = 0; i < SAMPLES; i++)
	{
		double rnd = dist(rng);
		if (fabs(rnd - mean) < EPS)
			meanpoints++;
		if ((fabs(rnd - mean - sqrt(2) * sigma) < EPS) ||
			(fabs(rnd - mean + sqrt(2) * sigma) < EPS))
			epoints++;
	}

	cout << "Expected f(mean)/f(mean+sqrt(2)*sigma): " << M_E << endl;
	cout << "Computed: " << meanpoints * 2.0 / epoints << endl;

	return 0;
}


#ifndef STAN_MATH_PRIM_FUN_CONSTANTS_HPP
#define STAN_MATH_PRIM_FUN_CONSTANTS_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/fun/inv.hpp>
#include <boost/math/constants/constants.hpp>
#include <cmath>
#include <limits>

namespace stan {
namespace math {

// TODO(anyone) Use constexpr when moving to C++17

/**
 * Return the base of the natural logarithm.
 *
 * @return Base of natural logarithm.
 */
inline double e() { return boost::math::constants::e<double>(); }

/**
 * Return the value of pi.
 *
 * @return Pi.
 */
inline double pi() { return boost::math::constants::pi<double>(); }

/**
 * Smallest positive value.
 */
const double EPSILON = std::numeric_limits<double>::epsilon();

/**
 * Positive infinity.
 */
const double INFTY = std::numeric_limits<double>::infinity();

/**
 * Negative infinity.
 */
const double NEGATIVE_INFTY = -INFTY;

/**
 * (Quiet) not-a-number value.
 */
const double NOT_A_NUMBER = std::numeric_limits<double>::quiet_NaN();

/**
 * Twice the value of \f$ \pi \f$,
 * \f$ 2\pi \f$.
 */
const double TWO_PI = 2.0 * pi();

/**
 * The natural logarithm of 0,
 * \f$ \log 0 \f$.
 */
const double LOG_ZERO = std::log(0.0);

/**
 * The natural logarithm of machine precision \f$ \epsilon \f$,
 * \f$ \log \epsilon \f$.
 */
const double LOG_EPSILON = std::log(EPSILON);

/**
 * The natural logarithm of \f$ \pi \f$,
 * \f$ \log \pi \f$.
 */
const double LOG_PI = std::log(pi());

/**
 * The natural logarithm of 0.5,
 * \f$ \log 0.5 \f$.
 */
const double LOG_HALF = std::log(0.5);

/**
 * The natural logarithm of 2,
 * \f$ \log 2 \f$.
 */
const double LOG_TWO = std::log(2.0);

/**
 * The natural logarithm of 2 plus the natural logarithm of \f$ \pi \f$,
 * \f$ \log(2\pi) \f$.
 */
const double LOG_TWO_PI = LOG_TWO + LOG_PI;

/**
 * The value of one quarter the natural logarithm of \f$ \pi \f$,
 * \f$ \log(\pi) / 4 \f$.
 */
const double LOG_PI_OVER_FOUR = 0.25 * LOG_PI;

/**
 * The natural logarithm of the square root of \f$ \pi \f$,
 * \f$ \log(sqrt{\pi}) \f$.
 */
const double LOG_SQRT_PI = std::log(std::sqrt(pi()));

/**
 * The natural logarithm of 10,
 * \f$ \log 10 \f$.
 */
const double LOG_TEN = std::log(10.0);

/**
 * The value of the square root of 2,
 * \f$ \sqrt{2} \f$.
 */
const double SQRT_TWO = std::sqrt(2.0);

/**
 * The value of the square root of \f$ \pi \f$,
 * \f$ \sqrt{\pi} \f$.
 */
const double SQRT_PI = std::sqrt(pi());

/**
 * The value of the square root of \f$ 2\pi \f$,
 * \f$ \sqrt{2\pi} \f$.
 */
const double SQRT_TWO_PI = std::sqrt(TWO_PI);

/**
 * The square root of 2 divided by the square root of \f$ \pi \f$,
 * \f$ \sqrt{2} / \sqrt{\pi} \f$.
 */
const double SQRT_TWO_OVER_SQRT_PI = SQRT_TWO / SQRT_PI;

/**
 * The value of 1 over the square root of 2,
 * \f$ 1 / \sqrt{2} \f$.
 */
const double INV_SQRT_TWO = inv(SQRT_TWO);

/**
 * The value of 1 over the square root of \f$ \pi \f$,
 * \f$ 1 / \sqrt{\pi} \f$.
 */
const double INV_SQRT_PI = inv(SQRT_PI);

/**
 * The value of 1 over the square root of \f$ 2\pi \f$,
 * \f$ 1 / \sqrt{2\pi} \f$.
 */
const double INV_SQRT_TWO_PI = inv(SQRT_TWO_PI);

/**
 * The value of 2 over the square root of \f$ \pi \f$,
 * \f$ 2 / \sqrt{\pi} \f$.
 */
const double TWO_OVER_SQRT_PI = 2.0 / SQRT_PI;

/**
 * The value of half the natural logarithm 2,
 * \f$ \log(2) / 2 \f$.
 */
const double HALF_LOG_TWO = 0.5 * LOG_TWO;

/**
 * The value of half the natural logarithm \f$ 2\pi \f$,
 * \f$ \log(2\pi) / 2 \f$.
 */
const double HALF_LOG_TWO_PI = 0.5 * LOG_TWO_PI;

/**
 * The value of minus the natural logarithm of the square root of \f$ 2\pi \f$,
 * \f$ -\log(\sqrt{2\pi}) \f$.
 */
const double NEG_LOG_SQRT_TWO_PI = -std::log(SQRT_TWO_PI);

/**
 * Largest rate parameter allowed in Poisson RNG
 */
const double POISSON_MAX_RATE = std::pow(2.0, 30);

/**
 * Return positive infinity.
 *
 * @return Positive infinity.
 */
inline double positive_infinity() { return INFTY; }

/**
 * Return negative infinity.
 *
 * @return Negative infinity.
 */
inline double negative_infinity() { return NEGATIVE_INFTY; }

/**
 * Return (quiet) not-a-number.
 *
 * @return Quiet not-a-number.
 */
inline double not_a_number() { return NOT_A_NUMBER; }

/**
 * Returns the difference between 1.0 and the next value
 * representable.
 *
 * @return Minimum positive number.
 */
inline double machine_precision() { return EPSILON; }

/**
 * Returns the natural logarithm of ten.
 *
 * @return Natural logarithm of ten.
 */
inline double log10() { return LOG_TEN; }

/**
 * Returns the square root of two.
 *
 * @return Square root of two.
 */
inline double sqrt2() { return SQRT_TWO; }

}  // namespace math
}  // namespace stan

#endif


/*!
 * @file matrix_coeff.hpp
 * @brief Contains implementation of diffusion coefficient.
 * @author Konrad Simon
 * @date August 2019
 */

#ifndef INCLUDE_MATRIX_COEFF_HPP_
#define INCLUDE_MATRIX_COEFF_HPP_

// Deal.ii
#include <deal.II/base/tensor_function.h>

// STL
#include <cmath>
#include <fstream>

// My Headers
#include "coefficients.h"

namespace Coefficients
{
using namespace dealii;

/*!
 * @class MatrixCoeff
 * @brief Diffusion coefficient.
 *
 * Class implements a matrix valued diffusion coefficient.
 * This coefficient must be positive definite.
 */
template <int dim>
class MatrixCoeff : public TensorFunction<2,dim>
{
public:
	MatrixCoeff ();

	virtual Tensor<2, dim> value(const Point<dim> &point) const override;
	virtual void value_list(const std::vector<Point<dim>> &points,
			std::vector<Tensor<2,dim>>  &values) const override;

private:
	const int k = 21;
	const double scale_factor = 0.9999999;

	const double alpha = PI_D/3,
				beta = PI_D/6,
				gamma = PI_D/4;
	Tensor<2,dim> rot;
};

template <>
MatrixCoeff<2>::MatrixCoeff ()
:
TensorFunction<2,2> ()
{
	rot[0][0] = cos(alpha);
	rot[0][1] = sin(alpha);
	rot[1][0] = -sin(alpha);
	rot[1][1] = cos(alpha);
}

template <>
MatrixCoeff<3>::MatrixCoeff ()
:
TensorFunction<2,3> ()
{
	rot[0][0] = cos(alpha)*cos(gamma) - sin(alpha)*cos(beta)*sin(gamma);
	rot[0][1] = -cos(alpha)*sin(gamma) - sin(alpha)*cos(beta)*cos(gamma);
	rot[0][2] = sin(alpha)*sin(beta);
	rot[1][0] = sin(alpha)*cos(gamma) + cos(alpha)*cos(beta)*sin(gamma);
	rot[1][1] = -sin(alpha)*sin(gamma) + cos(alpha)*cos(beta)*cos(gamma);
	rot[1][2] = -cos(alpha)*sin(beta);
	rot[2][0] = sin(beta)*sin(gamma);
	rot[2][1] = sin(beta)*cos(gamma);
	rot[2][2] = cos(beta);
}

template <int dim>
Tensor<2, dim>
MatrixCoeff<dim>::value(const Point<dim> &p) const
{
	Tensor<2, dim> value;
	value.clear();

	for (unsigned int d=0; d<dim; ++d)
	{
		value[d][d] = 1.0 * (1.0 - scale_factor*(
				0.5*sin(2*PI_D*k*p(0))
				+ 0.5*sin(2*PI_D*k*p(1))
				) ); /* Must be positive definite. */
	}

	value = rot * value * transpose (rot);

	return value;
}

template <int dim>
void
MatrixCoeff<dim>::value_list(const std::vector<Point<dim>> &points,
		std::vector<Tensor<2,dim>>  &values) const
{
	Assert (points.size() == values.size(),
			ExcDimensionMismatch (points.size(), values.size()) );

	for ( unsigned int p=0; p<points.size(); ++p)
	{
		values[p].clear();

		for (unsigned int d=0; d<dim; ++d)
		{
			values[p][d][d] = 1.0 * (1.0 - scale_factor*(
					0.5*sin(2*PI_D*k*points[p](0))
					+ 0.5*sin(2*PI_D*k*points[p](1))
					) ); /* Must be positive definite. */
		}

		values[p] = rot * values[p] * transpose (rot);
	}
}

} // end namespace Coefficients

#endif /* INCLUDE_MATRIX_COEFF_HPP_ */