////////////////////////////////////////////////////////////////////////////////
#include <polyfem/RBFWithQuadratic.hpp>
#include <polyfem/Types.hpp>
#include <polyfem/MatrixUtils.hpp>
#include <polyfem/Logger.hpp>

#include <igl/Timer.h>

#include <Eigen/Dense>

#include <iostream>
#include <fstream>
#include <array>
////////////////////////////////////////////////////////////////////////////////

// #define VERBOSE

using namespace polyfem;

namespace
{

	// Harmonic kernel
	double kernel(const bool is_volume, const double r)
	{
		if (r < 1e-8)
		{
			return 0;
		}

		if (is_volume)
		{
			return 1 / r;
		}
		else
		{
			return log(r);
		}
	}

	double kernel_prime(const bool is_volume, const double r)
	{
		if (r < 1e-8)
		{
			return 0;
		}

		if (is_volume)
		{
			return -1 / (r * r);
		}
		else
		{
			return 1 / r;
		}
	}

	// Biharmonic kernel (2d only)
	// double kernel(const bool is_volume, const double r) {
	// 	assert(!is_volume);
	// 	if (r < 1e-8) { return 0; }

	// 	return r * r * (log(r)-1);
	// }

	// double kernel_prime(const bool is_volume, const double r) {
	// 	assert(!is_volume);
	// 	if (r < 1e-8) { return 0; }

	// 	return r * ( 2 * log(r) - 1);
	// }

} // anonymous namespace

////////////////////////////////////////////////////////////////////////////////

//output is std::array<Eigen::MatrixXd, 5> &strong rhs(q(x_i) er)
void RBFWithQuadratic::setup_monomials_strong_2d(const int dim, const AssemblerUtils &assembler, const std::string &assembler_name, const Eigen::MatrixXd &pts, const QuadratureVector &da, std::array<Eigen::MatrixXd, 5> &strong)
{
	//a(u,v) = a(q er, phi_j es) = <rhs(q(x_i) er) , phi_j(x_i) es >
	// (not a(phi_j es, q er))

	DiffScalarBase::setVariableCount(2);
	AutodiffHessianPt pt(dim);
	for (int i = 0; i < 5; ++i)
	{
		strong[i].resize(dim * dim, pts.rows());
		strong[i].setZero();
	}

	Eigen::MatrixXd tmp;

	for (int i = 0; i < pts.rows(); ++i)
	{
		//loop for er
		for (int d = 0; d < dim; ++d)
		{
			pt((d + 1) % dim) = AutodiffScalarHessian(0);
			//for d = 0 pt(q, 0), for d = 1 pt=(0, q)

			//x
			pt(d) = AutodiffScalarHessian(0, pts(i, 0));	 //pt=(x, 0) or pt=(0, x)
			tmp = assembler.compute_rhs(assembler_name, pt); //in R^dim
			for (int d1 = 0; d1 < dim; ++d1)
				strong[0](d * dim + d1, i) = tmp(d1) * da(i);

			//y
			pt(d) = AutodiffScalarHessian(1, pts(i, 1));
			tmp = assembler.compute_rhs(assembler_name, pt);
			for (int d1 = 0; d1 < dim; ++d1)
				strong[1](d * dim + d1, i) = tmp(d1) * da(i);

			//xy
			pt(d) = AutodiffScalarHessian(0, pts(i, 0)) * AutodiffScalarHessian(1, pts(i, 1));
			tmp = assembler.compute_rhs(assembler_name, pt);
			for (int d1 = 0; d1 < dim; ++d1)
				strong[2](d * dim + d1, i) = tmp(d1) * da(i);

			//x^2
			pt(d) = AutodiffScalarHessian(0, pts(i, 0)) * AutodiffScalarHessian(0, pts(i, 0));
			tmp = assembler.compute_rhs(assembler_name, pt);
			for (int d1 = 0; d1 < dim; ++d1)
				strong[3](d * dim + d1, i) = tmp(d1) * da(i);

			//y^2
			pt(d) = AutodiffScalarHessian(1, pts(i, 1)) * AutodiffScalarHessian(1, pts(i, 1));
			tmp = assembler.compute_rhs(assembler_name, pt);
			for (int d1 = 0; d1 < dim; ++d1)
				strong[4](d * dim + d1, i) = tmp(d1) * da(i);
		}
	}
}

void RBFWithQuadratic::setup_monomials_vals_2d(const int star_index, const Eigen::MatrixXd &pts, ElementAssemblyValues &vals)
{
	assert(star_index + 5 <= vals.basis_values.size());
	//x
	vals.basis_values[star_index + 0].val = pts.col(0);
	vals.basis_values[star_index + 0].grad = Eigen::MatrixXd(pts.rows(), pts.cols());
	vals.basis_values[star_index + 0].grad.col(0).setOnes();
	vals.basis_values[star_index + 0].grad.col(1).setZero();

	//y
	vals.basis_values[star_index + 1].val = pts.col(1);
	vals.basis_values[star_index + 1].grad = Eigen::MatrixXd(pts.rows(), pts.cols());
	vals.basis_values[star_index + 1].grad.col(0).setZero();
	vals.basis_values[star_index + 1].grad.col(1).setOnes();

	//xy
	vals.basis_values[star_index + 2].val = pts.col(0).array() * pts.col(1).array();
	vals.basis_values[star_index + 2].grad = Eigen::MatrixXd(pts.rows(), pts.cols());
	vals.basis_values[star_index + 2].grad.col(0) = pts.col(1);
	vals.basis_values[star_index + 2].grad.col(1) = pts.col(0);

	//x^2
	vals.basis_values[star_index + 3].val = pts.col(0).array() * pts.col(0).array();
	vals.basis_values[star_index + 3].grad = Eigen::MatrixXd(pts.rows(), pts.cols());
	vals.basis_values[star_index + 3].grad.col(0) = 2 * pts.col(0);
	vals.basis_values[star_index + 3].grad.col(1).setZero();

	//y^2
	vals.basis_values[star_index + 4].val = pts.col(1).array() * pts.col(1).array();
	vals.basis_values[star_index + 4].grad = Eigen::MatrixXd(pts.rows(), pts.cols());
	vals.basis_values[star_index + 4].grad.col(0).setZero();
	vals.basis_values[star_index + 4].grad.col(1) = 2 * pts.col(1);

	for (size_t i = star_index; i < star_index + 5; ++i)
	{
		vals.basis_values[i].grad_t_m = vals.basis_values[i].grad;
	}

	// for(size_t i = star_index; i < star_index + 5; ++i)
	// {
	// 	vals.basis_values[i].grad_t_m = Eigen::MatrixXd(pts.rows(), pts.cols());
	// 	for(int k = 0; k < vals.jac_it.size(); ++k)
	// 		vals.basis_values[i].grad_t_m.row(k) = vals.basis_values[i].grad.row(k) * vals.jac_it[k];
	// }
}

RBFWithQuadratic::RBFWithQuadratic(
	const AssemblerUtils &assembler,
	const std::string &assembler_name,
	const Eigen::MatrixXd &centers,
	const Eigen::MatrixXd &collocation_points,
	const Eigen::MatrixXd &local_basis_integral,
	const Quadrature &quadr,
	Eigen::MatrixXd &rhs,
	bool with_constraints)
	: centers_(centers)
{
	// centers_.resize(0, centers.cols());
	compute_weights(assembler, assembler_name, collocation_points, local_basis_integral, quadr, rhs, with_constraints);
}

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

void RBFWithQuadratic::basis(const int local_index, const Eigen::MatrixXd &samples, Eigen::MatrixXd &val) const
{
	Eigen::MatrixXd tmp;
	bases_values(samples, tmp);
	val = tmp.col(local_index);
}

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

void RBFWithQuadratic::grad(const int local_index, const Eigen::MatrixXd &samples, Eigen::MatrixXd &val) const
{
	Eigen::MatrixXd tmp;
	const int dim = centers_.cols();
	val.resize(samples.rows(), dim);
	for (int d = 0; d < dim; ++d)
	{
		bases_grads(d, samples, tmp);
		val.col(d) = tmp.col(local_index);
	}
}

////////////////////////////////////////////////////////////////////////////////

void RBFWithQuadratic::bases_values(const Eigen::MatrixXd &samples, Eigen::MatrixXd &val) const
{
	// Compute A
	Eigen::MatrixXd A;
	compute_kernels_matrix(samples, A);

	// Multiply by the weights
	val = A * weights_;
}

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

void RBFWithQuadratic::bases_grads(const int axis, const Eigen::MatrixXd &samples, Eigen::MatrixXd &val) const
{
	const int num_kernels = centers_.rows();
	const int dim = (is_volume() ? 3 : 2);

	// Compute ∇xA
	Eigen::MatrixXd A_prime(samples.rows(), num_kernels + 1 + dim + dim * (dim + 1) / 2);
	A_prime.setZero();

	for (int j = 0; j < num_kernels; ++j)
	{
		A_prime.col(j) = (samples.rowwise() - centers_.row(j)).rowwise().norm().unaryExpr([this](double x) { return kernel_prime(is_volume(), x) / x; });
		A_prime.col(j) = (samples.col(axis).array() - centers_(j, axis)) * A_prime.col(j).array();
	}
	// Linear terms
	A_prime.middleCols(num_kernels + 1 + axis, 1).setOnes();
	// Mixed terms
	if (dim == 2)
	{
		A_prime.col(num_kernels + 1 + dim) = samples.col(1 - axis);
	}
	else
	{
		A_prime.col(num_kernels + 1 + dim + axis) = samples.col((axis + 1) % dim);
		A_prime.col(num_kernels + 1 + dim + (axis + 2) % dim) = samples.col((axis + 2) % dim);
	}
	// Quadratic terms
	A_prime.rightCols(dim).col(axis) = 2.0 * samples.col(axis);

	// Apply weights
	val = A_prime * weights_;
}

////////////////////////////////////////////////////////////////////////////////
//
// For each FEM basis φ that is nonzero on the element E, we want to
// solve the least square system A w = rhs, where:
//     ┏                                     ┓
//     ┃ ψ_k(pi) ... 1 xi yi xi*yi xi^2 yi^2 ┃
// A = ┃   ┊        ┊  ┊  ┊   ┊    ┊    ┊    ┃ ∊ ℝ^{#S x (#K+1+dim+dim*(dim+1)/2)}
//     ┃   ┊        ┊  ┊  ┊   ┊    ┊    ┊    ┃
//     ┗                                     ┛
//     ┏                                 ┓^⊤
// w = ┃ w_k ... a00 a10 a01 a11 a20 a02 ┃   ∊ ℝ^{#K+1+dim+dim*(dim+1)/2}
//     ┗                                 ┛
// - A is the RBF kernels evaluated over the collocation points (#S)
// - b is the expected value of the basis sampled on the boundary (#S)
// - w is the weight of the kernels defining the basis
// - pi = (xi, yi) is the i-th collocation point
//
// Moreover, we want to impose a constraint on the weight vector w so that each
// monomial Q(x,y) = x^α*y^β with α+β <= 2 is in the span of the FEM bases {φ_j}_j.
//
// In the case of Laplace's equation, we recall the weak form of the PDE as:
//
//   Find u such that: ∫_Ω Δu v = - ∫_Ω ∇u·∇v   ∀ v
//
// For our bases to exactly represent a monomial Q(x,y), it means that its
// approximation by the finite element bases {φ_j}_j must be equal to Q(x,y).
// In particular, for any φ_j that is nonzero on the polyhedral element E, we must have:
//
//   ∫_{𝘅 in Ω} ΔQ(𝘅) φ_j(𝘅) d𝘅  = - ∫_{𝘅 \in Ω} ∇Q(𝘅)·∇φ_j(𝘅) d𝘅     (1)
//
// Now, for each of the 5 non-constant monomials (9 in 3D), we need to compute
// Δ(x^α*y^β). For (α,β) ∊ {(1,0), (0,1), (1,1), (2,0), (0,2)}, this yields
// the following equalities:
//
//     Δx  = 0      (2a)
//     Δy  = 0      (2b)
//     Δxy = 0      (2c)
//     Δx² = 1      (2d)
//     Δy² = 1      (2e)
//
// If we plug these back into (1), and split the integral between the polyhedral
// element E and Ω\E, we obtain the following constraints:
//
// ∫_E ∇Q·∇φ_j + ∫_E ΔQ φ_j = - ∫_{Ω\E} ∇Q·∇φ_j - ∫_{Ω\E} ΔQ φ_j    (3)
//
// Note that the right-hand side of (3) is already known, since no two polyhedral
// cells are adjacent to each other, and the bases overlapping a polyhedron vanish
// on the boundary of the domain ∂Ω. This right-hand side is computed in advance
// and passed to our functions as in argument `local_basis_integral`.
//
// The left-hand side of equation (3) reduces to the following (in 2D):
//
//     ∫_E ∇x(φ_j) = c10                       (4a)
//     ∫_E ∇y(φ_j) = c01                       (4b)
//     ∫_E (y·∇x(φ_j) + y·∇x(φ_jj)) = c11      (4c)
//     ∫_E 2x·∇x(φ_j) + ∫_E 2 φ_j = c20        (4d)
//     ∫_E 2y·∇y(φ_j) + ∫_E 2 φ_j = c02        (4e)
//
// The next step is to express the basis φ_j in terms of the harmonic kernels and
// quadratic polynomials:
//
//     φ_j(x,y) = Σ_k w_k ψ_k(x,y) + a00 + a10*x + a01*y + a11*x*y + a20*x² + a02*y²
//
// The five equations in (4) become:
//
//		Σ_j w_k ∫∇x(ψ_k) = ∫ Δ q10  (Σ_j w_k (ψ_k) + a00) + Σ_j w_k ∫∇q10 . ∇(ψ_k + a00)
//    Σ_j w_k ∫∇x(ψ_k) + a10 |E| + a11 ∫y + a20 ∫2x = c10
//    Σ_j w_k ∫∇y(ψ_k) + a01 |E| + a11 ∫x + a02 ∫2y = c01
//    Σ_j w_k (∫y·∇x(ψ_k) + ∫x·∇y(ψ_k)) + a10 ∫y + a01 ∫x + a11 (∫x²+∫y²) + a20 2∫xy + a02 2∫xy = c11
//    Σ_j w_k (2∫x·∇x(ψ_k) + 2ψ_k) + a10 4∫x + a01 2∫y + a11 4∫xy + a20 6∫x² + a02 2∫y² = c20
//    Σ_j w_k (2∫y·∇y(ψ_k) + 2ψ_k) + a10 2∫x + a01 4∫y + a11 4∫xy + a20 2∫x² + a02 6∫y² = c02
//  	Σ_j w_k (2∫y·∇y(ψ_k) + 2ψ_k) = ∫ Δ q20  (Σ_j w_k (ψ_k) + a00) + Σ_j w_k ∫∇q20 . ∇(ψ_k + a00) = ∫ -2  (Σ_j w_k (ψ_k) + a00) + Σ_j w_k ∫2x ∇x(ψ_k)
//
// This system gives us a relationship between the fives a10, a01, a11, a20, a02
// and the rest of the w_k + a constant translation term. We can write down the
// corresponding system:
//
//       a10   a01   a11   a20   a02
//     ┏                              ┓             ┏     ┓
//     ┃ |E|         ∫y    2∫x        ┃             ┃ w_k ┃
//     ┃                              ┃             ┃  ┊  ┃
//     ┃       |E|   ∫x          2∫y  ┃             ┃  ┊  ┃
//     ┃                              ┃             ┃  ┊  ┃
// M = ┃  ∫y   ∫x  ∫x²+∫y² 2∫xy  2∫xy ┃ = \tilde{L} ┃  ┊  ┃ + \tilde{t}
//     ┃                              ┃             ┃  ┊  ┃
//     ┃ 4∫x  2∫y  4∫xy    6∫x²  2∫y² ┃             ┃  ┊  ┃
//     ┃                              ┃             ┃w_#K ┃
//     ┃ 2∫x  4∫y  4∫xy    2∫x²  6∫y² ┃             ┃ a00 ┃
//     ┗                              ┛             ┗     ┛
//
// Now, if we want to express w as w = Lv + t, and solve our least-square
// system as before, we need to invert M and compute L and t in terms of
// \tilde{L} and \tilde{t}
//
//     ┏                  ┓
//     ┃   1              ┃
//     ┃       1          ┃
//     ┃          ·       ┃
// L = ┃             ·    ┃ ∊ ℝ^{ (#K+1+dim+dim*(dim+1)/2) x (#K+1}) }
//     ┃                1 ┃
//     ┃ M^{-1} \tilde{L} ┃
//     ┗                  ┛
//     ┏                  ┓
//     ┃        0         ┃
//     ┃        ┊         ┃
// t = ┃        ┊         ┃ ∊ ℝ^{#K+1+dim+dim*(dim+1)/2}
//     ┃        0         ┃
//     ┃ M^{-1} \tilde{t} ┃
//     ┗                  ┛
// After solving the new least square system A L v = rhs - A t, we can retrieve
// w = L v
//
////////////////////////////////////////////////////////////////////////////////

void RBFWithQuadratic::compute_kernels_matrix(const Eigen::MatrixXd &samples, Eigen::MatrixXd &A) const
{
	// Compute A
	const int num_kernels = centers_.rows();
	const int dim = (is_volume() ? 3 : 2);

	A.resize(samples.rows(), num_kernels + 1 + dim + dim * (dim + 1) / 2);
	for (int j = 0; j < num_kernels; ++j)
	{
		A.col(j) = (samples.rowwise() - centers_.row(j)).rowwise().norm().unaryExpr([this](double x) { return kernel(is_volume(), x); });
	}
	A.col(num_kernels).setOnes();				  // constant term
	A.middleCols(num_kernels + 1, dim) = samples; // linear terms
	if (dim == 2)
	{
		A.middleCols(num_kernels + dim + 1, 1) = samples.rowwise().prod(); // mixed terms
	}
	else if (dim == 3)
	{
		A.middleCols(num_kernels + dim + 1, 3) = samples;
		A.middleCols(num_kernels + dim + 1 + 0, 1).array() *= samples.col(1).array();
		A.middleCols(num_kernels + dim + 1 + 1, 1).array() *= samples.col(2).array();
		A.middleCols(num_kernels + dim + 1 + 2, 1).array() *= samples.col(0).array();
	}
	A.rightCols(dim) = samples.array().square(); // quadratic terms
}

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

void RBFWithQuadratic::compute_constraints_matrix_2d_old(
	const int num_bases,
	const Quadrature &quadr,
	const Eigen::MatrixXd &local_basis_integral,
	Eigen::MatrixXd &L,
	Eigen::MatrixXd &t) const
{
	const int num_kernels = centers_.rows();
	const int dim = centers_.cols();
	assert(dim == 2);

	// K_cst = ∫ψ_k
	// K_lin = ∫∇x(ψ_k), ∫∇y(ψ_k)
	// K_mix = ∫y·∇x(ψ_k), ∫x·∇y(ψ_k)
	// K_sqr = ∫x·∇x(ψ_k), ∫y·∇y(ψ_k)
	Eigen::VectorXd K_cst = Eigen::VectorXd::Zero(num_kernels);
	Eigen::MatrixXd K_lin = Eigen::MatrixXd::Zero(num_kernels, dim);
	Eigen::MatrixXd K_mix = Eigen::MatrixXd::Zero(num_kernels, dim);
	Eigen::MatrixXd K_sqr = Eigen::MatrixXd::Zero(num_kernels, dim);
	for (int j = 0; j < num_kernels; ++j)
	{
		// ∫∇x(ψ_k)(p) = Σ_q (xq - xk) * 1/r * h'(r) * wq
		// - xq is the x coordinate of the q-th quadrature point
		// - wq is the q-th quadrature weight
		// - r is the distance from pq to the kernel center
		// - h is the RBF kernel (scalar function)
		for (int q = 0; q < quadr.points.rows(); ++q)
		{
			const RowVectorNd p = quadr.points.row(q) - centers_.row(j);
			const double r = p.norm();
			const RowVectorNd gradPhi = p * kernel_prime(is_volume(), r) / r * quadr.weights(q);
			K_cst(j) += kernel(is_volume(), r) * quadr.weights(q);
			K_lin.row(j) += gradPhi;
			K_mix(j, 0) += quadr.points(q, 1) * gradPhi(0);
			K_mix(j, 1) += quadr.points(q, 0) * gradPhi(1);
			K_sqr.row(j) += (quadr.points.row(q).array() * gradPhi.array()).matrix();
		}
	}

	// I_lin = ∫x, ∫y
	// I_mix = ∫xy
	// I_sqr = ∫x², ∫y²
	Eigen::RowVectorXd I_lin = (quadr.points.array().colwise() * quadr.weights.array()).colwise().sum();
	Eigen::RowVectorXd I_mix = (quadr.points.rowwise().prod().array() * quadr.weights.array()).colwise().sum();
	Eigen::RowVectorXd I_sqr = (quadr.points.array().square().colwise() * quadr.weights.array()).colwise().sum();
	double volume = quadr.weights.sum();

	// std::cout << I_lin << std::endl;
	// std::cout << I_mix << std::endl;
	// std::cout << I_sqr << std::endl;

	// Compute M
	Eigen::Matrix<double, 5, 5> M;
	M << volume, 0, I_lin(1), 2 * I_lin(0), 0,
		0, volume, I_lin(0), 0, 2 * I_lin(1),
		I_lin(1), I_lin(0), I_sqr(0) + I_sqr(1), 2 * I_mix(0), 2 * I_mix(0),
		4 * I_lin(0), 2 * I_lin(1), 4 * I_mix(0), 6 * I_sqr(0), 2 * I_sqr(1),
		2 * I_lin(0), 4 * I_lin(1), 4 * I_mix(0), 2 * I_sqr(0), 6 * I_sqr(1);
	Eigen::FullPivLU<Eigen::Matrix<double, 5, 5>> lu(M);
	assert(lu.isInvertible());

	// show_matrix_stats(M);

	// Compute L
	L.resize(num_kernels + 1 + dim + dim * (dim + 1) / 2, num_kernels + 1);
	L.setZero();
	L.diagonal().setOnes();

	L.block(num_kernels + 1, 0, dim, num_kernels) = -K_lin.transpose();
	L.block(num_kernels + 1 + dim, 0, 1, num_kernels) = -K_mix.transpose().colwise().sum();
	L.block(num_kernels + 1 + dim + 1, 0, dim, num_kernels) = -2.0 * (K_sqr.colwise() + K_cst).transpose();
	L.bottomRightCorner(dim, 1).setConstant(-2.0 * volume);
	// j \in [0, 4]
	// i \in [0, num_kernels]
	// ass_val = [q_10, q_01, q_11, q_20, q_02, psi_0, ..., psi_k]

	// strong rows is the evaluation at quadrature points
	// strong.col(0) = pde(q_10) (probably 0)
	// strong.col(4) = pde(q_02) (it is 2 for laplacian)
	//L.block(num_kernels + 1 + i, j) =  +/- assembler.assemble(ass_val, j, 5 + i) +/- (strong.col(j).array() * ass_val.basis_values[5+i].val.array() * quadr.weights.array()).sum();

	L.block(num_kernels + 1, 0, 5, num_kernels + 1) = lu.solve(L.block(num_kernels + 1, 0, 5, num_kernels + 1));
	// std::cout << L.bottomRightCorner(10, 10) << std::endl;

	// Compute t
	t.resize(L.rows(), num_bases);
	t.setZero();
	t.bottomRows(5) = local_basis_integral.transpose();
	t.bottomRows(5) = lu.solve(weights_.bottomRows(5));
}

void RBFWithQuadratic::compute_constraints_matrix_2d(
	const AssemblerUtils &assembler,
	const std::string &assembler_name,
	const int num_bases,
	const Quadrature &quadr,
	const Eigen::MatrixXd &local_basis_integral,
	Eigen::MatrixXd &L,
	Eigen::MatrixXd &t) const
{
	const int num_kernels = centers_.rows();
	const int space_dim = centers_.cols();
	const int assembler_dim = assembler.is_tensor(assembler_name) ? 2 : 1;
	assert(space_dim == 2);

	std::array<Eigen::MatrixXd, 5> strong;

	// ass_val = [q_10, q_01, q_11, q_20, q_02, psi_0, ..., psi_k]
	ElementAssemblyValues ass_val;
	ass_val.has_parameterization = false;
	ass_val.basis_values.resize(5 + num_kernels);

	//evaluating monomial and grad of monomials at quad points
	setup_monomials_vals_2d(0, quadr.points, ass_val);
	setup_monomials_strong_2d(assembler_dim, assembler, assembler_name, quadr.points, quadr.weights.array(), strong);

	//evaluating psi and grad psi at quadr points
	for (int j = 0; j < num_kernels; ++j)
	{
		ass_val.basis_values[5 + j].val = Eigen::MatrixXd(quadr.points.rows(), 1);
		ass_val.basis_values[5 + j].grad = Eigen::MatrixXd(quadr.points.rows(), quadr.points.cols());

		for (int q = 0; q < quadr.points.rows(); ++q)
		{
			const RowVectorNd p = quadr.points.row(q) - centers_.row(j);
			const double r = p.norm();

			ass_val.basis_values[5 + j].val(q) = kernel(is_volume(), r);
			ass_val.basis_values[5 + j].grad.row(q) = p * kernel_prime(is_volume(), r) / r;
		}
	}

	for (size_t i = 5; i < ass_val.basis_values.size(); ++i)
	{
		ass_val.basis_values[i].grad_t_m = ass_val.basis_values[i].grad;
	}

	Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, 0, 10, 10> M(5 * assembler_dim, 5 * assembler_dim);
	for (int i = 0; i < 5; ++i)
	{
		for (int j = 0; j < 5; ++j)
		{
			const auto tmp = assembler.local_assemble(assembler_name, ass_val, i, j, quadr.weights);

			for (int d1 = 0; d1 < assembler_dim; ++d1)
			{
				for (int d2 = 0; d2 < assembler_dim; ++d2)
				{
					const int loc_index = d1 * assembler_dim + d2;
					M(i * assembler_dim + d1, j * assembler_dim + d2) = tmp(loc_index) + (strong[i].row(loc_index).transpose().array() * ass_val.basis_values[j].val.array()).sum();
				}
			}
		}
	}

	Eigen::FullPivLU<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, 0, 10, 10>> lu(M);
	assert(lu.isInvertible());

	// Compute L
	L.resize((num_kernels + 1 + space_dim + space_dim * (space_dim + 1) / 2) * assembler_dim, (num_kernels + 1) * assembler_dim);
	L.setZero();
	L.diagonal().setOnes();

	for (int i = 0; i < 5; ++i)
	{
		for (int j = 0; j < num_kernels; ++j)
		{
			const auto tmp = assembler.local_assemble(assembler_name, ass_val, i, 5 + j, quadr.weights);
			for (int d1 = 0; d1 < assembler_dim; ++d1)
			{
				for (int d2 = 0; d2 < assembler_dim; ++d2)
				{
					const int loc_index = d1 * assembler_dim + d2;
					L((num_kernels + 1 + i) * assembler_dim + d1, j * assembler_dim + d2) = -tmp(loc_index) - (strong[i].row(loc_index).transpose().array() * ass_val.basis_values[5 + j].val.array()).sum();
					// L(num_kernels + 1 + i*assembler_dim + d1, j*assembler_dim + d2) =  -assembler.local_assemble(assembler_name, ass_val, i, 5 + j, quadr.weights)(0) - (strong[i].transpose().array() * ass_val.basis_values[5+j].val.array()).sum();
				}
			}
		}
		for (int d1 = 0; d1 < assembler_dim; ++d1)
		{
			for (int d2 = 0; d2 < assembler_dim; ++d2)
			{
				const int loc_index = d1 * assembler_dim + d2;
				L(num_kernels + 1 + i * assembler_dim + d1, assembler_dim * num_kernels + d2) = -strong[i].row(loc_index).sum();
			}
		}

		// L(num_kernels + 1 + i*assembler_dim + d1, assembler_dim*num_kernels) =  - strong[i].sum();
	}

	L.block((num_kernels + 1) * assembler_dim, 0, 5 * assembler_dim, (num_kernels + 1) * assembler_dim) = lu.solve(L.block((num_kernels + 1) * assembler_dim, 0, 5 * assembler_dim, (num_kernels + 1) * assembler_dim));

	// Compute t
	//t == weights_
	t.resize(L.rows(), num_bases * assembler_dim);
	t.setZero();
	t.bottomRows(5 * assembler_dim) = lu.solve(local_basis_integral.transpose());
}

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

void RBFWithQuadratic::compute_constraints_matrix_3d(
	const AssemblerUtils &assembler,
	const std::string &assembler_name,
	const int num_bases,
	const Quadrature &quadr,
	const Eigen::MatrixXd &local_basis_integral,
	Eigen::MatrixXd &L,
	Eigen::MatrixXd &t) const
{
	const int num_kernels = centers_.rows();
	const int dim = centers_.cols();
	assert(dim == 3);
	assert(local_basis_integral.cols() == 9);

	// K_cst = ∫ψ_k
	// K_lin = ∫∇x(ψ_k), ∫∇y(ψ_k), ∫∇z(ψ_k)
	// K_mix = ∫(y·∇x(ψ_k)+x·∇y(ψ_k)), ∫(z·∇y(ψ_k)+y·∇z(ψ_k)), ∫(x·∇z(ψ_k)+z·∇x(ψ_k))
	// K_sqr = ∫x·∇x(ψ_k), ∫y·∇y(ψ_k), ∫z·∇z(ψ_k)
	Eigen::VectorXd K_cst = Eigen::VectorXd::Zero(num_kernels);
	Eigen::MatrixXd K_lin = Eigen::MatrixXd::Zero(num_kernels, dim);
	Eigen::MatrixXd K_mix = Eigen::MatrixXd::Zero(num_kernels, dim);
	Eigen::MatrixXd K_sqr = Eigen::MatrixXd::Zero(num_kernels, dim);
	for (int j = 0; j < num_kernels; ++j)
	{
		// ∫∇x(ψ_k)(p) = Σ_q (xq - xk) * 1/r * h'(r) * wq
		// - xq is the x coordinate of the q-th quadrature point
		// - wq is the q-th quadrature weight
		// - r is the distance from pq to the kernel center
		// - h is the RBF kernel (scalar function)
		for (int q = 0; q < quadr.points.rows(); ++q)
		{
			const RowVectorNd p = quadr.points.row(q) - centers_.row(j);
			const double r = p.norm();
			const RowVectorNd gradPhi = p * kernel_prime(is_volume(), r) / r * quadr.weights(q);
			K_cst(j) += kernel(is_volume(), r) * quadr.weights(q);
			K_lin.row(j) += gradPhi;
			for (int d = 0; d < dim; ++d)
			{
				K_mix(j, d) += quadr.points(q, (d + 1) % dim) * gradPhi(d) + quadr.points(q, d) * gradPhi((d + 1) % dim);
			}
			K_sqr.row(j) += (quadr.points.row(q).array() * gradPhi.array()).matrix();
		}
	}

	// I_lin = ∫x, ∫y, ∫z
	// I_sqr = ∫x², ∫y², ∫z²
	// I_mix = ∫xy, ∫yz, ∫zx
	Eigen::RowVectorXd I_lin = (quadr.points.array().colwise() * quadr.weights.array()).colwise().sum();
	Eigen::RowVectorXd I_sqr = (quadr.points.array().square().colwise() * quadr.weights.array()).colwise().sum();
	Eigen::RowVectorXd I_mix(3);
	I_mix(0) = (quadr.points.col(0).array() * quadr.points.col(1).array() * quadr.weights.array()).sum();
	I_mix(1) = (quadr.points.col(1).array() * quadr.points.col(2).array() * quadr.weights.array()).sum();
	I_mix(2) = (quadr.points.col(2).array() * quadr.points.col(0).array() * quadr.weights.array()).sum();
	double volume = quadr.weights.sum();

	// std::cout << I_lin << std::endl;
	// std::cout << I_mix << std::endl;
	// std::cout << I_sqr << std::endl;

	// Compute M
	Eigen::Matrix<double, 9, 9> M;
	M << volume, 0, 0, I_lin(1), 0, I_lin(2), 2 * I_lin(0), 0, 0,
		0, volume, 0, I_lin(0), I_lin(2), 0, 0, 2 * I_lin(1), 0,
		0, 0, volume, 0, I_lin(1), I_lin(0), 0, 0, 2 * I_lin(2),
		I_lin(1), I_lin(0), 0, I_sqr(0) + I_sqr(1), I_mix(2), I_mix(1), 2 * I_mix(0), 2 * I_mix(0), 0,
		0, I_lin(2), I_lin(1), I_mix(2), I_sqr(1) + I_sqr(2), I_mix(0), 0, 2 * I_mix(1), 2 * I_mix(1),
		I_lin(2), 0, I_lin(0), I_mix(1), I_mix(0), I_sqr(2) + I_sqr(0), 2 * I_mix(2), 0, 2 * I_mix(2),
		2 * I_lin(0), 0, 0, 2 * I_mix(0), 0, 2 * I_mix(2), 4 * I_sqr(0), 0, 0,
		0, 2 * I_lin(1), 0, 2 * I_mix(0), 2 * I_mix(1), 0, 0, 4 * I_sqr(1), 0,
		0, 0, 2 * I_lin(2), 0, 2 * I_mix(1), 2 * I_mix(2), 0, 0, 4 * I_sqr(2);
	Eigen::Matrix<double, 1, 9> M_rhs;
	M_rhs.segment<3>(0) = I_lin;
	M_rhs.segment<3>(3) = I_mix;
	M_rhs.segment<3>(6) = I_sqr;
	// M_rhs << I_lin, I_mix, I_sqr;
	M.bottomRows(dim).rowwise() += 2.0 * M_rhs;
	Eigen::FullPivLU<Eigen::Matrix<double, 9, 9>> lu(M);
	assert(lu.isInvertible());

	// show_matrix_stats(M);

	// Compute L
	L.resize(num_kernels + 1 + dim + dim * (dim + 1) / 2, num_kernels + 1);
	L.setZero();
	L.diagonal().setOnes();
	L.block(num_kernels + 1, 0, dim, num_kernels) = -K_lin.transpose();
	L.block(num_kernels + 1 + dim, 0, dim, num_kernels) = -K_mix.transpose();
	L.block(num_kernels + 1 + dim + dim, 0, dim, num_kernels) = -2.0 * (K_sqr.colwise() + K_cst).transpose();
	L.bottomRightCorner(dim, 1).setConstant(-2.0 * volume);
	L.block(num_kernels + 1, 0, 9, num_kernels + 1) = lu.solve(L.block(num_kernels + 1, 0, 9, num_kernels + 1));
	// std::cout << L.bottomRightCorner(10, 10) << std::endl;

	// Compute t
	t.resize(L.rows(), num_bases);
	t.setZero();
	t.bottomRows(9) = local_basis_integral.transpose();
	t.bottomRows(9) = lu.solve(weights_.bottomRows(9));
}

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

void RBFWithQuadratic::compute_weights(const AssemblerUtils &assembler, const std::string &assembler_name, const Eigen::MatrixXd &samples,
									   const Eigen::MatrixXd &local_basis_integral, const Quadrature &quadr,
									   Eigen::MatrixXd &rhs, bool with_constraints)
{
#ifdef VERBOSE
	logger().trace("#kernel centers: {}", centers_.rows());
	logger().trace("#collocation points: {}", samples.rows());
	logger().trace("#quadrature points: {}", quadr.weights.size());
	logger().trace("#non-vanishing bases: {}", rhs.cols());
#endif

	if (!with_constraints)
	{
		// Compute A
		Eigen::MatrixXd A;
		compute_kernels_matrix(samples, A);

		// Solve the system
		const int num_kernels = centers_.rows();
		logger().trace("-- Solving system of size {}x{}", num_kernels, num_kernels);
		weights_ = (A.transpose() * A).ldlt().solve(A.transpose() * rhs);
		logger().trace("-- Solved!");

		return;
	}

	const int num_bases = rhs.cols();

	// Compute A
	Eigen::MatrixXd A;
	compute_kernels_matrix(samples, A);

	// Compute L and t
	// Note that t is stored into `weights_` for memory efficiency reasons
	Eigen::MatrixXd L;
	if (is_volume())
	{
		compute_constraints_matrix_3d(assembler, assembler_name, num_bases, quadr, local_basis_integral, L, weights_);
	}
	else
	{
		compute_constraints_matrix_2d(assembler, assembler_name, num_bases, quadr, local_basis_integral, L, weights_);
	}

	// Compute b = rhs - A t
	Eigen::MatrixXd b = rhs - A * weights_;

// Solve the system
#ifdef VERBOSE
	logger().trace("-- Solving system of size {}x{}", L.cols(), L.cols());
#endif
	auto ldlt = (L.transpose() * A.transpose() * A * L).ldlt();
	if (ldlt.info() == Eigen::NumericalIssue)
	{
		logger().error("-- WARNING: Numerical issues when solving the harmonic least square.");
	}
	weights_ += L * ldlt.solve(L.transpose() * A.transpose() * b);
#ifdef VERBOSE
	logger().trace("-- Solved!");
#endif

#ifdef VERBOSE
	logger().trace("-- Mean residual: {}", (A * weights_ - rhs).array().abs().colwise().maxCoeff().mean());
#endif

#if 0
	Eigen::MatrixXd MM, x, dx, val;
	basis(0, quadr.points, val);
	grad(0, quadr.points, MM);
	int dim = (is_volume() ? 3 : 2);
	for (int d = 0; d < dim; ++d) {
		// basis(0, quadr.points, x);
		// auto asd = quadr.points;
		// asd.col(d).array() += 1e-7;
		// basis(0, asd, dx);
		// std::cout << (dx - x) / 1e-7 - MM.col(d) << std::endl;
		std::cout << (MM.col(d).array() * quadr.weights.array()).sum() - local_basis_integral(0, d) << std::endl;
		std::cout << ((
				MM.col((d+1)%dim).array() * quadr.points.col(d).array()
				+ MM.col(d).array() * quadr.points.col((d+1)%dim).array()
			) * quadr.weights.array()).sum() - local_basis_integral(0, (dim == 2 ? 2 : (dim+d) )) << std::endl;
		std::cout << 2.0 * (
				(quadr.points.col(d).array() * MM.col(d).array()
				+ val.array())
			* quadr.weights.array()
			).sum() - local_basis_integral(0, (dim == 2 ? (3 + d) : (dim+dim+d))) << std::endl;
	}
#endif
}


//-------------------------------------------------------------------------//
//
// Copyright 2017 Sascha Kaden
//
// 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 UTILGEO_HPP
#define UTILGEO_HPP

#include <cmath>

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

#include <ippp/types.h>
#include <ippp/util/UtilVec.hpp>

namespace ippp {
namespace util {

constexpr double pi() {
    return 3.141592653589793;
}

constexpr double twoPi() {
    return 3.141592653589793 * 2;
}

constexpr double halfPi() {
    return 3.141592653589793 / 2;
}

constexpr double toRad() {
    return 3.141592653589793 / 180;
}

constexpr double toDeg() {
    return 180 / 3.141592653589793;
}

/*!
*  \brief      Create 2D rotation matrix from rad
*  \author     Sascha Kaden
*  \param[in]  deg
*  \param[out] rotation matrix
*  \date       2016-11-15
*/
static Matrix2 getRotMat2D(const double rad) {
    Eigen::Rotation2D<double> R(rad);
    return R.toRotationMatrix();
}

/*!
*  \brief      Create 3D rotation matrix from rad
*  \author     Sascha Kaden
*  \param[in]  deg in x direction
*  \param[in]  deg in y direction
*  \param[in]  deg in z direction
*  \param[out] rotation matrix
*  \date       2016-11-15
*/
static Matrix3 getRotMat3D(const double radX, const double radY, const double radZ) {
    Matrix3 R;
    R = Eigen::AngleAxisd(radX, Eigen::Vector3d::UnitX()) * Eigen::AngleAxisd(radY, Eigen::Vector3d::UnitY()) *
        Eigen::AngleAxisd(radZ, Eigen::Vector3d::UnitZ());
    return R;
}

/*!
*  \brief      Create transformation matrix T from rotation R and translation t
*  \author     Sascha Kaden
*  \param[in]  rotation matrix
*  \param[in]  translatin matrix
*  \param[out] transformation matrix
*  \date       2016-08-25
*/
static Transform createTransform(const Matrix3 &R, const Vector3 &t) {
    Transform T;
    T = Translation(t) * R;
    return T;
}

/*!
*  \brief      Decompose transformation matrix T in rotation R and translation t
*  \author     Sascha Kaden
*  \param[in]  transformation matrix
*  \param[out] rotation matrix
*  \param[out] translation matrix
*  \date       2016-08-25
*/
static void decomposeT(const Matrix4 &T, Matrix3 &R, Vector3 &t) {
    R = T.block<3, 3>(0, 0);
    t = T.block<3, 1>(0, 3);
}

/*!
*  \brief      Convert pose config to transformation matrix
*  \author     Sascha Kaden
*  \param[in]  pose Vector
*  \param[out] transformation matrix
*  \date       2016-07-07
*/
static Transform poseVecToTransform(const Vector6 &pose) {
    Transform T;
    T = Translation(Vector3(pose[0], pose[1], pose[2])) * Eigen::AngleAxisd(pose[3], Eigen::Vector3d::UnitX()) * Eigen::AngleAxisd(pose[4], Eigen::Vector3d::UnitY()) *
        Eigen::AngleAxisd(pose[5], Eigen::Vector3d::UnitZ());
    return T;
}

/*!
*  \brief      Convert transformation matrix into poseVec
*  \author     Sascha Kaden
*  \param[in]  transformation matrix
*  \param[out] pose Vector (angles)
*  \date       2016-07-07
*/
static Vector6 transformToVec(const Transform &T) {
    Vector3 vec(T.translation());
    Vector3 euler(T.rotation().eulerAngles(0, 1, 2));
    return util::append<3, 3>(vec, euler);
}

/*!
*  \brief      Compute the normal from the plane of three points.
*  \author     Sascha Kaden
*  \param[in]  point one
*  \param[in]  point two
*  \param[in]  point three
*  \param[out] normal Vector
*  \date       2017-04-07
*/
static Vector3 computeNormal(const Vector3 &p1, const Vector3 &p2, const Vector3 &p3) {
    Vector3 v = p2 - p1;
    Vector3 w = p3 - p1;
    double nx = (v[1] * w[2]) - (v[2] * w[1]);
    double ny = (v[2] * w[0]) - (v[0] * w[2]);
    double nz = (v[0] * w[1]) - (v[1] * w[0]);
    Vector3 normal(nx, ny, nz);
    return normal.normalized();
}

/*!
*  \brief      Transforms an AABB with the passed transformations and return the new AABB.
*  \details    The new AABB has a larger size as the original and the AABB is no more tight!
*  \author     Sascha Kaden
*  \param[in]  original AABB
*  \param[in]  Transform
*  \param[out] transformed AABB
*  \date       2017-06-21
*/
static AABB transformAABB(const AABB &aabb, const Transform &T) {
    Vector3 min = aabb.min();
    Vector3 max = aabb.max();
    Vector4 min4  = util::append<3>(min, 1);
    Vector4 max4  = util::append<3>(max, 1);
    min4 = T * min4;
    max4 = T * max4;
    return AABB(Vector3(min4[0], min4[1], min4[2]), Vector3(max4[0], max4[1], max4[2]));

//    Vector3 center(T.translation());
//    Vector3 radius = Vector3::Zero(3, 1);
//    for (size_t i = 0; i < 3; i++) {
//        for (size_t j = 0; j < 3; j++) {
//            center[i] += T(i, j) * aabb.center()[j];
//            radius[i] += std::abs(T(i, j)) * aabb.diagonal()[j] / 2;
//        }
//    }
//    return AABB(center - radius, center + radius);
}

/*!
*  \brief      Translate an AABB with the passed transformations.
*  \details    The new AABB has a larger size as the original and the AABB is no more tight!
*  \author     Sascha Kaden
*  \param[in]  original aabb
*  \param[in]  pair with rotation and transformation
*  \param[out] transformed aabb
*  \date       2017-06-21
*/
static AABB translateAABB(const AABB &a, const Transform &T) {
    AABB result(a);
    result.translate(T.translation());
    return result;
}

/*!
*  \brief      Remove duplicate vectors from the passed reference list.
*  \author     Sascha Kaden
*  \param[in]  list of vectors
*  \date       2017-04-07
*/
static void removeDuplicates(std::vector<Vector3> &vectors) {
    // sort vector list
    struct {
        bool operator()(Vector3 a, Vector3 b) {
            return a.x() < b.x();
        }
    } customCompare;
    std::sort(vectors.begin(), vectors.end(), customCompare);

    // remove duplicates
    for (auto vec = vectors.begin(); vec != vectors.end(); ++vec) {
        int i = 1;
        while (vec + i != vectors.end() && vec->x() - (vec + i)->x() < 0.01) {
            if ((*vec - *(vec + i)).squaredNorm() < 0.0001)
                vectors.erase(vec + i);
            else
                ++i;
        }
    }
}

/*!
*  \brief      Convert Vec of deg angles to Vec of rad
*  \author     Sascha Kaden
*  \param[in]  Vector of deg
*  \param[out] Vector of rad
*  \date       2016-07-07
*/
template <unsigned int dim>
Vector<dim> degToRad(Vector<dim> deg) {
    for (unsigned int i = 0; i < dim; ++i)
        deg[i] *= toRad();
    return deg;
}

/*!
*  \brief      Convert Vec of rad angles to Vec of deg
*  \author     Sascha Kaden
*  \param[in]  Vector of rad
*  \param[out] Vector of deg
*  \date       2016-07-07
*/
template <unsigned int dim>
Vector<dim> radToDeg(Vector<dim> rad) {
    for (unsigned int i = 0; i < dim; ++i)
        rad[i] *= toDeg();
    return rad;
}

/*!
*  \brief      Convert degree to radian
*  \author     Sascha Kaden
*  \param[in]  deg
*  \param[out] rad
*  \date       2016-11-16
*/
static double degToRad(const double deg) {
    return deg * toRad();
}

} /* namespace util */
} /* namespace ippp */

#endif    // UTILGEO_HPP


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Surface_mesh.h>
#include <CGAL/convex_hull_3.h>
#include <Eigen/Dense>
#include <vtkPolyDataReader.h>
#include <vtkPolyDataWriter.h>
#include <vtkPolyData.h>
#include <vtkCellArray.h>
#include <vtkDoubleArray.h>
#include <vtkSmartPointer.h>

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef K::Point_3 Point;
typedef CGAL::Surface_mesh<Point> Surface_mesh;
typedef Surface_mesh::Vertex_index Vertex;
typedef Surface_mesh::Edge_index Edge;
typedef Surface_mesh::Face_index Face;
typedef Surface_mesh::Halfedge_index Halfedge;
typedef Eigen::Vector3d Vector3d;
typedef Eigen::VectorXd VectorXd;
typedef Eigen::Matrix3d Matrix3d;
typedef Eigen::Matrix3Xd Matrix3Xd;
typedef Eigen::Map<Matrix3Xd> Map3Xd;

int main(){
    clock_t t1;
    t1 = clock();
    for(auto i=0; i < 10000; ++i){
        vtkNew<vtkPolyDataReader> reader;
        reader->SetFileName("Bad.vtk");
        reader->Update();
        auto poly = reader->GetOutput();
        auto N = poly->GetNumberOfPoints();
        auto pts = static_cast<double_t*>(poly->GetPoints()->GetData()->GetVoidPointer(0));
        Map3Xd points(pts,3,N);

        // Project points to unit sphere
        points.colwise().normalize();

        // Reset the center of the sphere to origin by translating
        Vector3d center = points.rowwise().mean();
        points = points.colwise() - center;

        std::vector<Point> spherePoints(N);
        for( auto i=0; i < N; ++i){
            spherePoints[i] = Point(points(0,i),points(1,i),points(2,i));
        }

        // Make a mesh object
        Surface_mesh sm;

        // Calculate the convex hull
        CGAL::convex_hull_3(spherePoints.begin(),
                            spherePoints.end(), sm);

        // To extract the surface
        // Write to a vtk file
        vtkNew<vtkCellArray> triangles;
        for( auto f : sm.faces() ){
            triangles->InsertNextCell(3);
	    auto he = sm.halfedge( f );
            for( auto j=0; j < 3; ++j){
                triangles->InsertCellPoint( static_cast<size_t>(sm.target(he)) );
		he = sm.next( he );
            }
        }
        poly->SetPolys(triangles);
        vtkNew<vtkPolyDataWriter> writer;
        writer->SetFileName("CH_Mesh.vtk");
        writer->SetInputData(poly);
        writer->Write();
    }
    float diff(static_cast<float>(clock()) - static_cast<float>(t1));
    std::cout << "Time elapsed : " << diff / CLOCKS_PER_SEC
              << " seconds" << std::endl;
    return 0;
}


///////////////////////////////////////////////////////////////////////////////
/// @brief      Tests periods.hpp
///
///
/// @file
/// @author     olivier

#include <iostream>

#include <boost/test/unit_test.hpp>
#include <boost/foreach.hpp>

#include "plato/math/matrix.hpp"
#include "plato/misc/periods.hpp"

using namespace boost;
using namespace unit_test;

using namespace plato::misc;
using namespace plato::math;

void period_unit_test_func()
{
    typedef plato::misc::period<int> period_type;

    /// An empty period object
    period_type period_1;

    /// A period object
    period_type period_2(2,5);

    BOOST_CHECK_EQUAL(period_1.start(), 0);
    BOOST_CHECK_EQUAL(period_1.stop() , 0);
    BOOST_CHECK_EQUAL(period_1.span() , 0);

    BOOST_CHECK_EQUAL(period_2.start(), 2);
    BOOST_CHECK_EQUAL(period_2.stop() , 5);
    BOOST_CHECK_EQUAL(period_2.span() , 3);

    /// Copy constructing a new period object and checking operator==
    period_type period_3(period_2);
    BOOST_CHECK(period_2 == period_3);

    /// Check operator!=
    BOOST_CHECK(period_type(2,5) != period_type(3,5));
    BOOST_CHECK(period_type(2,5) != period_type(2,6));

    /// Check operator=
    period_type period_4 = period_3;
    BOOST_CHECK(period_3 == period_3);

    /// Check repr()
    BOOST_CHECK_NO_THROW(period_1.repr());

    /// Checks a non-valid periods
    BOOST_CHECK_THROW(period_type p(6,2), std::runtime_error);
}

void periods_unit_test_func()
{
    typedef periods<int> periods_type;
    typedef periods_type::period_type period_type;

    /// An empty periods object
    periods_type periods_1;

    /// A periods object with 10 periods with span 1, starting at 0
    periods_type periods_2(10);

    /// A periods object with periods with span 1, starting at 5 and stopping at 15
    periods_type periods_3(5,15);

    /// A periods object with periods of span 2, starting at 7 and stopping at 17
    periods_type periods_4(7,17,2);

    /// A periods object with periods of span 2, starting at 7 and stopping at 16
    periods_type periods_5(7,16,2);

    /// A periods object with periods of different spans
    plato::math::vector<int> offsets(8);
    offsets << 0, 7, 14, 30, 60, 90, 180, 365;
    periods_type periods_6(offsets);

    /// Checking start, stop and size
    BOOST_CHECK_EQUAL(periods_1.start(),      0);
    BOOST_CHECK_EQUAL(periods_1.stop() ,      0);
    BOOST_CHECK_EQUAL(periods_1.real_stop(),  0);
    BOOST_CHECK_EQUAL(periods_1.size() ,      0u);

    BOOST_CHECK_EQUAL(periods_2.start(),      0);
    BOOST_CHECK_EQUAL(periods_2.stop() ,     10);
    BOOST_CHECK_EQUAL(periods_2.real_stop(), 10);
    BOOST_CHECK_EQUAL(periods_2.size() ,     10u);

    BOOST_CHECK_EQUAL(periods_3.start(),      5);
    BOOST_CHECK_EQUAL(periods_3.stop() ,     15);
    BOOST_CHECK_EQUAL(periods_3.real_stop(), 15);
    BOOST_CHECK_EQUAL(periods_3.size() ,     10u);

    BOOST_CHECK_EQUAL(periods_4.start(),      7);
    BOOST_CHECK_EQUAL(periods_4.stop() ,     17);
    BOOST_CHECK_EQUAL(periods_4.real_stop(), 17);
    BOOST_CHECK_EQUAL(periods_4.size() ,      5u);

    BOOST_CHECK_EQUAL(periods_5.start(),      7);
    BOOST_CHECK_EQUAL(periods_5.stop() ,     16);
    BOOST_CHECK_EQUAL(periods_5.real_stop(), 17);
    BOOST_CHECK_EQUAL(periods_5.size() ,     5u);

    BOOST_CHECK_EQUAL(periods_6.start(),      0);
    BOOST_CHECK_EQUAL(periods_6.stop() ,    365);
    BOOST_CHECK_EQUAL(periods_6.real_stop(),365);
    BOOST_CHECK_EQUAL(periods_6.size() ,     7u);

    /// Checking operator==
    BOOST_CHECK(periods_type(10) == periods_type(0,10));
    BOOST_CHECK(periods_type(10) == periods_type(0,10,1));

    /// periods_4 and periods_5 have a different stop offsets,
    /// but same real_stop offsets and since they produce the
    /// same periods, they should be considered equal. Checking that...
    BOOST_CHECK(periods_4 == periods_5);

    /// Check operator!=
    BOOST_CHECK(periods_type(0,10,1) != periods_type(1,10,1));
    BOOST_CHECK(periods_type(0,10,1) != periods_type(0,11,1));
    BOOST_CHECK(periods_type(0,10,1) != periods_type(0,10,2));

    /// Check operator=
    periods_type periods_7 = periods_6;
    BOOST_CHECK(periods_6 == periods_7);

    /// Check repr()
    BOOST_CHECK_NO_THROW(periods_7.repr());

    /// Check iteration
    periods_type p(periods_7);
    BOOST_CHECK(*(p.begin())              == period_type(  0,   7));
    BOOST_CHECK(*(++p.begin())            == period_type(  7,  14));
    BOOST_CHECK(*(++++p.begin())          == period_type( 14,  30));
    BOOST_CHECK(*(++++++p.begin())        == period_type( 30,  60));
    BOOST_CHECK(*(++++++++p.begin())      == period_type( 60,  90));
    BOOST_CHECK(*(++++++++++p.begin())    == period_type( 90, 180));
    BOOST_CHECK(*(++++++++++++p.begin())  == period_type(180, 365));
    BOOST_CHECK(  ++++++++++++++p.begin() == p.end());
    BOOST_CHECK(*(--p.end())              == period_type(180, 365));
    BOOST_CHECK(*(----p.end())            == period_type( 90, 180));
    BOOST_CHECK(*(------p.end())          == period_type( 60,  90));
    BOOST_CHECK(*(--------p.end())        == period_type( 30,  60));
    BOOST_CHECK(*(----------p.end())      == period_type( 14,  30));
    BOOST_CHECK(*(------------p.end())    == period_type(  7,  14));
    BOOST_CHECK(*(--------------p.end())  == period_type(  0,   7));

    /// Check random access
    BOOST_CHECK(p.begin()                 == p.begin() + 0);
    BOOST_CHECK(++p.begin()               == p.begin() + 1);
    BOOST_CHECK(++++p.begin()             == p.begin() + 2);
    BOOST_CHECK(++++++p.begin()           == p.begin() + 3);
    BOOST_CHECK(++++++++p.begin()         == p.begin() + 4);
    BOOST_CHECK(++++++++++p.begin()       == p.begin() + 5);
    BOOST_CHECK(++++++++++++p.begin()     == p.begin() + 6);
    BOOST_CHECK(++++++++++++++p.begin()   == p.begin() + 7);
    BOOST_CHECK(p.begin()                 == p.end()   - 7);
    BOOST_CHECK(++p.begin()               == p.end()   - 6);
    BOOST_CHECK(++++p.begin()             == p.end()   - 5);
    BOOST_CHECK(++++++p.begin()           == p.end()   - 4);
    BOOST_CHECK(++++++++p.begin()         == p.end()   - 3);
    BOOST_CHECK(++++++++++p.begin()       == p.end()   - 2);
    BOOST_CHECK(++++++++++++p.begin()     == p.end()   - 1);
    BOOST_CHECK(++++++++++++++p.begin()   == p.end()   - 0);

    /// Check BOOST_FOREACH
    std::size_t count(0);
    BOOST_FOREACH(period_type period, p)
    {
        BOOST_CHECK_EQUAL(period, period_type(
                                      (*(p.begin() + count)).start()
                                    , (*(p.begin() + count)).stop()
                                  )
                         );
        ++count;
    }
    BOOST_CHECK_EQUAL(count, static_cast<std::size_t>(p.size()));

    /// Check some non-valid periods
    BOOST_CHECK_THROW(periods_type p(0,10,-1), std::runtime_error);
    BOOST_CHECK_THROW(periods_type p(10,0, 1), std::runtime_error);
    BOOST_CHECK_THROW(periods_type p(10,0,-1), std::runtime_error);
    plato::math::vector<int> o(8);
    o << 0, 7, 14, 60, 30, 90, 180, 365;
    BOOST_CHECK_THROW(periods_type p(o), std::runtime_error);
}

///////////////////////////////////////////////////////////////////////////////
// init_unit_test_suite
//
test_suite* init_unit_test_suite( int argc, char* argv[] )
{
    test_suite *test = BOOST_TEST_SUITE("plato::misc::periods test");

    test->add(BOOST_TEST_CASE(&period_unit_test_func));
    test->add(BOOST_TEST_CASE(&periods_unit_test_func));

    return test;
}


/*
 [auto_generated]
 boost/numeric/odeint/stepper/runge_kutta_cash_karp54.hpp

 [begin_description]
 Implementation of the Runge Kutta Cash Karp 5(4) method. It uses the generic error stepper.
 [end_description]

 Copyright 2009-2011 Karsten Ahnert
 Copyright 2009-2011 Mario Mulansky

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

#ifndef BOOST_NUMERIC_ODEINT_STEPPER_RUNGE_KUTTA_CASH_KARP54_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_RUNGE_KUTTA_CASH_KARP54_HPP_INCLUDED

#include <boost/fusion/container/vector.hpp>
#include <boost/fusion/container/generation/make_vector.hpp>

#include <boost/numeric/odeint/stepper/explicit_error_generic_rk.hpp>
#include <boost/numeric/odeint/algebra/range_algebra.hpp>
#include <boost/numeric/odeint/algebra/default_operations.hpp>
#include <boost/numeric/odeint/algebra/algebra_dispatcher.hpp>
#include <boost/numeric/odeint/algebra/operations_dispatcher.hpp>

#include <boost/numeric/odeint/util/state_wrapper.hpp>
#include <boost/numeric/odeint/util/is_resizeable.hpp>
#include <boost/numeric/odeint/util/resizer.hpp>

#include <boost/array.hpp>

namespace boost {
namespace numeric {
namespace odeint {

#ifndef DOXYGEN_SKIP
template< class Value = double >
struct rk54_ck_coefficients_a1 : boost::array< Value , 1 >
{
    rk54_ck_coefficients_a1( void )
    {
        (*this)[0] = static_cast< Value >( 1 )/static_cast< Value >( 5 );
    }
};

template< class Value = double >
struct rk54_ck_coefficients_a2 : boost::array< Value , 2 >
{
    rk54_ck_coefficients_a2( void )
    {
        (*this)[0] = static_cast<Value>( 3 )/static_cast<Value>( 40 );
        (*this)[1] = static_cast<Value>( 9 )/static_cast<Value>( 40 );
    }
};

template< class Value = double >
struct rk54_ck_coefficients_a3 : boost::array< Value , 3 >
{
    rk54_ck_coefficients_a3( void )
    {
        (*this)[0] = static_cast<Value>( 3 )/static_cast<Value>( 10 );
        (*this)[1] = static_cast<Value>( -9 )/static_cast<Value>( 10 );
        (*this)[2] = static_cast<Value>( 6 )/static_cast<Value>( 5 );
    }
};

template< class Value = double >
struct rk54_ck_coefficients_a4 : boost::array< Value , 4 >
{
    rk54_ck_coefficients_a4( void )
    {
        (*this)[0] = static_cast<Value>( -11 )/static_cast<Value>( 54 );
        (*this)[1] = static_cast<Value>( 5 )/static_cast<Value>( 2 );
        (*this)[2] = static_cast<Value>( -70 )/static_cast<Value>( 27 );
        (*this)[3] = static_cast<Value>( 35 )/static_cast<Value>( 27 );
    }
};

template< class Value = double >
struct rk54_ck_coefficients_a5 : boost::array< Value , 5 >
{
    rk54_ck_coefficients_a5( void )
    {
        (*this)[0] = static_cast<Value>( 1631 )/static_cast<Value>( 55296 );
        (*this)[1] = static_cast<Value>( 175 )/static_cast<Value>( 512 );
        (*this)[2] = static_cast<Value>( 575 )/static_cast<Value>( 13824 );
        (*this)[3] = static_cast<Value>( 44275 )/static_cast<Value>( 110592 );
        (*this)[4] = static_cast<Value>( 253 )/static_cast<Value>( 4096 );
    }
};

template< class Value = double >
struct rk54_ck_coefficients_b : boost::array< Value , 6 >
{
    rk54_ck_coefficients_b( void )
    {
        (*this)[0] = static_cast<Value>( 37 )/static_cast<Value>( 378 );
        (*this)[1] = static_cast<Value>( 0 );
        (*this)[2] = static_cast<Value>( 250 )/static_cast<Value>( 621 );
        (*this)[3] = static_cast<Value>( 125 )/static_cast<Value>( 594 );
        (*this)[4] = static_cast<Value>( 0 );
        (*this)[5] = static_cast<Value>( 512 )/static_cast<Value>( 1771 );
    }
};

template< class Value = double >
struct rk54_ck_coefficients_db : boost::array< Value , 6 >
{
    rk54_ck_coefficients_db( void )
    {
        (*this)[0] = static_cast<Value>( 37 )/static_cast<Value>( 378 ) - static_cast<Value>( 2825 )/static_cast<Value>( 27648 );
        (*this)[1] = static_cast<Value>( 0 );
        (*this)[2] = static_cast<Value>( 250 )/static_cast<Value>( 621 ) - static_cast<Value>( 18575 )/static_cast<Value>( 48384 );
        (*this)[3] = static_cast<Value>( 125 )/static_cast<Value>( 594 ) - static_cast<Value>( 13525 )/static_cast<Value>( 55296 );
        (*this)[4] = static_cast<Value>( -277 )/static_cast<Value>( 14336 );
        (*this)[5] = static_cast<Value>( 512 )/static_cast<Value>( 1771 ) - static_cast<Value>( 1 )/static_cast<Value>( 4 );
    }
};

template< class Value = double >
struct rk54_ck_coefficients_c : boost::array< Value , 6 >
{
    rk54_ck_coefficients_c( void )
    {
        (*this)[0] = static_cast<Value>(0);
        (*this)[1] = static_cast<Value>( 1 )/static_cast<Value>( 5 );
        (*this)[2] = static_cast<Value>( 3 )/static_cast<Value>( 10 );
        (*this)[3] = static_cast<Value>( 3 )/static_cast<Value>( 5 );
        (*this)[4] = static_cast<Value>( 1 );
        (*this)[5] = static_cast<Value>( 7 )/static_cast<Value>( 8 );
    }
};
#endif

template<
    class State ,
    class Value = double ,
    class Deriv = State ,
    class Time = Value ,
    class Algebra = typename algebra_dispatcher< State >::algebra_type ,
    class Operations = typename operations_dispatcher< State >::operations_type ,
    class Resizer = initially_resizer
    >
#ifndef DOXYGEN_SKIP
class runge_kutta_cash_karp54 : public explicit_error_generic_rk< 6 , 5 , 5 , 4 ,
        State , Value , Deriv , Time , Algebra , Operations , Resizer >
#else
class runge_kutta_cash_karp54 : public explicit_error_generic_rk
#endif
{

public:
#ifndef DOXYGEN_SKIP
    typedef explicit_error_generic_rk< 6 , 5 , 5 , 4 , State , Value , Deriv , Time ,
                               Algebra , Operations , Resizer > stepper_base_type;
#endif
    typedef typename stepper_base_type::state_type state_type;
    typedef typename stepper_base_type::value_type value_type;
    typedef typename stepper_base_type::deriv_type deriv_type;
    typedef typename stepper_base_type::time_type time_type;
    typedef typename stepper_base_type::algebra_type algebra_type;
    typedef typename stepper_base_type::operations_type operations_type;
    typedef typename stepper_base_type::resizer_type resizer_typ;

    #ifndef DOXYGEN_SKIP
    typedef typename stepper_base_type::stepper_type stepper_type;
    typedef typename stepper_base_type::wrapped_state_type wrapped_state_type;
    typedef typename stepper_base_type::wrapped_deriv_type wrapped_deriv_type;
    #endif

    runge_kutta_cash_karp54( const algebra_type &algebra = algebra_type() ) : stepper_base_type(
        boost::fusion::make_vector( rk54_ck_coefficients_a1<Value>() ,
                                 rk54_ck_coefficients_a2<Value>() ,
                                 rk54_ck_coefficients_a3<Value>() ,
                                 rk54_ck_coefficients_a4<Value>() ,
                                 rk54_ck_coefficients_a5<Value>() ) ,
            rk54_ck_coefficients_b<Value>() , rk54_ck_coefficients_db<Value>() , rk54_ck_coefficients_c<Value>() ,
            algebra )
    { }
};

/********** DOXYGEN **********/

/**
 * \class runge_kutta_cash_karp54
 * \brief The Runge-Kutta Cash-Karp method.
 *
 * The Runge-Kutta Cash-Karp method is one of the standard methods for
 * solving ordinary differential equations, see
 * en.wikipedia.org/wiki/Cash-Karp_methods.
 * The method is explicit and fulfills the Error Stepper concept. Step size control
 * is provided but continuous output is not available for this method.
 *
 * This class derives from explicit_error_stepper_base and inherits its interface via CRTP (current recurring template pattern).
 * Furthermore, it derivs from explicit_error_generic_rk which is a generic Runge-Kutta algorithm with error estimation.
 * For more details see explicit_error_stepper_base and explicit_error_generic_rk.
 *
 * \tparam State The state type.
 * \tparam Value The value type.
 * \tparam Deriv The type representing the time derivative of the state.
 * \tparam Time The time representing the independent variable - the time.
 * \tparam Algebra The algebra type.
 * \tparam Operations The operations type.
 * \tparam Resizer The resizer policy type.
 */

    /**
     * \fn runge_kutta_cash_karp54::runge_kutta_cash_karp54( const algebra_type &algebra )
     * \brief Constructs the runge_kutta_cash_karp54 class. This constructor can be used as a default
     * constructor if the algebra has a default constructor.
     * \param algebra A copy of algebra is made and stored inside explicit_stepper_base.
     */
}
}
}

#endif // BOOST_NUMERIC_ODEINT_STEPPER_RUNGE_KUTTA_CASH_KARP54_HPP_INCLUDED


//  (C) Copyright Matt Borland 2022.
//  Distributed under the Boost Software License, Version 1.0.
//  (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)

#include "math_unit_test.hpp"
#include <limits>
#include <boost/math/special_functions/logaddexp.hpp>
#include <boost/math/constants/constants.hpp>

template <typename Real>
void test()
{
    using boost::math::logaddexp;
    using std::log;
    using std::exp;

    constexpr Real nan_val = std::numeric_limits<Real>::quiet_NaN();
    constexpr Real inf_val = std::numeric_limits<Real>::infinity();

    // NAN
    CHECK_NAN(logaddexp(nan_val, Real(1)));
    CHECK_NAN(logaddexp(Real(1), nan_val));
    CHECK_NAN(logaddexp(nan_val, nan_val));

    // INF
    CHECK_EQUAL(logaddexp(inf_val, Real(1)), inf_val);
    CHECK_EQUAL(logaddexp(Real(1), inf_val), inf_val);
    CHECK_EQUAL(logaddexp(inf_val, inf_val), inf_val);

    // Equal values
    constexpr Real ln2 = boost::math::constants::ln_two<Real>();
    CHECK_ULP_CLOSE(Real(2) + ln2, logaddexp(Real(2), Real(2)), 1);
    CHECK_ULP_CLOSE(Real(1e-50) + ln2, logaddexp(Real(1e-50), Real(1e-50)), 1);

    // Spot check
    // https://numpy.org/doc/stable/reference/generated/numpy.logaddexp.html
    // Calculated at higher precision using wolfram alpha
    Real x1 = 1e-50l;
    Real x2 = 2.5e-50l;
    Real spot1 = static_cast<Real>(exp(x1));
    Real spot2 = static_cast<Real>(exp(x2));
    Real spot12 = logaddexp(x1, x2);

    CHECK_ULP_CLOSE(log(spot1 + spot2), spot12, 1);
}

int main (void)
{
    test<float>();
    test<double>();
    test<long double>();
    return boost::math::test::report_errors();
}


#include <CGAL/Simple_cartesian.h>
#include <CGAL/Polyhedron_3.h>

#include <iostream>
#include <list>

#include <boost/graph/kruskal_min_spanning_tree.hpp>

typedef CGAL::Simple_cartesian<double>                       Kernel;
typedef Kernel::Vector_3                                     Vector;
typedef Kernel::Point_3                                      Point;
typedef CGAL::Polyhedron_3<Kernel>                           Polyhedron;

typedef boost::graph_traits<Polyhedron>::vertex_descriptor vertex_descriptor;
typedef boost::graph_traits<Polyhedron>::vertex_iterator   vertex_iterator;
typedef boost::graph_traits<Polyhedron>::edge_descriptor   edge_descriptor;

// The BGL makes heavy use of indices associated to the vertices
// We use a std::map to store the index

typedef std::map<vertex_descriptor,int> Vertex_index_map;
Vertex_index_map vertex_index_map;

// A std::map is not a property map, because it is not lightweight
typedef boost::associative_property_map<Vertex_index_map> Vertex_index_pmap;
Vertex_index_pmap vertex_index_pmap(vertex_index_map);

void
kruskal(const Polyhedron& P)
{
  // associate indices to the vertices
  vertex_iterator vb, ve;
  int index = 0;

  // boost::tie assigns the first and second element of the std::pair
  // returned by boost::vertices to the variables vb and ve
  for(boost::tie(vb, ve)=vertices(P); vb!=ve; ++vb){
    vertex_index_pmap[*vb]= index++;
  }

  // We use the default edge weight which is the length of the edge
  // This property map is defined in graph_traits_Polyhedron_3.h

  // In the function call you can see a named parameter: vertex_index_map
  std::list<edge_descriptor> mst;

  boost::kruskal_minimum_spanning_tree(P,
                                       std::back_inserter(mst),
                                       boost::vertex_index_map(vertex_index_pmap));

  std::cout << "#VRML V2.0 utf8\n"
    "Shape {\n"
    "  appearance Appearance {\n"
    "    material Material { emissiveColor 1 0 0}}\n"
    "    geometry\n"
    "    IndexedLineSet {\n"
    "      coord Coordinate {\n"
    "        point [ \n";

  for(boost::tie(vb, ve) = vertices(P); vb!=ve; ++vb){
    std::cout <<  "        " << (*vb)->point() << "\n";
  }

  std::cout << "        ]\n"
               "     }\n"
    "      coordIndex [\n";

  for(std::list<edge_descriptor>::iterator it = mst.begin(); it != mst.end(); ++it)
  {
    edge_descriptor e = *it ;
    vertex_descriptor s = source(e,P);
    vertex_descriptor t = target(e,P);
    std::cout << "      " << vertex_index_pmap[s] << ", " << vertex_index_pmap[t] <<  ", -1\n";
  }

  std::cout << "]\n"
    "  }#IndexedLineSet\n"
    "}# Shape\n";
}

int main() {

  Polyhedron P;
  Point a(1,0,0);
  Point b(0,1,0);
  Point c(0,0,1);
  Point d(0,0,0);

  P.make_tetrahedron(a,b,c,d);

  kruskal(P);

  return 0;
}


#include <blitz/array.h>
#include <blitz/timer.h>

BZ_USING_NAMESPACE(blitz)

BZ_DECLARE_STENCIL4(acoustic2D_stencil,P1,P2,P3,c)
  P3 = 2 * P2 + c * Laplacian2D(P2) - P1;
BZ_END_STENCIL

int benchmark(int N, int nIterations, int blockSize)
{
    Array<float,2> P1, P2, P3, c;
    allocateArrays(shape(N,N), P1, P2, P3, c);

    // Initial conditions: obviously in a real application these
    // wouldn't be zeroed...
    Range I(0,blockSize-1), J(0,blockSize-1);

    P1(I,J) = 0;
    P2(I,J) = 0;
    P3(I,J) = 0;
    c(I,J) = 0;

    for (int i=0; i < nIterations; ++i)
    {
        // Apply the stencil object to the arrays
        applyStencil(acoustic2D_stencil(), P1(I,J), P2(I,J), P3(I,J), c(I,J));

        // Set [P1,P2,P3] <- [P2,P3,P1] to set up for the next
        // time step
        cycleArrays(P1,P2,P3);
    }

    return 0;
}

int main()
{
    Timer timer;

    cout << "N\tMflops" << endl;

    const int blockSize = 27;

    for (int N=2000; N < 2100; ++N)
    {
        double stencilPoints = pow(blockSize-2,2.0);
        int nIterations = 5000;

        timer.start();
        benchmark(N, nIterations, blockSize);
        timer.stop();

        double flops = (4 + 7) * stencilPoints * nIterations;
        double Mflops = flops / timer.elapsedSeconds() / 1.0E+6;
        cout << N << "\t" << Mflops << endl;
    }

    return 0;
}


/**
 * \file se2_localization_ukfm.cpp
 *
 *  Created on: Dec 10, 2018
 *     \author: artivis
 *
 *  ---------------------------------------------------------
 *  This file is:
 *  (c) 2021 Jeremie Deray
 *
 *  This file is part of `manif`, a C++ template-only library
 *  for Lie theory targeted at estimation for robotics.
 *  Manif is:
 *  (c) 2018 Jeremie Deray @ IRI-UPC, Barcelona
 *  ---------------------------------------------------------
 *
 *  ---------------------------------------------------------
 *  Demonstration example:
 *
 *  2D Robot localization based on fixed beacons.
 *
 *  See se3_localization.cpp for the 3D equivalent.
 *  See se3_sam.cpp for a more advanced example performing smoothing and mapping.
 *  ---------------------------------------------------------
 *
 *  This demo showcases an application of an Unscented Kalman Filter on Manifold,
 *  based on the paper
 *  'A Code for Unscented Kalman Filtering on Manifolds (UKF-M)'
 *  [https://arxiv.org/pdf/2002.00878.pdf], M. Brossard, A. Barrau and S. Bonnabel.
 *
 *  The following is an abstract of the example hereafter.
 *  Please consult the aforemention paper for better UKF-M reference
 *  and the paper Sola-18, [https://arxiv.org/abs/1812.01537] for general
 *  Lie group reference.
 *
 *
 *  We consider a robot in the plane surrounded by a small
 *  number of punctual landmarks or _beacons_.
 *  The robot receives control actions in the form of axial
 *  and angular velocities, and is able to measure the location
 *  of the beacons w.r.t its own reference frame.
 *
 *  The robot pose X is in SE(2) and the beacon positions b_k in R^2,
 *
 *          | cos th  -sin th   x |
 *      X = | sin th   cos th   y |  // position and orientation
 *          |   0        0      1 |
 *
 *      b_k = (bx_k, by_k)           // lmk coordinates in world frame
 *
 *  The control signal u is a twist in se(2) comprising longitudinal
 *  velocity v and angular velocity w, with no lateral velocity
 *  component, integrated over the sampling time dt.
 *
 *      u = (v*dt, 0, w*dt)
 *
 *  The control is corrupted by additive Gaussian noise u_noise,
 *  with covariance
 *
 *    Q = diagonal(sigma_v^2, sigma_s^2, sigma_w^2).
 *
 *  This noise accounts for possible lateral slippage u_s
 *  through a non-zero value of sigma_s,
 *
 *  At the arrival of a control u, the robot pose is updated
 *  with X <-- X * Exp(u) = X + u.
 *
 *  Landmark measurements are of the range and bearing type,
 *  though they are put in Cartesian form for simplicity.
 *  Their noise n is zero mean Gaussian, and is specified
 *  with a covariances matrix R.
 *  We notice the rigid motion action y = h(X,b) = X^-1 * b
 *  (see appendix C),
 *
 *      y_k = (brx_k, bry_k)       // lmk coordinates in robot frame
 *
 *  We consider the beacons b_k situated at known positions.
 *  We define the pose to estimate as X in SE(2).
 *  The estimation error dx and its covariance P are expressed
 *  in the tangent space at X.
 *
 *  All these variables are summarized again as follows
 *
 *    X   : robot pose, SE(2)
 *    u   : robot control, (v*dt ; 0 ; w*dt) in se(2)
 *    Q   : control perturbation covariance
 *    b_k : k-th landmark position, R^2
 *    y   : Cartesian landmark measurement in robot frame, R^2
 *    R   : covariance of the measurement noise
 *
 *  The motion and measurement models are
 *
 *    X_(t+1) = f(X_t, u) = X_t * Exp ( w )     // motion equation
 *    y_k     = h(X, b_k) = X^-1 * b_k          // measurement equation
 *
 *  The algorithm below comprises first a simulator to
 *  produce measurements, then uses these measurements
 *  to estimate the state, using a Lie-based error-state Kalman filter.
 *
 *  This file has plain code with only one main() function.
 *  There are no function calls other than those involving `manif`.
 *
 *  Printing simulated state and estimated state together
 *  with an unfiltered state (i.e. without Kalman corrections)
 *  allows for evaluating the quality of the estimates.
 */

#include "manif/SE2.h"

#include <Eigen/Cholesky>

#include <vector>

#include <iostream>
#include <iomanip>
#include <tuple>

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

using namespace Eigen;

typedef Array<double, 2, 1> Array2d;
typedef Array<double, 3, 1> Array3d;

template <typename Scalar>
struct Weights
{
  Weights() = default;
  ~Weights() = default;

  Weights(const Scalar l, const Scalar alpha)
  {
    using std::sqrt;

    const Scalar m = (alpha * alpha - 1) * l;
    const Scalar ml = m + l;

    sqrt_d_lambda = sqrt(ml);
    wj = Scalar(1) / (Scalar(2) * (ml));
    wm = m / (ml);
    w0 = m / (ml) + Scalar(3) - alpha * alpha;
  }

  Scalar sqrt_d_lambda;
  Scalar wj;
  Scalar wm;
  Scalar w0;
};

using Weightsd = Weights<double>;

template <typename Scalar>
std::tuple<Weights<Scalar>, Weights<Scalar>, Weights<Scalar>>
compute_sigma_weights(const Scalar state_size,
                      const Scalar propagation_noise_size,
                      const Scalar alpha_0,
                      const Scalar alpha_1,
                      const Scalar alpha_2)
{
  assert(state_size>0);
  assert(propagation_noise_size>0);
  assert(alpha_0>=1e-3 && alpha_0<=1);
  assert(alpha_1>=1e-3 && alpha_1<=1);
  assert(alpha_2>=1e-3 && alpha_2<=1);

  return std::make_tuple(Weights<Scalar>(state_size, alpha_0),
                         Weights<Scalar>(propagation_noise_size, alpha_1),
                         Weights<Scalar>(state_size, alpha_2));
}

int main()
{
    std::srand((unsigned int) time(0));

    // START CONFIGURATION
    //
    //
    const int NUMBER_OF_LMKS_TO_MEASURE = 3;
    constexpr int DoF = manif::SE2d::DoF;
    constexpr int SystemNoiseSize = manif::SE2d::DoF;
    // Measurement Dim
    constexpr int Rp = 2;

    // Define the robot pose element and its covariance
    manif::SE2d X            = manif::SE2d::Identity(),
                X_simulation = manif::SE2d::Identity(),
                X_unfiltered = manif::SE2d::Identity();
    Matrix3d    P            = Matrix3d::Identity() * 1e-6;

    // Define a control vector and its noise and covariance
    manif::SE2Tangentd  u_simu, u_est, u_unfilt;
    Vector3d            u_nom, u_noisy, u_noise;
    Array3d             u_sigmas;
    Matrix3d            U, Uchol;

    u_nom    << 0.1, 0.0, 0.05;
    u_sigmas << 0.1, 0.1, 0.1;
    U        = (u_sigmas * u_sigmas).matrix().asDiagonal();
    Uchol    = U.llt().matrixL();

    // Define three landmarks in R^2
    Eigen::Vector2d b;
    const std::vector<Eigen::Vector2d> landmarks{
      Eigen::Vector2d(2.0,  0.0),
      Eigen::Vector2d(2.0,  1.0),
      Eigen::Vector2d(2.0, -1.0)
    };

    // Define the beacon's measurements
    Vector2d                  y, y_bar, y_noise;
    Matrix<double, Rp, 2*DoF> yj;
    Array2d                   y_sigmas;
    Matrix2d                  R;
    std::vector<Vector2d>     measurements(landmarks.size());

    y_sigmas << 0.01, 0.01;
    R        = (y_sigmas * y_sigmas).matrix().asDiagonal();

    // Declare UFK variables
    Array3d alpha;
    alpha << 1e-3, 1e-3, 1e-3;

    Weightsd w_d, w_q, w_u;
    std::tie(w_d, w_q, w_u) = compute_sigma_weights<double>(
      DoF, Rp, alpha(0), alpha(1), alpha(2)
    );

    // Declare some temporaries

    manif::SE2d X_new;
    Matrix3d P_new;
    manif::SE2d s_j_p, s_j_m;
    Vector3d xi_mean;
    Vector3d w_p, w_m;

    Matrix2d P_yy;
    Matrix<double, DoF, 2*DoF> xij;
    Matrix<double, DoF, 2> P_xiy;

    Vector2d                e, z;   // expectation, innovation
    Matrix<double, 3, 2>    K;      // Kalman gain
    manif::SE2Tangentd      dx;     // optimal update step, or error-state

    Matrix<double, DoF, DoF> xis;
    Matrix<double, DoF, DoF*2> xis_new;
    Matrix<double, DoF, SystemNoiseSize*2> xis_new2;

    //
    //
    // CONFIGURATION DONE

    // DEBUG
    cout << std::fixed   << std::setprecision(4) << std::showpos << endl;
    cout << "X STATE     :    X      Y    THETA" << endl;
    cout << "----------------------------------" << endl;
    cout << "X initial   : " << X_simulation.log().coeffs().transpose() << endl;
    cout << "----------------------------------" << endl;
    // END DEBUG

    // START TEMPORAL LOOP
    //
    //

    // Make 10 steps. Measure up to three landmarks each time.
    for (int t = 0; t <10; t++)
    {
        //// I. Simulation ###############################################################################

        /// simulate noise
        u_noise = u_sigmas * Array3d::Random();             // control noise
        u_noisy = u_nom + u_noise;                          // noisy control

        u_simu   = u_nom;
        u_est    = u_noisy;
        u_unfilt = u_noisy;

        /// first we move - - - - - - - - - - - - - - - - - - - - - - - - - - - -
        X_simulation = X_simulation + u_simu;               // overloaded X.rplus(u) = X * exp(u)

        /// then we measure all landmarks - - - - - - - - - - - - - - - - - - - -
        for (std::size_t i = 0; i < landmarks.size(); i++)
        {
            b = landmarks[i];                               // lmk coordinates in world frame

            /// simulate noise
            y_noise = y_sigmas * Array2d::Random();         // measurement noise

            y = X_simulation.inverse().act(b);              // landmark measurement, before adding noise
            y = y + y_noise;                                // landmark measurement, noisy
            measurements[i] = y;                            // store for the estimator just below
        }

        //// II. Estimation ###############################################################################

        /// First we move - - - - - - - - - - - - - - - - - - - - - - - - - - - -

        X_new = X + u_est;                        // X * exp(u)

        // set sigma points
        xis = w_d.sqrt_d_lambda * P.llt().matrixL().toDenseMatrix();

        // sigma points on manifold
        for (int i = 0; i < DoF; ++i)
        {
          s_j_p = X + manif::SE2Tangentd( xis.col(i));
          s_j_m = X + manif::SE2Tangentd(-xis.col(i));

          xis_new.col(i) = (X_new.lminus(s_j_p + u_est)).coeffs();
          xis_new.col(i + DoF) = (X_new.lminus(s_j_m + u_est)).coeffs();
        }

        // compute covariance
        xi_mean = w_d.wj * xis_new.rowwise().sum();
        xis_new.colwise() -= xi_mean;

        P_new = w_d.wj * xis_new * xis_new.transpose() +
                w_d.w0 * xi_mean * xi_mean.transpose();

        // sigma points on manifold
        for (int i = 0; i < SystemNoiseSize; ++i)
        {
          w_p =  w_q.sqrt_d_lambda * Uchol.col(i);
          w_m = -w_q.sqrt_d_lambda * Uchol.col(i);

          xis_new2.col(i) = (X_new.lminus(X + (u_est + w_p))).coeffs();
          xis_new2.col(i + SystemNoiseSize) = (X_new.lminus(X + (u_est + w_m))).coeffs();
        }

        xi_mean = w_q.wj * xis_new2.rowwise().sum();
        xis_new2.colwise() -= xi_mean;

        U = w_q.wj * xis_new2 * xis_new2.transpose() +
            w_q.w0 * xi_mean * xi_mean.transpose();

        P = P_new + U;

        X = X_new;

        /// Then we correct using the measurements of each lmk - - - - - - - - -
        for (int i = 0; i < NUMBER_OF_LMKS_TO_MEASURE; i++)
        {
            // landmark
            b = landmarks[i];                               // lmk coordinates in world frame

            // measurement
            y = measurements[i];                            // lmk measurement, noisy

            // expectation
            e = X.inverse().act(b);

            // set sigma points
            xis = w_u.sqrt_d_lambda * P.llt().matrixL().toDenseMatrix();

            // compute measurement sigma points
            for (int d = 0; d < DoF; ++d)
            {
              s_j_p = X + manif::SE2Tangentd( xis.col(d));
              s_j_m = X + manif::SE2Tangentd(-xis.col(d));

              yj.col(d) = s_j_p.inverse().act(b);
              yj.col(d + DoF) = s_j_m.inverse().act(b);
            }

            // measurement mean
            y_bar = w_u.wm * e + w_u.wj * yj.rowwise().sum();

            yj.colwise() -= y_bar;
            e -= y_bar;

            // compute covariance and cross covariance matrices
            P_yy = w_u.w0 * e * e.transpose() +
                   w_u.wj * yj * yj.transpose() + R;

            xij << xis, -xis;
            P_xiy = w_u.wj * xij * yj.transpose();

            // Kalman gain
            K = P_yy.colPivHouseholderQr().solve(P_xiy.transpose()).transpose();

            // innovation
            z = y - y_bar;

            // Correction step
            dx = K * z;                                     // dx is in the tangent space at X

            // Update
            X = X + dx;                                     // overloaded X.rplus(dx) = X * exp(dx)
            P = P - K * P_yy * K.transpose();
        }

        //// III. Unfiltered ##############################################################################

        // move also an unfiltered version for comparison purposes
        X_unfiltered = X_unfiltered + u_unfilt;

        //// IV. Results ##############################################################################

        // DEBUG
        cout << "X simulated : " << X_simulation.log().coeffs().transpose() << "\n";
        cout << "X estimated : " << X.log().coeffs().transpose() << "\n";
        cout << "X unfilterd : " << X_unfiltered.log().coeffs().transpose() << "\n";
        cout << "----------------------------------" << endl;
        // END DEBUG

    }

    //
    //
    // END OF TEMPORAL LOOP. DONE.

    return 0;
}


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

#include <Eigen/Dense>

namespace Eigen
{

typedef Matrix<double, 6, 1> Vector6d;
typedef Matrix<double, 6, 6> Matrix6d;

}

#endif // EIGEN_UTILITIES_TYPES_H


/* =========================================================================
   Copyright (c) 2012-2014, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
                             -----------------
               ViennaFEM - The Vienna Finite Element Method Library
                             -----------------

   Author:     Karl Rupp                          rupp@iue.tuwien.ac.at

   License:    MIT (X11), see file LICENSE in the ViennaFEM base directory
============================================================================ */

// remove assert() statements and the like in order to get reasonable performance
#ifndef NDEBUG
  #define NDEBUG
#endif

#include <iostream>
#include <vector>
#include <stdlib.h>
#include <assert.h>

// ViennaFEM includes:
#include "viennafem/forwards.h"
#include "viennafem/fem.hpp"
#include "viennafem/io/vtk_writer.hpp"

// ViennaGrid includes:
#include "viennagrid/forwards.hpp"
#include "viennagrid/config/default_configs.hpp"
#include "viennagrid/io/netgen_reader.hpp"

// ViennaData includes:
#include "viennadata/api.hpp"

#include "viennamath/expression.hpp"
#include "viennamath/manipulation/eval.hpp"
#include "viennamath/manipulation/substitute.hpp"
#include "viennamath/manipulation/diff.hpp"

#include "viennamath/runtime/equation.hpp"
#include "viennamath/manipulation/apply_coordinate_system.hpp"

// Boost.uBLAS includes:
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/ublas/operation_sparse.hpp>

//ViennaCL includes:
#ifndef VIENNACL_HAVE_UBLAS
 #define VIENNACL_HAVE_UBLAS
#endif

#include "viennacl/linalg/bicgstab.hpp"
#include "viennacl/linalg/norm_2.hpp"
#include "viennacl/linalg/prod.hpp"

using namespace viennamath;

//
// The strain tensor: eps_ij = 0.5 * (du_i/dx_j + du_j/dx_i)
//
template <typename InterfaceType>
std::vector< rt_expr<InterfaceType> > strain_tensor(std::vector< rt_function_symbol<InterfaceType> > const & u)
{
  typedef rt_variable<InterfaceType>     Variable;

  //
  // a 3x3 matrix representing the strain tensor
  //
  std::vector< rt_expr<InterfaceType> > result(9);

  Variable x(0);
  Variable y(1);
  Variable z(2);

  //first row:
  result[0] =        diff(u[0], x);
  result[1] = 0.5 * (diff(u[0], y) + diff(u[1], x));
  result[2] = 0.5 * (diff(u[0], z) + diff(u[2], x));

  //second row:
  result[3] = 0.5 * (diff(u[1], x) + diff(u[0], y));
  result[4] =        diff(u[1], y);
  result[5] = 0.5 * (diff(u[1], z) + diff(u[2], y));

  //third row:
  result[6] = 0.5 * (diff(u[2], x) + diff(u[0], z));
  result[7] = 0.5 * (diff(u[2], y) + diff(u[1], z));
  result[8] =        diff(u[2], z);

  return result;
}

//
// The stress tensor: sigma = 2 \mu eps + \lambda trace(eps) Id  for St. Venent-Kirchhoff material
// can be replaced with other expressions for plasticity and the like
//
template <typename InterfaceType>
std::vector< rt_expr<InterfaceType> > stress_tensor(std::vector< rt_function_symbol<InterfaceType> > const & v)
{
  //
  // a 3x3 matrix representing the stress tensor
  //
  std::vector< rt_expr<InterfaceType> > result(9);
  std::vector< rt_expr<InterfaceType> > strain = strain_tensor(v);

  double mu = 0.5;
  double lambda = 1;

  //The entries are in the following written

  //add 2 \mu eps:
  for (size_t i=0; i<9; ++i)
    result[i] = (2*mu) * strain[i];
    //result[i] = viennamath::constant<>(0);

  //add trace(eps) * Id:
  result[0] = (2*mu) * strain[0] + lambda * (strain[0] + strain[4] + strain[8]);
  result[4] = (2*mu) * strain[4] + lambda * (strain[0] + strain[4] + strain[8]);
  result[8] = (2*mu) * strain[8] + lambda * (strain[0] + strain[4] + strain[8]);

  /*result[0] = lambda * (strain[0] + strain[4] + strain[8]);
  result[4] = lambda * (strain[0] + strain[4] + strain[8]);
  result[8] = lambda * (strain[0] + strain[4] + strain[8]);*/

  return result;
}

//
// Provides the operation a : b, where a and b are tensors
//
template <typename InterfaceType>
rt_expr<InterfaceType> tensor_reduce(std::vector< rt_expr<InterfaceType> > lhs, std::vector< rt_expr<InterfaceType> > rhs)
{
  rt_expr<InterfaceType> ret = lhs[0] * rhs[0];

  for (size_t i=1; i<rhs.size(); ++i)
    ret = ret + lhs[i] * rhs[i];

  return ret;
}

//
// Writes displacements to domain
//
template <typename DomainT, typename StorageT, typename VectorT>
void apply_displacements(DomainT& domain, StorageT& storage, VectorT const & result)
{
  typedef typename viennagrid::result_of::element<DomainT, viennagrid::vertex_tag>::type           VertexType;
  typedef typename viennagrid::result_of::element_range<DomainT, viennagrid::vertex_tag>::type     VertexContainer;
  typedef typename viennagrid::result_of::iterator<VertexContainer>::type                          VertexIterator;

  typedef viennafem::mapping_key          MappingKeyType;
  typedef viennafem::boundary_key         BoundaryKeyType;

  MappingKeyType map_key(0);
  BoundaryKeyType bnd_key(0);

  std::cout << "* apply_displacements(): Writing computed displacements onto domain" << std::endl;
  VertexContainer vertices = viennagrid::elements<VertexType>(domain);
  for (VertexIterator vit = vertices.begin();
      vit != vertices.end();
      ++vit)
  {
    long cur_index = viennadata::access<MappingKeyType, long>(storage, map_key, *vit);
    if (cur_index > -1)
    {
      viennagrid::point(domain, *vit)[0] += result[cur_index+0];
      viennagrid::point(domain, *vit)[1] += result[cur_index+1];
      viennagrid::point(domain, *vit)[2] += result[cur_index+2];
    }
    else
    {
      if (viennadata::access<BoundaryKeyType, std::vector<double> >(storage, bnd_key, *vit).size() > 0)
      {
        viennagrid::point(domain, *vit)[0] += viennadata::access<BoundaryKeyType, std::vector<double> >(storage, bnd_key, *vit)[0];
        viennagrid::point(domain, *vit)[1] += viennadata::access<BoundaryKeyType, std::vector<double> >(storage, bnd_key, *vit)[1];
        viennagrid::point(domain, *vit)[2] += viennadata::access<BoundaryKeyType, std::vector<double> >(storage, bnd_key, *vit)[2];
      }
    }
  }
}

int main()
{
  typedef viennagrid::hexahedral_3d_mesh                                                  DomainType;
  typedef viennagrid::result_of::segmentation<DomainType>::type                           SegmentationType;
  typedef viennagrid::result_of::element<DomainType, viennagrid::vertex_tag>::type        VertexType;
  typedef viennagrid::result_of::element_range<DomainType, viennagrid::vertex_tag>::type  VertexContainer;
  typedef viennagrid::result_of::iterator<VertexContainer>::type                          VertexIterator;

  typedef boost::numeric::ublas::compressed_matrix<viennafem::numeric_type>  MatrixType;
  typedef boost::numeric::ublas::vector<viennafem::numeric_type>             VectorType;

  typedef viennamath::function_symbol   FunctionSymbol;
  typedef viennamath::equation          Equation;
  typedef viennamath::expr              Expression;

  typedef viennafem::boundary_key      BoundaryKey;

  std::cout << "*********************************************************" << std::endl;
  std::cout << "*****     Demo for LAME equation with ViennaFEM     *****" << std::endl;
  std::cout << "*********************************************************" << std::endl;

  //
  // Create a domain from file
  //
  DomainType my_domain;
  SegmentationType segments(my_domain);

  //
  // Create a storage object
  //
  typedef viennadata::storage<> StorageType;
  StorageType   storage;

  try
  {
    viennagrid::io::netgen_reader my_reader;
    my_reader(my_domain, segments, "../examples/data/cube343_hex.mesh");
  }
  catch (...)
  {
    std::cerr << "File-Reader failed. Aborting program..." << std::endl;
    exit(EXIT_FAILURE);
  }

  MatrixType system_matrix;
  VectorType load_vector;

  // the unknown function (vector valued, so one for each of the three components..
  std::vector< FunctionSymbol > u(3);
  u[0] = FunctionSymbol(0, unknown_tag<>());
  u[1] = FunctionSymbol(1, unknown_tag<>());
  u[2] = FunctionSymbol(2, unknown_tag<>());

  std::vector< FunctionSymbol > v(3);
  v[0] = FunctionSymbol(0, test_tag<>());
  v[1] = FunctionSymbol(1, test_tag<>());
  v[2] = FunctionSymbol(2, test_tag<>());

  //
  // Step 1: Define the classical Lame equation
  //             (lambda + mu) div(u) div(v) + mu grad(u):grad(v) = F
  // with force F set to 0.
  //
  // Minimization problem: \int eps : sigma dx = \int F \cdot u dx
  //

  std::vector< Expression > strain = strain_tensor(u);
  std::vector< Expression > stress = stress_tensor(v);

  Equation weak_form_lame = make_equation( integral(symbolic_interval(), tensor_reduce( strain, stress )),
                                           //=
                                           integral(symbolic_interval(), viennamath::rt_constant<double>(1.0) * v[2])
                                         );

  std::cout << "Weak form of Lame equation: " << std::endl;
  std::cout << weak_form_lame << std::endl;

  std::vector<double> bnd_data_right(3);
  bnd_data_right[0] = 0.2; //small displacement into x-direction prescribed

  VertexContainer vertices = viennagrid::elements<VertexType>(my_domain);
  for (VertexIterator vit = vertices.begin();
      vit != vertices.end();
      ++vit)
  {
    //boundary for first equation: Homogeneous Dirichlet everywhere
    if (viennagrid::point(my_domain, *vit)[0] == 0.0 || viennagrid::point(my_domain, *vit)[0] == 1.0 )
      viennafem::set_dirichlet_boundary(storage, *vit, 0);

    if (viennagrid::point(my_domain, *vit)[0] == 1.0)
    {
      viennafem::set_dirichlet_boundary(storage, *vit, bnd_data_right);
      viennadata::access<BoundaryKey, double>(storage, BoundaryKey(0), *vit) = bnd_data_right[0]; //this is for the moment used for the VTK writer
    }
  }

  //
  // Create PDE solver functors: (discussion about proper interface required)
  //
  viennafem::pde_assembler<StorageType> fem_assembler(storage);

  //
  // Assemble and solve system and write solution vector to pde_result:
  // (discussion about proper interface required. Introduce a pde_result class?)
  //
  fem_assembler(viennafem::make_linear_pde_system(weak_form_lame,
                                                  u,
                                                  viennafem::make_linear_pde_options(0,
                                                                                viennafem::lagrange_tag<1>(),
                                                                                viennafem::lagrange_tag<1>())
                                                 ),
                my_domain,
                system_matrix,
                load_vector
               );

  VectorType displacements = viennacl::linalg::solve(system_matrix, load_vector, viennacl::linalg::bicgstab_tag());
  std::cout << "* solve(): Residual: " << norm_2(prod(system_matrix, displacements) - load_vector) << std::endl;

  apply_displacements(my_domain, storage, displacements);
  viennafem::io::write_solution_to_VTK_file(displacements, "lame_hex", my_domain, segments, storage, 0);

  std::cout << "*****************************************" << std::endl;
  std::cout << "* Lame solver finished successfully! *" << std::endl;
  std::cout << "*****************************************" << std::endl;
  return EXIT_SUCCESS;
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    Rot3Q.cpp
 * @brief   Rotation (internal: quaternion representation*)
 * @author  Richard Roberts
 */

#include <gtsam/config.h> // Get GTSAM_USE_QUATERNIONS macro

#ifdef GTSAM_USE_QUATERNIONS

#include <boost/math/constants/constants.hpp>
#include <gtsam/geometry/Rot3.h>
#include <cmath>

using namespace std;

namespace gtsam {

  /* ************************************************************************* */
  Rot3::Rot3() : quaternion_(Quaternion::Identity()) {}

  /* ************************************************************************* */
  Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) :
      quaternion_((Matrix3() <<
          col1.x(), col2.x(), col3.x(),
          col1.y(), col2.y(), col3.y(),
          col1.z(), col2.z(), col3.z()).finished()) {}

  /* ************************************************************************* */
  Rot3::Rot3(double R11, double R12, double R13,
      double R21, double R22, double R23,
      double R31, double R32, double R33) :
        quaternion_((Matrix3() <<
            R11, R12, R13,
            R21, R22, R23,
            R31, R32, R33).finished()) {}

  /* ************************************************************************* */
  Rot3::Rot3(const gtsam::Quaternion& q) :
      quaternion_(q) {
  }

  /* ************************************************************************* */
  Rot3 Rot3::Rx(double t) {
    return gtsam::Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitX()));
  }

  /* ************************************************************************* */
  Rot3 Rot3::Ry(double t) {
    return gtsam::Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitY()));
  }

  /* ************************************************************************* */
  Rot3 Rot3::Rz(double t) {
    return gtsam::Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitZ()));
  }

  /* ************************************************************************* */
  Rot3 Rot3::RzRyRx(double x, double y, double z) { return Rot3(
      gtsam::Quaternion(Eigen::AngleAxisd(z, Eigen::Vector3d::UnitZ())) *
      gtsam::Quaternion(Eigen::AngleAxisd(y, Eigen::Vector3d::UnitY())) *
      gtsam::Quaternion(Eigen::AngleAxisd(x, Eigen::Vector3d::UnitX())));
  }

  /* ************************************************************************* */
  Rot3 Rot3::operator*(const Rot3& R2) const {
    return Rot3(quaternion_ * R2.quaternion_);
  }

  /* ************************************************************************* */
  // TODO: Could we do this? It works in Rot3M but not here, probably because
  // here we create an intermediate value by calling matrix()
  // const Eigen::Transpose<const Matrix3> Rot3::transpose() const {
  Matrix3 Rot3::transpose() const {
    return matrix().transpose();
  }

  /* ************************************************************************* */
  Point3 Rot3::rotate(const Point3& p,
        OptionalJacobian<3,3> H1,  OptionalJacobian<3,3> H2) const {
    const Matrix3 R = matrix();
    if (H1) *H1 = R * skewSymmetric(-p.x(), -p.y(), -p.z());
    if (H2) *H2 = R;
    const Vector3 r = R * p;
    return Point3(r.x(), r.y(), r.z());
  }

  /* ************************************************************************* */
  Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3, 3> H) {
    return traits<gtsam::Quaternion>::Logmap(R.quaternion_, H);
  }

  /* ************************************************************************* */
  Rot3 Rot3::ChartAtOrigin::Retract(const Vector3& omega, ChartJacobian H) {
    static const CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE;
    if (mode == Rot3::EXPMAP) return Expmap(omega, H);
    else throw std::runtime_error("Rot3::Retract: unknown mode");
  }

  /* ************************************************************************* */
  Vector3 Rot3::ChartAtOrigin::Local(const Rot3& R, ChartJacobian H) {
    static const CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE;
    if (mode == Rot3::EXPMAP) return Logmap(R, H);
    else throw std::runtime_error("Rot3::Local: unknown mode");
  }

  /* ************************************************************************* */
  Matrix3 Rot3::matrix() const {return quaternion_.toRotationMatrix();}

  /* ************************************************************************* */
  Point3 Rot3::r1() const { return Point3(quaternion_.toRotationMatrix().col(0)); }

  /* ************************************************************************* */
  Point3 Rot3::r2() const { return Point3(quaternion_.toRotationMatrix().col(1)); }

  /* ************************************************************************* */
  Point3 Rot3::r3() const { return Point3(quaternion_.toRotationMatrix().col(2)); }

  /* ************************************************************************* */
  gtsam::Quaternion Rot3::toQuaternion() const { return quaternion_; }

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

} // namespace gtsam

#endif


/* ---------------------------------------------------------------------
 *
 * Copyright (C) 1999 - 2019 by the deal.II authors
 *
 * This file is part of the deal.II library.
 *
 * The deal.II library is free software; you can use it, redistribute
 * it, and/or modify it under the terms of the GNU Lesser General
 * Public License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * The full text of the license can be found in the file LICENSE.md at
 * the top level of the deal.II distribution.
 *
 * ---------------------------------------------------------------------
 *
 * based on deal.II step-1
 */

#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/grid_out.h>
#include <deal.II/grid/tria.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>

#include <cmath>
#include <fstream>
#include <iostream>

using namespace dealii;

void
first_grid()
{
  Triangulation<2> triangulation;

  GridGenerator::hyper_cube(triangulation);
  triangulation.refine_global(4);

  std::ofstream out("grid-1.svg");
  GridOut       grid_out;
  grid_out.write_svg(triangulation, out);
  std::cout << "Grid written to grid-1.svg" << std::endl;
}

void
second_grid()
{
  Triangulation<2> triangulation;

  const Point<2> center(1, 0);
  const double   inner_radius = 0.5, outer_radius = 1.0;
  GridGenerator::hyper_shell(
    triangulation, center, inner_radius, outer_radius, 10);
  for (unsigned int step = 0; step < 5; ++step)
    {
      for (auto &cell : triangulation.active_cell_iterators())
        {
          for (const auto v : cell->vertex_indices())
            {
              const double distance_from_center =
                center.distance(cell->vertex(v));

              if (std::fabs(distance_from_center - inner_radius) <=
                  1e-6 * inner_radius)
                {
                  cell->set_refine_flag();
                  break;
                }
            }
        }

      triangulation.execute_coarsening_and_refinement();
    }

  std::ofstream out("grid-2.svg");
  GridOut       grid_out;
  grid_out.write_svg(triangulation, out);

  std::cout << "Grid written to grid-2.svg" << std::endl;
}

int
main()
{
  first_grid();
  second_grid();
}


/*
	Copyright 2020 Patrick Owen

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

#include "VectorMath.h"
#include <unsupported/Eigen/MatrixFunctions>
#include <Eigen/SVD>

Matrix4d VectorMath::hyperbolicSvdUnitary(const Matrix4d& matrix) {
	return matrix * (hyperbolicTranspose(matrix) * matrix).sqrt().inverse();
}

Matrix4d VectorMath::sphericalSvdUnitary(const Matrix4d& matrix) {
	Eigen::JacobiSVD<Matrix4d> svd(matrix, Eigen::ComputeThinU | Eigen::ComputeThinV);
	return svd.matrixU() * svd.matrixV().adjoint();
}