text:
////////////////////////////////////////////////////////////////////////////////
#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
}

max_stars_repo_name: danielepanozzo/polyfem

max_issues_repo_name: danielepanozzo/polyfem

max_forks_repo_name: danielepanozzo/polyfem


text:
#ifndef GRAPHITE_INFOFIELDPARSER_H
#define GRAPHITE_INFOFIELDPARSER_H

/*
#include "core/util/Types.h"

#include <string>

#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix_core.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>
#include <boost/spirit/include/phoenix_object.hpp>
#include <boost/fusion/include/adapt_struct.hpp>
#include <boost/fusion/include/io.hpp>

namespace graphite
{

	namespace qi = boost::spirit::qi;

	template <typename Iterator>
	struct InfoFieldParser
		: qi::grammar<Iterator, std::map<std::string, std::string>()>
	{
		InfoFieldParser()
			: InfoFieldParser::base_type(query)
			{
				query =  pair >> *((qi::lit(';')) >> pair);
				pair  =  key >> -('=' >> value);
				key   =  qi::char_("a-zA-Z_") >> *qi::char_("a-zA-Z_0-9");
				value = +(qi::char_(",a-zA-Z_0-9-"));
			}
		qi::rule<Iterator, std::map<std::string, std::string>()> query;
		qi::rule<Iterator, std::pair<std::string, std::string>()> pair;
		qi::rule<Iterator, std::string()> key, value;
	};

} // end namespace graphite

*/
#endif //GRAPHITE_INFOFIELDPARSER_H

ext: hpp


text:
#include "testsuite.h"
#include <blitz/array.h>

BZ_USING_NAMESPACE(blitz)

int main()
{
    // NEEDS_WORK: resize

    // One-dimensional resizeAndPreserve
    Array<int,1> A(5);
    A = 0, 1, 2, 3, 4;
    A.resizeAndPreserve(10);
    A(Range(5,toEnd)) = 0;

    BZTEST(A.lbound(0) == 0);
    BZTEST(A.ubound(0) == 9);

    Array<int,1> B(5);
    B = 0, 1, 2, 3, 4;

    BZTEST(all(A(Range(0,4)) == B));
    BZTEST(all(A(Range(5,9)) == 0));

    A.resizeAndPreserve(5);
    BZTEST(all(A == B));
    A.resizeAndPreserve(2);
    BZTEST((A(0) == 0) && (A(1) == 1));
    BZTEST(A.lbound(0) == 0);
    BZTEST(A.ubound(0) == 1);

    // Two-dimensional resizeAndPreserve, with Fortran-style arrays
    Array<int,2> C(3,3,FortranArray<2>());
    C = 0, 1, 2,
        3, 4, 5,
        6, 7, 8;
    C.resizeAndPreserve(2,4);
    // valgrind here correctly complains about use of uninitialized
    // values, because the (i,4) elements are uninitialized. harmless.
    BZTEST(all(C(Range(1,2),Range(1,4)) == C(Range(1,2),Range(1,4))));
    BZTEST(C.lbound(0) == 1);
    BZTEST(C.lbound(1) == 1);
    BZTEST(C.ubound(0) == 2);
    BZTEST(C.ubound(1) == 4);

    // Three dimensional resize
    Array<int,3> G(4,5,6);
    G.resize(7,8,9);
    BZTEST(G.extent(0) == 7);
    BZTEST(G.extent(1) == 8);
    BZTEST(G.extent(2) == 9);
    return 0;
}

ext: cpp

max_stars_repo_name: fraguela/depspawn

max_issues_repo_name: MSV-Project/IBAMR

max_forks_repo_name: MSV-Project/IBAMR


text:
/**
 * @file finitevolumesineconslaw_main.cc
 * @brief NPDE homework "FiniteVolumeSineConsLaw" code
 * @author Oliver Rietmann
 * @date 25.04.2019
 * @copyright Developed at ETH Zurich
 */

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

#include "finitevolumesineconslaw.h"

using namespace FiniteVolumeSineConsLaw;

const static Eigen::IOFormat CSVFormat(Eigen::FullPrecision,
                                       Eigen::DontAlignCols, ", ", "\n");

int main() {
  /* SAM_LISTING_BEGIN_1 */
  unsigned int N = 600;
  unsigned int M = 200;
  double h = 12.0 / N;
  Eigen::VectorXd x =
      Eigen::VectorXd::LinSpaced(N, -6.0 + 0.5 * h, 6.0 - 0.5 * h);

  std::ofstream file;

  // without reaction term
  Eigen::VectorXd ufinal = solveSineConsLaw(&sineClawRhs, N, M);
#if SOLUTION
  file.open("ufinal.csv");
  file << x.transpose().format(CSVFormat) << std::endl;
  file << ufinal.transpose().format(CSVFormat) << std::endl;
  file.close();

  std::cout << "Generated " CURRENT_BINARY_DIR "/ufinal.csv" << std::endl;
  std::system("python3 " CURRENT_SOURCE_DIR "/plot.py " CURRENT_BINARY_DIR
              "/ufinal.csv " CURRENT_BINARY_DIR "/ufinal.eps");
#else
  //====================
  // Your code goes here
  // Use std::ofstream to write the solution to
  // the file "ufinal.csv". To plot this
  // file you may uncomment the following line:
  // std::system("python3 " CURRENT_SOURCE_DIR "/plot.py " CURRENT_BINARY_DIR
  // "/ufinal.csv " CURRENT_BINARY_DIR "/ufinal.eps");
  //====================
#endif
  /* SAM_LISTING_END_1 */

  // with reaction term: -c * u(x, t), where c = 1.0
  double c = 1.0;
  auto bind_c = [c](const Eigen::VectorXd &mu) {
    return sineClawReactionRhs(mu, c);
  };
  Eigen::VectorXd ufinal_reaction = solveSineConsLaw(bind_c, N, M);
#if SOLUTION
  file.open("ufinal_reaction.csv");
  file << x.transpose().format(CSVFormat) << std::endl;
  file << ufinal_reaction.transpose().format(CSVFormat) << std::endl;
  file.close();

  std::cout << "Generated " CURRENT_BINARY_DIR "/ufinal_reaction.csv"
            << std::endl;
  std::system("python3 " CURRENT_SOURCE_DIR "/plot.py " CURRENT_BINARY_DIR
              "/ufinal_reaction.csv " CURRENT_BINARY_DIR
              "/ufinal_reaction.eps");
#else
  //====================
  // Your code goes here
  // Use std::ofstream to write the solution to
  // the file "ufinal_reaction.csv". To plot this
  // file you may uncomment the following line:
  // std::system("python3 " CURRENT_SOURCE_DIR "/plot.py " CURRENT_BINARY_DIR
  // "/ufinal_reaction.csv " CURRENT_BINARY_DIR "/ufinal_reaction.eps");
  //====================
#endif

  // Finding the optimal timestep (no reaction term)
  unsigned int M_small = findTimesteps();
  std::cout
      << "The smallest number of timesteps keeping the solution in [0, 2] is "
      << M_small << std::endl;

  return 0;
}

ext: cc


text:
#include "Acceptor.h"
#include "Loop.h"
#include "InetAddress.h"

#include <boost/bind.hpp>

using namespace Reactor;

Acceptor::Acceptor(Loop* loop, const InetAddress& listenAddr)
  : loop_(loop),
    acceptSocket_(::socket(AF_INET,
                  SOCK_STREAM | SOCK_NONBLOCK | SOCK_CLOEXEC,
                  IPPROTO_TCP)),
    acceptEvent_(loop, acceptSocket_.fd()),
    listenning_(false)
{
  acceptSocket_.setReuseAddr(true);
  acceptSocket_.bindAddress(listenAddr);
  acceptEvent_.setReadCallback(boost::bind(&Acceptor::handleRead, this));
}

void Acceptor::listen()
{
  listenning_ = true;
  acceptSocket_.listen();
  acceptEvent_.enableReading();
}

void Acceptor::handleRead()
{
  InetAddress peerAddr(0);
  int connfd = acceptSocket_.accept(&peerAddr);
  if (connfd >= 0) {
    if (newConnectionCallback_) {
      newConnectionCallback_(connfd, peerAddr);
    } else {
      ::close(connfd);
    }
  }
}