/*
 *  Sparse linear solver.
 *  Copyright (C) 2004 Bruno Levy
 *  Copyright (C) 2005-2015 Blender Foundation
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 *
 *  If you modify this software, you should include a notice giving the
 *  name of the person performing the modification, the date of modification,
 *  and the reason for such modification.
 */

#include "linear_solver.h"

#include <Eigen/Sparse>

#include <algorithm>
#include <cassert>
#include <cstdlib>
#include <iostream>
#include <vector>

/* Eigen data structures */

typedef Eigen::SparseMatrix<double, Eigen::ColMajor> EigenSparseMatrix;
typedef Eigen::SparseLU<EigenSparseMatrix> EigenSparseLU;
typedef Eigen::VectorXd EigenVectorX;
typedef Eigen::Triplet<double> EigenTriplet;

/* Linear Solver data structure */

struct LinearSolver
{
	struct Coeff
	{
		Coeff()
		{
			index = 0;
			value = 0.0;
		}

		int index;
		double value;
	};

	struct Variable
	{
		Variable()
		{
			memset(value, 0, sizeof(value));
			locked = false;
			index = 0;
		}

		double value[4];
		bool locked;
		int index;
		std::vector<Coeff> a;
	};

	enum State
	{
		STATE_VARIABLES_CONSTRUCT,
		STATE_MATRIX_CONSTRUCT,
		STATE_MATRIX_SOLVED
	};

	LinearSolver(int num_rows_, int num_variables_, int num_rhs_, bool lsq_)
	{
		assert(num_variables_ > 0);
		assert(num_rhs_ <= 4);

		state = STATE_VARIABLES_CONSTRUCT;
		m = 0;
		n = 0;
		sparseLU = NULL;
		num_variables = num_variables_;
		num_rhs = num_rhs_;
		num_rows = num_rows_;
		least_squares = lsq_;

		variable.resize(num_variables);
	}

	~LinearSolver()
	{
		delete sparseLU;
	}

	State state;

	int n;
	int m;

	std::vector<EigenTriplet> Mtriplets;
	EigenSparseMatrix M;
	EigenSparseMatrix MtM;
	std::vector<EigenVectorX> b;
	std::vector<EigenVectorX> x;

	EigenSparseLU *sparseLU;

	int num_variables;
	std::vector<Variable> variable;

	int num_rows;
	int num_rhs;

	bool least_squares;
};

LinearSolver *EIG_linear_solver_new(int num_rows, int num_columns, int num_rhs)
{
	return new LinearSolver(num_rows, num_columns, num_rhs, false);
}

LinearSolver *EIG_linear_least_squares_solver_new(int num_rows, int num_columns, int num_rhs)
{
	return new LinearSolver(num_rows, num_columns, num_rhs, true);
}

void EIG_linear_solver_delete(LinearSolver *solver)
{
	delete solver;
}

/* Variables */

void EIG_linear_solver_variable_set(LinearSolver *solver, int rhs, int index, double value)
{
	solver->variable[index].value[rhs] = value;
}

double EIG_linear_solver_variable_get(LinearSolver *solver, int rhs, int index)
{
	return solver->variable[index].value[rhs];
}

void EIG_linear_solver_variable_lock(LinearSolver *solver, int index)
{
	if (!solver->variable[index].locked) {
		assert(solver->state == LinearSolver::STATE_VARIABLES_CONSTRUCT);
		solver->variable[index].locked = true;
	}
}

void EIG_linear_solver_variable_unlock(LinearSolver *solver, int index)
{
	if (solver->variable[index].locked) {
		assert(solver->state == LinearSolver::STATE_VARIABLES_CONSTRUCT);
		solver->variable[index].locked = false;
	}
}

static void linear_solver_variables_to_vector(LinearSolver *solver)
{
	int num_rhs = solver->num_rhs;

	for (int i = 0; i < solver->num_variables; i++) {
		LinearSolver::Variable* v = &solver->variable[i];
		if (!v->locked) {
			for (int j = 0; j < num_rhs; j++)
				solver->x[j][v->index] = v->value[j];
		}
	}
}

static void linear_solver_vector_to_variables(LinearSolver *solver)
{
	int num_rhs = solver->num_rhs;

	for (int i = 0; i < solver->num_variables; i++) {
		LinearSolver::Variable* v = &solver->variable[i];
		if (!v->locked) {
			for (int j = 0; j < num_rhs; j++)
				v->value[j] = solver->x[j][v->index];
		}
	}
}

/* Matrix */

static void linear_solver_ensure_matrix_construct(LinearSolver *solver)
{
	/* transition to matrix construction if necessary */
	if (solver->state == LinearSolver::STATE_VARIABLES_CONSTRUCT) {
		int n = 0;

		for (int i = 0; i < solver->num_variables; i++) {
			if (solver->variable[i].locked)
				solver->variable[i].index = ~0;
			else
				solver->variable[i].index = n++;
		}

		int m = (solver->num_rows == 0)? n: solver->num_rows;

		solver->m = m;
		solver->n = n;

		assert(solver->least_squares || m == n);

		/* reserve reasonable estimate */
		solver->Mtriplets.clear();
		solver->Mtriplets.reserve(std::max(m, n)*3);

		solver->b.resize(solver->num_rhs);
		solver->x.resize(solver->num_rhs);

		for (int i = 0; i < solver->num_rhs; i++) {
			solver->b[i].setZero(m);
			solver->x[i].setZero(n);
		}

		linear_solver_variables_to_vector(solver);

		solver->state = LinearSolver::STATE_MATRIX_CONSTRUCT;
	}
}

void EIG_linear_solver_matrix_add(LinearSolver *solver, int row, int col, double value)
{
	if (solver->state == LinearSolver::STATE_MATRIX_SOLVED)
		return;

	linear_solver_ensure_matrix_construct(solver);

	if (!solver->least_squares && solver->variable[row].locked);
	else if (solver->variable[col].locked) {
		if (!solver->least_squares)
			row = solver->variable[row].index;

		LinearSolver::Coeff coeff;
		coeff.index = row;
		coeff.value = value;
		solver->variable[col].a.push_back(coeff);
	}
	else {
		if (!solver->least_squares)
			row = solver->variable[row].index;
		col = solver->variable[col].index;

		/* direct insert into matrix is too slow, so use triplets */
		EigenTriplet triplet(row, col, value);
		solver->Mtriplets.push_back(triplet);
	}
}

/* Right hand side */

void EIG_linear_solver_right_hand_side_add(LinearSolver *solver, int rhs, int index, double value)
{
	linear_solver_ensure_matrix_construct(solver);

	if (solver->least_squares) {
		solver->b[rhs][index] += value;
	}
	else if (!solver->variable[index].locked) {
		index = solver->variable[index].index;
		solver->b[rhs][index] += value;
	}
}

/* Solve */

bool EIG_linear_solver_solve(LinearSolver *solver)
{
	/* nothing to solve, perhaps all variables were locked */
	if (solver->m == 0 || solver->n == 0)
		return true;

	bool result = true;

	assert(solver->state != LinearSolver::STATE_VARIABLES_CONSTRUCT);

	if (solver->state == LinearSolver::STATE_MATRIX_CONSTRUCT) {
		/* create matrix from triplets */
		solver->M.resize(solver->m, solver->n);
		solver->M.setFromTriplets(solver->Mtriplets.begin(), solver->Mtriplets.end());
		solver->Mtriplets.clear();

		/* create least squares matrix */
		if (solver->least_squares)
			solver->MtM = solver->M.transpose() * solver->M;

		/* convert M to compressed column format */
		EigenSparseMatrix& M = (solver->least_squares)? solver->MtM: solver->M;
		M.makeCompressed();

		/* perform sparse LU factorization */
		EigenSparseLU *sparseLU = new EigenSparseLU();
		solver->sparseLU = sparseLU;

		sparseLU->compute(M);
		result = (sparseLU->info() == Eigen::Success);

		solver->state = LinearSolver::STATE_MATRIX_SOLVED;
	}

	if (result) {
		/* solve for each right hand side */
		for (int rhs = 0; rhs < solver->num_rhs; rhs++) {
			/* modify for locked variables */
			EigenVectorX& b = solver->b[rhs];

			for (int i = 0; i < solver->num_variables; i++) {
				LinearSolver::Variable *variable = &solver->variable[i];

				if (variable->locked) {
					std::vector<LinearSolver::Coeff>& a = variable->a;

					for (int j = 0; j < a.size(); j++)
						b[a[j].index] -= a[j].value*variable->value[rhs];
				}
			}

			/* solve */
			if (solver->least_squares) {
				EigenVectorX Mtb = solver->M.transpose() * b;
				solver->x[rhs] = solver->sparseLU->solve(Mtb);
			}
			else {
				EigenVectorX& b = solver->b[rhs];
				solver->x[rhs] = solver->sparseLU->solve(b);
			}

			if (solver->sparseLU->info() != Eigen::Success)
				result = false;
		}

		if (result)
			linear_solver_vector_to_variables(solver);
	}

	/* clear for next solve */
	for (int rhs = 0; rhs < solver->num_rhs; rhs++)
		solver->b[rhs].setZero(solver->m);

	return result;
}

/* Debugging */

void EIG_linear_solver_print_matrix(LinearSolver *solver)
{
	std::cout << "A:" << solver->M << std::endl;

	for (int rhs = 0; rhs < solver->num_rhs; rhs++)
		std::cout << "b " << rhs << ":" << solver->b[rhs] << std::endl;

	if (solver->MtM.rows() && solver->MtM.cols())
		std::cout << "AtA:" << solver->MtM << std::endl;
}


#include <engine/Vectormath.hpp>
#include <engine/Manifoldmath.hpp>
#include <utility/Constants.hpp>
#include <utility/Logging.hpp>
#include <utility/Exception.hpp>

#include <Eigen/Dense>

#include <GenEigsSolver.h>  // Also includes <MatOp/DenseGenMatProd.h>
#include <GenEigsRealShiftSolver.h>

#include <array>

namespace C = Utility::Constants;

#ifndef SPIRIT_USE_CUDA

namespace Engine
{
    namespace Manifoldmath
    {
        void project_parallel(vectorfield & vf1, const vectorfield & vf2)
        {
            vectorfield vf3 = vf1;
            project_orthogonal(vf3, vf2);
            // TODO: replace the loop with Vectormath Kernel
            #pragma omp parallel for
            for (unsigned int i = 0; i < vf1.size(); ++i)
                vf1[i] -= vf3[i];
        }

        void project_orthogonal(vectorfield & vf1, const vectorfield & vf2)
        {
            scalar x = Vectormath::dot(vf1, vf2);
            // TODO: replace the loop with Vectormath Kernel
            #pragma omp parallel for
            for (unsigned int i=0; i<vf1.size(); ++i)
                vf1[i] -= x*vf2[i];
        }

        void invert_parallel(vectorfield & vf1, const vectorfield & vf2)
        {
            scalar x = Vectormath::dot(vf1, vf2);
            // TODO: replace the loop with Vectormath Kernel
            #pragma omp parallel for
            for (unsigned int i=0; i<vf1.size(); ++i)
                vf1[i] -= 2*x*vf2[i];
        }

        void invert_orthogonal(vectorfield & vf1, const vectorfield & vf2)
        {
            vectorfield vf3 = vf1;
            project_orthogonal(vf3, vf2);
            // TODO: replace the loop with Vectormath Kernel
            #pragma omp parallel for
            for (unsigned int i = 0; i < vf1.size(); ++i)
                vf1[i] -= 2 * vf3[i];
        }

        void project_tangential(vectorfield & vf1, const vectorfield & vf2)
        {
            #pragma omp parallel for
            for (unsigned int i = 0; i < vf1.size(); ++i)
                vf1[i] -= vf1[i].dot(vf2[i]) * vf2[i];
        }

        scalar dist_geodesic(const vectorfield & v1, const vectorfield & v2)
        {
            scalar dist = 0;
            #pragma omp parallel for reduction(+:dist)
            for (unsigned int i = 0; i < v1.size(); ++i)
                dist += pow(Vectormath::angle(v1[i], v2[i]), 2);
            return sqrt(dist);
        }

        /*
        Calculates the 'tangent' vectors, i.e.in crudest approximation the difference between an image and the neighbouring
        */
        void Tangents(std::vector<std::shared_ptr<vectorfield>> configurations, const std::vector<scalar> & energies, std::vector<vectorfield> & tangents)
        {
            int noi = configurations.size();
            int nos = (*configurations[0]).size();

            for (int idx_img = 0; idx_img < noi; ++idx_img)
            {
                auto& image = *configurations[idx_img];

                // First Image
                if (idx_img == 0)
                {
                    auto& image_plus = *configurations[idx_img + 1];
                    Vectormath::set_c_a( 1, image_plus, tangents[idx_img]);
                    Vectormath::add_c_a(-1, image,      tangents[idx_img]);
                }
                // Last Image
                else if (idx_img == noi - 1)
                {
                    auto& image_minus = *configurations[idx_img - 1];
                    Vectormath::set_c_a( 1, image,       tangents[idx_img]);
                    Vectormath::add_c_a(-1, image_minus, tangents[idx_img]);
                }
                // Images Inbetween
                else
                {
                    auto& image_plus  = *configurations[idx_img + 1];
                    auto& image_minus = *configurations[idx_img - 1];

                    // Energies
                    scalar E_mid = 0, E_plus = 0, E_minus = 0;
                    E_mid   = energies[idx_img];
                    E_plus  = energies[idx_img + 1];
                    E_minus = energies[idx_img - 1];

                    // Vectors to neighbouring images
                    vectorfield t_plus(nos), t_minus(nos);

                    Vectormath::set_c_a( 1, image_plus, t_plus);
                    Vectormath::add_c_a(-1, image,      t_plus);

                    Vectormath::set_c_a( 1, image,       t_minus);
                    Vectormath::add_c_a(-1, image_minus, t_minus);

                    // Near maximum or minimum
                    if ((E_plus < E_mid && E_mid > E_minus) || (E_plus > E_mid && E_mid < E_minus))
                    {
                        // Get a smooth transition between forward and backward tangent
                        scalar E_max = std::max(std::abs(E_plus - E_mid), std::abs(E_minus - E_mid));
                        scalar E_min = std::min(std::abs(E_plus - E_mid), std::abs(E_minus - E_mid));

                        if (E_plus > E_minus)
                        {
                            Vectormath::set_c_a(E_max, t_plus,  tangents[idx_img]);
                            Vectormath::add_c_a(E_min, t_minus, tangents[idx_img]);
                        }
                        else
                        {
                            Vectormath::set_c_a(E_min, t_plus,  tangents[idx_img]);
                            Vectormath::add_c_a(E_max, t_minus, tangents[idx_img]);
                        }
                    }
                    // Rising slope
                    else if (E_plus > E_mid && E_mid > E_minus)
                    {
                        Vectormath::set_c_a(1, t_plus,  tangents[idx_img]);
                    }
                    // Falling slope
                    else if (E_plus < E_mid && E_mid < E_minus)
                    {
                        Vectormath::set_c_a(1, t_minus,  tangents[idx_img]);
                        //tangents = t_minus;
                        for (int i = 0; i < nos; ++i)
                        {
                            tangents[idx_img][i] = t_minus[i];
                        }
                    }
                    // No slope(constant energy)
                    else
                    {
                        Vectormath::set_c_a(1, t_plus,  tangents[idx_img]);
                        Vectormath::add_c_a(1, t_minus, tangents[idx_img]);
                    }
                }

                // Project tangents into tangent planes of spin vectors to make them actual tangents
                project_tangential(tangents[idx_img], image);

                // Normalise in 3N - dimensional space
                Manifoldmath::normalize(tangents[idx_img]);

            }// end for idx_img
        }// end Tangents
    }
}

#endif

namespace Engine
{
    namespace Manifoldmath
    {
        scalar norm(const vectorfield & vf)
        {
            scalar x = Vectormath::dot(vf, vf);
            return std::sqrt(x);
        }

        void normalize(vectorfield & vf)
        {
            scalar sc = 1.0/norm(vf);
            Vectormath::scale(vf, sc);
        }

        MatrixX tangential_projector(const vectorfield & image)
        {
            int nos = image.size();
            int size = 3*nos;

            // Get projection matrix M=1-S, blockwise S=x*x^T
            MatrixX proj = MatrixX::Identity(size, size);
            for (int i = 0; i < nos; ++i)
            {
                proj.block<3, 3>(3*i, 3*i) -= image[i] * image[i].transpose();
            }

            return proj;
        }

        // This gives an orthogonal matrix of shape (3N, 2N), meaning M^T=M^-1 or M^T*M=1.
        // This assumes that the vectors of vf are normalized and that basis is 3N x 2N
        // It can be used to transform a vector into or back from the tangent space of a
        //      sphere w.r.t. euclidean 3N space.
        // It is generated by column-wise normalization of the Jacobi matrix for the
        //      transformation from (unit-)spherical coordinates to euclidean.
        // It therefore consists of the local basis vectors of the spherical coordinates
        //      of a unit sphere, represented in 3N, as the two columns of the matrix.
        void tangent_basis_spherical(const vectorfield & vf, MatrixX & basis)
        {
            Vector3 tmp, etheta, ephi;
            basis.setZero();
            for (unsigned int i=0; i < vf.size(); ++i)
            {
                if (vf[i][2] > 1-1e-8)
                {
                    tmp = Vector3{1, 0, 0};
                    basis.block<3,1>(3*i,2*i)   = (tmp - tmp.dot(vf[i])*vf[i]).normalized();
                    tmp = Vector3{0, 1, 0};
                    basis.block<3,1>(3*i,2*i+1) = (tmp - tmp.dot(vf[i])*vf[i]).normalized();
                }
                else if (vf[i][2] < -1+1e-8)
                {
                    tmp = Vector3{1, 0, 0};
                    basis.block<3,1>(3*i,2*i)   = (tmp - tmp.dot(vf[i])*vf[i]).normalized();
                    tmp = Vector3{0, -1, 0};
                    basis.block<3,1>(3*i,2*i+1) = (tmp - tmp.dot(vf[i])*vf[i]).normalized();
                }
                else
                {
                    scalar rxy = std::sqrt( 1 - vf[i][2]*vf[i][2] );
                    scalar z_rxy = vf[i][2] / rxy;

                    // Note: these are not unit vectors, but derivatives!
                    etheta = Vector3{  vf[i][0]*z_rxy, vf[i][1]*z_rxy, -rxy };
                    ephi   = Vector3{ -vf[i][1]/rxy,   vf[i][0]/rxy,    0   };

                    basis.block<3,1>(3*i,2*i)   = (etheta - etheta.dot(vf[i])*vf[i]).normalized();
                    basis.block<3,1>(3*i,2*i+1) = (ephi   - ephi.dot(vf[i])*vf[i]).normalized();
                }
            }
        }

        // This calculates the basis via calculation of cross products
        // This assumes that the vectors of vf are normalized and that basis is 3N x 2N
        void tangent_basis_cross(const vectorfield & vf, MatrixX & basis)
        {
            basis.setZero();
            for(int i=0; i < vf.size(); ++i)
            {
                if (std::abs(vf[i].z()) > 1-1e-8)
                {
                    basis.block<3,1>(3*i,2*i)   = Vector3{0,1,0}.cross(vf[i]).normalized();
                    basis.block<3,1>(3*i,2*i+1) = vf[i].cross(basis.block<3,1>(3*i,2*i));
                }
                else
                {
                    basis.block<3,1>(3*i,2*i)   = Vector3{0,0,1}.cross(vf[i]).normalized();
                    basis.block<3,1>(3*i,2*i+1) = vf[i].cross(basis.block<3,1>(3*i,2*i));
                }
            }
        }

        // This calculates the basis via orthonormalization to a random vector
        // This assumes that the vectors of vf are normalized and that basis is 3N x 2N
        void tangent_basis_righthanded(const vectorfield & vf, MatrixX & basis)
        {
            int size = vf.size();
            basis.setZero();

            // vf should be 3N
            // basis should be 3N x 2N

            // e1 and e2 will form a righthanded vectorset with the axis (though not orthonormal!)
            Vector3 e1, e2, v1;
            Vector3 ex{1,0,0}, ey{0,1,0}, ez{0,0,1};

            for(int i=0; i < size; ++i)
            {
                auto& axis = vf[i];

                // Choose orthogonalisation basis for Grahm-Schmidt
                //      We will need two vectors with which the axis always forms the
                //      same orientation (händigkeit des vektor-dreibeins)
                // If axis_z=0 its in the xy-plane
                //      the vectors should be: axis, ez, (axis x ez)
                if (axis[2] == 0)
                {
                    e1 = ez;
                    e2 = axis.cross(ez);
                }
                // Else its either above or below the xy-plane.
                //      if its above the xy-plane, it points in z-direction
                //      the vectors should be: axis, ex, -ey
                else if (axis[2] > 0)
                {
                    e1 = ex;
                    e2 = -ey;
                }
                //      if its below the xy-plane, it points in -z-direction
                //      the vectors should be: axis, ex, ey
                else if (axis[2] < 0)
                {
                    e1 = ex;
                    e2 = ey;
                }

                // First vector: orthogonalize e1 w.r.t. axis
                v1 = ( e1 - e1.dot(axis) * axis ).normalized();
                basis.block<3,1>(3*i,2*i)   = v1;

                // Second vector: orthogonalize e2 w.r.t. axis and v1
                basis.block<3,1>(3*i,2*i+1) = ( e2 - e2.dot(axis)*axis - e2.dot(v1)*v1 ).normalized();
            }
        }

        // This gives the Jacobian matrix for the transformation from (unit-)spherical
        // to euclidean coordinates. It consists of the derivative vectors d/d_theta
        // and d/d_phi as the two columns of the matrix.
        void spherical_to_cartesian_jacobian(const vectorfield & vf, MatrixX & jacobian)
        {
            Vector3 tmp, etheta, ephi;
            jacobian.setZero();
            for (unsigned int i=0; i < vf.size(); ++i)
            {
                if (vf[i][2] > 1-1e-8)
                {
                    tmp = Vector3{1, 0, 0};
                    jacobian.block<3,1>(3*i,2*i)   = (tmp - tmp.dot(vf[i])*vf[i]).normalized();
                    tmp = Vector3{0, 1, 0};
                    jacobian.block<3,1>(3*i,2*i+1) = (tmp - tmp.dot(vf[i])*vf[i]).normalized();
                }
                else if (vf[i][2] < -1+1e-8)
                {
                    tmp = Vector3{1, 0, 0};
                    jacobian.block<3,1>(3*i,2*i)   = (tmp - tmp.dot(vf[i])*vf[i]).normalized();
                    tmp = Vector3{0, -1, 0};
                    jacobian.block<3,1>(3*i,2*i+1) = (tmp - tmp.dot(vf[i])*vf[i]).normalized();
                }
                else
                {
                    scalar rxy = std::sqrt( 1 - vf[i][2]*vf[i][2] );
                    scalar z_rxy = vf[i][2] / rxy;

                    // Note: these are not unit vectors, but derivatives!
                    etheta = Vector3{  vf[i][0]*z_rxy, vf[i][1]*z_rxy, -rxy };
                    ephi   = Vector3{ -vf[i][1],       vf[i][0],        0   };

                    jacobian.block<3,1>(3*i,2*i)   = etheta - etheta.dot(vf[i])*vf[i];
                    jacobian.block<3,1>(3*i,2*i+1) = ephi   - ephi.dot(vf[i])*vf[i];
                }
            }
        }

        // The Hessian matrix of the transformation from spherical to euclidean coordinates
        void spherical_to_cartesian_hessian(const vectorfield & vf, MatrixX & gamma_x, MatrixX & gamma_y, MatrixX & gamma_z)
        {
            int nos = vf.size();
            gamma_x.setZero();
            gamma_y.setZero();
            gamma_z.setZero();

            for (unsigned int i=0; i < nos; ++i)
            {
                scalar z_rxy = vf[i][2] / std::sqrt(1+1e-6-vf[i][2]*vf[i][2]);

                gamma_x.block<2,2>(2*i,2*i) << -vf[i][0],       -vf[i][1]*z_rxy,
                                                -vf[i][1]*z_rxy, -vf[i][0];

                gamma_y.block<2,2>(2*i,2*i) << -vf[i][1],        vf[i][0]*z_rxy,
                                                vf[i][0]*z_rxy, -vf[i][1];

                gamma_z.block<2,2>(2*i,2*i) << -vf[i][2], 0,
                                                0,        0;
            }
        }

        // The (2Nx2N) Christoffel symbols of the transformation from (unit-)spherical coordinates to euclidean
        void spherical_to_cartesian_christoffel_symbols(const vectorfield & vf, MatrixX & gamma_theta, MatrixX & gamma_phi)
        {
            using std::cos;
            using std::sin;
            using std::acos;
            using std::tan;
            using std::atan2;

            int nos = vf.size();
            gamma_theta = MatrixX::Zero(2*nos, 2*nos);
            gamma_phi   = MatrixX::Zero(2*nos, 2*nos);

            for (unsigned int i=0; i < nos; ++i)
            {
                scalar theta = acos(vf[i][2]);
                scalar phi   = atan2(vf[i][1], vf[i][0]);
                scalar cot = 0;
                if (std::abs(theta) > 1e-4)
                    cot = -tan(C::Pi_2 + theta);

                gamma_theta(2*i+1,2*i+1) = -sin(theta)*cos(theta);

                gamma_phi(2*i+1,2*i)   = cot;
                gamma_phi(2*i,2*i+1)   = cot;
            }
        }

        void hessian_bordered(const vectorfield & image, const vectorfield & gradient, const MatrixX & hessian, MatrixX & tangent_basis, MatrixX & hessian_out)
        {
            // Calculates a 3Nx3N matrix in the bordered Hessian approach and transforms it into the tangent basis,
            // making the result a 2Nx2N matrix. The bordered Hessian's Lagrange multipliers assume a local extremum.

            int nos = image.size();
            MatrixX tmp_3N = hessian;

            VectorX lambda(nos);
            for (int i=0; i<nos; ++i)
                lambda[i] = image[i].dot(gradient[i]);

            for (int i=0; i<nos; ++i)
            {
                for (int j=0; j<3; ++j)
                {
                    tmp_3N(3*i+j,3*i+j) -= lambda(i);
                }
            }

            // Calculate the basis transformation matrix
            tangent_basis = MatrixX::Zero(3*nos, 2*nos);
            tangent_basis_spherical(image, tangent_basis);

            // Result is a 2Nx2N matrix
            hessian_out = tangent_basis.transpose() * tmp_3N * tangent_basis;
        }

        void hessian_projected(const vectorfield & image, const vectorfield & gradient, const MatrixX & hessian, MatrixX & tangent_basis, MatrixX & hessian_out)
        {
            // Calculates a 3Nx3N matrix in the projector approach and transforms it into the tangent basis,
            // making the result a 2Nx2N matrix

            int nos = image.size();
            hessian_out.setZero();

            // Calculate projector matrix
            auto P = tangential_projector(image);

            // Calculate tangential projection of Hessian
            hessian_out = P * hessian * P;

            // Calculate correction terms
            for (unsigned int i = 0; i < nos; ++i)
            {
                hessian_out.block<3, 3>(3*i, 3*i) -=    P.block<3, 3>(3*i, 3*i) * ( image[i].dot(gradient[i]) )
                                                    + ( P.block<3, 3>(3*i, 3*i) * gradient[i] ) * image[i].transpose();
            }

            // Calculate the basis transformation matrix
            tangent_basis = MatrixX::Zero(3*nos, 2*nos);
            tangent_basis_spherical(image, tangent_basis);

            // Result is a 2Nx2N matrix
            hessian_out = tangent_basis.transpose() * hessian_out * tangent_basis;
        }

        void hessian_weingarten(const vectorfield & image, const vectorfield & gradient, const MatrixX & hessian, MatrixX & tangent_basis, MatrixX & hessian_out)
        {
            // Calculates a 3Nx3N matrix in the Weingarten map approach and transforms it into the tangent basis,
            // making the result a 2Nx2N matrix

            int nos = image.size();
            hessian_out.setZero();

            // Calculate projector matrix
            auto P = tangential_projector(image);

            // Calculate tangential projection of Hessian
            hessian_out = P * hessian;

            // Add the Weingarten map
            for (unsigned int i = 0; i < nos; ++i)
            {
                MatrixX proj = MatrixX::Identity(3, 3);
                hessian_out.block<3, 3>(3*i, 3*i) -=  MatrixX::Identity(3, 3) * image[i].dot(gradient[i]);
            }

            // Calculate the basis transformation matrix
            tangent_basis = MatrixX::Zero(3*nos, 2*nos);
            tangent_basis_spherical(image, tangent_basis);

            // Result is a 2Nx2N matrix
            hessian_out = tangent_basis.transpose() * hessian_out * tangent_basis;
        }

        void hessian_spherical(const vectorfield & image, const vectorfield & gradient, const MatrixX & hessian, MatrixX & hessian_out)
        {
            // Calculates a 2Nx2N hessian matrix containing second order spherical derivatives

            int nos = image.size();

            MatrixX jacobian   = MatrixX::Zero(3*nos, 2*nos);
            MatrixX sph_hess_x = MatrixX::Zero(2*nos, 2*nos);
            MatrixX sph_hess_y = MatrixX::Zero(2*nos, 2*nos);
            MatrixX sph_hess_z = MatrixX::Zero(2*nos, 2*nos);

            // Calculate coordinate transformation jacobian
            Engine::Manifoldmath::spherical_to_cartesian_jacobian(image, jacobian);

            // Calculate coordinate transformation Hessian
            Engine::Manifoldmath::spherical_to_cartesian_hessian(image, sph_hess_x, sph_hess_y, sph_hess_z);

            // Calculate transformed Hessian
            hessian_out = jacobian.transpose() * hessian * jacobian;
            for (int i=0; i < nos; ++i)
            {
                hessian_out.block<2,2>(2*i, 2*i) += gradient[i][0] * sph_hess_x.block<2,2>(2*i, 2*i)
                                                    + gradient[i][1] * sph_hess_y.block<2,2>(2*i, 2*i)
                                                    + gradient[i][2] * sph_hess_z.block<2,2>(2*i, 2*i);
            }

        }

        void hessian_covariant(const vectorfield & image, const vectorfield & gradient, const MatrixX & hessian, MatrixX & hessian_out)
        {
            // Calculates a 2Nx2N covariant hessian matrix containing second order spherical derivatives
            // and correction terms (containing Christoffel symbols)

            int nos = image.size();

            // Calculate coordinate transformation jacobian
            MatrixX jacobian(3*nos, 2*nos);
            Engine::Manifoldmath::spherical_to_cartesian_jacobian(image, jacobian);

            // Calculate the gradient in spherical coordinates
            Eigen::Ref<const VectorX> grad = Eigen::Map<const VectorX>(gradient[0].data(), 3 * nos);
            VectorX gradient_spherical = jacobian.transpose() * grad;

            // Calculate the Hessian in spherical coordinates
            hessian_spherical(image, gradient, hessian, hessian_out);

            // Calculate the Christoffel symbols for spherical coordinates
            MatrixX christoffel_theta = MatrixX::Zero(2*nos, 2*nos);
            MatrixX christoffel_phi   = MatrixX::Zero(2*nos, 2*nos);
            Engine::Manifoldmath::spherical_to_cartesian_christoffel_symbols(image, christoffel_theta, christoffel_phi);

            // Calculate the covariant Hessian
            for (int i=0; i < nos; ++i)
            {
                hessian_out.block<2,2>(2*i, 2*i) -= gradient_spherical[2*i]   * christoffel_theta.block<2,2>(2*i, 2*i)
                                                    + gradient_spherical[2*i+1] * christoffel_phi.block<2,2>(2*i, 2*i);
            }
        }

    }
}


#include <algorithm>
#include <cmath>
#include <cstring>
#include <fstream>
#include <sstream>
#include <iostream>
#include <iterator>
#include <string>
#include <vector>
#include <map>
#include <boost/dynamic_bitset.hpp>
#include <boost/program_options.hpp>

namespace po = boost::program_options;

using namespace std;
using namespace boost;

class Corpus {
public:
    string text;
    dynamic_bitset<> utt_boundaries;
    dynamic_bitset<> boundaries;
    map<char, double> p_phonemes;

    Corpus(string path)
    {
        load_file({path});
        compute_p_phonemes();
    }

    Corpus(string path, string trained_path,
           dynamic_bitset<> &trained_boundaries)
    {
        load_file({path, trained_path});
        compute_p_phonemes();

        // set the trained boundaries as unchangable utterance boundaries
        trained_boundaries.resize(text.size() + 1, false);
        utt_boundaries |= trained_boundaries;
    }

    double p0(string word, double p_hash=0.5)
    {
        double p = 1;
        for (char ph : word)
            p *= p_phonemes[ph];

        return p_hash * pow(1 - p_hash, word.size() - 1) * p;
    }

    vector<string> get_words()
    {
        vector<string> words;
        dynamic_bitset<> bounds = utt_boundaries | boundaries;

        for (int i = 0; i < text.size(); ++i) {
            if (bounds[i])
                words.push_back(string());

            words.back().append(1, text[i]);
        }

        return words;
    }

private:
    virtual void load_file(const vector<string> &paths)
    {
        string line;
        stringstream text_stream;
        vector<int> utt_indices = {0};

        for (string path : paths) {
            fstream in(path);

            while (getline(in, line)) {
                text_stream << line;
                utt_indices.push_back(utt_indices.back() + line.size());
            }

            in.close();
        }

        text = text_stream.str();

        utt_boundaries = dynamic_bitset<>(text.size() + 1);
        boundaries = dynamic_bitset<>(text.size() + 1);

        for (int index : utt_indices) {
            utt_boundaries[index] = 1;
        }
    }

    void compute_p_phonemes()
    {
        for (char phoneme : text) {
            // only compute if the char isn't in the map yet
            if (p_phonemes.find(phoneme) == p_phonemes.end())
                p_phonemes[phoneme] = count(text.begin(), text.end(), phoneme) /
                    (double)text.size();
        }
    }
};

map<string, int> histogram(const vector<string> &vec)
{
    map<string, int> counts;
    vector<string> unique_words;

    unique_copy(vec.begin(), vec.end(), back_inserter(unique_words));

    for (const string &word : unique_words) {
        if (counts.find(word) == counts.end())
            counts[word] = 1;
        else
            counts[word] += 1;
    }

    return counts;
}

void find_enclosing_boundaries(dynamic_bitset<> bounds, int i,
                               int *lower, int *upper)
{
    *lower = i - 1;
    while (!bounds[*lower])
        *lower -= 1;

    *upper = i + 1;
    while (!bounds[*upper])
        *upper += 1;
}

void gibbs_iteration(Corpus &corpus, double rho=2.0, double alpha=0.5,
                     double p_hash=0.5)
{
    dynamic_bitset<> bounds = corpus.utt_boundaries | corpus.boundaries;
    vector<string> words = corpus.get_words();
    map<string, int> word_counts = histogram(words);

    for (int i = 0; i < corpus.text.size(); ++i) {
        char phoneme = corpus.text[i];

        if (corpus.utt_boundaries[i])
            continue;

        int lower, upper;
        find_enclosing_boundaries(bounds, i, &lower, &upper);

        string w1 = corpus.text.substr(lower, (upper - lower));
        string w2 = corpus.text.substr(lower, (i - lower));
        string w3 = corpus.text.substr(i, (upper - i));

        double n_;
        if (!bounds[i])
            n_ = words.size();
        else
            n_ = words.size() - 1;

        double n_dollar = corpus.utt_boundaries.count() - 1;
        double nu = corpus.utt_boundaries[upper] ? n_dollar : n_ - n_dollar;

        double p_h1_factor1;
        if (!bounds[i])
            p_h1_factor1 = (word_counts[w1] - 1 + alpha * corpus.p0(w1, p_hash)) / (n_ + alpha);
        else
            p_h1_factor1 = (word_counts[w1] + alpha * corpus.p0(w1, p_hash)) / (n_ + alpha);

        double p_h1_factor2 = (nu + rho/2) / (n_ + rho);

        double p_h2_factor1, p_h2_factor3;
        if (!bounds[i]) {
            p_h2_factor1 = (word_counts[w2] + alpha * corpus.p0(w2, p_hash)) / (n_ + alpha);
            p_h2_factor3 = ((word_counts[w3] + (w2 == w3 ? 1 : 0) + alpha *
                             corpus.p0(w3, p_hash)) / (n_ + 1 + alpha));
        } else {
            p_h2_factor1 = (word_counts[w2] - 1 + alpha * corpus.p0(w2, p_hash)) / (n_ + alpha);
            p_h2_factor3 = ((word_counts[w3] - 1 + (w2 == w3 ? 1 : 0) + alpha *
                             corpus.p0(w3, p_hash)) / (n_ + 1 + alpha));
        }

        double p_h2_factor2 = (n_ - n_dollar + rho/2) / (n_ + rho);
        double p_h2_factor4 = ((nu + (w2 == w3 ? 1 : 0) + rho/2) / (n_ + 1 + rho));

        double p_h1 = p_h1_factor1 * p_h1_factor2;
        double p_h2 = p_h2_factor1 * p_h2_factor2 * p_h2_factor3 * p_h2_factor4;

        if (p_h2 > p_h1)
            corpus.boundaries[i] = 1;
        else
            corpus.boundaries[i] = 0;
    }
}

void write_boundaries(const Corpus &corpus, string filename)
{
    ofstream out(filename);
    out << "[";

    for (int i = 0; i < corpus.text.size(); ++i) {
        if (i != 0)
            out << ", ";
        out << corpus.boundaries[i];
    }

    out << "]";
    out.close();
}

dynamic_bitset<> read_boundaries(string path)
{
    fstream in(path);
    string line;
    getline(in, line);
    in.close();

    auto end_iter = remove_if(line.begin(), line.end(),
                              [](char ch) {return !(ch == '1' || ch == '0');});
    line.erase(end_iter, line.end());

    return dynamic_bitset<>(line);
}

string generate_filepath(const string &out_dir, double alpha, double p_hash, int i=-1)
{
    stringstream s;
    s << out_dir << "/";
    if (i >= 0)
        s << "iter_" << i << "_";
    s << alpha << "_" << p_hash << ".txt";

    return s.str();
}

int main(int argc, char *argv[])
{
    string out_dir;
    string train_path;
    string test_path;
    string boundaries;
    double alpha;
    double p_hash;
    vector<int> eval_points;

    po::options_description desc("Usage");
    desc.add_options()
        ("alpha,a", po::value<double>(&alpha)->default_value(0.5), "alpha parameter")
        ("p_hash,ph", po::value<double>(&p_hash)->default_value(0.5), "p# parameter")
        ("out_dir", po::value<string>(&out_dir), "path to write output to")
        ("train_path", po::value<string>(&train_path), "path to load training data")
        ("test_path", po::value<string>(&test_path), "path to load test data")
        ("test", "apply a learned model to test data (requires test_path and boundaries)")
        ("help", "print usage information")
        ("boundaries", po::value<string>(&boundaries), "path to a learned set of boundaries")
        (",n", po::value<vector<int> >(&eval_points)->multitoken(), "points to evaluate");

    po::variables_map opts;
    po::store(po::parse_command_line(argc, argv, desc), opts);
    po::notify(opts);

    if (opts.count("help")) {
        cout << desc << endl;
        return 0;
    }

    string out_path = generate_filepath(out_dir, alpha, p_hash);

    Corpus *corpus;
    if (opts.count("test")) {
        cout << "Testing" << endl;
        auto bounds = read_boundaries(boundaries);
        corpus = new Corpus(train_path, test_path, bounds);
    } else {
        corpus = new Corpus(train_path);
    }

    // calculate the maximum iteration
    int i_max = *max_element(eval_points.begin(), eval_points.end());

    for (int i = 0; i < i_max; ++i) {
        cout << "Iteration " << i << endl;
        gibbs_iteration(*corpus, 2, alpha, p_hash);

        if (find(eval_points.begin(), eval_points.end(), i) != eval_points.end()) {
            string iterpath = generate_filepath(out_dir, alpha, p_hash, i);
            write_boundaries(*corpus, iterpath);
        }
    }

    write_boundaries(*corpus, out_path);

    return 0;
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2010 Roland Lichters
 Copyright (C) 2014 Jose Aparicio

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#include <qlo/qladdindefines.hpp>
#include <qlo/credit.hpp>
#include <qlo/enumerations/factories/termstructuresfactory.hpp>

#include <ql/instruments/stock.hpp>
#include <ql/quote.hpp>
#include <ql/currencies/europe.hpp>
#include <ql/time/daycounter.hpp>
#include <ql/time/daycounters/actual360.hpp>
#include <ql/termstructures/credit/interpolatedhazardratecurve.hpp>
#include <ql/termstructures/yield/piecewiseyieldcurve.hpp>
#include <ql/termstructures/credit/piecewisedefaultcurve.hpp>
#include <ql/math/interpolations/backwardflatinterpolation.hpp>
#include <ql/math/interpolations/loginterpolation.hpp>
#include <ql/pricingengines/credit/midpointcdsengine.hpp>

#include <ql/experimental/credit/riskybond.hpp>
#include <ql/experimental/credit/syntheticcdo.hpp>
#include <ql/experimental/credit/midpointcdoengine.hpp>
#include <ql/experimental/credit/nthtodefault.hpp>
#include <ql/experimental/credit/integralntdengine.hpp>

#include <boost/algorithm/string/case_conv.hpp>
#include <boost/make_shared.hpp>

#include <ql/settings.hpp>

using boost::algorithm::to_upper_copy;

namespace QuantLibAddin {

    Issuer::Issuer(
            const boost::shared_ptr<ObjectHandler::ValueObject>& properties,
            const boost::shared_ptr<QuantLib::DefaultProbabilityTermStructure>& dfts,
            const boost::shared_ptr<QuantLib::DefaultEventSet>& evtSet,
            bool permanent
        )
        : ObjectHandler::LibraryObject<QuantLib::Issuer>(properties, permanent) {

        std::vector<QuantLib::Issuer::key_curve_pair> curves(1, std::make_pair(
            QuantLib::NorthAmericaCorpDefaultKey(QuantLib::EURCurrency(),
                                                     QuantLib::SeniorSec,
                                                     QuantLib::Period(),
                                                     1. // amount threshold
                                                     ),
            QuantLib::Handle<QuantLib::DefaultProbabilityTermStructure>(dfts)
        ));
        libraryObject_ = boost::shared_ptr<QuantLib::Issuer>(new QuantLib::Issuer(curves, *evtSet));
    }

    DefaultEventSet::DefaultEventSet(
            const boost::shared_ptr<ObjectHandler::ValueObject>& properties,
            const std::string& eventType,
            const QuantLib::Date& eventDate,
            const QuantLib::Currency& cur,
            QuantLib::Seniority sen,
            const QuantLib::Date& settlementDate,
            QuantLib::Real settledRecovery,
            bool permanent
        )
    : ObjectHandler::LibraryObject<QuantLib::DefaultEventSet>(properties, permanent) {
        // if no match return empty set
        libraryObject_ = boost::shared_ptr<QuantLib::DefaultEventSet>(new QuantLib::DefaultEventSet());

        // only one recovery parsed by now; bankruptcy events need the whole
        //  set or they fail to construct.
        std::map<QuantLib::Seniority, QuantLib::Real> rrs;
        rrs.insert(std::pair<QuantLib::Seniority, QuantLib::Real>(sen, settledRecovery));
        if(eventType==std::string("FailureToPayEvent")) {
            libraryObject_->insert(boost::shared_ptr<QuantLib::FailureToPayEvent> (
                new QuantLib::FailureToPayEvent(eventDate, cur, sen, 1.e7,
                //implSettlemt,
                settlementDate,
                rrs)));
        }else if(eventType==std::string("BankruptcyEvent")){
            libraryObject_->insert(boost::shared_ptr<QuantLib::BankruptcyEvent> (
                new QuantLib::BankruptcyEvent(eventDate, cur, sen,
                settlementDate,
                rrs)));
        }
    }

    /* Code essentially copied from QuantLibAddIn::SimpleQuote.
         Same considerations mentioned there apply. Here the
         Seniority of the quote is considered a fixed
         property, not a market value.
    */
    RecoveryRateQuote::RecoveryRateQuote(
            const boost::shared_ptr<ObjectHandler::ValueObject>& properties,
            QuantLib::Seniority sen,
            QuantLib::Real value,
            bool permanent) : Quote(properties, permanent) {
        libraryObject_ = recoveryQuote_ = boost::shared_ptr<QuantLib::RecoveryRateQuote>(
            new QuantLib::RecoveryRateQuote(value, sen));
    }

   // CreditDefaultSwap::CreditDefaultSwap(
   //           const boost::shared_ptr<ObjectHandler::ValueObject>& properties,
   //           QuantLib::Protection::Side side,
   //           QuantLib::Real notional,
   //           QuantLib::Rate upfront,
   //           QuantLib::Rate spread,
   //           const boost::shared_ptr<QuantLib::Schedule>& schedule,
   //           QuantLib::BusinessDayConvention paymentConvention,
   //           const QuantLib::DayCounter& dayCounter,
   //           bool settlesAccrual,
   //           bool paysAtDefaultTime,
   //           const QuantLib::Date& protectionStart,
   //           const QuantLib::Date& upfrontDate,
   //           bool permanent)
   //     : Instrument(properties, permanent) {
			//// dirty way to decide if this is constructed through a run only version
			//if(upfrontDate == QuantLib::Null<QuantLib::Date>() && upfront == 0.) {
			//	libraryObject_ = boost::shared_ptr<QuantLib::CreditDefaultSwap>(
			//				new QuantLib::CreditDefaultSwap(side,
			//												notional,
			//												spread,
			//												*schedule,
			//												paymentConvention,
			//												dayCounter,
			//												settlesAccrual,
			//												paysAtDefaultTime,
			//												protectionStart,
			//												boost::shared_ptr<QuantLib::Claim>()));
			//}else{
			//	libraryObject_ = boost::shared_ptr<QuantLib::CreditDefaultSwap>(
			//				new QuantLib::CreditDefaultSwap(side,
			//												notional,
			//												upfront,
			//												spread,
			//												*schedule,
			//												paymentConvention,
			//												dayCounter,
			//												settlesAccrual,
			//												paysAtDefaultTime,
			//												protectionStart,
			//												upfrontDate,
			//												boost::shared_ptr<QuantLib::Claim>()));
   //         }
   // }
   //
    MidPointCdsEngine::MidPointCdsEngine(
            const boost::shared_ptr<ObjectHandler::ValueObject>& properties,
            const QuantLib::Handle<QuantLib::DefaultProbabilityTermStructure>& defaultTS,
            QuantLib::Real recoveryRate,
            const QuantLib::Handle<QuantLib::YieldTermStructure>& yieldTS,
            bool permanent)
        : PricingEngine(properties, permanent) {
        libraryObject_ = boost::shared_ptr<QuantLib::PricingEngine>(new
              QuantLib::MidPointCdsEngine(defaultTS, recoveryRate, yieldTS));
    }

    SpreadCdsHelper::SpreadCdsHelper(
            const boost::shared_ptr<ObjectHandler::ValueObject>& properties,
            const QuantLib::Handle<QuantLib::Quote>& quote,
            const QuantLib::Period& period,
            QuantLib::Natural settlementDays,
            const QuantLib::Calendar& calendar,
            QuantLib::Frequency frequency,
            QuantLib::BusinessDayConvention paymentConvention,
            QuantLib::DateGeneration::Rule rule,
            const QuantLib::DayCounter& dayCounter,
            QuantLib::Real recoveryRate,
            const QuantLib::Handle<QuantLib::YieldTermStructure>& yieldTS,
            bool settlesAccrual,
            bool paysAtDefaultTime,
            bool permanent) : DefaultProbabilityHelper(properties, permanent) {

        libraryObject_ = boost::shared_ptr<QuantLib::DefaultProbabilityHelper>(new
		       QuantLib::SpreadCdsHelper(quote,
						 period,
						 settlementDays,
						 calendar,
						 frequency,
						 paymentConvention,
						 rule,
						 dayCounter,
						 recoveryRate,
						 yieldTS,
						 settlesAccrual,
						 paysAtDefaultTime));
    }

    UpfrontCdsHelper::UpfrontCdsHelper(
            const boost::shared_ptr<ObjectHandler::ValueObject>& properties,
            const QuantLib::Handle<QuantLib::Quote>& quote,
            QuantLib::Rate runningSpread,
            const QuantLib::Period& period,
            QuantLib::Natural settlementDays,
            const QuantLib::Calendar& calendar,
            QuantLib::Frequency frequency,
            QuantLib::BusinessDayConvention paymentConvention,
            QuantLib::DateGeneration::Rule rule,
            const QuantLib::DayCounter& dayCounter,
            QuantLib::Real recoveryRate,
            const QuantLib::Handle<QuantLib::YieldTermStructure>& yieldTS,
            QuantLib::Natural upfrontSettlementDays,
            bool settlesAccrual,
            bool paysAtDefaultTime,
            bool permanent) : DefaultProbabilityHelper(properties, permanent) {
        libraryObject_ = boost::shared_ptr<QuantLib::DefaultProbabilityHelper>(new
		       QuantLib::UpfrontCdsHelper(quote,
                                          runningSpread,
                                          period,
                                          settlementDays,
                                          calendar,
                                          frequency,
                                          paymentConvention,
                                          rule,
                                          dayCounter,
                                          recoveryRate,
                                          yieldTS,
                                          upfrontSettlementDays,
                                          settlesAccrual,
                                          paysAtDefaultTime));
    }

    HazardRateCurve::HazardRateCurve(
            const boost::shared_ptr<ObjectHandler::ValueObject>& properties,
            const std::vector<QuantLib::Date>& dates,
            const std::vector<QuantLib::Rate>& hazardRates,
            const QuantLib::DayCounter& dayCounter,
            bool permanent)
        : DefaultProbabilityTermStructure(properties, permanent) {
        QL_REQUIRE(!dates.empty(), "no input dates given");
        QL_REQUIRE(dates.size() == hazardRates.size(),
                   "vector sizes differ");
        libraryObject_ = boost::shared_ptr<QuantLib::Extrapolator>(
        new QuantLib::InterpolatedHazardRateCurve<QuantLib::BackwardFlat>(
 				 dates, hazardRates, dayCounter));
    }

    PiecewiseHazardRateCurve::PiecewiseHazardRateCurve(
            const boost::shared_ptr<ObjectHandler::ValueObject>& properties,
            const std::vector<boost::shared_ptr<QuantLib::DefaultProbabilityHelper> >& helpers,
            const QuantLib::DayCounter& dayCounter,
            const QuantLib::Calendar& calendar,
            const std::string& interpolator,
            QuantLib::Real accuracy,
            bool permanent)
        : DefaultProbabilityTermStructure(properties, permanent) {

        if(interpolator == std::string("LINEAR")){
            libraryObject_ = boost::shared_ptr<QuantLib::Extrapolator>(new
                   QuantLib::PiecewiseDefaultCurve<QuantLib::HazardRate,
                        QuantLib::Linear>(
                            0,
                            calendar,
                            helpers,
                            dayCounter));
        }else if(interpolator == std::string("BACKWARDFLAT")) {
            libraryObject_ = boost::shared_ptr<QuantLib::Extrapolator>(new
                   QuantLib::PiecewiseDefaultCurve<QuantLib::HazardRate,
                        QuantLib::BackwardFlat>(
                            0,
                            calendar,
                            helpers,
                            dayCounter));
        }else{
            QL_FAIL("Unrecognised interpolator");
        }

        libraryObject_->enableExtrapolation();
    }

    // ptr type check here would correspond to template spez in the
    //   subscribers factory solution.
    const std::vector<QuantLib::Date>& PiecewiseHazardRateCurve::dates() const {
        typedef QuantLib::PiecewiseDefaultCurve<QuantLib::HazardRate, QuantLib::BackwardFlat> flat_curve;
        typedef QuantLib::PiecewiseDefaultCurve<QuantLib::HazardRate, QuantLib::Linear> lin_curve;
        boost::shared_ptr<flat_curve> ptrBF =
            boost::dynamic_pointer_cast<flat_curve>(libraryObject_);
        if(ptrBF) return ptrBF->dates();
        boost::shared_ptr<lin_curve> ptrLIN =
            boost::dynamic_pointer_cast<lin_curve>(libraryObject_);
        if(ptrLIN) return ptrLIN->dates();
        QL_FAIL("Unable to cast default probability term structure.");
    }

    const std::vector<QuantLib::Real>& PiecewiseHazardRateCurve::data() const {
        typedef QuantLib::PiecewiseDefaultCurve<QuantLib::HazardRate, QuantLib::BackwardFlat> flat_curve;
        typedef QuantLib::PiecewiseDefaultCurve<QuantLib::HazardRate, QuantLib::Linear> lin_curve;
        boost::shared_ptr<flat_curve> ptrBF =
            boost::dynamic_pointer_cast<flat_curve>(libraryObject_);
        if(ptrBF) return ptrBF->data();
        boost::shared_ptr<lin_curve> ptrLIN =
            boost::dynamic_pointer_cast<lin_curve>(libraryObject_);
        if(ptrLIN) return ptrLIN->data();
        QL_FAIL("Unable to cast default probability term structure.");
        }

    PiecewiseFlatForwardCurve::PiecewiseFlatForwardCurve(
            const boost::shared_ptr<ObjectHandler::ValueObject>& properties,
            const QuantLib::Date& referenceDate,
            const std::vector<boost::shared_ptr<QuantLib::RateHelper> >& helpers,
            const QuantLib::DayCounter& dayCounter,
            QuantLib::Real accuracy,
            bool permanent)
        : YieldTermStructure(properties, permanent) {
        libraryObject_ = boost::shared_ptr<QuantLib::Extrapolator>(new
               QuantLib::PiecewiseYieldCurve<QuantLib::Discount,QuantLib::LogLinear>(referenceDate, helpers, dayCounter));
    }

    RiskyFixedBond::RiskyFixedBond(
        const boost::shared_ptr<ObjectHandler::ValueObject>& properties,
        std::string name,
        QuantLib::Currency ccy,
        QuantLib::Real recoveryRate,
        QuantLib::Handle<QuantLib::DefaultProbabilityTermStructure> defaultTS,
        const boost::shared_ptr<QuantLib::Schedule>& schedule,
        QuantLib::Real rate,
        QuantLib::DayCounter dayCounter,
        QuantLib::BusinessDayConvention paymentConvention,
        QuantLib::Real notional,
        QuantLib::Handle<QuantLib::YieldTermStructure> yieldTS,
        QuantLib::Date npvDate, // unused by now
        bool permanent)
    : Instrument(properties, permanent) {

        std::vector<QuantLib::Real> notionals(1,notional);

        libraryObject_ = boost::shared_ptr<QuantLib::RiskyFixedBond>(
            new QuantLib::RiskyFixedBond(
                    name,ccy,recoveryRate,defaultTS,*schedule,rate,dayCounter,
                    paymentConvention,notionals,yieldTS///, npvDate
                                       ));

    }

    SyntheticCDO::SyntheticCDO(
        const boost::shared_ptr<ObjectHandler::ValueObject>& properties,
        const boost::shared_ptr<QuantLib::Basket>& bskt,
        QuantLib::Protection::Side side,
        const boost::shared_ptr<QuantLib::Schedule>& schedule,
        QuantLib::Rate upfront,
        QuantLib::Rate spread,
        const QuantLib::DayCounter& dayCounter,
        QuantLib::BusinessDayConvention paymentConvention,
        bool permanent)
    : Instrument(properties, permanent) {
		libraryObject_ = boost::make_shared<QuantLib::SyntheticCDO>(
            bskt,side,*schedule,upfront,spread,dayCounter,paymentConvention);
    }

    MidPointCDOEngine::MidPointCDOEngine(
        const boost::shared_ptr<ObjectHandler::ValueObject>& properties,
        const QuantLib::Handle<QuantLib::YieldTermStructure>& discTS,
        bool permanent)
    : PricingEngine(properties, permanent) {
		libraryObject_ =
            boost::make_shared<QuantLib::MidPointCDOEngine>(discTS);
    }

    NthToDefault::NthToDefault(
          const boost::shared_ptr<ObjectHandler::ValueObject>& properties,
          const boost::shared_ptr<QuantLib::Basket>& bskt,
          QuantLib::Size order,
          QuantLib::Protection::Side side,
          const boost::shared_ptr<QuantLib::Schedule>& schedule,
          QuantLib::Rate upfront,
          QuantLib::Rate spread,
          const QuantLib::DayCounter& dayCounter,
          QuantLib::Real notional,
          bool paysAccrual,
          bool permanent)
    : Instrument(properties, permanent) {
		libraryObject_ = boost::make_shared<QuantLib::NthToDefault>(
            bskt,order,side,*schedule,upfront,spread,dayCounter,
            notional,paysAccrual);
    }

    IntegralNtdEngine::IntegralNtdEngine(
        const boost::shared_ptr<ObjectHandler::ValueObject>& properties,
        const QuantLib::Period& step,
        const QuantLib::Handle<QuantLib::YieldTermStructure>& discTS,
        bool permanent)
    : PricingEngine(properties, permanent) {
		libraryObject_ =
            boost::make_shared<QuantLib::IntegralNtdEngine>(step, discTS);
    }

}


#include <armadillo>
#include <sys/sysinfo.h>
#include "energy.h"
#include "mcpdft.h"
#include "libMem.h"
#include "openrdmConfig.h"
#include <string>
//#include "HDF5_Read_Contiguous.h"
//#include "HDF5_Write_Contiguous.h"
//#include "HDF5_Read_Compact.h"
//#include "HDF5_Write_Compact.h"
//#include "HDF5_Read_Chunked.h"
//#include "HDF5_Write_Chunked.h"
//#include "HDF5ContiguousFactory.h"
//#include "HDF5CompactFactory.h"
//#include "HDF5ChunkedFactory.h"
#include "HDF5Client.h"
#ifdef WITH_LIBXC
   #include <xc.h>
#else
   #include "functional.h"
#endif

namespace mcpdft {

   // calculates the MCPDFT energy
   double mcpdft_energy(MCPDFT *mc,
		        std::string &functional,
                        const arma::mat &D1a,
                        const arma::mat &D1b,
                        const arma::mat &D2ab) {

      // Query memory information from linux
      struct sysinfo info;
      sysinfo(&info);

      // Calculating the amount of available memory
      LibMem *libmem;
      libmem = new LibMem();
      libmem->query_system_memory(&info);

      double tot_energy = 0.0;

      // getting the value of the classical energy
      double eclass = mc->get_eclass();
      // printf("eclass = %-20.15lf\n",eclass);

      // building the one electron densities rho_a(r) and rho_b(r)
      mc->build_rho();

      // building the on-top pair density pi(r,r)
      mc->build_pi(D2ab);

      // building the R(r) factor for density translation
      mc->build_R();

      // translate the one-electron densities
      mc->translate();

      size_t npts = mc->get_npts();
      arma::vec tr_rhoa(mc->get_tr_rhoa());
      arma::vec tr_rhob(mc->get_tr_rhob());
      arma::vec tr_sigma_aa(npts, arma::fill::zeros);
      arma::vec tr_sigma_ab(npts, arma::fill::zeros);
      arma::vec tr_sigma_bb(npts, arma::fill::zeros);
      if(mc->is_gga()) {
         tr_sigma_aa = mc->get_tr_sigma_aa();
         tr_sigma_ab = mc->get_tr_sigma_ab();
         tr_sigma_bb = mc->get_tr_sigma_bb();
      }
#ifdef WITH_LIBXC
      arma::vec tr_rho(tr_rhoa);
      arma::vec ex(npts);
      arma::vec ec(npts);
      arma::vec W(mc->get_w());
      tr_rho = tr_rho + tr_rhob;
      double * rhop = tr_rho.memptr();
      double * exp = ex.memptr();
      double * ecp = ec.memptr();

      double Ex = 0.0;
      double Ec = 0.0;

      xc_func_type func_x;
      if(xc_func_init(&func_x,XC_LDA_X, XC_UNPOLARIZED) != 0){
        xc_lda_exc(&func_x, npts, rhop, exp);
      }

      double ans = 0.0;
      for (int p = 0; p < npts; p++) {
          if (tr_rho(p) > 1.0e-20)
             ans += W(p) * ex(p) * tr_rho(p);
      }
      Ex=ans;

      xc_func_type func_c;
      if(xc_func_init(&func_c,XC_LDA_C_VWN_RPA, XC_UNPOLARIZED) != 0){
        xc_lda_exc(&func_c, npts, rhop, ecp);
      }

      ans = 0.0;
      for (int p = 0; p < npts; p++) {
          if (tr_rho(p) > 1.0e-20)
             ans += W(p) * ec(p) * tr_rho(p);
      }
      Ec=ans;
      xc_func_end(&func_x);
      xc_func_end(&func_c);
#else
      Functional* func = new Functional;

      double Ex = 0.0;
      double Ec = 0.0;
      if (functional == "SVWN") {
          Ex = func->EX_LSDA(mc, tr_rhoa, tr_rhob);
          Ec = func->EC_VWN3(mc, tr_rhoa, tr_rhob);
      }else{
          Ex = func->EX_PBE(mc, tr_rhoa, tr_rhob, tr_sigma_aa, tr_sigma_bb);
          Ec = func->EC_PBE(mc, tr_rhoa, tr_rhob, tr_sigma_aa, tr_sigma_ab, tr_sigma_bb);
      }

      delete func;
#endif

      size_t nbfs = mc->get_nbfs();
      size_t nbfs2 = nbfs * nbfs;

      arma::mat d1a(nbfs, nbfs, arma::fill::zeros);
      arma::mat d1b(nbfs, nbfs, arma::fill::zeros);
      arma::mat d2ab(nbfs2, nbfs2, arma::fill::zeros);

      HDF5Client* h5client = new HDF5Client();

      HDF5Client::factory_mode mode(HDF5Client::factory_mode::WRITE);
      h5client->factory_client(H5D_COMPACT,mode,D1a,D1b,D2ab);

      mode = HDF5Client::factory_mode::READ;
      h5client->factory_client(H5D_COMPACT,mode,D1a,D1b,D2ab);

      delete h5client;

      // IOFactory* iof;
      // IRead* ird;
      // IWrite* iwt;

//    //   iof = new HDF5ContiguousFactory;
//    //   iof = new HDF5CompactFactory;
      // iof = new HDF5ChunkedFactory;
      // iwt = iof->create_IWrite();
      // iwt->write_rdms(D1a,D1b,D2ab);
      // ird = iof->create_IRead();
      // ird->read_rdms(d1a,d1b,d2ab);
      // delete iof;
      // IWrite* h5w = iof->create_IWrite();
      // h5w->write_opdm(D1a,D1b);
      // DiskRW dskrw;
      // dskrw.write_opdm(D1a,D1b);
      // dskrw.write_tpdm(D2ab);
      // size_t nbfs = mc->get_nbfs();
      // size_t nbfs2 = nbfs * nbfs;
      // try{
      //    arma::mat d1a(nbfs, nbfs, arma::fill::zeros);
      //    arma::mat d1b(nbfs, nbfs, arma::fill::zeros);
      //    arma::mat d2ab(nbfs2, nbfs2, arma::fill::zeros);
      //    dskrw.read_opdm(d1a,d1b);
      //    // d1a.print("D1a =");
      //    // d1b.print("D1b =");
      //    dskrw.read_tpdm(d2ab);
      //    // d2ab.print("D2ab =");
      // } catch(const char* err_msg) {
      //    printf("%s\n",err_msg);
      // }

      printf("------------------------------------------\n");
      printf("   Classical energy = %-20.12lf\n", eclass);
      printf("   Ex               = %-20.12lf\n", Ex);
      printf("   Ec               = %-20.12lf\n", Ec);
      printf("------------------------------------------\n\n");

      tot_energy += eclass;
      tot_energy += Ex;
      tot_energy += Ec;

      delete libmem;

      return tot_energy;
   }

}


#include <iostream>
#include <string>
#include <experimental/filesystem>
#include <armadillo>
#include <stdexcept>

#include "exciton_transfer.h"
#include "cnt.h"
#include "constants.h"
#include "../helper/progress.hpp"

// calculate and plot Q matrix element between two exciton bands
void exciton_transfer::save_Q_matrix_element(const int i_n_principal, const int f_n_principal)
{
  // set the donor and accdeptor cnts
  const cnt& init_cnt = *_cnts[0];
  const cnt& final_cnt = *_cnts[1];

  // set the donor and acceptor exciton structs
  const cnt::exciton_struct& i_exciton = init_cnt.A2_singlet();
  const cnt::exciton_struct& f_exciton = final_cnt.A2_singlet();

  std::cout << "\n...calculating Q matrix element\n";

  // some error checking
  if ((i_n_principal >= i_exciton.n_principal) and (f_n_principal >= f_exciton.n_principal)){
    throw std::invalid_argument("i_n_principal or f_n_principla are two large.");
  }

  // find lists of relevant states in donor and acceptor excitons
  auto get_exciton_band = [](const auto& exciton, const int i_principal){
    std::vector<ex_state> states;
    for (int ik_cm_idx=0; ik_cm_idx<exciton.nk_cm; ik_cm_idx++)
    {
      ex_state state(exciton,ik_cm_idx,i_principal);
      states.emplace_back(state);
    }
    return states;
  };
  const auto i_relevant_states = get_exciton_band(i_exciton,i_n_principal);
  const auto f_relevant_states = get_exciton_band(f_exciton,f_n_principal);

  // match the states based on their energy
  std::vector<matching_states> state_pairs = match_all_states(i_relevant_states, f_relevant_states);

  // get min of ik_cm_idx from relevant states indices
  auto get_min_ik_cm_idx = [](const auto& relevant_states){
    int min_idx = 1.e9;
    for (const auto& state: relevant_states)
    {
      min_idx = (min_idx < state.ik_cm_idx) ? min_idx : state.ik_cm_idx;
    }
    return min_idx;
  };
  // get max of ik_cm_idx from relevant states indices
  auto get_max_ik_cm_idx = [](const auto& relevant_states){
    int max_idx = -1.e9;
    for (const auto& state: relevant_states)
    {
      max_idx = (max_idx > state.ik_cm_idx) ? max_idx : state.ik_cm_idx;
    }
    return max_idx;
  };
  int i_min_idx = get_min_ik_cm_idx(i_relevant_states);
  int i_max_idx = get_max_ik_cm_idx(i_relevant_states);
  int f_min_idx = get_min_ik_cm_idx(f_relevant_states);
  int f_max_idx = get_max_ik_cm_idx(f_relevant_states);

  arma::cx_mat Q_mat(i_max_idx-i_min_idx+1, f_max_idx-f_min_idx+1, arma::fill::zeros);
  arma::vec init_ik_cm(i_max_idx-i_min_idx+1, arma::fill::zeros);
  arma::vec final_ik_cm(f_max_idx-f_min_idx+1, arma::fill::zeros);

  progress_bar prog(state_pairs.size(), "calculate Q");

  for (const auto& pair:state_pairs)
  {
    Q_mat(pair.i.ik_cm_idx-i_min_idx, pair.f.ik_cm_idx-f_min_idx) = calculate_Q(pair);
    init_ik_cm(pair.i.ik_cm_idx-i_min_idx) = pair.i.ik_cm;
    final_ik_cm(pair.f.ik_cm_idx-f_min_idx) = pair.f.ik_cm;
    prog.step();
  }

  arma::mat tmp;

  // save the real part matrix element Q to a file
  tmp= arma::real(Q_mat);
  std::string filename = _directory.path() / "matrix_element_q.real.dat";
  tmp.save(filename,arma::arma_ascii);

  // save the imaginary part matrix element Q to a file
  tmp= arma::imag(Q_mat);
  filename = _directory.path() / "matrix_element_q.imag.dat";
  tmp.save(filename,arma::arma_ascii);

  // save ik_cm of the inital states
  filename = _directory.path() / "matrix_element_q.init_ik_cm.dat";
  init_ik_cm.save(filename,arma::arma_ascii);

  // save ik_cm of the final states
  filename = _directory.path() / "matrix_element_q.final_ik_cm.dat";
  final_ik_cm.save(filename,arma::arma_ascii);

  std::cout << "\n...calculated and saved Q matrix element\n";

}

// calculate and plot J matrix element between two exciton bands
void exciton_transfer::save_J_matrix_element(const int i_n_principal, const int f_n_principal)
{
  // set geometrical properties
  double z_shift = 1.5e-9;
  double theta = 0;
  std::array<double,2> axis_shifts = {0., 0.};

  // set the donor and accdeptor cnts
  const cnt& init_cnt = *_cnts[0];
  const cnt& final_cnt = *_cnts[1];

  // set the donor and acceptor exciton structs
  const cnt::exciton_struct& i_exciton = init_cnt.A2_singlet();
  const cnt::exciton_struct& f_exciton = final_cnt.A2_singlet();

  std::cout << "\n...calculating J matrix element\n";
  // std::cout << "initial exciton ik_cm_range: [" << i_exciton.ik_cm_range[0] << "," << i_exciton.ik_cm_range[1] << "]\n";
  // std::cout << "final exciton ik_cm_range: [" << f_exciton.ik_cm_range[0] << "," << f_exciton.ik_cm_range[1] << "]\n";

  // some error checking
  if ((i_n_principal >= i_exciton.n_principal) and (f_n_principal >= f_exciton.n_principal)){
    throw std::invalid_argument("i_n_principal or f_n_principla are two large.");
  }

  // find lists of relevant states in donor and acceptor excitons
  auto get_exciton_band = [](const auto& exciton, const int i_principal){
    std::vector<ex_state> states;
    // int ik_cm_min = exciton.ik_cm_range[0];
    // int ik_cm_max = exciton.ik_cm_range[1];
    int ik_cm_min = -50;
    int ik_cm_max = -ik_cm_min+1;
    for (int ik_cm=ik_cm_min; ik_cm<ik_cm_max; ik_cm++)
    {
      ex_state state(exciton,ik_cm-exciton.ik_cm_range[0],i_principal);
      states.emplace_back(state);
    }
    return states;
  };
  const auto i_relevant_states = get_exciton_band(i_exciton,i_n_principal);
  const auto f_relevant_states = get_exciton_band(f_exciton,f_n_principal);

  std::cout << "initial exciton length: " << i_exciton.cnt_obj->length_in_meter()*1e9 << " [nm]\n";
  std::cout << "final exciton length: " << f_exciton.cnt_obj->length_in_meter()*1e9 << " [nm]\n";

  // match the states based on their energy
  std::vector<matching_states> state_pairs = match_all_states(i_relevant_states, f_relevant_states);

  // get min of ik_cm_idx from relevant states indices
  auto get_min_ik_cm_idx = [](const auto& relevant_states){
    int min_idx = 1.e9;
    for (const auto& state: relevant_states)
    {
      min_idx = (min_idx < state.ik_cm_idx) ? min_idx : state.ik_cm_idx;
    }
    return min_idx;
  };
  // get max of ik_cm_idx from relevant states indices
  auto get_max_ik_cm_idx = [](const auto& relevant_states){
    int max_idx = -1.e9;
    for (const auto& state: relevant_states)
    {
      max_idx = (max_idx > state.ik_cm_idx) ? max_idx : state.ik_cm_idx;
    }
    return max_idx;
  };
  int i_min_idx = get_min_ik_cm_idx(i_relevant_states);
  int i_max_idx = get_max_ik_cm_idx(i_relevant_states);
  int f_min_idx = get_min_ik_cm_idx(f_relevant_states);
  int f_max_idx = get_max_ik_cm_idx(f_relevant_states);

  arma::cx_mat J_mat(i_max_idx-i_min_idx+1, f_max_idx-f_min_idx+1, arma::fill::zeros);
  arma::vec init_ik_cm(i_max_idx-i_min_idx+1, arma::fill::zeros);
  arma::vec final_ik_cm(f_max_idx-f_min_idx+1, arma::fill::zeros);

  // int count = 0;
  // int number_of_pairs = state_pairs.size();
  progress_bar prog(state_pairs.size(),"calculate J");

  for (const auto& pair:state_pairs)
  {
    // count++;
    // prog.step(count, number_of_pairs, "calculate J");
    prog.step();
    // std::cout << "calculate J: " << count << "/" << number_of_pairs << "\n";

    J_mat(pair.i.ik_cm_idx-i_min_idx, pair.f.ik_cm_idx-f_min_idx) = calculate_J(pair, axis_shifts, z_shift, theta);
    init_ik_cm(pair.i.ik_cm_idx-i_min_idx) = pair.i.ik_cm;
    final_ik_cm(pair.f.ik_cm_idx-f_min_idx) = pair.f.ik_cm;
  }

  arma::mat tmp;

  // save the real part matrix element Q to a file
  tmp= arma::real(J_mat);
  std::string filename = _directory.path() / "matrix_element_j.real.dat";
  tmp.save(filename,arma::arma_ascii);

  // save the imaginary part matrix element Q to a file
  tmp= arma::imag(J_mat);
  filename = _directory.path() / "matrix_element_j.imag.dat";
  tmp.save(filename,arma::arma_ascii);

  // save ik_cm of the inital states
  filename = _directory.path() / "matrix_element_j.init_ik_cm.dat";
  init_ik_cm.save(filename,arma::arma_ascii);

  // save ik_cm of the final states
  filename = _directory.path() / "matrix_element_j.final_ik_cm.dat";
  final_ik_cm.save(filename,arma::arma_ascii);

  std::cout << "\n...calculated and saved J matrix element\n";

}

// get the energetically relevant states in the form a vector of ex_state structs
std::vector<exciton_transfer::ex_state> exciton_transfer::get_relevant_states(const cnt::exciton_struct& exciton, const double min_energy)
{
  const double threshold_population = 1.e-3;
  const double threshold_energy = min_energy+std::abs(std::log(threshold_population) * constants::kb*_temperature);

  std::vector<ex_state> relevant_states;

  for (int i_n=0; i_n<exciton.n_principal; i_n++)
  {
    for (int ik_cm_idx=0; ik_cm_idx<exciton.nk_cm; ik_cm_idx++)
    {
      if (exciton.energy(ik_cm_idx,i_n) <= threshold_energy)
      {
        relevant_states.emplace_back(ex_state(exciton,ik_cm_idx,i_n));
      }
    }
  }

  // sort relevant states in order of their energies
  std::sort(relevant_states.begin(),relevant_states.end(), \
              [](const auto& s1, const auto& s2) {
                return s1.energy < s2.energy;
              }
            );

  // calculate normalization factor
  double normalization_factor = 0.;
  for (const auto& state: relevant_states)
  {
    double delta_e = (state.energy-min_energy);
    normalization_factor += std::exp(-delta_e/(constants::kb*_temperature));
  }

  // std::cout << "\n...calculated relevant states\n";
  // std::cout << "number of relevant states: " << relevant_states.size() << "\n";
  // for (const auto& state: relevant_states)
  // {
  //   double delta_e = (state.energy-min_energy);
  //   std::cout << "[" << state.ik_cm_idx << "," << state.i_principal <<"] --> energy:" << delta_e/constants::eV
  //             << " , population:" << std::exp(-delta_e/(constants::kb*_temperature))/normalization_factor << "\n";
  // }

  return relevant_states;
}

// calculate Q()
std::complex<double> exciton_transfer::calculate_Q(const matching_states& pair) const
{
  // lambda function to calculate part of the Q matrix element that relates to each pair.
  auto Q_partial = [](const ex_state& state)
  {
    const int ic = 1;
    const int iv = 0;

    const arma::vec dA = {0,0};
    const arma::vec& dB = *(state.exciton->aCC_vec);
    const arma::cx_vec exp_factor({std::exp(std::complex<double>(0.,+1.)*arma::dot(state.ik_cm*state.dk_l(),dA)),\
                                   std::exp(std::complex<double>(0.,+1.)*arma::dot(state.ik_cm*state.dk_l(),dB))});

    std::complex<double> Q_partial = 0;
    for (int ik_c_idx=0; ik_c_idx<state.exciton->nk_c; ik_c_idx++)
    {
      const arma::cx_vec& Cc = state.elec_struct->wavefunc(state.ik_idx(1,ik_c_idx)).slice(state.ik_idx(0,ik_c_idx)).col(ic);
      const arma::cx_vec& Cv = state.elec_struct->wavefunc(state.ik_idx(3,ik_c_idx)).slice(state.ik_idx(2,ik_c_idx)).col(iv);
      Q_partial += state.psi(ik_c_idx)*arma::accu(Cc%arma::conj(Cv)%exp_factor);
    }

    return Q_partial;

  };

  double coeff = (std::pow(constants::q0,2)*pair.i.cnt_obj->Au()*pair.f.cnt_obj->Au())/\
                 (16*std::pow(constants::pi,3)*constants::eps0*pair.i.cnt_obj->radius()*pair.f.cnt_obj->radius()*\
                  std::sqrt(pair.i.cnt_obj->length_in_meter()*pair.f.cnt_obj->length_in_meter()));

  return std::complex<double>(coeff)*std::conj(Q_partial(pair.i))*Q_partial(pair.f);

}

// calculate J()
std::complex<double> exciton_transfer::calculate_J(const matching_states& pair, const std::array<double,2>& shifts_along_axis, const double& z_shift, const double& angle) const
{
  // make position of all atoms in the entire cnt length in 3d space
  auto make_Ru_3d = [](const cnt &m_cnt, const double shift_along_axis, const double z_shift, const double angle) {
    int n_atoms_in_cnt_unit_cell = m_cnt.pos_u_3d().n_rows;
    int total_number_of_atoms = m_cnt.pos_u_3d().n_rows * m_cnt.length_in_cnt_unit_cell();
    arma::mat all_atoms(total_number_of_atoms, 3);

    for (int i = 0; i < m_cnt.length_in_cnt_unit_cell(); i++) {
      all_atoms.rows(i * n_atoms_in_cnt_unit_cell, (i + 1) * n_atoms_in_cnt_unit_cell - 1) = m_cnt.pos_u_3d().each_row() + i * m_cnt.t_vec_3d().t();
    }

    // make the cnt center at the middle
    double y_max = all_atoms.col(1).max();
    double y_min = all_atoms.col(1).min();
    all_atoms.col(1) -= ((y_max + y_min) / 2.);

    // shift the center of the cnt axis along it's axis
    all_atoms.col(1) += shift_along_axis;

    // shift the atoms along the z axis
    all_atoms.col(2) += z_shift;

    // rotate by angle around the z axis
    for (unsigned int i = 0; i < all_atoms.n_rows; i++) {
      double x = all_atoms(i, 0) * std::cos(angle) - all_atoms(i, 1) * std::sin(angle);
      double y = all_atoms(i, 0) * std::sin(angle) + all_atoms(i, 1) * std::cos(angle);
      all_atoms(i, 0) = x;
      all_atoms(i, 1) = y;
    }
    return all_atoms;
  };

  // make position of all atoms in the entire cnt length in 2d space of unrolled cnt
  auto make_Ru_2d = [](const cnt& m_cnt) {
    int n_atoms_in_cnt_unit_cell = m_cnt.pos_u_2d().n_rows;
    int total_number_of_atoms = m_cnt.pos_u_2d().n_rows * m_cnt.length_in_cnt_unit_cell();
    arma::mat all_atoms(total_number_of_atoms,2);

    for (int i=0; i<m_cnt.length_in_cnt_unit_cell(); i++) {
      all_atoms.rows(i*n_atoms_in_cnt_unit_cell,(i+1)*n_atoms_in_cnt_unit_cell-1) = m_cnt.pos_u_2d().each_row() + i * m_cnt.t_vec().t();
    }

    return all_atoms;
  };

  arma::mat i_Ru_3d = make_Ru_3d(*(pair.i.cnt_obj), shifts_along_axis[0], 0, 0);
  arma::mat f_Ru_3d = make_Ru_3d(*(pair.f.cnt_obj), shifts_along_axis[1], z_shift, angle);

  arma::mat i_Ru_2d = make_Ru_2d(*(pair.i.cnt_obj));
  arma::mat f_Ru_2d = make_Ru_2d(*(pair.f.cnt_obj));

  std::complex<double> J = 0;
  const std::complex<double> i1(0,1);

  // prebuild exponential factor for the inner loop
  arma::cx_vec f_exp(f_Ru_2d.n_rows);
  for (unsigned int j=0; j<f_Ru_2d.n_rows; j++) {
    f_exp(j) = std::exp(+i1*arma::dot(pair.f.ik_cm*pair.f.dk_l(),f_Ru_2d.row(j)));
  }

  for (unsigned int i=0; i<i_Ru_2d.n_rows; i++)
  {
    std::complex<double> i_exp = std::exp(-i1*arma::dot(pair.i.ik_cm*pair.i.dk_l(),i_Ru_2d.row(i)));
    for (unsigned int j=0; j<f_Ru_2d.n_rows; j++)
    {
      J += i_exp*f_exp(j)/(arma::norm(i_Ru_3d.row(i)-f_Ru_3d.row(j)));
    }
  }
  return J;
}

// calculate first order transfer rate
double exciton_transfer::first_order(const double& z_shift, const std::array<double,2> axis_shifts, const double& theta, const bool& show_results)
{
  const cnt& donor = *_cnts[0];
  const cnt& acceptor = *_cnts[1];

  double min_energy = donor.A2_singlet().energy.min();

  const cnt::exciton_struct& d_exciton = donor.A2_singlet();
  const cnt::exciton_struct& a_exciton = acceptor.A2_singlet();

  // find lists of relevant states in donor and acceptor excitons
  std::vector<ex_state> d_relevant_states = get_relevant_states(d_exciton,min_energy);
  std::vector<ex_state> a_relevant_states = get_relevant_states(a_exciton,min_energy);

  double Z = 0;
  for (const auto& state:d_relevant_states) {
    Z += std::exp(-state.energy/(constants::kb*_temperature));
  }

  // match the states based on their energy
  std::vector<matching_states> state_pairs = match_states(d_relevant_states, a_relevant_states);

  // std::cout << "number of matching states: " << state_pairs.size() << std::endl;

  // int i=0;

  double transfer_rate = 0;

  progress_bar prog(state_pairs.size(),"calculate first-order exciton transfer rate", !show_results);
  for (const auto& pair:state_pairs) {
    prog.step();
    std::complex<double> Q = calculate_Q(pair);
    std::complex<double> J = calculate_J(pair, axis_shifts, z_shift, theta);
    double M = std::abs(Q * J) / std::sqrt(pair.i.cnt_obj->length_in_meter() * pair.f.cnt_obj->length_in_meter());
    // double M = std::abs(Q * J);

    // std::cout << std::showpos;
    // std::cout << i   << "---------> ";
    // std::cout << "(" << pair.i.ik_cm << "," << pair.f.ik_cm << ")" << " E(i)=" << pair.i.energy << " , E(f)=" << pair.f.energy;
    // std::cout << " , Q = " << std::abs(Q);
    // std::cout << " , J = " << std::abs(J);
    // std::cout << " , M = " << std::abs(M) << std::endl;
    // i++;

    transfer_rate += (2*constants::pi/constants::hb)*(std::exp(-pair.i.energy/(constants::kb*_temperature))/Z)*std::pow(M,2)*lorentzian(pair.i.energy-pair.f.energy);
  }

  if (show_results) {
    std::cout << "\n\n";
    std::cout << "cnt lengths: " << _cnts[0]->length_in_meter()*1.e9 << " [nm], " << _cnts[1]->length_in_meter()*1.e9 << " [nm]\n";
    std::cout << "center to center distance: " << z_shift*1.e9 << " [nm]\n";
    std::cout << "cnt1 radius: " << _cnts[0]->radius()*1.e9 << " [nm], cnt2 radius: " << _cnts[1]->radius()*1.e9 << " [nm]\n";
    std::cout << "wall to wall distance: " << (z_shift - _cnts[0]->radius() - _cnts[1]->radius())*1.e9 << " [nm]\n";
    std::cout << "theta: " << theta/constants::pi*180 << " [degrees]\n";
    std::cout << "axis shifts: " << axis_shifts[0]*1e9 << " [nm] and " << axis_shifts[1]*1e9 << " [nm]\n";
    std::cout << "exciton transfer rate: " << transfer_rate << "\n";
  }

  return transfer_rate;
}

// calculate first order transfer rate for varying angle
void exciton_transfer::calculate_first_order_vs_angle(const arma::vec& angle_vec ,const double& z_shift, const std::array<double,2> axis_shifts) {
  int n_theta = angle_vec.n_elem;
  arma::vec transfer_rate(arma::size(angle_vec), arma::fill::zeros);

  progress_bar prog(n_theta, "first order transfer rate versus angle");
  for (int i=0; i<n_theta; i++)
  {
    prog.step();
    transfer_rate(i) = first_order(z_shift, axis_shifts, angle_vec(i));
  }

  // save the transfer rate
  std::string filename = _directory.path() / "first_order_transfer_rate_vs_angle.dat";
  transfer_rate.save(filename,arma::arma_ascii);

  // save theta vector
  filename = _directory.path() / "first_order_transfer_rate_vs_angle.theta.dat";
  angle_vec.save(filename,arma::arma_ascii);

  std::cout << "\n\n";
  std::cout << "cnt lengths: " << _cnts[0]->length_in_meter()*1.e9 << " [nm], " << _cnts[1]->length_in_meter()*1.e9 << " [nm]\n";
  std::cout << "center to center distance: " << z_shift*1.e9 << " [nm]\n";
  std::cout << "cnt1 radius: " << _cnts[0]->radius()*1.e9 << " [nm], cnt2 radius: " << _cnts[1]->radius()*1.e9 << " [nm]\n";
  std::cout << "wall to wall distance: " << (z_shift - _cnts[0]->radius() - _cnts[1]->radius())*1.e9 << " [nm]\n";
  std::cout << "axis shifts: " << axis_shifts[0]*1e9 << " [nm] and " << axis_shifts[1]*1e9 << " [nm]\n";
  std::cout << "max transfer rate: " << transfer_rate.max()/1e12 << " [1/ps] at " << angle_vec(transfer_rate.index_max())*180/constants::pi << " [degrees]\n";
  std::cout << "min transfer rate: " << transfer_rate.min()/1e12 << " [1/ps] at " << angle_vec(transfer_rate.index_min())*180/constants::pi << " [degrees]\n";
}

// calculate first order transfer rate for center to center distance
void exciton_transfer::calculate_first_order_vs_zshift(const arma::vec& z_shift_vec, const std::array<double,2> axis_shifts, const double& theta) {
  int n = z_shift_vec.n_elem;
  arma::vec transfer_rate(arma::size(z_shift_vec), arma::fill::zeros);

  progress_bar prog(n, "first order transfer rate versus z_shift");
  for (int i=0; i<n; i++)
  {
    prog.step();
    transfer_rate(i) = first_order(z_shift_vec(i), axis_shifts, theta);
  }

  // save the transfer rate
  std::string filename = _directory.path() / "first_order_transfer_rate_vs_zshift.dat";
  transfer_rate.save(filename,arma::arma_ascii);

  // save theta vector
  filename = _directory.path() / "first_order_transfer_rate_vs_zshift.distance.dat";
  z_shift_vec.save(filename,arma::arma_ascii);

  std::cout << "\n\n";
  std::cout << "cnt lengths: " << _cnts[0]->length_in_meter()*1.e9 << " [nm], " << _cnts[1]->length_in_meter()*1.e9 << " [nm]\n";
  std::cout << "cnt1 radius: " << _cnts[0]->radius()*1.e9 << " [nm], cnt2 radius: " << _cnts[1]->radius()*1.e9 << " [nm]\n";
  std::cout << "theta: " << theta*180/constants::pi << " [degrees]\n";
  std::cout << "axis shifts: " << axis_shifts[0]*1e9 << " [nm] and " << axis_shifts[1]*1e9 << " [nm]\n";
  std::cout << "max transfer rate: " << transfer_rate.max() << " [1/s] at " << z_shift_vec(transfer_rate.index_max())*1e9 << " [nm]\n";
  std::cout << "min transfer rate: " << transfer_rate.min() << " [1/s] at " << z_shift_vec(transfer_rate.index_min())*1e9 << " [nm]\n";
}

// calculate first order transfer rate for varying axis shift for initial cnt
void exciton_transfer::calculate_first_order_vs_axis_shift_1(const arma::vec& axis_shift_vec_1, const double axis_shift_2, const double z_shift, const double& theta) {
  int n = axis_shift_vec_1.n_elem;
  arma::vec transfer_rate(arma::size(axis_shift_vec_1), arma::fill::zeros);

  progress_bar prog(n, "first order transfer rate versus axis shift of initial cnt");
  for (int i=0; i<n; i++)
  {
    prog.step();
    std::array<double,2> axis_shifts = {axis_shift_vec_1(i),axis_shift_2};
    transfer_rate(i) = first_order(z_shift, axis_shifts, theta);
  }

  // save the transfer rate
  std::string filename = _directory.path() / "first_order_transfer_rate_vs_axis_shift_1.dat";
  transfer_rate.save(filename,arma::arma_ascii);

  // save theta vector
  filename = _directory.path() / "first_order_transfer_rate_vs_axis_shift_1.shift.dat";
  axis_shift_vec_1.save(filename,arma::arma_ascii);

  std::cout << "\n\n";
  std::cout << "cnt lengths: " << _cnts[0]->length_in_meter()*1.e9 << " [nm], " << _cnts[1]->length_in_meter()*1.e9 << " [nm]\n";
  std::cout << "cnt1 radius: " << _cnts[0]->radius()*1.e9 << " [nm], cnt2 radius: " << _cnts[1]->radius()*1.e9 << " [nm]\n";
  std::cout << "theta: " << theta*180/constants::pi << " [degrees]\n";
  std::cout << "center to center distance: " << z_shift*1.e9 << " [nm]\n";
  std::cout << "max transfer rate: " << transfer_rate.max() << " [1/s] at " << axis_shift_vec_1(transfer_rate.index_max())*1e9 << " [nm]\n";
  std::cout << "min transfer rate: " << transfer_rate.min() << " [1/s] at " << axis_shift_vec_1(transfer_rate.index_min())*1e9 << " [nm]\n";
}

// calculate first order transfer rate for varying axis shift for final cnt
void exciton_transfer::calculate_first_order_vs_axis_shift_2(const arma::vec& axis_shift_vec_2, const double axis_shift_1, const double z_shift, const double& theta) {
  int n = axis_shift_vec_2.n_elem;
  arma::vec transfer_rate(arma::size(axis_shift_vec_2), arma::fill::zeros);

  progress_bar prog(n, "first order transfer rate versus axis shift of final cnt");
  for (int i=0; i<n; i++)
  {
    prog.step();
    std::array<double,2> axis_shifts = {axis_shift_1, axis_shift_vec_2(i)};
    transfer_rate(i) = first_order(z_shift, axis_shifts, theta);
  }

  // save the transfer rate
  std::string filename = _directory.path() / "first_order_transfer_rate_vs_axis_shift_2.dat";
  transfer_rate.save(filename,arma::arma_ascii);

  // save theta vector
  filename = _directory.path() / "first_order_transfer_rate_vs_axis_shift_2.shift.dat";
  axis_shift_vec_2.save(filename,arma::arma_ascii);

  std::cout << "\n\n";
  std::cout << "cnt lengths: " << _cnts[0]->length_in_meter()*1.e9 << " [nm], " << _cnts[1]->length_in_meter()*1.e9 << " [nm]\n";
  std::cout << "cnt1 radius: " << _cnts[0]->radius()*1.e9 << " [nm], cnt2 radius: " << _cnts[1]->radius()*1.e9 << " [nm]\n";
  std::cout << "theta: " << theta*180/constants::pi << " [degrees]\n";
  std::cout << "center to center distance: " << z_shift*1.e9 << " [nm]\n";
  std::cout << "max transfer rate: " << transfer_rate.max() << " [1/s] at " << axis_shift_vec_2(transfer_rate.index_max())*1e9 << " [nm]\n";
  std::cout << "min transfer rate: " << transfer_rate.min() << " [1/s] at " << axis_shift_vec_2(transfer_rate.index_min())*1e9 << " [nm]\n";
}

typedef std::experimental::filesystem::path path_t;
void exciton_transfer::save_atom_locations(path_t path, const std::array<double, 2> &shifts_along_axis, const double &z_shift, const double &angle, std::string prefix) {

  std::cout << "saving 3d atom locations for donor and acceptor cnts...";

  // make position of all atoms in the entire cnt length in 3d space
  auto make_Ru_3d = [](const cnt &m_cnt, const double shift_along_axis, const double z_shift, const double angle) {
    int n_atoms_in_cnt_unit_cell = m_cnt.pos_u_3d().n_rows;
    int total_number_of_atoms = m_cnt.pos_u_3d().n_rows * m_cnt.length_in_cnt_unit_cell();
    arma::mat all_atoms(total_number_of_atoms, 3);

    for (int i = 0; i < m_cnt.length_in_cnt_unit_cell(); i++) {
      all_atoms.rows(i * n_atoms_in_cnt_unit_cell, (i + 1) * n_atoms_in_cnt_unit_cell - 1) = m_cnt.pos_u_3d().each_row() + i*m_cnt.t_vec_3d().t();
    }

    // make the cnt center at the middle
    double y_max = all_atoms.col(1).max();
    double y_min = all_atoms.col(1).min();
    all_atoms.col(1) -= ((y_max + y_min) / 2.);

    // shift the center of the cnt axis along it's axis
    all_atoms.col(1) += shift_along_axis;

    // shift the atoms along the z axis
    all_atoms.col(2) += z_shift;

    // rotate by angle around the z axis
    for (unsigned int i = 0; i < all_atoms.n_rows; i++)
    {
      double x = all_atoms(i, 0) * std::cos(angle) - all_atoms(i, 1) * std::sin(angle);
      double y = all_atoms(i, 0) * std::sin(angle) + all_atoms(i, 1) * std::cos(angle);
      all_atoms(i, 0) = x;
      all_atoms(i, 1) = y;
    }
    return all_atoms;
  };

  const cnt* donor = _cnts[0];
  const cnt* acceptor = _cnts[1];

  arma::mat i_Ru_3d = make_Ru_3d(*donor, shifts_along_axis[0], 0, 0);
  arma::mat f_Ru_3d = make_Ru_3d(*acceptor, shifts_along_axis[1], z_shift, angle);

  path /= "3d_coord"+prefix;

  std::ofstream file;

  file.open(std::string(path) + ".cnt1.dat");
  file << i_Ru_3d;
  file.close();

  file.open(std::string(path) + ".cnt2.dat");
  file << f_Ru_3d;
  file.close();

  std::cout << "done!!!" << std::endl;
}


// Distributed under the MIT License.
// See LICENSE.txt for details.

#include "Evolution/Systems/ScalarAdvection/FiniteDifference/MonotisedCentral.hpp"

#include <array>
#include <boost/functional/hash.hpp>
#include <cstddef>
#include <memory>
#include <pup.h>
#include <utility>
#include <vector>

#include "DataStructures/DataBox/Prefixes.hpp"
#include "DataStructures/FixedHashMap.hpp"
#include "DataStructures/Index.hpp"
#include "DataStructures/Tensor/IndexType.hpp"
#include "DataStructures/Variables.hpp"
#include "Domain/Structure/Direction.hpp"
#include "Domain/Structure/Element.hpp"
#include "Domain/Structure/ElementId.hpp"
#include "Domain/Structure/MaxNumberOfNeighbors.hpp"
#include "Domain/Structure/Side.hpp"
#include "Evolution/Systems/ScalarAdvection/FiniteDifference/ReconstructWork.tpp"
#include "Evolution/Systems/ScalarAdvection/FiniteDifference/Reconstructor.hpp"
#include "Evolution/Systems/ScalarAdvection/Tags.hpp"
#include "NumericalAlgorithms/FiniteDifference/MonotisedCentral.hpp"
#include "NumericalAlgorithms/Spectral/Mesh.hpp"
#include "Utilities/GenerateInstantiations.hpp"
#include "Utilities/Gsl.hpp"
#include "Utilities/TMPL.hpp"

namespace ScalarAdvection::fd {
template <size_t Dim>
MonotisedCentral<Dim>::MonotisedCentral(CkMigrateMessage* const msg)
    : Reconstructor<Dim>(msg) {}

template <size_t Dim>
std::unique_ptr<Reconstructor<Dim>> MonotisedCentral<Dim>::get_clone() const {
  return std::make_unique<MonotisedCentral>(*this);
}

template <size_t Dim>
void MonotisedCentral<Dim>::pup(PUP::er& p) {
  Reconstructor<Dim>::pup(p);
}

template <size_t Dim>
PUP::able::PUP_ID MonotisedCentral<Dim>::my_PUP_ID = 0;

template <size_t Dim>
template <typename TagsList>
void MonotisedCentral<Dim>::reconstruct(
    const gsl::not_null<std::array<Variables<TagsList>, Dim>*>
        vars_on_lower_face,
    const gsl::not_null<std::array<Variables<TagsList>, Dim>*>
        vars_on_upper_face,
    const Variables<tmpl::list<Tags::U>>& volume_vars,
    const Element<Dim>& element,
    const FixedHashMap<
        maximum_number_of_neighbors(Dim),
        std::pair<Direction<Dim>, ElementId<Dim>>, std::vector<double>,
        boost::hash<std::pair<Direction<Dim>, ElementId<Dim>>>>& neighbor_data,
    const Mesh<Dim>& subcell_mesh) const {
  reconstruct_work(
      vars_on_lower_face, vars_on_upper_face,
      [](auto upper_face_vars_ptr, auto lower_face_vars_ptr,
         const auto& volume_variables, const auto& ghost_cell_vars,
         const auto& subcell_extents, const size_t number_of_variables) {
        ::fd::reconstruction::monotised_central(
            upper_face_vars_ptr, lower_face_vars_ptr, volume_variables,
            ghost_cell_vars, subcell_extents, number_of_variables);
      },
      volume_vars, element, neighbor_data, subcell_mesh, ghost_zone_size());
}

template <size_t Dim>
template <typename TagsList>
void MonotisedCentral<Dim>::reconstruct_fd_neighbor(
    const gsl::not_null<Variables<TagsList>*> vars_on_face,
    const Variables<tmpl::list<Tags::U>>& volume_vars,
    const Element<Dim>& element,
    const FixedHashMap<
        maximum_number_of_neighbors(Dim),
        std::pair<Direction<Dim>, ElementId<Dim>>, std::vector<double>,
        boost::hash<std::pair<Direction<Dim>, ElementId<Dim>>>>& neighbor_data,
    const Mesh<Dim>& subcell_mesh,
    const Direction<Dim> direction_to_reconstruct) const {
  reconstruct_fd_neighbor_work(
      vars_on_face,
      [](const auto tensor_component_on_face_ptr,
         const auto& tensor_component_volume,
         const auto& tensor_component_neighbor, const auto& subcell_extents,
         const auto& ghost_data_extents,
         const auto& local_direction_to_reconstruct) {
        ::fd::reconstruction::reconstruct_neighbor<
            Side::Lower,
            ::fd::reconstruction::detail::MonotisedCentralReconstructor>(
            tensor_component_on_face_ptr, tensor_component_volume,
            tensor_component_neighbor, subcell_extents, ghost_data_extents,
            local_direction_to_reconstruct);
      },
      [](const auto tensor_component_on_face_ptr,
         const auto& tensor_component_volume,
         const auto& tensor_component_neighbor, const auto& subcell_extents,
         const auto& ghost_data_extents,
         const auto& local_direction_to_reconstruct) {
        ::fd::reconstruction::reconstruct_neighbor<
            Side::Upper,
            ::fd::reconstruction::detail::MonotisedCentralReconstructor>(
            tensor_component_on_face_ptr, tensor_component_volume,
            tensor_component_neighbor, subcell_extents, ghost_data_extents,
            local_direction_to_reconstruct);
      },
      volume_vars, element, neighbor_data, subcell_mesh,
      direction_to_reconstruct, ghost_zone_size());
}

template <size_t Dim>
bool operator==(const MonotisedCentral<Dim>& /*lhs*/,
                const MonotisedCentral<Dim>& /*rhs*/) {
  return true;
}

template <size_t Dim>
bool operator!=(const MonotisedCentral<Dim>& lhs,
                const MonotisedCentral<Dim>& rhs) {
  return not(lhs == rhs);
}

#define DIM(data) BOOST_PP_TUPLE_ELEM(0, data)
#define TAGS_LIST(data)                                                       \
  tmpl::list<Tags::U,                                                         \
             ::Tags::Flux<Tags::U, tmpl::size_t<DIM(data)>, Frame::Inertial>, \
             Tags::VelocityField<DIM(data)>>

#define INSTANTIATION(r, data)                                                \
  template class MonotisedCentral<DIM(data)>;                                 \
  template bool operator==                                                    \
      <DIM(data)>(const MonotisedCentral<DIM(data)>& /*lhs*/,                 \
                  const MonotisedCentral<DIM(data)>& /*rhs*/);                \
  template bool operator!=<DIM(data)>(const MonotisedCentral<DIM(data)>& lhs, \
                                      const MonotisedCentral<DIM(data)>& rhs);

GENERATE_INSTANTIATIONS(INSTANTIATION, (1, 2))
#undef INSTANTIATION

#define INSTANTIATION(r, data)                                                 \
  template void MonotisedCentral<DIM(data)>::reconstruct(                      \
      gsl::not_null<std::array<Variables<TAGS_LIST(data)>, DIM(data)>*>        \
          vars_on_lower_face,                                                  \
      gsl::not_null<std::array<Variables<TAGS_LIST(data)>, DIM(data)>*>        \
          vars_on_upper_face,                                                  \
      const Variables<tmpl::list<Tags::U>>& volume_vars,                       \
      const Element<DIM(data)>& element,                                       \
      const FixedHashMap<                                                      \
          maximum_number_of_neighbors(DIM(data)),                              \
          std::pair<Direction<DIM(data)>, ElementId<DIM(data)>>,               \
          std::vector<double>,                                                 \
          boost::hash<std::pair<Direction<DIM(data)>, ElementId<DIM(data)>>>>& \
          neighbor_data,                                                       \
      const Mesh<DIM(data)>& subcell_mesh) const;                              \
  template void MonotisedCentral<DIM(data)>::reconstruct_fd_neighbor(          \
      gsl::not_null<Variables<TAGS_LIST(data)>*> vars_on_face,                 \
      const Variables<tmpl::list<Tags::U>>& volume_vars,                       \
      const Element<DIM(data)>& element,                                       \
      const FixedHashMap<                                                      \
          maximum_number_of_neighbors(DIM(data)),                              \
          std::pair<Direction<DIM(data)>, ElementId<DIM(data)>>,               \
          std::vector<double>,                                                 \
          boost::hash<std::pair<Direction<DIM(data)>, ElementId<DIM(data)>>>>& \
          neighbor_data,                                                       \
      const Mesh<DIM(data)>& subcell_mesh,                                     \
      const Direction<DIM(data)> direction_to_reconstruct) const;

GENERATE_INSTANTIATIONS(INSTANTIATION, (1, 2))

#undef INSTANTIATION
#undef TAGS_LIST
#undef DIM
}  // namespace ScalarAdvection::fd