#include "tools.hpp"

#include <Eigen/Geometry>

using namespace std ;
using namespace Eigen ;
using namespace vsim ;

static float polygon_area(const vector<Vector2f> &contour)
{
    uint n = contour.size();

    float A = 0.0f;

    for( uint p=n-1, q=0; q<n; p=q++ )
    {
        A+= contour[p].x()*contour[q].y() - contour[q].x()*contour[p].y();
    }

    return A*0.5f;
}

/*
     InsideTriangle decides if a point P is Inside of the triangle
     defined by A, B, C.
   */
bool inside_triangle(float Ax, float Ay,
                                 float Bx, float By,
                                 float Cx, float Cy,
                                 float Px, float Py)

{
    float ax, ay, bx, by, cx, cy, apx, apy, bpx, bpy, cpx, cpy;
    float cCROSSap, bCROSScp, aCROSSbp;

    ax = Cx - Bx;  ay = Cy - By;
    bx = Ax - Cx;  by = Ay - Cy;
    cx = Bx - Ax;  cy = By - Ay;
    apx= Px - Ax;  apy= Py - Ay;
    bpx= Px - Bx;  bpy= Py - By;
    cpx= Px - Cx;  cpy= Py - Cy;

    aCROSSbp = ax*bpy - ay*bpx;
    cCROSSap = cx*apy - cy*apx;
    bCROSScp = bx*cpy - by*cpx;

    return ((aCROSSbp >= 0.0f) && (bCROSScp >= 0.0f) && (cCROSSap >= 0.0f));
}

bool snip(const vector<Vector2f> &contour, int u,int v,int w,int n, vector<uint> &V)
{
    static const float EPSILON = 1.0e-10f;

    uint p;
    float Ax, Ay, Bx, By, Cx, Cy, Px, Py;

    Ax = contour[V[u]].x();
    Ay = contour[V[u]].y();

    Bx = contour[V[v]].x();
    By = contour[V[v]].y();

    Cx = contour[V[w]].x();
    Cy = contour[V[w]].y();

    if ( EPSILON > (((Bx-Ax)*(Cy-Ay)) - ((By-Ay)*(Cx-Ax))) ) return false;

    for ( p=0 ; p<n ; p++ )
    {
        if ( (p == u) || (p == v) || (p == w) ) continue;
        Px = contour[V[p]].x();
        Py = contour[V[p]].y();
        if ( inside_triangle(Ax,Ay,Bx,By,Cx,Cy,Px,Py) ) return false;
    }

    return true;
}

static float triangle_area(const Vector3f &v1, const Vector3f &v2, const Vector3f &v3) {
    Vector3f n1, n2 ;

    n1 = v1 - v2 ;
    n2 = v1 - v3 ;
    return  n1.cross(n2).norm() ;
}

Eigen::Hyperplane<float, 3> fitPlaneToPoints(const vector<Vector3f> &pts) {
    uint n_pts = pts.size() ;
    assert(n_pts >= 3) ;

    if ( n_pts == 3 )
        return Eigen::Hyperplane<float, 3>::Through(pts[0], pts[1], pts[2]) ;
    else {
        Eigen::Map<Matrix<float,Dynamic,3,RowMajor> > mat((float *)pts.data(), pts.size(), 3);
        VectorXf centroid = mat.colwise().mean();
        MatrixXf centered = mat.rowwise() - centroid.adjoint() ;
        MatrixXf cov = (centered.adjoint() * centered) / double(mat.rows() - 1);
        JacobiSVD<Matrix3f> svd(cov, ComputeFullU);
        Vector3f normal = svd.matrixU().col(2);
        return Eigen::Hyperplane<float, 3>(normal, centroid) ;
    }
}

bool triangulate(const vector<Vector3f> &pts, vector<uint32_t> &result)
{
    if ( pts.size() < 3 ) return false ;

    if ( pts.size() == 3 ) {

        result.push_back(0) ; result.push_back(1) ; result.push_back(2) ;
        return true ;
    }

    // fit plane to points

    Eigen::Hyperplane<float, 3> plane = fitPlaneToPoints(pts) ;

    Vector3f na, nb, nz = plane.normal() ;
    double q = sqrt(nz.x() * nz.x() + nz.y() * nz.y()) ;
    if ( q < 1.0e-4 )
    {
        na = Vector3f(1, 0, 0) ;
        nb = nz.cross(na) ;
    }
    else {
        na = Vector3f(nz.y()/q, -nz.x()/q, 0) ;
        nb = Vector3f(nz.x() * nz.z()/q, nz.y() * nz.z()/q, -q) ;
    }

    // project points to plane

    vector<Vector2f> contour ;
    for(uint i=0 ; i<pts.size() ; i++ ) {
        const Vector3f &pt = pts[i] ;
        contour.push_back(Vector2f(na.dot(pt), nb.dot(pt))) ;
    }

    /* allocate and initialize list of Vertices in polygon */

    uint n = contour.size();

    vector<uint> V(n) ;

    /* we want a counter-clockwise polygon in V */

    if (  polygon_area(contour) > 0 )
        for ( uint v=0; v<n; v++) V[v] = v;
    else
        for(int v=0; v<n; v++) V[v] = (n-1)-v;

    uint nv = n;

    /*  remove nv-2 Vertices, creating 1 triangle every time */
    uint count = 2*nv;   /* error detection */

    for(int m=0, v=nv-1; nv>2; )
    {
        /* if we loop, it is probably a non-simple polygon */
        if (0 >= (count--))
        {
            //** Triangulate: ERROR - probable bad polygon!
            return false;
        }

        /* three consecutive vertices in current polygon, <u,v,w> */
        uint u = v  ; if (nv <= u) u = 0;     /* previous */
        v = u+1; if (nv <= v) v = 0;     /* new v    */
        uint w = v+1; if (nv <= w) w = 0;     /* next     */

        if ( snip(contour,u,v,w,nv,V) )
        {
            uint a,b,c,s,t;

            /* true names of the vertices */
            a = V[u]; b = V[v]; c = V[w];

            /* output Triangle */
            result.push_back( a );
            result.push_back( b );
            result.push_back( c );

            m++;

            /* remove v from remaining polygon */
            for(s=v,t=v+1;t<nv;s++,t++) V[s] = V[t]; nv--;

            /* resest error detection counter */
            count = 2*nv;
        }
    }

    return true;
}

void flatten_mesh(const Mesh &mesh, std::vector<Vector3f> &vertices, std::vector<Vector3f> &normals, std::vector<Vector3f> &colors,
                  std::vector<Vector2f> tex_coords[])
{
    vector<Vector3f> cnormals ;

    if ( mesh.normals_.empty() && mesh.ptype_ == Mesh::Triangles )
        compute_normals(mesh.vertices_, mesh.vertex_indices_, cnormals );

    for( uint v=0 ; v<mesh.vertex_indices_.size() ; v++) {

        uint32_t vidx = mesh.vertex_indices_[v] ;
        const Vector3f &pos = mesh.vertices_[vidx] ;
        vertices.push_back(pos) ;

        if ( !cnormals.empty() ) {
            const Vector3f &normal = cnormals[vidx] ;
            normals.push_back(normal) ;
        }
        else if ( !mesh.normal_indices_.empty() ) {
            uint32_t nidx = mesh.normal_indices_[v] ;
            const Vector3f &normal = mesh.normals_[nidx] ;
            normals.push_back(normal) ;
        } else if ( !mesh.normals_.empty() ){
            const Vector3f &norm = mesh.normals_[vidx] ;
            normals.push_back(norm) ;
        }

        if ( !mesh.colors_.empty() ) {
            if ( !mesh.color_indices_.empty() ) {
                uint32_t cidx = mesh.color_indices_[v] ;
                const Vector3f &color = mesh.colors_[cidx] ;
                colors.push_back(color) ;
            }
            else {
                const Vector3f &color = mesh.colors_[vidx] ;
                colors.push_back(color) ;
            }
        }

        for( uint t=0 ; t<MAX_MESH_TEXTURES ; t++ ) {
            if ( !mesh.tex_coords_[t].empty() ) {
                if ( !mesh.tex_coord_indices_[t].empty() ) {
                    uint32_t tidx = mesh.tex_coord_indices_[t][v] ;
                    const Vector2f &uv = mesh.tex_coords_[t][tidx] ;
                    tex_coords[t].push_back(uv) ;
                }
                else {
                    const Vector2f &uv = mesh.tex_coords_[t][vidx] ;
                    tex_coords[t].push_back(uv) ;
                }
            }
        }
    }
}

static Vector3f normal_triangle(const Vector3f &v1, const Vector3f &v2, const Vector3f &v3)
{
    Vector3f n1, n2 ;

    n1 = v1 - v2 ;
    n2 = v1 - v3 ;
    return  n1.cross(n2).normalized() ;

}
void compute_normals(const vector<Vector3f> &vertices, const vector<uint> &indices, vector<Vector3f> &vtx_normals)
{
    vtx_normals.resize(vertices.size()) ;
    for( int i=0 ; i<vertices.size() ; i++ ) vtx_normals[i] = Vector3f::Zero() ;

    for( int i=0 ; i<indices.size() ; i+=3 )
    {
        uint idx0 = indices[i] ;
        uint idx1 = indices[i+1] ;
        uint idx2 = indices[i+2] ;
        Vector3f n = normal_triangle(vertices[idx0], vertices[idx1], vertices[idx2]) ;

        vtx_normals[idx0] += n ;
        vtx_normals[idx1] += n ;
        vtx_normals[idx2] += n ;
    }

    for( int i=0 ; i<vertices.size() ; i++ ) vtx_normals[i].normalize() ;

}


/**
 * \file inc/dcs/math/function/bell.hpp
 *
 * \brief Compute the n-th Bell number.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 *

 *
 * Copyright 2014 Marco Guazzone (marco.guazzone@gmail.com)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef DCS_MATH_FUNCTION_BELL_HPP
#define DCS_MATH_FUNCTION_BELL_HPP

#include <dcs/debug.hpp>
#include <boost/math/special_functions/binomial.hpp>
#include <vector>

namespace dcs { namespace math {

namespace detail { namespace /*<unnamed>*/ {

/**
 * \brief Compute the n-th Bell's number with the recursive formula.
 *
 * \tparam RealT The real type of the return value
 * \param n The order of the Bell's number
 * \return the n-th Bell's number
 *
 * The n-th Bell number \f$B_n\f$ can be computed as:
 * \f{equation}
 *  B_n=sum_(k=0)^(n-1){\binom{n-1}{k} B_k}
 * \f}
 *
 * \note This is a naive implementation that should not be used in practice.
 */
template <typename RealT>
RealT bell_rec(unsigned int n)
{
	if (n <= 1)
	{
		return 1;
	}

	RealT Bn = 0;
	for (unsigned int k = 0; k < n; ++k)
	{
		Bn += ::boost::math::binomial_coefficient<RealT>(n-1, k)*bell_rec<RealT>(k);
	}

	return Bn;
}

/**
 * \brief Compute the n-th Bell's number with the triangle method.
 *
 * \tparam RealT The real type of the return value
 * \param n The order of the Bell's number
 * \return the n-th Bell's number
 *
 * For details about the triangle method see [1].
 *
 * References:
 * -# J. Shallit
 *    "A triangle for the Bell numbers",
 *    In: A collection of manuscripts related to the Fibonacci sequence (eds. V.E. Hoggatt, Jr. and M. Bicknell-Johnson), pp. 69-71, Fibonacci Association, 1980
 *    [http://www.fq.math.ca/Books/Collection/shallit.pdf]
 * .
 */
template <typename RealT>
RealT bell_triangle(unsigned int n)
{
	if (n <= 1)
	{
		return 1;
	}

	::std::vector<RealT> Tup(n); // The upper row of Bell triangle
	::std::vector<RealT> Tlo(n); // The lower row of Bell triangle

    Tup[0] = 1;
	for (unsigned int i = 1; i < n; ++i)
	{
		Tlo[0] = Tup[i-1];
		for (unsigned int k = 1; k <= i; ++k)
		{
			Tlo[k] = Tlo[k-1]+Tup[k-1];
			Tup[k-1] = Tlo[k-1];
		}
		Tup[i] = Tlo[i];
	}

DCS_DEBUG_TRACE("B("<<n<<")="<<Tlo.back());
	return Tlo.back();
}

}} // Namespace detail::<unnamed>

#if 0 // Naive implementation, only used for testing/debugging purpose
/**
 * \brief Compute the n-th Bell number
 *
 * \tparam RealT The real type of the return value
 * \param n The order of the Bell's number
 * \return the n-th Bell's number
 *
 * The n-th Bell number \f$B_n\f$ is the number of ways a set of n elements can
 * be partitioned into nonempty subsets.
 * \f$B_n\f$ can be computed as:
 * \f{equation}
 *  B_n=sum_(k=0)^(n-1){\binom{n-1}{k} B_k}
 * \f}
 */
template <typename RealT>
RealT bell_naive(unsigned int n)
{
	return detail::bell_rec<RealT>(n);
}
#endif // if 0

/**
 * \brief Compute the n-th Bell number
 *
 * \tparam RealT The real type of the return value
 * \param n The order of the Bell's number
 * \return the n-th Bell's number
 *
 * The n-th Bell number \f$B_n\f$ (also called the exponential
 * number) is the number of ways a set of n elements can be partitioned
 * into nonempty subsets.
 * \f$B_n\f$ can be computed as:
 * \f{equation}
 *  B_n=sum_(k=0)^(n-1){\binom{n-1}{k} B_k}
 * \f}
 */
template <typename RealT>
RealT bell(unsigned int n)
{
	return detail::bell_triangle<RealT>(n);
}

}} // Namespace dcs::math

#endif // DCS_MATH_FUNCTION_BELL_HPP


#ifndef INVERT_MATRIX_GJ_HPP
#define INVERT_MATRIX_GJ_HPP

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

#define abs(x) ((x) < 0 ? -x : x)

namespace ublas = boost::numeric::ublas;

template<class T>
bool invert_matrix_gj(const ublas::matrix<T>& input, ublas::matrix<T>& inverse) {
    typedef ublas::matrix<T> tmatrix;
    typedef ublas::vector<T> tvector;
    typedef ublas::identity_matrix<T> imatrix;

    T eps = 1e-10;

    int h = input.size1();
    int w = input.size2();

    // get a working copy of the input and augment it by the identity matrix
    tmatrix A(h, w + h);
    imatrix I(h, h);
    for (int i = 0; i < A.size1(); i++) {
	for (int j = 0; j < A.size2(); j++) {
	    if (j < input.size2()) {
		A(i, j) = input(i, j);
	    } else {
		A(i, j) = I(i, j - input.size2());
	    }
	}
    }

    // do gauss-jordan
    for (int y = 0; y < h; y++) {
	// find max pivot
	int maxrow = y;
	for (int y2 = y+1; y2 < h; y2++) {
	    if (abs(A(y2, y)) > abs(A(maxrow,y))) {
		maxrow = y2;
	    }
	}
	// swap row y with maxrow
	ublas::matrix_row<tmatrix> current(A, y);
	ublas::matrix_row<tmatrix> toswap (A, maxrow);
	current.swap(toswap);
	// check for singularity
	if (abs(A(y, y)) <= eps) {
	    return false;
	}
	// eliminate row y
	for (int y2 = y+1; y2 < h; y2++) {
	    T c = A(y2, y) / A(y, y);
	    for (int x = y; x < w+h; x++) {
		A(y2, x) -= A(y, x) * c;
	    }
	}
    }
    // backsubstitution
    for (int y = h - 1; y >= 0; y--) {
	T c = A(y, y);
	for (int y2 = 0; y2 < y; y2++) {
	    for (int x = w+h - 1; x >= 0; x--) {
		A(y2, x) -= A(y, x) * A(y2, y) / c;
	    }
	}
	A(y, y) /= c;
	for (int x = h; x < w+h; x++) {
	    A(y, x) /= c;
	}
    }
    // get out the inverted part
    ublas::matrix_range<tmatrix> orig_range(A, ublas::range(0, h), ublas::range(h, w+h));
    inverse = orig_range;
    return true;
}

#endif/*INVERT_MATRIX_GJ_HPP*/


#include <algorithm>
#include <cmath>
#include <stdexcept>

#include <Eigen/QR>
#include <Euclid/Geometry/Spectral.h>

namespace Euclid
{

template<typename Mesh>
void WKS<Mesh>::build(const Mesh& mesh, unsigned k)
{
    _mesh = &mesh;
    auto n = spectrum(mesh, k, _loglambda, _phi2);
    // abs fix numerical error
    _lambda_max = std::abs(_loglambda(n - 1));
    _lambda_min = std::abs(_loglambda(1));
    _loglambda = _loglambda.array().abs().log().matrix().eval();
    _phi2 = _phi2.array().square().matrix().eval();
}

template<typename Mesh>
void WKS<Mesh>::build(const Mesh& mesh,
                      const Vec* eigenvalues,
                      const Mat* eigenfunctions)
{
    _mesh = &mesh;
    // abs fix numerical error
    _lambda_max = std::abs(eigenvalues->coeff(eigenvalues->size() - 1));
    _lambda_min = std::abs(eigenvalues->coeff(1));
    _loglambda = (*eigenvalues).array().abs().log().matrix().eval();
    _phi2 = (*eigenfunctions).array().square().matrix().eval();
}

template<typename Mesh>
template<typename Derived>
void WKS<Mesh>::compute(Eigen::ArrayBase<Derived>& wks,
                        unsigned escales,
                        float emin,
                        float emax,
                        float sigma)
{
    if (emin >= emax || sigma <= 0) {
        // the parameters described in paper form a linear system
        Eigen::Matrix3f A;
        Eigen::Vector3f B;
        A << 1.0f, 0.0f, -2.0f, 0.0f, 1.0f, 2.0f, 7.0f, -7.0f, escales + 0.0f;
        B << std::log(_lambda_min), std::log(_lambda_max), 0.0f;
        Eigen::Vector3f x = A.colPivHouseholderQr().solve(B);
        emin = x(0);
        emax = x(1);
        sigma = x(2);
        EASSERT(emin < 0);
        EASSERT(emax > emin);
        EASSERT(sigma > 0);
    }
    auto estep = (emax - emin) / escales;
    auto edenom = 0.5f / (sigma * sigma);
    auto vimap = get(boost::vertex_index, *_mesh);
    auto nv = num_vertices(*_mesh);
    wks.derived().resize(escales, nv);

    for (auto v : vertices(*_mesh)) {
        auto idx = get(vimap, v);
        for (size_t i = 0; i < escales; ++i) {
            auto e = emin + estep * i;
            auto wks_e = static_cast<FT>(0);
            auto ce = static_cast<FT>(0);
            for (int j = 1; j < _loglambda.size(); ++j) {
                auto exp = std::exp(-std::pow(e - _loglambda(j), 2) * edenom);
                ce += exp;
                wks_e += exp * _phi2(idx, j);
            }
            wks(i, idx) = wks_e / ce;
        }
    }
}

} // namespace Euclid


/*******************************************************************
Author: David Ge (dge893@gmail.com, aka Wei Ge)
Last modified: 11/02/2020
Allrights reserved by David Ge

Modifications
Date            Author      Description
---------------------------------------------

********************************************************************/
#include <malloc.h>

#include <math.h>
#define _USE_MATH_DEFINES // for C++
#include <cmath>

#include "HiMatrix.h"

#include <boost/multiprecision/cpp_dec_float.hpp>
using namespace boost::multiprecision;
HiMatrixTools::HiMatrixTools()
{

}

// Function to get cofactor of A[p][q] in temp[][]. n is current
// dimension of A[][]
void HiMatrixTools::HIgetCofactor(void *inA, void *intemp, int p, int q, int n)
{
	int i = 0, j = 0;
	int rowx = 0, ix = 0;
	cpp_dec_float_100 *A = (cpp_dec_float_100 *)inA;
	cpp_dec_float_100 *temp = (cpp_dec_float_100*)intemp;
	// Looping for each element of the matrix
	for (int row = 0; row < n; row++)
	{
		if (row != p)
		{
			for (int col = 0; col < n; col++)
			{
				//  Copying into temporary matrix only those element
				//  which are not in given row and column
				if (col != q)
				{
					temp[ix + j] = A[rowx + col];//A[row][col];

					j++;
					// Row is filled, so increase row index and
					// reset col index
					if (j == n - 1)
					{
						j = 0;
						i++;
						ix += (n - 1);
					}
				}
			}
		}
		rowx += n;
	}
}

/* Recursive function for finding determinant of matrix.
n is current dimension of A[][]. */
void HiMatrixTools::HIdeterminant(void *inA,void *outret, int n)
{
	cpp_dec_float_100 D = 0; // Initialize result
	cpp_dec_float_100 *A = (cpp_dec_float_100 *)inA;
	cpp_dec_float_100 *ret = (cpp_dec_float_100 *)outret;
	//  Base case : if matrix contains single element
	if (n == 1)
	{
		ret[0] = A[0];
		return;
	}
	cpp_dec_float_100 det;
	cpp_dec_float_100 *temp = (cpp_dec_float_100 *)malloc((n - 1)*(n - 1)*sizeof(cpp_dec_float_100));// [N][N]; // To store cofactors
	if (temp == NULL)
	{
		throw;
	}
	int sign = 1;  // To store sign multiplier

	// Iterate for each element of first row
	for (int f = 0; f < n; f++)
	{
		// Getting Cofactor of A[0][f]
		HIgetCofactor(A, temp, 0, f, n);
		//D += sign * A[0][f] * determinant(temp, n - 1);
		if (sign > 0)
		{
			HIdeterminant(temp, &det, n - 1);
			D += A[f] * det;
		}
		else
		{
			HIdeterminant(temp, &det, n - 1);
			D -= A[f] * det;
		}
		// terms are to be added with alternate sign
		sign = -sign;
	}
	free(temp);
	ret[0] = D;
	return;
}

// Function to get adjoint of A[N][N] in adj[N][N].
void HiMatrixTools::HIadjoint(void *inA, void *inadj, int N)
{
	cpp_dec_float_100 *A = (cpp_dec_float_100 *)inA;
	cpp_dec_float_100 *adj = (cpp_dec_float_100 *)inadj;
	if (N == 1)
	{
		adj[0] = 1;
		return;
	}

	// temp is used to store cofactors of A[][]
	int sign = 1;
	int ix;
	cpp_dec_float_100 det;
	cpp_dec_float_100 *temp = (cpp_dec_float_100 *)malloc((N - 1)*(N - 1)*sizeof(cpp_dec_float_100));

	for (int i = 0; i<N; i++)
	{
		ix = 0;
		for (int j = 0; j<N; j++)
		{
			// Get cofactor of A[i][j]
			HIgetCofactor(A, temp, i, j, N);

			// sign of adj[j][i] positive if sum of row
			// and column indexes is even.
			sign = ((i + j) % 2 == 0) ? 1 : -1;

			// Interchanging rows and columns to get the
			// transpose of the cofactor matrix
			//adj[j+ix] = (sign)*(determinant(temp, N-1));
			if (sign > 0)
			{
				HIdeterminant(temp, &det, N - 1);
				adj[i + ix] = det;
			}
			else
			{
				HIdeterminant(temp, &det, N - 1);
				adj[i + ix] = -det;
			}
			ix += N;
		}

	}
	free(temp);
}

// Function to calculate and store inverse, returns false if
// matrix is singular
bool HiMatrixTools::HIinverse(void *inA, void *inInverse, int N)
{
	cpp_dec_float_100 *A = (cpp_dec_float_100 *)inA;
	cpp_dec_float_100 *Inverse = (cpp_dec_float_100 *)inInverse;
	// Find determinant of A[][]
	cpp_dec_float_100 det;
	double ddet;
	HIdeterminant(A, &det, N);
	ddet = det.convert_to<double>();
	if (abs(ddet) < 1.1e-10)
	{
		//cout << "Singular matrix, can't find its inverse";
		return false;
	}

	// Find adjoint
	cpp_dec_float_100 *adj = (cpp_dec_float_100 *)malloc(N*N*sizeof(cpp_dec_float_100));
	if (adj == NULL) throw;
	HIadjoint(A, adj, N);

	// Find Inverse using formula "inverse(A) = adj(A)/det(A)"
	int ix = 0;
	for (int i = 0; i<N; i++)
	{
		for (int j = 0; j<N; j++)
		{
			Inverse[ix + j] = adj[ix + j] / det;
		}
		ix += N;
	}
	free(adj);
	return true;
}

void HiMatrixTools::HIMatrixMultiply(void *inA, void *inB, void *inC, int N)
{
	int i, j, ix, k, kx;
	cpp_dec_float_100 *A = (cpp_dec_float_100 *)inA;
	cpp_dec_float_100 *B = (cpp_dec_float_100 *)inB;
	cpp_dec_float_100 *C = (cpp_dec_float_100 *)inC;
	ix = 0;
	for (i = 0; i<N; i++)
	{
		for (j = 0; j<N; j++)
		{
			C[ix + j] = 0.0;
			kx = 0;
			for (k = 0; k<N; k++)
			{
				C[ix + j] += A[ix + k] * B[kx + j];
				kx += N;
			}
		}
		ix += N;
	}
}
double HiMatrixTools::HIinverseError(void *inA, void *inInverse, int N, double *errMax)
{
	double err = 0.0;
	size_t k;
	cpp_dec_float_100 *A = (cpp_dec_float_100 *)inA;
	cpp_dec_float_100 *B = (cpp_dec_float_100 *)inInverse;
	cpp_dec_float_100 *C = (cpp_dec_float_100 *)malloc(N*N*sizeof(cpp_dec_float_100));
	if (C == NULL)
		throw;
	cpp_dec_float_100 e0,em = 0.0;
	HIMatrixMultiply(A, B, C, N);
	k = 0;
	for ( int i = 0; i < N; i++)
	{
		for ( int j = 0; j < N; j++)
		{
			if (i == j)
			{
				e0 = 1.0 - C[k];
			}
			else
			{
				e0 = C[k];
			}
			err += abs(e0.convert_to<double>());
			if (e0 > em)
			{
				em = e0;
			}
			k++;
		}
	}
	free(C);
	*errMax = em.convert_to<double>();
	return err;
}


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

#include "utils.hpp"

using namespace std;
using namespace Eigen;

int main(void)
{
    // Make skew symmetric matrix
    Vector3d v;
    v << 8, 5, 6;

    cout << "== The vector ==" << endl;
    cout << v << endl;

    Matrix3d v_hat;
    Vector::wedge<double>(v_hat,v);
    cout << "== The skew symmetric matrix ==" << endl;
    cout << v_hat << endl;

    // Example Vector
    Vector3d a;
    a << 2, 5, 7;

    cout << "== Example vector ==" << endl;
    cout << a << endl;

    // Make Cross
    cout << "== v_hat * a ==" << endl;
    cout << v_hat * a << endl;
    cout << "== v x a ==" << endl;
    cout << v.cross(a) << endl;
    cout << "== -v_hat * a ==" << endl;
    cout << -v_hat * a << endl;
    cout << "== a x v ==" << endl;
    cout << a.cross(v) << endl;

    return 0;
}


#include <ql/option.hpp>
#include <ql/handle.hpp>
#include <ql/quote.hpp>
#include <ql/quotes/simplequote.hpp>
#include <ql/termstructures/yieldtermstructure.hpp>
#include <ql/termstructures/yield/flatforward.hpp>

#include <ql/termstructures/volatility/equityfx/blackconstantvol.hpp>
#include <ql/termstructures/volatility/equityfx/blackvoltermstructure.hpp>
#include <ql/time/calendars/target.hpp>
#include <ql/time/daycounters/actual360.hpp>
#include <boost/make_shared.hpp>
#include <iostream>
#include <ql/math/distributions/normaldistribution.hpp>

using std::cout;
using std::endl;
using std::vector;
using boost::shared_ptr;
using boost::make_shared;
using namespace QuantLib;

struct blackscholes {
    double value;
    double delta;
    double gamma;
    double theta;

};

blackscholes black_scholes_formula(double S, double K, double T,
                                   double r, double sigma, bool call);

using std::sqrt;
using std::exp;
using std::log;

namespace {
    QuantLib::CumulativeNormalDistribution N;
    QuantLib::NormalDistribution n;
}

blackscholes black_scholes_formula(double S, double K, double T,
                                   double r, double sigma, bool call) {
    double d1 = (1/(sigma*sqrt(T))) * (log(S/K) + (r+sigma*sigma/2)*T);
    double d2 = d1 - sigma*sqrt(T);

    blackscholes results;
    if (call) {
        cout << "Call is called: ";
        results.value = N(d1)*S - N(d2)*K*exp(-r*T);
        results.delta = N(d1);
        results.theta = -(S*n(d1)*sigma)/(2*sqrt(T)) - r*K*exp(-r*T)*N(d2);
    } else {
        cout << "Put is called: ";
        results.value = N(-d2)*K*exp(-r*T) -N(-d1)*S;
        results.delta = -N(-d1);
        results.theta = -(S*n(d1)*sigma)/(2*sqrt(T)) + r*K*exp(-r*T)*N(-d2);
    }
    results.gamma = n(d1)/(S*sigma*sqrt(T));

    return results;
}

class EuropeanOption : public Instrument {

  public:
    EuropeanOption(Real strike, Option::Type, const Date& exerciseDate,
                   const Handle<Quote>& u,
                   const Handle<YieldTermStructure>& r,
                   const Handle<BlackVolTermStructure>& sigma);
    bool isExpired() const;
    void performCalculations() const;
    Real delta() const;
    Real gamma() const;
    Real theta() const;
  private:
    Real strike_;
    Option::Type type_;
    const Date& exerciseDate_;
    const Handle<Quote>& u_;
    const Handle<YieldTermStructure>& r_;
    const Handle<BlackVolTermStructure>& sigma_;
    mutable Real delta_;
    mutable Real gamma_;
    mutable Real theta_;

};

EuropeanOption ::EuropeanOption(Real strike, Option::Type type, const Date& exerciseDate, const Handle<Quote>& u,
                                const Handle<YieldTermStructure>& r, const Handle<BlackVolTermStructure>& sigma)
: strike_(strike), type_(type), exerciseDate_(exerciseDate), u_(u), r_(r), sigma_(sigma){
    cout << "Type is " <<type_<<endl;
    registerWith(u_);
    registerWith(r_);
    registerWith(sigma_);
}

bool EuropeanOption::isExpired() const {
    Date today = Settings::instance().evaluationDate();
    return today >= exerciseDate_;
}

void EuropeanOption::performCalculations() const {
    DayCounter dayCounter = r_->dayCounter();
    Date today = Settings::instance().evaluationDate();
    Time T = dayCounter.yearFraction(today, exerciseDate_);
    Rate r = r_->zeroRate(T, Continuous);
    Volatility sigma = sigma_->blackVol(T, strike_);
    blackscholes results = black_scholes_formula(u_->value(), strike_, T, r, sigma,
                                             type_ == Option::Call);

    NPV_ = results.value;
    delta_ = results.delta;
    gamma_ = results.gamma;
    theta_ = results.theta;
}

Real EuropeanOption::delta() const {

    // before returning delta_, we ensure that it is calculated: checking calculated flag (whether instrument is
    // upto date) or it's going to trigger the calculation in case its not.
    calculate();
    return delta_;
}

Real EuropeanOption::gamma() const {

    // before returning gamma_, we ensure that it is calculated: checking calculated flag (whether instrument is
    // upto date) or it's going to trigger the calculation in case its not.
    calculate();
    return gamma_;
}

Real EuropeanOption::theta() const {

    // before returning theta_, we ensure that it is calculated: checking calculated flag (whether instrument is
    // upto date) or it's going to trigger the calculation in case its not.
    calculate();
    return theta_;
}

int main() {
    Date today(1, May, 2022);
    Settings::instance().evaluationDate() = today;

    Real strike = 100.0;
    Option::Type type = Option::Call;
    Date exerciseDate = today + 360*3;

    shared_ptr<SimpleQuote> u = make_shared<SimpleQuote>(120.0);
    Handle<Quote> U(u);

    shared_ptr<YieldTermStructure> r =
        make_shared<FlatForward>(today, 0.01, Actual360());
    RelinkableHandle<YieldTermStructure> R(r);

    shared_ptr<BlackVolTermStructure> sigma =
        make_shared<BlackConstantVol>(today, TARGET(), 0.20, Actual360());
    Handle<BlackVolTermStructure> Sigma(sigma);

    EuropeanOption option(strike, type, exerciseDate, U, R, Sigma);

    cout << "option value: " << option.NPV() << endl;
    cout << "delta: " << option.delta() << endl;
    cout << "gamma: " << option.gamma() << endl;
    cout << "theta: " << option.theta() << endl;

    u->setValue(115.0);
    cout << "option value after u decreases: " << option.NPV() << endl;

    /*
     * OUTPUT:
     *
        Type is Call
        option value: Call is called: 29.0795
        delta: 0.784103
        gamma: 0.00704601
        theta: -2.67938
        option value after u decreases: Call is called: 25.2511
     */
    return 0;
}


// Copyright John Maddock 2006, 2007
// Copyright Paul A. Bristow 2007

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

#include <iostream>
using std::cout; using std::endl;
using std::left; using std::fixed; using std::right; using std::scientific;
#include <iomanip>
using std::setw;
using std::setprecision;
#include <boost/math/distributions/chi_squared.hpp>
#include <boost/math/distributions/normal.hpp>
#include <boost/math/special_functions/erf.hpp>

double upper_confidence_limit_on_std_deviation(
        double Sd,    // Sample Standard Deviation
        unsigned N,   // Sample size
        double pds)   // Probability
{
  // Calculate confidence intervals for the standard deviation.
  // For example if we set the confidence limit to
  // 0.95, we know that if we repeat the sampling
  // 100 times, then we expect that the true standard deviation
  // will be between out limits on 95 occations.
  // Note: this is not the same as saying a 95%
  // confidence interval means that there is a 95%
  // probability that the interval contains the true standard deviation.
  // The interval computed from a given sample either
  // contains the true standard deviation or it does not.
  // See http://www.itl.nist.gov/div898/handbook/eda/section3/eda358.htm

  // using namespace boost::math;
  using boost::math::chi_squared;
  using boost::math::quantile;
  using boost::math::complement;

  // Start by declaring the distribution we'll need:
  chi_squared dist(N - 1);

  // Calculate limits:
  double lower_limit = ((N - 1) * Sd * Sd / quantile(complement(dist, 1 - pds))); // Needs to be checked if its pds or pds/2
  double upper_limit = ((N - 1) * Sd * Sd / quantile(dist, 1 - pds));
  std::cout << "Lower Limit = " << lower_limit << "\n";
  std::cout << "Upper Limit = " << upper_limit << "\n";

  return upper_limit;
}

double upper_confidence_limit_gaussian(
        double Sd,    // Sample Standard Deviation
        double pdv)   // Probability
{
  // Calculate confidence intervals for the standard deviation.
  // For example if we set the confidence limit to
  // 0.95, we know that if we repeat the sampling
  // 100 times, then we expect that the true standard deviation
  // will be between out limits on 95 occations.
  // Note: this is not the same as saying a 95%
  // confidence interval means that there is a 95%
  // probability that the interval contains the true standard deviation.
  // The interval computed from a given sample either
  // contains the true standard deviation or it does not.
  // See http://www.itl.nist.gov/div898/handbook/eda/section3/eda358.htm

  // using namespace boost::math;
  using boost::math::normal;
  using boost::math::quantile;
  using boost::math::complement;

  // Start by declaring the distribution we'll need:
  normal normdist(0, Sd);

  // Calculate limits:
  double upper_limit = quantile(normdist, pdv);

  return upper_limit;
}

double get_inverse_error_func(double probability)
{
  // Calculate the inverse gaussian error function for a given probability
  // Check if probability value is in a valid range (absolute value)
  if(probability > 1 && probability < -1)
    return 0;

  return boost::math::erf_inv(probability);
}


/**
 * @file
 * @brief We integrate a function over a square using quadrature rules of a
 * fixed degree and refine the mesh to achieve convergence.
 * @author Raffael Casagrande
 * @date   2018-09-02 05:17:45
 * @copyright MIT License
 */

#include <lf/geometry/geometry.h>
#include <lf/io/io.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/quad/quad.h>

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

#include "lf/refinement/test/refinement_test_utils.h"

template <class F>
double integrate(const lf::mesh::Mesh& mesh, lf::quad::quadDegree_t degree,
                 F f) {
  double result = 0.;
  auto qr_tria = lf::quad::make_QuadRule(lf::base::RefEl::kTria(), degree);
  auto qr_quad = lf::quad::make_QuadRule(lf::base::RefEl::kQuad(), degree);

  Eigen::MatrixXd points(1, 1);
  Eigen::VectorXd weights(1);

  for (const auto* e : mesh.Entities(0)) {
    if (e->RefEl() == lf::base::RefEl::kTria()) {
      points = qr_tria.Points();
      weights = qr_tria.Weights();
    } else {
      points = qr_quad.Points();
      weights = qr_quad.Weights();
    }
    auto mapped_points = e->Geometry()->Global(points);
    auto integration_elements = e->Geometry()->IntegrationElement(points);
    for (Eigen::Index j = 0; j < points.cols(); ++j) {
      result += f(mapped_points.col(j)) * weights(j) * integration_elements(j);
    }
  }
  return result;
}

int main(int argc, char** argv) {
  // define allowed command line arguments:
  namespace po = boost::program_options;
  po::options_description desc("Allowed options");
  // clang-format off
  desc.add_options()
  ("help", "produce this help message")
  ("quad_degree", po::value<int>()->default_value(3), "The degree of the local quadrature rule.")
  ("max_level", po::value<int>()->default_value(5), "The number of refinement levels")
  ;
  // clang-format on
  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, desc), vm);
  po::notify(vm);
  if (vm.count("help") != 0U) {
    std::cout << desc << std::endl;
    return 1;
  }

  int max_level = vm["max_level"].as<int>();
  int quad_degree = vm["quad_degree"].as<int>();

  // Create a three element mesh that contains two triangles and one
  // quadrilateral
  lf::mesh::hybrid2d::MeshFactory mesh_factory(2);
  mesh_factory.AddPoint(Eigen::Vector2d{0, 0});
  mesh_factory.AddPoint(Eigen::Vector2d{0.5, 0});
  mesh_factory.AddPoint(Eigen::Vector2d{1, 0});
  mesh_factory.AddPoint(Eigen::Vector2d{1, 1});
  mesh_factory.AddPoint(Eigen::Vector2d{0.5, 1});
  mesh_factory.AddPoint(Eigen::Vector2d{0, 1});
  Eigen::MatrixXd node_coords(2, 3);
  node_coords << 0, 0.5, 0.5, 0, 0, 1;
  mesh_factory.AddEntity(lf::base::RefEl::kTria(),
                         std::vector<lf::base::size_type>{0, 1, 4},
                         std::make_unique<lf::geometry::TriaO1>(node_coords));
  node_coords << 0, 0.5, 0, 0, 1, 1;
  mesh_factory.AddEntity(lf::base::RefEl::kTria(),
                         std::vector<lf::base::size_type>{0, 4, 5},
                         std::make_unique<lf::geometry::TriaO1>(node_coords));
  node_coords = Eigen::MatrixXd(2, 4);
  node_coords << 0.5, 1, 1, 0.5, 0, 0, 1, 1;
  mesh_factory.AddEntity(lf::base::RefEl::kQuad(),
                         std::vector<lf::base::size_type>{1, 2, 3, 4},
                         std::make_unique<lf::geometry::QuadO1>(node_coords));

  auto base_mesh = mesh_factory.Build();

  // parameters:
  auto pi = boost::math::constants::pi<double>();
  auto f = [&](const Eigen::Vector2d& x) {
    return std::sin(pi * x(0)) * std::pow(std::cos(pi * x(1)), 2);
  };
  auto exact_integral = 1. / pi;

  auto mesh = base_mesh;
  auto errors = Eigen::VectorXd(max_level + 1);
  for (int level = 0; level <= max_level; ++level) {
    lf::io::VtkWriter vtk_writer(mesh,
                                 "level" + std::to_string(level) + ".vtk");

    auto approx = integrate(*mesh, quad_degree, f);
    errors(level) = std::abs(approx - exact_integral);

    lf::refinement::MeshHierarchy mh(
        mesh, std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2));
    mh.RefineRegular();
    mesh = mh.getMesh(1);
  }

  std::cout << "measured errors for each level: " << std::endl;
  std::cout << errors << std::endl << std::endl;

  // estimate the rate of convergence:
  Eigen::MatrixXd A(max_level + 1, 2);
  A.col(0).setOnes();
  A.col(1) = (-Eigen::ArrayXd::LinSpaced(max_level + 1, 1, max_level + 1) *
              std::log(2))
                 .matrix();

  Eigen::VectorXd b = errors.transpose().array().log().matrix();

  // consider only the three smallest meshes:
  A = A.bottomRows(3);
  b = b.bottomRows(3);

  Eigen::VectorXd x =
      A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
  std::cout << "estimated order of convergence = " << x(1) << std::endl;
}


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

using namespace std;
using namespace Eigen;

int main(){
    const int n = 100;
    MatrixXd a = MatrixXd::Random(n,n);
    MatrixXd A;
    A.noalias() = a * a.transpose();
    VectorXd b = VectorXd::Random(n);
    VectorXd x;

    cout<<"直接求逆"<<endl;
    auto t0 = chrono::steady_clock::now();
    x = A.inverse()*b;
    auto t1 = chrono::steady_clock::now();
    cout<<"Cost time: "<< chrono::duration<double,std::milli>(t1-t0).count()<<"ms"<<endl;
    cout<<"x= "<<x.transpose()<<endl;

    cout<<"QR 分解"<<endl;
    auto t2 = chrono::steady_clock::now();
    x = A.colPivHouseholderQr().solve(b);
    auto t3 = chrono::steady_clock::now();
    cout<<"Cost time: "<< chrono::duration<double,std::milli>(t3-t2).count()<<"ms"<<endl;
    cout<<"x= "<<x.transpose()<<endl;

    cout<<"Cholesky 分解"<<endl;
    auto t4 = chrono::steady_clock::now();
    x = A.ldlt().solve(b);
    auto t5 = chrono::steady_clock::now();
    cout<<"Cost time: "<< chrono::duration<double,std::milli>(t5-t4).count()<<"ms"<<endl;
    cout<<"x= "<<x.transpose()<<endl;

    return 0;
}


#include <bits/stdc++.h>
#include <boost/random.hpp>
#include "util.h"

using namespace std;

void Util::generate_divisors(const vector< pair<int,int> > &prime_factorization, vector<int> &ret, int at, int sofar) {
  if (at == prime_factorization.size()) {
    ret.push_back(sofar);
  } else {
    generate_divisors(prime_factorization, ret, at+1, sofar);
    for (int i = 0; i < prime_factorization[at].second; ++i) {
      sofar *= prime_factorization[at].first;
      generate_divisors(prime_factorization, ret, at+1, sofar);
    }
  }
}

// calculate generator for multiplicative group of F_q, q prime
int Util::primitive_root(int q, boost::random::mt19937 &rng) {
  int val = q-1;
  vector< pair<int, int> > prime_factorization; // factorize q-1;
  for (int d = 2; d*d <= q; ++d)
    if (val % d == 0) {
      int cnt = 0;
      while (val % d == 0) {
	++cnt;
	val /= d;
      }
      prime_factorization.push_back(make_pair(d, cnt));
    }
  if (val != 1)
    prime_factorization.push_back(make_pair(val, 1));
  vector<int> divisors;
  generate_divisors(prime_factorization, divisors, 0, 1);
  boost::uniform_int<> unif_int = boost::uniform_int<>(1, q - 1);
  while (true) {
    // a random element of F_p^* has probability 1-o(1) of being a generator
    int z = unif_int(rng);
    bool bad = false;
    for (int div : divisors) {
      if ((div == 1) || (div == q-1))
	continue;
      if (fast_mod_pow(z, div, q) == 1) {
	bad = true;
	break;
      }
    }
    if (!bad)
      return z;
  }
}

vector<int> Util::hadamard_transform(vector<int> x) {
  int n = x.size();
  if (n == 1)
    return x;
  else {
    assert((n&1) == 0);
    vector<int> ret(n);
    vector<int> xl(n/2);
    vector<int> xh(n/2);
    for (int i = 0; i < n/2; ++i)
      xl[i] = x[i];
    for (int i = n/2; i < n; ++i)
      xh[i-n/2] = x[i];
    xl = hadamard_transform(xl);
    xh = hadamard_transform(xh);
    for (int i = 0; i < n/2; ++i) {
      ret[i] = xl[i] + xh[i];
      ret[n/2+i] = xl[i] - xh[i];
    }
    return ret;
  }
}

int Util::fast_mod_pow(int x, int n, int q) {
  if (n==0)
    return 1;
  else {
    int z = fast_mod_pow(x, n/2, q);
    z = ((int64_t)z * z) % q;
    if (n&1 == 1)
      z = ((int64_t)z * x) % q;
    return z;
  }
}

vector<int> Util::compute_inverse_table(int q, boost::random::mt19937 &rng) {
  // primitive root of finite field F_q, allows us to calculate table of all
  // compute table of all F_q multiplicative inverses in O(q) time; naively
  // would be O(q log q), which is O(k log k) for t=2 since q=Theta(k) for t=2
  assert(is_prime(q));
  int z = primitive_root(q, rng);
  vector<int> zpows(q), ret(q);
  zpows[0] = 1;
  for (int i = 1; i < q; ++i)
    zpows[i] = ((int64_t)zpows[i-1] * z) % q;
  for (int i = 0; i < q-1; ++i)
    ret[zpows[i]] = zpows[q-i-1];
  return ret;
}

int Util::first_non_zero(const vector<int> &v) {
  int i = 0;
  while ((i<v.size()) && !v[i])
    ++i;
  if (i == v.size())
    return -1;
  else
    return i;
}

// normal a nonzero vector so that it's canonical (first non-entry is a 1)
void Util::canonicalize(vector<int> &v, int q, const vector<int> &qinv) {
  int i = first_non_zero(v);
  assert(i != -1);
  int g = qinv[v[i]];
  for (int j = i; j < v.size(); ++j)
    v[j] = ((int64_t)v[j] * g) % q;
}

vector<int> Util::canonicalized_index_to_vec(int j, int l, int q) {
  // given an index in {0,...,K-1}, return the corresponding canonical vector
  // recall a canonical vector is a vector in {0,...,q-1}^t which is nonzero,
  // and its first nonzero entry (going from smallest vector index to largest)
  // has the value 1
  int tot = 0;
  int qpow = 1;
  int num_zeroes = l - 1;
  while (j >= tot + qpow) {
    tot += qpow;
    qpow *= q;
    num_zeroes -= 1;
  }
  vector<int> ret(l);
  ret[num_zeroes] = 1;
  int r = j - tot;
  int at = l - 1;
  while (r > 0) {
    ret[at] = r % q;
    r /= q;
    at -= 1;
  }
  return ret;
}

int Util::canonicalized_vec_to_index(const vector<int> &v, int q, const vector<int> &qpows) {
  // given a canonical vector in {0,...,q-1}^t, return its index in
  // {0,...,K-1}
  int leftmost_one = Util::first_non_zero(v);
  assert((leftmost_one!=-1) && (v[leftmost_one]==1));
  int ret = (qpows[v.size() - 1 - leftmost_one] - 1) / (q - 1);
  int c = 0;
  for (int i = leftmost_one + 1; i < v.size(); ++i)
    c = (c*q) + v[i];
  return ret + c;
}

// vindex is index (1-based indexing) to a canonical vector v in F_q^l
// returns a 3dim-vector (v[0], g, vsuff_index), where g is the value of the
// first non-zero entry of vsuffix (vector v with v[0] removed) or 1 if
// vsuffix is the 0 vector. vsuff_index is the index of vsuffix amongst all
// canonical vectors in F_q^{l-1}
vector<int> Util::decompose_canonical_vector(int vindex, int l, int q,
					     const vector<int> &qpows, const vector<int> &qinv) {
  assert(l > 0);
  vector<int> v(l), vsuffix(l - 1); // vindex and its suffix as vectors
  if (vindex) {
    v = Util::canonicalized_index_to_vec(vindex - 1, l, q);
    assert(Util::canonicalized_vec_to_index(v, q, qpows) == vindex - 1);
    for (int i = 1; i < v.size(); ++i)
      vsuffix[i - 1] = v[i];
  }
  int g = 1; // value of first nonzero entry in vbsuffix, else 1 if it's all zeroes
  bool vsuffix_all_zeroes = true;
  for (int i = 0; i < vsuffix.size(); ++i)
    if (vsuffix[i]) {
      g = vsuffix[i];
      vsuffix_all_zeroes = false;
      break;
    }
  if (!vsuffix_all_zeroes)
    Util::canonicalize(vsuffix, q, qinv); // multiplies vsuffix through by g^{-1} entry-wise (mod q)
  int vsuff_index = 0; // a number in the range [0, (q^{l-1}-1)/(q-1) + 1]
  if (!vsuffix_all_zeroes) {
    vsuff_index = Util::canonicalized_vec_to_index(vsuffix, q, qpows) + 1;
    assert(Util::canonicalized_index_to_vec(vsuff_index - 1, l - 1, q) == vsuffix);
  }
  assert(vsuff_index <= (qpows[l - 1] - 1) / (q - 1) + 1);
  return vector<int>( { v[0], g, vsuff_index } );
}

int Util::mod_dot_product(const vector<int> &u, const vector<int> &v, int q) {
  assert(u.size() == v.size());
  int ret = 0;
  for (int i = 0; i < u.size(); ++i)
    (ret += ((int64_t)u[i]*v[i]) % q) %= q;
  return ret;
}

bool Util::is_prime(int q) {
  if (q < 2) return false;
  else if (q == 2) return true;
  else if (q % 2 == 0) return false;
  else {
    for (int d = 3; d < q; d += 2) {
      if ((int64_t)d*d > q) break;
      if (q % d == 0) return false;
    }
    return true;
  }
}

// pick a random nonzero vector u in F_q^t amongst those with <u,v> = s
// we assume here that v is not zero (else this isn't possible for all s)
vector<int> Util::randvec_with_specified_dot_product(const vector<int>& v, int s, int q,
						     const vector<int> &qinv, boost::random::mt19937 &rng) {
  boost::uniform_int<> unif_int = boost::uniform_int<>(0, q - 1);
  vector<int> ret(v.size());
  // pick ret to be a random element of (F_q)^t \ {0} s.t. <ret,v> = s mod q
  while (true) {
    for (int i = 0; i < v.size(); ++i)
      ret[i] = unif_int(rng);
    if (accumulate(ret.begin(), ret.end(), 0) == 0)
      continue;

    int i = first_non_zero(v);
    assert(i != -1);

    // compute the dot product of ret with v, ignoring index i
    int z = 0;
    for (int j = 0; j < v.size(); ++j)
      if (j != i)
	z = (z + (int64_t)v[j]*ret[j]) % q;
    // now set ret[i] to make the dot product s
    // we need z + ret[i]*v[i] = s, so ret[i] = (s - z)*v[i]^{-1}
    ret[i] = ((int64_t)(q + s - z) * qinv[v[i]]) % q;
    // be careful; ret might now be the 0 vector. try again if it is.
    if (accumulate(ret.begin(), ret.end(), 0))
      break;
  }
#ifdef DEBUG
  // make sure the dot product is actually what we said it would be
  int calc = 0;
  for (int i = 0; i < v.size(); ++i)
    (calc += (int64_t)v[i]*ret[i]) %= q;
  assert(calc == s);
#endif
  return ret;
}


/*-------------ConjGrad.cpp---------------------------------------------------//
*
*              Conjugate Gradient Method
*
* Purpose: This file intends to solve a simple linear algebra equation:
*              Ax = b
*              Where A is a square Matrix of dimension N and x, b are vectors
*              of length N.
*
*   Notes: This will use the eigen LA library for solving things.
*
*-----------------------------------------------------------------------------*/

#include <iostream>
#include <Eigen/Core>
#include <memory>
#include <cassert>
#include <cmath>

using namespace Eigen;

// Function for Conj Gradient -- All the big stuff happens here
void conjgrad(const Matrix2d &A, const Vector2d &b, Vector2d &x_0,
              double thresh);

/*----------------------------------------------------------------------------//
* MAIN
*-----------------------------------------------------------------------------*/

int main(){

    double thresh = 0.001;

    Matrix2d a(2,2);
    Vector2d b, x;

    a(0,0) = 4; a(1,0) = 1; a(0,1) = 1; a(1,1) = 3;
    x(0) = 2; x(1) = 1;
    b(0) = 1; b(1) = 2;

    conjgrad(a, b, x, thresh);

    std::cout << "Conjugate Gradient output with threshold " << thresh << '\n';

    std::cout << x(0) << '\t' << x(1) <<'\n';

}

/*----------------------------------------------------------------------------//
* SUBROUTINES
*-----------------------------------------------------------------------------*/

// Function for Conj Gradient -- All the big stuff happens here
void conjgrad(const Matrix2d &A, const Vector2d &b, Vector2d &x_0,
              double thresh){

    Vector2d r, p;
    double alpha, diff, beta, temp1, temp2;

    // definingin first r
    Vector2d r_0 = b - (A * x_0);

    // setting x arbitrarily high to start
    Vector2d x = x_0 * 4;

    // *grumble... gustorn... grumble*
    Vector2d p_0 = r_0;

    diff = (x - x_0).norm();
    while (diff > thresh){

        // Note, matmul will output a Matrix2d with one element.
        temp1 = r_0.transpose() * r_0;
        temp2 = p_0.transpose() * A * p_0;
        alpha = temp1 / temp2;

        x = x_0 + (p_0 * alpha);
        r = r_0 - (A * alpha * p_0);

        temp1 = r.transpose() * r;
        temp2 = r_0.transpose() * r_0;

        beta = temp1 / temp2;

        p = r + (p_0 * beta);

        diff = (x - x_0).norm();;

        // set all the values to naughts
        x_0 = x;
        r_0 = r;
        p_0 = p;
    }
}


#include<iostream>
#include<fstream>
#include<vector>
#include <Eigen/Dense>
#include<string>

using Eigen::MatrixXd;
using namespace std;

void print_vector(vector<vector<double> > v);
void print_matrix_vector(vector<double> v);
MatrixXd* read_matrix(string filename);
void write_matrix(string filename, MatrixXd m);

int main(){

    MatrixXd* data = read_matrix("83ppl.csv");

    // Data arrangement:
    int Nsize = (*data).rows();
    MatrixXd x_data(Nsize, 1);
    MatrixXd y_data(Nsize, 1);
    x_data << (*data).col(3);
    y_data << (*data).col(2);
    x_data = x_data.array() / y_data.array();

    // Linear model:--------------------------------------------------------------------
    // int Msize = 2;
    // MatrixXd A(Nsize,Msize);
    // MatrixXd b(Nsize,1);

    // A << x_data, MatrixXd::Ones(Nsize,1);
    // b << y_data;

    // MatrixXd At = A.transpose();
    // MatrixXd X(Msize,1);
    // X = (At*A).inverse() * At * b;

    // double m = X(0);
    // double c = X(1);
    // cout<<"slope m: "<<m<<endl<<"const c: "<<c<<endl;

    // MatrixXd Yi_Xi(350,2);
    // for(int i=0; i<350; i++){
    //     Yi_Xi(i,0) = m*i + c;
    //     Yi_Xi(i,1) = i;
    // }
    // write_matrix("linear_model.txt", Yi_Xi);

    // Exp model--------------------------------------------------------------------

    // int Msize = 2;
    // MatrixXd A(Nsize,Msize);
    // MatrixXd b(Nsize,1);

    // A << x_data, MatrixXd::Ones(Nsize,1);
    // b << y_data.array().log();

    // MatrixXd At = A.transpose();
    // MatrixXd X(Msize,1);
    // X = (At*A).inverse() * At * b;

    // long double a1 = exp(X(1)), b1 = X(0);
    // cout<<"Exp a: "<<  a1 <<"Exp b: "<< b1<<endl;

    // MatrixXd Yi_Xi(350,2);
    // for(int i=0; i<350; i++){
    //     Yi_Xi(i,0) = a1 * exp(b1*i);
    //     Yi_Xi(i,1) = i;
    // }
    // write_matrix("exp_model.txt", Yi_Xi);

    // x-Exp model--------------------------------------------------------------------

    // int Msize = 2;
    // MatrixXd A(Nsize,Msize);
    // MatrixXd b(Nsize,1);

    // A << x_data.array().log() , MatrixXd::Ones(Nsize,1);
    // b << y_data.array().log();

    // MatrixXd At = A.transpose();
    // MatrixXd X(Msize,1);
    // X = (At*A).inverse() * At * b;

    // double a1 = exp(X(1)), b1 = X(0);
    // cout<<"x-Exp a: "<< a1  <<"x-Exp b: "<< b1 <<endl;

    // MatrixXd Yi_Xi(350,2);
    // for(int i=0; i<350; i++){
    //     Yi_Xi(i,0) = a1 * pow(i, b1);
    //     Yi_Xi(i,1) = i;
    // }
    // write_matrix("x_exp_model.txt", Yi_Xi);

    // Loagrithmic model--------------------------------------------------------------------

    // int Msize = 2;
    // MatrixXd A(Nsize,Msize);
    // MatrixXd b(Nsize,1);

    // A << x_data.array().log() , MatrixXd::Ones(Nsize,1);
    // b << y_data;

    // MatrixXd At = A.transpose();
    // MatrixXd X(Msize,1);
    // X = (At*A).inverse() * At * b;
    // double a1 = X(0);
    // double b1 = X(1);
    // cout<<"Log a: "<<  a1 <<"Log b: "<< b1 <<endl;

    // MatrixXd Yi_Xi(350,2);
    // for(int i=0; i<350; i++){
    //     Yi_Xi(i,0) = a1 * log(i) + b1;
    //     Yi_Xi(i,1) = i;
    // }
    // write_matrix("log_model.txt", Yi_Xi);

    // inv model--------------------------------------------------------------------

    int Msize = 1;
    MatrixXd A(Nsize,Msize);
    MatrixXd b(Nsize,1);

    A << MatrixXd::Ones(Nsize,1);
    b << y_data.array() * x_data.array();

    MatrixXd At = A.transpose();
    MatrixXd X(Msize,1);
    X = (At*A).inverse() * At * b;
    double a1 = X(0);
    cout<<"inv a: "<< a1 <<endl;

    MatrixXd Yi_Xi(350,2);
    for(int i=0; i<350; i++){
        Yi_Xi(i,0) = a1 / i;
        Yi_Xi(i,1) = i;
    }
    write_matrix("inv_model.txt", Yi_Xi);

    return 0;
}

// Supporting functions used:

MatrixXd* read_matrix(string filename){
    ifstream myfile;
    myfile.open(filename);
    if (!myfile.is_open()) throw runtime_error("Could not open file");
    string row_item;
    vector<vector<double> > rows;
    string line;
    while(myfile.good()){
        vector<double> row;
        getline(myfile, line);
        stringstream s(line);
        while (getline(s,row_item,',')){
            row.push_back(stod(row_item));
        }
        rows.push_back(row);
    }
    myfile.close();
    int nrows,ncols;
    nrows = rows.end() - rows.begin();
    ncols = rows[0].end() - rows[0].begin();
    MatrixXd* m = new MatrixXd(nrows,ncols);
    for(int i=0; i<nrows; i++){
        for (int j=0; j<ncols; j++){
            (*m)(i,j) = rows[i][j];
        }
    }
    // cout<<"The following is a matrix of "<<nrows<<" rows and "<<ncols<<" coloumns: "<<endl<<*m<<endl;
    return m;
}

void write_matrix(string filename, MatrixXd m){
    ofstream(myfile);
    myfile.open(filename);
    for(int i=0; i<m.rows(); i++){
        for(int j=0; j<m.cols(); j++){
            myfile << m(i,j) <<",";
        }
        myfile << endl;
    }
}

void print_vector(vector<vector<double> > v){
    vector<vector<double> >::iterator mvp;
    vector<double>::iterator vp;
    for(mvp= v.begin(); mvp!=v.end(); mvp++){
        for(vp = mvp->begin(); vp!= mvp->end(); vp++){
            cout<<*vp<<" ";
        }
        cout<<endl;
    }
}
void print_matrix_vector(vector<double> v){
    vector<double>::iterator it;
    for(it= v.begin(); it!= v.end(); it++){
        cout<<*it<<" ";
    }cout<<endl;
}


#include <chrono>
#include <functional>
#include <iostream>
#include <vector>

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

std::vector<Eigen::Triplet<double>> MakeTripletList(int n) {
	int nnz = 3 * n - 2;
	std::vector<Eigen::Triplet<double>> tripletList(nnz);

	for(int i = 0; i < n; i++) {
		tripletList.push_back(Eigen::Triplet<double>(i, i, 2));

		if(i > 0) {
			tripletList.push_back(Eigen::Triplet<double>(i, i - 1, -1));
		}

		if(i < n - 1) {
			tripletList.push_back(Eigen::Triplet<double>(i, i + 1, -1));
		}
	}

	return tripletList;
}

double Runtime(const std::function<void(void)> &f) {
	double runtime = std::numeric_limits<double>::max();
	std::chrono::duration<double, std::ratio<1> > duration;
	std::chrono::time_point<std::chrono::high_resolution_clock> start, end;

	for(int i = 0; i < 10; i++) {
		start = std::chrono::high_resolution_clock::now();
		f();
		end = std::chrono::high_resolution_clock::now();

		duration = end - start;
		runtime = std::min(runtime, duration.count());
	}

	return runtime;
}

template <class T>
std::ostream & operator<< (std::ostream &os, const std::vector<T> &v) {
	os << "[";
	if (!v.empty()) {
		os << v[0];
		for (int i = 1; i < v.size(); ++i) os << ", " << v[i];
	}
    os << "]";

    return os;
}

int main() {
	// print small example of the tridiagonal matrix
	int m = 4;
	std::vector<Eigen::Triplet<double>> tripletList = MakeTripletList(m);
	Eigen::SparseMatrix<double> S_(m, m);
	S_.setFromTriplets(tripletList.begin(), tripletList.end());
	std::cout << "If n = " << m << ", then T equals" << std::endl;
	std::cout << Eigen::MatrixXd(S_) << std::endl;

	// matrix sizes for benchmark
	std::vector<int> N = {64, 128, 256, 512};
	std::cout << "LU decomposition of T, where n = " << N << std::endl;

	// set up variables for runtime measurement
	std::vector<double> runtimeSparse;
	std::vector<double> runtimeDense;

	for (int n : N) {
		tripletList = MakeTripletList(n);

		// sparse LU decomposition
		Eigen::SparseMatrix<double> S(n, n);
		S.setFromTriplets(tripletList.begin(), tripletList.end());

		runtimeSparse.push_back(Runtime([&]() {
			Eigen::SparseLU<Eigen::SparseMatrix<double>> solver;
			solver.analyzePattern(S);
			solver.factorize(S);
		}));

		// dense LU decomposition
		Eigen::MatrixXd D(S);

		runtimeDense.push_back(Runtime([&]() {
			D.fullPivLu();
		}));
	}

	std::cout << "Runtime in seconds using storage format..." << std::endl;
	std::cout << "...sparse: " << runtimeSparse << std::endl;
	std::cout << "...dense:  " << runtimeDense << std::endl;

	return 0;
}


#include "MathUtil.h"
#include <Eigen/Geometry>
#include "Intersection.h"

namespace ptgl {

// https://github.com/s-nakaoka/choreonoid/src/Util/EigenUtil.cpp
Eigen::Matrix3d rotFromRpy(double r, double p, double y)
{
    const double cr = cos(r);
    const double sr = sin(r);
    const double cp = cos(p);
    const double sp = sin(p);
    const double cy = cos(y);
    const double sy = sin(y);

    Eigen::Matrix3d R;
    R << cp*cy, sr*sp*cy - cr*sy, cr*sp*cy + sr*sy,
         cp*sy, sr*sp*sy + cr*cy, cr*sp*sy - sr*cy,
         -sp  , sr*cp           , cr*cp;

    return R;
}

// https://github.com/s-nakaoka/choreonoid/src/Util/EigenUtil.cpp
Eigen::Vector3d omegaFromRot(const Eigen::Matrix3d& R)
{
    double alpha = (R(0,0) + R(1,1) + R(2,2) - 1.0) / 2.0;

    if(fabs(alpha - 1.0) < 1.0e-6) {   //th=0,2PI;
        return Eigen::Vector3d::Zero();

    } else {
        double th = acos(alpha);
        double s = sin(th);

        if (s < std::numeric_limits<double>::epsilon()) {   //th=PI
            return Eigen::Vector3d( sqrt((R(0,0)+1)*0.5)*th, sqrt((R(1,1)+1)*0.5)*th, sqrt((R(2,2)+1)*0.5)*th );
        }

        double k = -0.5 * th / s;

        return Eigen::Vector3d((R(1,2) - R(2,1)) * k,
                       (R(2,0) - R(0,2)) * k,
                       (R(0,1) - R(1,0)) * k);
    }
}

Eigen::Matrix3d rotFromAxisFromTo(const Eigen::Vector3d& from, const Eigen::Vector3d& to, const Eigen::Vector3d& axis)
{
    Eigen::Vector3d p0 = ptgl::calcIntersectionPointAndLine(from, Eigen::Vector3d::Zero(), axis);
    Eigen::Vector3d from0 = from - p0;
    Eigen::Vector3d to0 = to - p0;
    return rotFromTo(from0, to0);
}

Eigen::Matrix3d rotFromToSet(const Eigen::Vector3d& vecA0, const Eigen::Vector3d& vecB0, const Eigen::Vector3d& vecA1, const Eigen::Vector3d& vecB1)
{
    Eigen::Matrix3d R0 = ptgl::rotFromTo(vecA0, vecA1);
    Eigen::Vector3d tempVecB = R0 * vecB0;
    return rotFromAxisFromTo(tempVecB, vecB1, vecA1) * R0;
}

double angleFromAxisFromTo(const Eigen::Vector3d& from, const Eigen::Vector3d& to, const Eigen::Vector3d& axis) {
    Eigen::Vector3d p0 = ptgl::calcIntersectionPointAndLine(from, Eigen::Vector3d::Zero(), axis);
    Eigen::Vector3d from0 = from - p0;
    Eigen::Vector3d to0 = to - p0;

    Eigen::Quaterniond q;
    q.setFromTwoVectors(from0, to0);

    Eigen::AngleAxisd aa(q);
    Eigen::Vector3d c = aa.axis();
    double angle = aa.angle();

    Eigen::Vector3d d1 = c - axis;
    Eigen::Vector3d d2 = c + axis;
    return d1.squaredNorm() < d2.squaredNorm() ? angle : -angle;
};

double angleFromDirectionFromTo(const Eigen::Vector3d& from, const Eigen::Vector3d& to, const Eigen::Vector3d& direction) {
    Eigen::Quaterniond q;
    q.setFromTwoVectors(from, to);

    Eigen::AngleAxisd aa(q);
    Eigen::Vector3d c = aa.axis();
    double angle = aa.angle();

    Eigen::Vector3d d1 = c - direction;
    Eigen::Vector3d d2 = c + direction;
    return d1.squaredNorm() < d2.squaredNorm() ? angle : -angle;
};

// glm/gtc/matrix_transform.inl ortho
Eigen::Matrix4d ortho(double left, double right, double bottom, double top)
{
    Eigen::Matrix4d Result(Eigen::Matrix4d::Identity());
    Result(0,0) = 2.0 / (right - left);
    Result(1,1) = 2.0 / (top - bottom);
    Result(0,3) = - (right + left) / (right - left);
    Result(1,3) = - (top + bottom) / (top - bottom);
    return Result;
}

// glm/gtc/matrix_transform.inl  orthoRH_NO
Eigen::Matrix4d ortho(double left, double right, double bottom, double top, double zNear, double zFar)
{
    Eigen::Matrix4d Result(Eigen::Matrix4d::Identity());
    Result(0,0) = 2.0 / (right - left);
    Result(1,1) = 2.0 / (top - bottom);
    Result(2,2) = - 2.0 / (zFar - zNear);
    Result(0,3) = - (right + left) / (right - left);
    Result(1,3) = - (top + bottom) / (top - bottom);
    Result(2,3) = - (zFar + zNear) / (zFar - zNear);
    return Result;
}

// project/unproject

// glm/gtc/matrix_transform.inl  projectNO
// object(x,y,z) -> window(x,y,z)
Eigen::Vector3d project(const Eigen::Vector3d& object_p, const Eigen::Matrix4d& modelMatrix, const Eigen::Matrix4d& projMatrix, const Eigen::Vector4d& viewport)
{
    Eigen::Vector4d tmp = Eigen::Vector4d(object_p(0), object_p(1), object_p(2), 1.0);
    tmp = Eigen::Vector4d(modelMatrix * tmp);
    tmp = Eigen::Vector4d(projMatrix * tmp);

    tmp /= tmp.w();
    tmp = tmp.array() * 0.5 + 0.5;
    tmp[0] = tmp[0] * viewport[2] + viewport[0];
    tmp[1] = tmp[1] * viewport[3] + viewport[1];
    return Eigen::Vector3d(tmp[0], tmp[1], tmp[2]);
}

// glm/gtc/matrix_transform.inl  unProjectNO
// window(x,y,z) -> object(x,y,z)
Eigen::Vector3d unProject(const Eigen::Vector3d& wp, const Eigen::Matrix4d& modelMatrix, const Eigen::Matrix4d& projMatrix, const Eigen::Vector4d& viewport)
{
    Eigen::Matrix4d Inverse = Eigen::Matrix4d(projMatrix * modelMatrix).inverse();

    Eigen::Vector4d tmp = Eigen::Vector4d(wp(0), wp(1), wp(2), 1.0);
    tmp(0) = (tmp(0) - viewport[0]) / viewport[2];
    tmp(1) = (tmp(1) - viewport[1]) / viewport[3];
    tmp = Eigen::Vector4d(tmp.array() * 2.0 - 1.0);

    Eigen::Vector4d obj = Inverse * tmp;
    obj /= obj.w();

    return Eigen::Vector3d(obj[0], obj[1], obj[2]);
}

} /* namespace ptgl */


#pragma once
#include <Eigen/Core>
#include <Eigen/Sparse>
#include <vector>

#include "mass_matrix.hpp"

//----------------AssembleMatrixBegin----------------
//! Assemble the mass matrix
//! for the linear system.
//!
//!
//! @param[out] triplets will at the end contain the Galerkin matrix
//! @param[in] vertices a list of triangle vertices
//! @param[in] triangles a list of triangles
SparseMatrix assembleMassMatrix(
    const Eigen::MatrixXd &vertices,
    const Eigen::MatrixXi &triangles) {
	std::vector<Triplet> triplets;
	const int            numberOfElements = triangles.rows();
	SparseMatrix         M(vertices.rows(), vertices.rows());

	// (write your solution here)
	triplets.reserve(numberOfElements * 3 * 3);
	for (int i = 0; i < numberOfElements; ++i) {
		auto &indexSet = triangles.row(i);

		const auto &a = vertices.row(indexSet(0));
		const auto &b = vertices.row(indexSet(1));
		const auto &c = vertices.row(indexSet(2));

		Eigen::Matrix3d massMatrix;
		computeMassMatrix(massMatrix, a, b, c);

		for (int n = 0; n < 3; ++n) {
			for (int m = 0; m < 3; ++m) {
				triplets.emplace_back(indexSet(n), indexSet(m), massMatrix(n, m));
			}
		}
	}

	M.setFromTriplets(triplets.begin(), triplets.end());

	return M;
}
//----------------AssembleMatrixEnd----------------


/*=================================================================
* Create rotation around a line by an angle theta
*
* according to David Legland's matlab toolbox: geom3d
*
* Junjie Cao, 2013, jjcao1231@gmail.com
*
=================================================================*/

#include <mex.h>
#include <Eigen/Dense>
#include <vector>

Eigen::Matrix4d create_translation3d(Eigen::Vector3d center)
{
	double dx(center.coeff(0)),dy(center.coeff(1)),dz(center.coeff(2));
	Eigen::Matrix4d trans;
	trans << 1,0,0,dx,
		    0,1,0,dy,
			0,0,1,dz,
			0,0,0,1;
	return trans;
}
Eigen::Matrix4d recenter_transform3d(Eigen::Matrix4d &transfo, Eigen::Vector3d& center)
{
	//% remove former translation part
	Eigen::Matrix4d res = Eigen::Matrix4d::Identity();
	res.block(0, 0, 3, 3) = transfo.block(0, 0, 3, 3);

	//% create translations
	Eigen::Matrix4d t1 = create_translation3d(-center);
	Eigen::Matrix4d t2 = create_translation3d(center);

	//% compute translated transform
	res = t2*res*t1;
	return res;
}
Eigen::Matrix4d create_rotation3d_line_angle(Eigen::Vector3d& center,Eigen::Vector3d& v, double theta)
{
	//% normalize vector
	v.normalize();

	//% compute projection matrix P and anti-projection matrix
	Eigen::Matrix3d P = v * v.transpose();
	Eigen::Matrix3d Q;
	Q << 0, -v.coeff(2), v.coeff(1),
		v.coeff(2), 0, -v.coeff(0),
		-v.coeff(1), v.coeff(0), 0;
	Eigen::Matrix3d I = Eigen::Matrix3d::Identity();

	//% compute vectorial part of the transform
	Eigen::Matrix4d mat = Eigen::Matrix4d::Identity();
	mat.block(0, 0, 3, 3) = P + (I - P)*cos(theta) + Q*sin(theta);

	//% add translation coefficient
	mat = recenter_transform3d(mat, center);

	return mat;
}
void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray*prhs[])
{
	////////////////// parse input
	if ( nrhs != 3)
		mexErrMsgTxt("3 arguments needed!");

	double *center_, *vector_, *theta_;
	center_ = mxGetPr(prhs[0]);
	vector_ = mxGetPr(prhs[1]);
	theta_ = mxGetPr(prhs[2]);

	if ( mxGetN(prhs[0]) != 3)
		mexErrMsgTxt("center must be 1*3 matrix!");
	if ( mxGetN(prhs[1]) != 3)
		mexErrMsgTxt("vector must be 1*3 matrix!");

	Eigen::Vector3d center = Eigen::Map<Eigen::Vector3d>(center_);
	Eigen::Vector3d v = Eigen::Map<Eigen::Vector3d>(vector_);

	/////////////////////////////
	Eigen::Matrix4d rot = create_rotation3d_line_angle(center,v,*theta_);

	//////////////////////// output
	plhs[0] = mxCreateDoubleMatrix(4,4,mxREAL);
	double *rot_ = mxGetPr(plhs[0]);
	Eigen::Map<Eigen::Matrix4d>(rot_, 4, 4) = rot;  // may contain mem error
}


#pragma once

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Sparse>
//! Sparse Matrix type. Makes using this type easier.
typedef Eigen::SparseMatrix<double> SparseMatrix;

//! Used for filling the sparse matrix.
typedef Eigen::Triplet<double> Triplet;
#include "boundary.hpp"
#include "mass_matrix_assembly.hpp"
#include "neumann_boundary_assemble.hpp"
#include "stiffness_matrix_assembly.hpp"

//! Evolves the solution in time using the DIRK2 method.
//!
//! @param vertices the list of vertices that build the triangular mesh
//! @param triangles the list of indices that represent the triangular mesh
//! @param u0 the initial data
//! @param gamma the parameter gamma (has to do with the boundary conditions)
//! @param m number of timesteps to perform
//! @returns a pair which contains as first parameter the solution at time = 1,
//!          and second parameter the energy evolving over time.
std::pair<Eigen::VectorXd, std::vector<double>> radiativeTimeEvolutionImplicit(const Eigen::MatrixXd &vertices,
                                                                               const Eigen::MatrixXi &triangles,
                                                                               const Eigen::VectorXd &u0,
                                                                               const double           gamma,
                                                                               const int              m) {
	std::vector<double> energy;
	Eigen::VectorXd     u(u0.size());
	u = u0;

	double dt     = 1.0 / m;
	double lambda = 1 - 0.5 * std::sqrt(2);

	// Get the edges, this is used to compute the neumann boundary matrix
	Eigen::MatrixXi edges = getBoundaryEdges(vertices, triangles);

	// Define the needed matrices. Note that these are constant in time,
	// so they can be defined before the time loop. Also set up the solver
	// (write your solution here)

	SparseMatrix M = assembleMassMatrix(vertices, triangles);
	SparseMatrix A = assembleStiffnessMatrix(vertices, triangles);
	SparseMatrix B = assembleBoundaryMatrix(vertices, edges, gamma);
	A += B;

	Eigen::SimplicialLDLT<SparseMatrix> Solver;
	Solver.compute(M + dt * lambda * A);

	if (Solver.info() != Eigen::Success) {
		throw std::runtime_error("Could not decompose matix for SDIRK");
	}

	auto solveY1 = [&](const Eigen::VectorXd &mu) -> Eigen::VectorXd {
		return Solver.solve(M * mu);
	};

	auto solveY2 = [&](const Eigen::VectorXd &mu, const Eigen::VectorXd &Y1) -> Eigen::VectorXd {
		return Solver.solve(M * mu - dt * (1 - lambda) * A * Y1);
	};

	Eigen::SimplicialLDLT<SparseMatrix> SolverM;
	SolverM.compute(M);

	if (SolverM.info() != Eigen::Success) {
		throw std::runtime_error("Could not decompose matrix M");
	}

	Eigen::VectorXd Y1(u0.rows());
	Eigen::VectorXd Y2(u0.rows());
	for (int timestep = 0; timestep < m; ++timestep) {
		// Do one step forward in time
		// (write your solution here)
		Y1 = solveY1(u);
		Y2 = solveY2(u, Y1);
		u  = SolverM.solve(M * u - dt * A * ((1 - lambda) * Y1 + lambda * Y2)).eval();
		// Compute the energy for this level
		energy.push_back(u.sum() / u.rows());
	}

	return std::make_pair(u, energy);
}


#include <Eigen/Dense>
#include <cmath>
#include <vector>
#include "Adagrad.hpp"

#define EIGEN_MPL2_ONLY
using namespace Eigen;
using namespace std;

double Adagrad::sigma(const MatrixXd& _x, int i){
  return sigmoid(_x.row(i).dot(w));
}

double Adagrad::sigmoid(double z){
  return 1.0 / (1.0 + exp(-z));
}

Adagrad::Adagrad(int _N,int _d,MatrixXd _x,VectorXd _label,double _C,double _eta,int iter):
  N(_N),
  d(_d),
  X(_x),
  label(_label),
  C(_C),
  eta(_eta),
  iteration(iter),
  E(VectorXd::Zero(d)),
  w(VectorXd::Random(d))
{}

double Adagrad::Acc(vector<double>& pred,VectorXd &l){
  int t =0;
  double loss =0;
  for(int i = 0; i< pred.size();i++){
    loss += (l(i) - pred[i]) * (l(i) - pred[i]);
    int s = pred[i] > 0.5 ? 1 : 0;
    if(s == l(i)){
      t++;
    }
  }
  return (double)t/pred.size();
}

void Adagrad::train(){
  for(int iter = 0; iter< iteration; iter++){
    for(int i = 0; i < N; i++){
      double pred = sigma(X,i);
      for(int idx = 0; idx < d; idx++){
        if(X(i,idx) != 0){
          double grad = (pred - label(i)) * X(i,idx) + C * w(idx);
          E(idx) +=grad * grad;
          w(idx) -= (eta/sqrt(E(idx)))*grad;
        }
      }
    }
    vector<double> ret;
    predict(X,label,ret);
    iterscores.push_back(Acc(ret,label));
  }
}
void Adagrad::predict(MatrixXd& _x,VectorXd& _l,vector<double>& ret){
  for(int i = 0; i < _x.rows(); i++){
    ret.push_back(sigma(_x,i));
  }
}


// Released under the terms of the BSD License
// (C) 2014-2017
//   Analog Devices, Inc
//   Kevin Mehall <km@kevinmehall.net>
//   Ian Daniher <itdaniher@gmail.com>

#include <cmath>
#include <vector>

#include "debug.hpp"
#include <libsmu/libsmu.hpp>
#include <boost/math/constants/constants.hpp>

const double PI = boost::math::constants::pi<double>();

using namespace smu;

void Signal::constant(std::vector<float>& buf, uint64_t samples, float val)
{
	m_src = CONSTANT;
	m_src_v1 = val;

	for (unsigned i = 0; i < samples; i++) {
		buf.push_back(get_sample());
	}
}

void Signal::square(std::vector<float>& buf, uint64_t samples, float midpoint, float peak, double period, double phase, double duty)
{
	m_src = SQUARE;
	m_src_phase = phase;
	m_src_period = period;
	m_src_v1 = midpoint;
	m_src_v2 = peak;
	m_src_duty = duty;

	for (unsigned i = 0; i < samples; i++) {
		buf.push_back(get_sample());
	}
}

void Signal::sawtooth(std::vector<float>& buf, uint64_t samples, float midpoint, float peak, double period, double phase)
{
	m_src = SAWTOOTH;
	m_src_phase = phase;
	m_src_period = period;
	m_src_v1 = midpoint;
	m_src_v2 = peak;

	for (unsigned i = 0; i < samples; i++) {
		buf.push_back(get_sample());
	}
}

void Signal::stairstep(std::vector<float>& buf, uint64_t samples, float midpoint, float peak, double period, double phase)
{
	m_src = STAIRSTEP;
	m_src_phase = phase;
	m_src_period = period;
	m_src_v1 = midpoint;
	m_src_v2 = peak;

	for (unsigned i = 0; i < samples; i++) {
		buf.push_back(get_sample());
	}
}

void Signal::sine(std::vector<float>& buf, uint64_t samples, float midpoint, float peak, double period, double phase)
{
	m_src = SINE;
	m_src_phase = phase;
	m_src_period = period;
	m_src_v1 = midpoint;
	m_src_v2 = peak;

	for (unsigned i = 0; i < samples; i++) {
		buf.push_back(get_sample());
	}
}

void Signal::triangle(std::vector<float>& buf, uint64_t samples, float midpoint, float peak, double period, double phase)
{
	m_src = TRIANGLE;
	m_src_phase = phase;
	m_src_period = period;
	m_src_v1 = midpoint;
	m_src_v2 = peak;

	for (unsigned i = 0; i < samples; i++) {
		buf.push_back(get_sample());
	}
}

// Internal function to generate waveform values.
float Signal::get_sample()
{
	switch (m_src) {
		case CONSTANT:
			return m_src_v1;

		case SQUARE:
		case SAWTOOTH:
		case SINE:
		case STAIRSTEP:
		case TRIANGLE: {

			auto peak_to_peak = m_src_v2 - m_src_v1;
			auto phase = m_src_phase;
			auto norm_phase = phase / m_src_period;
			if (norm_phase < 0)
				norm_phase += 1;
			m_src_phase = fmod(m_src_phase + 1, m_src_period);

			switch (m_src) {
				case SQUARE:
					return (norm_phase < m_src_duty) ? m_src_v1 : m_src_v2;

				case SAWTOOTH: {
					float int_period = truncf(m_src_period);
					float int_phase = truncf(phase);
					float frac_period = m_src_period - int_period;
					float frac_phase = phase - int_phase;
					float max_int_phase;

					// Get the integer part of the maximum value phase will be set at.
					// For example:
					// - If m_src_period = 100.6, phase first value = 0.3 then
					//   phase will take values: 0.3, 1.3, ..., 98.3, 99.3, 100.3
					// - If m_src_period = 100.6, phase first value = 0.7 then
					//   phase will take values: 0.7, 1.7, ..., 98.7, 99.7
					if (frac_period <= frac_phase)
						max_int_phase = int_period - 1;
					else
						max_int_phase = int_period;
                    auto nphase = int_phase / max_int_phase;
                    if(nphase < 0)
                        nphase += 1;
                    return m_src_v2 - nphase * peak_to_peak;
				}

				case STAIRSTEP:
					return m_src_v2 - floorf(norm_phase * 10) * peak_to_peak / 9;

				case SINE:
                    return m_src_v1 + (1 + cos(norm_phase * 2 * PI)) * peak_to_peak / 2;

				case TRIANGLE:
					return m_src_v1 + fabs(1 - norm_phase * 2) * peak_to_peak;
				default:
					throw std::runtime_error("unknown waveform");
			}
		}
	}
	throw std::runtime_error("unknown waveform");
}


#include "Optimize.h"

#include <armadillo>
#include <functional>

arma::vec Gradient_Approximate( const arma::vec & x,
								std::function<double( const arma::vec & )> func,
								double resolution )
{
	arma::vec gradient_vector = arma::zeros( x.size() );
	for( auto i = 0; i < x.size(); i++ )
	{
		arma::vec offset = arma::zeros<arma::vec>( x.size() );
		offset( i ) = resolution;
		//double debug1 = func( x + offset );
		//double debug2 = func( x - offset );

		//gradient_vector( i ) = (func( x + offset ) - func( x - offset )) / (2 * resolution);
		gradient_vector( i ) = (func( x + offset ) - func( x - offset ));// / (2 * resolution);
	}

	return gradient_vector;
}

arma::mat Hessian_Approximate( const arma::vec & x,
							   std::function<double( const arma::vec & )> func,
							   double resolution )
{
	arma::mat hessian_matrix( x.size(), x.size() );
	const double denominator = 4 * resolution * resolution;
	for( int j = 0; j < x.size(); j++ )
	{
		arma::vec offset_j = arma::zeros<arma::vec>( x.size() );
		offset_j( j ) = resolution;
		for( int i = 0; i <= j; i++ )
		{
			arma::vec offset_i = arma::zeros<arma::vec>( x.size() );
			offset_i( i ) = resolution;
			//double debug1 = func( x + offset_i + offset_j );
			//double debug2 = func( x + offset_i - offset_j );
			//double debug3 = func( x - offset_i + offset_j );
			//double debug4 = func( x - offset_i - offset_j );
			double delta_f = func( x + offset_i + offset_j ) - func( x + offset_i - offset_j ) - func( x - offset_i + offset_j ) + func( x - offset_i - offset_j );
			if( abs( delta_f ) < 1E-9 )
				int i = 0;
			//double debug5 = delta_f / denominator;
			double result = delta_f;// / denominator;
			hessian_matrix( i, j ) = result;
			hessian_matrix( j, i ) = result;
		}
	}

	return hessian_matrix;
}

arma::vec Minimize_Function_Starting_Point( std::function<double( const arma::vec & )> function_to_minimize,
											const arma::vec & starting_point,
											int max_iteration_count,
											double attenuation_coefficient,
											double resolution,
											double biggest_step_size,
											std::function<void( arma::vec )> iteration_finished_callback )
{
	arma::vec current_guess = starting_point;
	arma::vec previous_direction = arma::zeros( current_guess.size() );
	for( int i = 0; i < max_iteration_count; i++ )
	{
		std::cout << "current_guess: " << function_to_minimize( current_guess ) << " =";
		for( double x : current_guess )
			std::cout << " " << x;
		std::cout << std::endl;

		arma::vec gradient = Gradient_Approximate( current_guess, function_to_minimize, resolution );
		//gradient = gradient( arma::span( 0, 1 ) );
		//std::cout << "gradient: " << gradient << std::endl;
		//arma::mat hessian = Hessian_Approximate( current_guess, function_to_minimize, resolution );
		////hessian = hessian( arma::span( 0, 1 ), arma::span( 0, 1 ) );
		////std::cout << "hessian: " << hessian << std::endl;
		//arma::mat inverse_hessian;
		//try
		//{
		//	inverse_hessian = arma::inv( hessian );
		//}
		//catch( ... )
		//{
		//	current_guess = 1.1 * current_guess;
		//	continue;
		//}
		//std::cout << "inverse_hessian: " << inverse_hessian << std::endl;

		//arma::vec move_vector = -attenuation_coefficient * (inverse_hessian * gradient) * (2 * resolution);
		//if( arma::norm( move_vector ) > 10 * resolution )
		//	move_vector = move_vector / arma::norm( move_vector ) * 10 * resolution;
		//for( int j = 0; j < current_guess.size(); j++ )
		//{
		//	if( abs( move_vector( j ) ) > 0.0005 * abs( current_guess( j ) ) )
		//		move_vector( j ) = 0.0005 * abs( current_guess( j ) ) * move_vector( j ) / abs( move_vector( j ) );
		//}
		arma::vec to_zero = -gradient / (2 * resolution) / function_to_minimize( current_guess );
		arma::vec move_vector = std::min( arma::norm( to_zero ), biggest_step_size ) * arma::normalise( to_zero );
		//double length = arma::norm( gradient ) / (2 * resolution);
		//arma::vec move_vector = -arma::normalise( gradient ) * std::min( length, 1000 * resolution );
		//arma::vec extend_thing = { 0, 0, 0 };
		//extend_thing( arma::span( 0, 1 ) ) = move_vector;
		//move_vector = extend_thing;
		//move_vector( 2 ) = 0;
		std::cout << "Move vector: ";
		for( double x : move_vector )
			std::cout << " " << x;
		std::cout << std::endl;
		current_guess = current_guess + move_vector;
		iteration_finished_callback( current_guess );
		//std::cout << "current_guess: " << current_guess( 0 ) << " " << current_guess( 1 ) << std::endl;
		if( arma::dot( move_vector, move_vector ) < resolution * resolution )
			break; // Quit out if we are barely moving anymore
		if( arma::dot( move_vector, previous_direction ) < 0 )
			biggest_step_size *= 0.9;

		previous_direction = move_vector;
	}

	return current_guess;
}

arma::mat Minimize_Function( std::function<double( const arma::vec & )> function_to_minimize,
							 const std::vector< std::tuple<double, double> > & bounds,
							 const int max_iteration_count )
{
	return arma::mat();
}

double Newtons_Method( std::function<double( double )> func, std::function<double( double )> derivative, double starting_point, double resolution, int max_iteration_count )
{
	double x_i = starting_point;
	double x_i_old;
	int iteration = 0;
	for( int iteration = 0; iteration < max_iteration_count; iteration++ )
	{
		x_i_old = x_i;
		x_i = x_i - func( x_i ) / derivative( x_i );
		if( abs( x_i - x_i_old ) < resolution )
		{
			return x_i;
		}
	}
}

double Binary_Search( std::function<double( double )> func, double left_most, double right_most, double resolution, int max_iteration_count )
{
	// Binary search
	double left = left_most, right = right_most;
	while( right - left > resolution )
	{
		double center = (right + left) / 2;
		if( func( center ) > 0 )
			right = center;
		else
			left = center;
	}
	double center = (right + left) / 2;

	return center;
}


// -*- coding: utf-16 -*-
#pragma once

/*! @file include/fractions.hpp
 *  This is a C++ Library header.
 */

#include <boost/operators.hpp>
#include <cmath>
#include <numeric>
#include <type_traits>

namespace fun
{

/*!
 * @brief Greatest common divider
 *
 * @tparam _Mn
 * @param[in] __m
 * @param[in] __n
 * @return _Mn
 */
template <typename Mn>
constexpr auto gcd(Mn _m, Mn _n) -> Mn
{
    return _m == 0 ? abs(_n) : _n == 0 ? abs(_m) : gcd(_n, _m % _n);
}

/*!
 * @brief Least common multiple
 *
 * @tparam _Mn
 * @param[in] __m
 * @param[in] __n
 * @return _Mn
 */
template <typename Mn>
constexpr auto lcm(Mn _m, Mn _n) -> Mn
{
    return (_m != 0 && _n != 0) ? (abs(_m) / gcd(_m, _n)) * abs(_n) : 0;
}

template <typename Z>
struct Fraction : boost::totally_ordered<Fraction<Z>,
                      boost::totally_ordered2<Fraction<Z>, Z,
                          boost::multipliable2<Fraction<Z>, Z,
                              boost::dividable2<Fraction<Z>, Z>>>>
{
    Z _numerator;
    Z _denominator;

    /*!
     * @brief Construct a new Fraction object
     *
     * @param[in] numerator
     * @param[in] denominator
     */
    constexpr Fraction(Z&& numerator, Z&& denominator) noexcept
        : _numerator {std::move(numerator)}
        , _denominator {std::move(denominator)}
    {
        this->normalize();
    }

    /*!
     * @brief Construct a new Fraction object
     *
     * @param[in] numerator
     * @param[in] denominator
     */
    constexpr Fraction(const Z& numerator, const Z& denominator)
        : _numerator {numerator}
        , _denominator {denominator}
    {
        this->normalize();
    }

    constexpr void normalize()
    {
        auto common = gcd(this->_numerator, this->_denominator);
        if (common == Z(1))
        {
            return;
        }
        // if (common == Z(0)) [[unlikely]] return; // both num and den are zero
        if (this->_denominator < Z(0))
        {
            common = -common;
        }
        this->_numerator /= common;
        this->_denominator /= common;
    }

    /*!
     * @brief Construct a new Fraction object
     *
     * @param[in] numerator
     */
    constexpr explicit Fraction(Z&& numerator) noexcept
        : _numerator {std::move(numerator)}
        , _denominator(Z(1))
    {
    }

    /*!
     * @brief Construct a new Fraction object
     *
     * @param[in] numerator
     */
    constexpr explicit Fraction(const Z& numerator)
        : _numerator {numerator}
        , _denominator(Z(1))
    {
    }

    /*!
     * @brief
     *
     * @return const Z&
     */
    constexpr auto numerator() const -> const Z&
    {
        return _numerator;
    }

    /*!
     * @brief
     *
     * @return const Z&
     */
    constexpr auto denominator() const -> const Z&
    {
        return _denominator;
    }

    /*!
     * @brief
     *
     * @return Fraction
     */
    constexpr auto abs() const -> Fraction
    {
        return Fraction(std::abs(_numerator), std::abs(_denominator));
    }

    /*!
     * @brief
     *
     */
    constexpr void reciprocal()
    {
        std::swap(_numerator, _denominator);
    }

    /*!
     * @brief
     *
     * @return Fraction
     */
    constexpr auto operator-() const -> Fraction
    {
        auto res = Fraction(*this);
        res._numerator = -res._numerator;
        return res;
    }

    /*!
     * @brief
     *
     * @param[in] frac
     * @return Fraction
     */
    constexpr auto operator+(const Fraction& frac) const -> Fraction
    {
        if (_denominator == frac._denominator)
        {
            return Fraction(_numerator + frac._numerator, _denominator);
        }
        auto d = _denominator * frac._denominator;
        auto n =
            frac._denominator * _numerator + _denominator * frac._numerator;
        return Fraction(n, d);
    }

    /*!
     * @brief
     *
     * @param[in] frac
     * @return Fraction
     */
    constexpr auto operator-(const Fraction& frac) const -> Fraction
    {
        return *this + (-frac);
    }

    /*!
     * @brief
     *
     * @param[in] frac
     * @return Fraction
     */
    constexpr auto operator*(const Fraction& frac) const -> Fraction
    {
        auto n = _numerator * frac._numerator;
        auto d = _denominator * frac._denominator;
        return Fraction(std::move(n), std::move(d));
    }

    /*!
     * @brief
     *
     * @param[in] frac
     * @return Fraction
     */
    constexpr auto operator/(Fraction frac) const -> Fraction
    {
        frac.reciprocal();
        return *this * frac;
    }

    /*!
     * @brief
     *
     * @param[in] i
     * @return Fraction
     */
    constexpr auto operator+(const Z& i) const -> Fraction
    {
        auto n = _numerator + _denominator * i;
        return Fraction(std::move(n), _denominator);
    }

    /*!
     * @brief
     *
     * @param[in] i
     * @return Fraction
     */
    constexpr auto operator-(const Z& i) const -> Fraction
    {
        return *this + (-i);
    }

    // /*!
    //  * @brief
    //  *
    //  * @param[in] i
    //  * @return Fraction
    //  */
    // constexpr Fraction operator*(const Z& i) const
    // {
    //     auto n = _numerator * i;
    //     return Fraction(n, _denominator);
    // }

    // /*!
    //  * @brief
    //  *
    //  * @param[in] i
    //  * @return Fraction
    //  */
    // constexpr Fraction operator/(const Z& i) const
    // {
    //     auto d = _denominator * i;
    //     return Fraction(_numerator, d);
    // }

    /*!
     * @brief
     *
     * @param[in] frac
     * @return Fraction
     */
    constexpr auto operator+=(const Fraction& frac) -> Fraction&
    {
        return *this = *this + frac;
    }

    /*!
     * @brief
     *
     * @param[in] frac
     * @return Fraction
     */
    constexpr auto operator-=(const Fraction& frac) -> Fraction&
    {
        return *this = *this - frac;
    }

    /*!
     * @brief
     *
     * @param[in] frac
     * @return Fraction
     */
    constexpr auto operator*=(const Fraction& frac) -> Fraction&
    {
        return *this = *this * frac;
    }

    /*!
     * @brief
     *
     * @param[in] frac
     * @return Fraction
     */
    constexpr auto operator/=(const Fraction& frac) -> Fraction&
    {
        return *this = *this / frac;
    }

    /*!
     * @brief
     *
     * @param[in] i
     * @return Fraction
     */
    constexpr auto operator+=(const Z& i) -> Fraction&
    {
        return *this = *this + i;
    }

    /*!
     * @brief
     *
     * @param[in] i
     * @return Fraction
     */
    constexpr auto operator-=(const Z& i) -> Fraction&
    {
        return *this = *this - i;
    }

    /*!
     * @brief
     *
     * @param[in] i
     * @return Fraction
     */
    constexpr auto operator*=(const Z& i) -> Fraction&
    {
        const auto common = gcd(i, this->_denominator);
        if (common == Z(1))
        {
            this->_numerator *= i;
        }
        // else if (common == Z(0)) [[unlikely]] // both i and den are zero
        // {
        //     this->_numerator = Z(0);
        // }
        else
        {
            this->_numerator *= (i / common);
            this->_denominator /= common;
        }
        return *this;
    }

    /*!
     * @brief
     *
     * @param[in] i
     * @return Fraction
     */
    constexpr auto operator/=(const Z& i) -> Fraction&
    {
        const auto common = gcd(this->_numerator, i);
        if (common == Z(1))
        {
            this->_denominator *= i;
        }
        // else if (common == Z(0)) [[unlikely]] // both i and num are zero
        // {
        //     this->_denominator = Z(0);
        // }
        else
        {
            this->_denominator *= (i / common);
            this->_numerator /= common;
        }
        return *this;
    }

    /*!
     * @brief Three way comparison
     *
     * @param[in] frac
     * @return auto
     */
    template <typename U>
    constexpr auto cmp(const Fraction<U>& frac) const
    {
        // if (_denominator == frac._denominator) {
        //     return _numerator - frac._numerator;
        // }
        return _numerator * frac._denominator - _denominator * frac._numerator;
    }

    constexpr auto operator==(const Fraction<Z>& rhs) const -> bool
    {
        if (this->_denominator == rhs._denominator)
        {
            return this->_numerator == rhs._numerator;
        }

        return (this->_numerator * rhs._denominator) ==
            (this->_denominator * rhs._numerator);
    }

    constexpr auto operator<(const Fraction<Z>& rhs) const -> bool
    {
        if (this->_denominator == rhs._denominator)
        {
            return this->_numerator < rhs._numerator;
        }

        return (this->_numerator * rhs._denominator) <
            (this->_denominator * rhs._numerator);
    }

    /**
     * @brief
     *
     */
    constexpr auto operator==(const Z& rhs) const -> bool
    {
        return this->_denominator == Z(1) && this->_numerator == rhs;
    }

    /**
     * @brief
     *
     */
    constexpr auto operator<(const Z& rhs) const -> bool
    {
        return this->_numerator < (this->_denominator * rhs);
    }

    /**
     * @brief
     *
     */
    constexpr auto operator>(const Z& rhs) const -> bool
    {
        return this->_numerator > (this->_denominator * rhs);
    }

    // /*!
    //  * @brief
    //  *
    //  * @return double
    //  */
    // constexpr explicit operator double()
    // {
    //     return double(_numerator) / _denominator;
    // }

    // /**
    //  * @brief
    //  *
    //  */
    // friend constexpr bool operator<(const Z& lhs, const Fraction<Z>& rhs)
    // {
    //     return lhs * rhs.denominator() < rhs.numerator();
    // }
};

/*!
 * @brief
 *
 * @param[in] c
 * @param[in] frac
 * @return Fraction<Z>
 */
template <typename Z>
constexpr auto operator+(const Z& c, const Fraction<Z>& frac) -> Fraction<Z>
{
    return frac + c;
}

/*!
 * @brief
 *
 * @param[in] c
 * @param[in] frac
 * @return Fraction<Z>
 */
template <typename Z>
constexpr auto operator-(const Z& c, const Fraction<Z>& frac) -> Fraction<Z>
{
    return c + (-frac);
}

// /*!
//  * @brief
//  *
//  * @param[in] c
//  * @param[in] frac
//  * @return Fraction<Z>
//  */
// template <typename Z>
// constexpr Fraction<Z> operator*(const Z& c, const Fraction<Z>& frac)
// {
//     return frac * c;
// }

/*!
 * @brief
 *
 * @param[in] c
 * @param[in] frac
 * @return Fraction<Z>
 */
template <typename Z>
constexpr auto operator+(int&& c, const Fraction<Z>& frac) -> Fraction<Z>
{
    return frac + c;
}

/*!
 * @brief
 *
 * @param[in] c
 * @param[in] frac
 * @return Fraction<Z>
 */
template <typename Z>
constexpr auto operator-(int&& c, const Fraction<Z>& frac) -> Fraction<Z>
{
    return (-frac) + c;
}

/*!
 * @brief
 *
 * @param[in] c
 * @param[in] frac
 * @return Fraction<Z>
 */
template <typename Z>
constexpr auto operator*(int&& c, const Fraction<Z>& frac) -> Fraction<Z>
{
    return frac * c;
}

/*!
 * @brief
 *
 * @tparam _Stream
 * @tparam Z
 * @param[in] os
 * @param[in] frac
 * @return _Stream&
 */
template <typename Stream, typename Z>
auto operator<<(Stream& os, const Fraction<Z>& frac) -> Stream&
{
    os << frac.numerator() << "/" << frac.denominator();
    return os;
}

// For template deduction
// Integral{Z} Fraction(const Z &, const Z &) -> Fraction<Z>;

} // namespace fun


#pragma once
#include <Eigen/Core>
#include <igl/readMESH.h>
#include <iostream>
#include <tuple>

#include "fem_solve.hpp"
#include "writer.hpp"

double f_lshape(double x, double y) {
	std::ignore = x;
	std::ignore = y;
	return 0;
}

double g_lshape(double x, double y) {
	double r = std::sqrt(x * x + y * y);

	double theta = std::atan2(y, x);

	// Adjust for the region where theta < 0
	if (theta < 0) {
		theta += 2 * M_PI;
	}

	return std::pow(r, 2.0 / 3.0) * std::sin(2 * theta / 3);
}

Eigen::Vector2d g_grad_lshape(double x, double y) {
	double r = std::sqrt(x * x + y * y);

	double theta = std::atan2(y, x);

	// Adjust for the region where theta < 0
	if (theta < 0) {
		theta += 2 * M_PI;
	}
	Eigen::Vector2d grad;
	grad << -2.0 / 3.0 * std::pow(r, -1.0 / 3.0) * std::sin(theta / 3), 2.0 / 3.0 * std::pow(r, -1.0 / 3.0) * std::cos(theta / 3);
	return grad;
}

void solveL(double r) {
	std::cout << "Solving L-shape" << std::endl;
	Vector u;

	Eigen::MatrixXd vertices;
	Eigen::MatrixXi triangles;
	Eigen::MatrixXi tetrahedra;

	igl::readMESH(NPDE_DATA_PATH "Lshape_5.mesh", vertices, tetrahedra, triangles);

	solveFiniteElement(u, vertices, triangles, f_lshape, constantFunction, g_lshape, r);

	writeToFile("Lshape_values.txt", u);
	writeMatrixToFile("Lshape_vertices.txt", vertices);
	writeMatrixToFile("Lshape_triangles.txt", triangles);
}