#ifndef ELASTY_FEM_HPP
#define ELASTY_FEM_HPP

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

// References:
//
// [1] Eftychios Sifakis and Jernej Barbic. 2012. FEM simulation of 3D deformable solids: a practitioner's guide to
// theory, discretization and model reduction. In ACM SIGGRAPH 2012 Courses (SIGGRAPH '12). Association for Computing
// Machinery, New York, NY, USA, Article 20, 1–50. DOI:https://doi.org/10.1145/2343483.2343501
//
// [2] Theodore Kim and David Eberle. 2020. Dynamic deformables: implementation and production practicalities. In ACM
// SIGGRAPH 2020 Courses (SIGGRAPH '20). Association for Computing Machinery, New York, NY, USA, Article 23, 1–182.
// DOI:https://doi.org/10.1145/3388769.3407490

namespace elasty::fem
{
    /// \brief Extract the rotational part of the given square matrix by performing polar decomposion.
    ///
    /// \details This implementation is based on SVD. It checks the determinant to avoid any reflection.
    template <typename Derived>
    Eigen::Matrix<typename Derived::Scalar, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime>
    extractRotation(const Eigen::MatrixBase<Derived>& F)
    {
        const auto svd   = F.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
        const auto Sigma = svd.singularValues();
        const auto U     = svd.matrixU();
        const auto V     = svd.matrixV();
        const auto R     = (U * V.transpose()).eval();

        assert(std::abs(std::abs(R.determinant()) - 1.0) < 1e-04);

        if constexpr (Derived::RowsAtCompileTime == 2)
        {
            // Just ignore reflection (if any)
            // TODO: Discuss whether this is a good strategy, or not
            return R;
        }
        else if constexpr (Derived::RowsAtCompileTime == 3)
        {
            // Correct reflection (if any)
            return (R.determinant() > 0) ? R : -R;
        }
    }

    /// \brief Calculate the first Lame parameter, $\lambda$.
    ///
    /// \details Reference: [1]
    template <typename Scalar> constexpr Scalar calcFirstLame(const Scalar youngs_modulus, const Scalar poisson_ratio)
    {
        return youngs_modulus * poisson_ratio / ((1.0 + poisson_ratio) * (1.0 - 2.0 * poisson_ratio));
    }

    /// \brief Calculate the second Lame parameter, $\mu$.
    ///
    /// \details Reference: [1]
    template <typename Scalar> constexpr Scalar calcSecondLame(const Scalar youngs_modulus, const Scalar poisson_ratio)
    {
        return youngs_modulus / (2.0 * (1.0 + poisson_ratio));
    }

    /// \brief Calculate either the "deformed" shape matrix (D_s) or "reference" shape matrix (D_m) of a triangle.
    ///
    /// \param x_0 A 2D vector.
    ///
    /// \param x_1 A 2D vector.
    ///
    /// \param x_2 A 2D vector.
    ///
    /// \details Reference: [1]
    template <typename Derived>
    Eigen::Matrix<typename Derived::Scalar, 2, 2> calc2dShapeMatrix(const Eigen::MatrixBase<Derived>& x_0,
                                                                    const Eigen::MatrixBase<Derived>& x_1,
                                                                    const Eigen::MatrixBase<Derived>& x_2)
    {
        using Mat = Eigen::Matrix<typename Derived::Scalar, 2, 2>;

        Mat shape_matrix;
        shape_matrix.col(0) = x_1 - x_0;
        shape_matrix.col(1) = x_2 - x_0;

        return shape_matrix;
    }

    /// \brief Calculate either the "deformed" shape matrix (D_s) or "reference" shape matrix (D_m) of a tetrahedron.
    ///
    /// \param x_0 A 3D vector.
    ///
    /// \param x_1 A 3D vector.
    ///
    /// \param x_2 A 3D vector.
    ///
    /// \param x_3 A 3D vector.
    ///
    /// \details Reference: [1]
    template <typename Derived>
    Eigen::Matrix<typename Derived::Scalar, 3, 3> calc3dShapeMatrix(const Eigen::MatrixBase<Derived>& x_0,
                                                                    const Eigen::MatrixBase<Derived>& x_1,
                                                                    const Eigen::MatrixBase<Derived>& x_2,
                                                                    const Eigen::MatrixBase<Derived>& x_3)
    {
        using Mat = Eigen::Matrix<typename Derived::Scalar, 3, 3>;

        Mat shape_matrix;
        shape_matrix.col(0) = x_1 - x_0;
        shape_matrix.col(1) = x_2 - x_0;
        shape_matrix.col(2) = x_3 - x_0;

        return shape_matrix;
    }

    /// \brief Calculate the area of a triangle in 2D
    template <typename Derived>
    typename Derived::Scalar calc2dTriangleArea(const Eigen::MatrixBase<Derived>& x_0,
                                                const Eigen::MatrixBase<Derived>& x_1,
                                                const Eigen::MatrixBase<Derived>& x_2)
    {
        const auto r_1 = x_1 - x_0;
        const auto r_2 = x_2 - x_0;

        return 0.5 * std::abs(r_1(0) * r_2(1) - r_2(0) * r_1(1));
    }

    /// \brief Calculate the volume of a tetrahedron
    template <typename Derived>
    typename Derived::Scalar calcTetrahedronVolume(const Eigen::MatrixBase<Derived>& x_0,
                                                   const Eigen::MatrixBase<Derived>& x_1,
                                                   const Eigen::MatrixBase<Derived>& x_2,
                                                   const Eigen::MatrixBase<Derived>& x_3)
    {
        const auto r_1 = x_1 - x_0;
        const auto r_2 = x_2 - x_0;
        const auto r_3 = x_3 - x_0;

        return std::abs(r_1.dot(r_2.cross(r_3))) / 6.0;
    }

    /// \brief Calculate the diagonal elements of the lumped mass matrix.
    ///
    /// \details This function takes the "barycentric" approach. See https://www.alecjacobson.com/weblog/?p=1146 .
    template <typename DerivedV, typename DerivedF>
    Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, 1>
    calcTriangleMeshLumpedMass(const Eigen::MatrixBase<DerivedV>& verts,
                               const Eigen::MatrixBase<DerivedF>& elems,
                               const typename DerivedV::Scalar    total_mass)
    {
        using Scalar = typename DerivedV::Scalar;
        using Vec    = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;

        assert(verts.cols() == 1);
        assert(verts.size() % 2 == 0);
        assert(elems.rows() == 3);

        const auto num_verts = verts.size() / 2;

        Scalar total_area = 0;
        Vec    masses     = Vec::Zero(verts.size());

        for (std::size_t elem_index = 0; elem_index < elems.cols(); ++elem_index)
        {
            const auto& indices = elems.col(elem_index);

            const Scalar area = calc2dTriangleArea(verts.template segment<2>(2 * indices[0]),
                                                   verts.template segment<2>(2 * indices[1]),
                                                   verts.template segment<2>(2 * indices[2]));

            const Scalar one_third_area = (1.0 / 3.0) * area;

            masses(2 * indices[0] + 0) += one_third_area;
            masses(2 * indices[0] + 1) += one_third_area;
            masses(2 * indices[1] + 0) += one_third_area;
            masses(2 * indices[1] + 1) += one_third_area;
            masses(2 * indices[2] + 0) += one_third_area;
            masses(2 * indices[2] + 1) += one_third_area;

            total_area += area;
        }

        assert(total_mass > 0);
        assert(total_area > 0);

        return (total_mass / total_area) * masses;
    }

    /// \brief Calculate the diagonal elements of the lumped mass matrix.
    template <typename DerivedV, typename DerivedF>
    Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, 1>
    calcTetraMeshLumpedMass(const Eigen::MatrixBase<DerivedV>& verts,
                            const Eigen::MatrixBase<DerivedF>& elems,
                            const typename DerivedV::Scalar    total_mass)
    {
        using Scalar = typename DerivedV::Scalar;
        using Vec    = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;

        assert(verts.cols() == 1);
        assert(verts.size() % 3 == 0);
        assert(elems.rows() == 4);

        const auto num_verts = verts.size() / 3;
        const auto num_elems = elems.cols();

        Scalar total_vol = 0;
        Vec    masses    = Vec::Zero(verts.size());

        for (std::size_t elem_index = 0; elem_index < num_elems; ++elem_index)
        {
            const auto& indices = elems.col(elem_index);

            const Scalar vol = calcTetrahedronVolume(verts.template segment<3>(3 * indices[0]),
                                                     verts.template segment<3>(3 * indices[1]),
                                                     verts.template segment<3>(3 * indices[2]),
                                                     verts.template segment<3>(3 * indices[3]));

            const Scalar one_fourth_vol = (1.0 / 4.0) * vol;

            masses.template segment<3>(3 * indices[0]) += one_fourth_vol * Eigen::Vector3d::Ones();
            masses.template segment<3>(3 * indices[1]) += one_fourth_vol * Eigen::Vector3d::Ones();
            masses.template segment<3>(3 * indices[2]) += one_fourth_vol * Eigen::Vector3d::Ones();
            masses.template segment<3>(3 * indices[3]) += one_fourth_vol * Eigen::Vector3d::Ones();

            total_vol += vol;
        }

        assert(total_mass > 0);
        assert(total_vol > 0);
        assert(masses.minCoeff() > 0.0);

        return (total_mass / total_vol) * masses;
    }

    /// \brief Calculate the Green strain tensor for a finite element.
    ///
    /// \param deform_grad The deformation gradient matrix, which should be either 2-by-2 (2D element in 2D), 3-by-3 (3D
    /// element in 3D), or 3-by-2 (2D element in 3D).
    template <typename Derived>
    Eigen::Matrix<typename Derived::Scalar, Derived::ColsAtCompileTime, Derived::ColsAtCompileTime>
    calcGreenStrain(const Eigen::MatrixBase<Derived>& deform_grad)
    {
        using Mat = Eigen::Matrix<typename Derived::Scalar, Derived::ColsAtCompileTime, Derived::ColsAtCompileTime>;

        return 0.5 * (deform_grad.transpose() * deform_grad - Mat::Identity());
    }

    /// \details Eq. 3.4 in [1]
    template <typename Derived>
    typename Derived::Scalar calcCoRotationalEnergyDensity(const Eigen::MatrixBase<Derived>& deform_grad,
                                                           const typename Derived::Scalar    first_lame,
                                                           const typename Derived::Scalar    second_lame)
    {
        using Mat = Eigen::Matrix<typename Derived::Scalar, Derived::ColsAtCompileTime, Derived::ColsAtCompileTime>;

        const auto R     = extractRotation(deform_grad);
        const auto S     = R.transpose() * deform_grad;
        const auto I     = Mat::Identity();
        const auto trace = (S - I).trace();

        assert(deform_grad.isApprox(R * S));

        return second_lame * (deform_grad - R).squaredNorm() + 0.5 * first_lame * trace * trace;
    }

    /// \details Eq. 3.5 in [1]
    template <typename Derived>
    Eigen::Matrix<typename Derived::Scalar, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime>
    calcCoRotationalPiolaStress(const Eigen::MatrixBase<Derived>& deform_grad,
                                const typename Derived::Scalar    first_lame,
                                const typename Derived::Scalar    second_lame)
    {
        using Mat = Eigen::Matrix<typename Derived::Scalar, Derived::ColsAtCompileTime, Derived::ColsAtCompileTime>;

        const auto R     = extractRotation(deform_grad);
        const auto S     = R.transpose() * deform_grad;
        const auto I     = Mat::Identity();
        const auto trace = (S - I).trace();

        assert(deform_grad.isApprox(R * S));

        return 2.0 * second_lame * (deform_grad - R) + first_lame * trace * R;
    }

    /// \details The first equation in Sec. 3.3 in [1]
    template <typename Derived>
    typename Derived::Scalar calcStVenantKirchhoffEnergyDensity(const Eigen::MatrixBase<Derived>& deform_grad,
                                                                const typename Derived::Scalar    first_lame,
                                                                const typename Derived::Scalar    second_lame)
    {
        const auto E     = calcGreenStrain(deform_grad);
        const auto trace = E.trace();

        return second_lame * E.squaredNorm() + 0.5 * first_lame * trace * trace;
    }

    /// \details Eq. 3.3 in [1]
    template <typename Derived>
    Eigen::Matrix<typename Derived::Scalar, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime>
    calcStVenantKirchhoffPiolaStress(const Eigen::MatrixBase<Derived>& deform_grad,
                                     const typename Derived::Scalar    first_lame,
                                     const typename Derived::Scalar    second_lame)
    {
        const auto E = calcGreenStrain(deform_grad);

        return 2.0 * second_lame * deform_grad * E + first_lame * E.trace() * deform_grad;
    }

    template <typename DerivedVec, typename DerivedMat>
    Eigen::Matrix<typename DerivedVec::Scalar, 2, 2>
    calc2dTriangleDeformGrad(const Eigen::MatrixBase<DerivedVec>& x_0,
                             const Eigen::MatrixBase<DerivedVec>& x_1,
                             const Eigen::MatrixBase<DerivedVec>& x_2,
                             const Eigen::MatrixBase<DerivedMat>& rest_shape_mat_inv)
    {
        const auto D_s = elasty::fem::calc2dShapeMatrix(x_0, x_1, x_2);
        const auto F   = D_s * rest_shape_mat_inv;

        return F;
    }

    template <typename DerivedVec, typename DerivedMat>
    Eigen::Matrix<typename DerivedVec::Scalar, 3, 3>
    calcTetrahedronDeformGrad(const Eigen::MatrixBase<DerivedVec>& x_0,
                              const Eigen::MatrixBase<DerivedVec>& x_1,
                              const Eigen::MatrixBase<DerivedVec>& x_2,
                              const Eigen::MatrixBase<DerivedVec>& x_3,
                              const Eigen::MatrixBase<DerivedMat>& rest_shape_mat_inv)
    {
        const auto D_s = elasty::fem::calc3dShapeMatrix(x_0, x_1, x_2, x_3);
        const auto F   = D_s * rest_shape_mat_inv;

        return F;
    }

    /// \brief Calculate analytic partial derivatives of the deformation gradient $\mathbf{F}$ with respect to the
    /// vertex positions $\mathbf{x}$ (i.e., $\frac{\partial \mathbf{F}}{\partial \mathbf{x}}$) and return it in the
    /// "flattened" format.
    ///
    /// \details The result is a 2-by-2-by-6 third-order tensor but flattened into a 4-by-6 matrix.
    ///
    /// Reference: [2, Appendix D]
    template <typename Derived>
    Eigen::Matrix<typename Derived::Scalar, 4, 6>
    calcVecTrianglePartDeformGradPartPos(const Eigen::MatrixBase<Derived>& rest_shape_mat_inv)
    {
        using Scalar = typename Derived::Scalar;

        // Analytics partial derivatives $\frac{\partial \mathbf{D}_\text{s}{\partial x_{i}}$
        Eigen::Matrix<Scalar, 2, 2> PDsPx[6];

        // x[0] (= x_0)
        PDsPx[0] << -1.0, -1.0, 0.0, 0.0;

        // x[1] (= y_0)
        PDsPx[1] << 0.0, 0.0, -1.0, -1.0;

        // x[2] (= x_1)
        PDsPx[2] << 1.0, 0.0, 0.0, 0.0;

        // x[3] (= y_1)
        PDsPx[3] << 0.0, 0.0, 1.0, 0.0;

        // x[4] (= x_2)
        PDsPx[4] << 0.0, 1.0, 0.0, 0.0;

        // x[5] (= y_2)
        PDsPx[5] << 0.0, 0.0, 0.0, 1.0;

        Eigen::Matrix<Scalar, 4, 6> vec_PFPx;
        for (std::size_t i = 0; i < 6; ++i)
        {
            const Eigen::Matrix<Scalar, 2, 2> PFPx_i = PDsPx[i] * rest_shape_mat_inv;

            vec_PFPx.col(i) = Eigen::Map<const Eigen::Matrix<Scalar, 4, 1>>(PFPx_i.data(), PFPx_i.size());
        }

        return vec_PFPx;
    }

    /// \brief Calculate analytic partial derivatives of the deformation gradient $\mathbf{F}$ with respect to the
    /// vertex positions $\mathbf{x}$ (i.e., $\frac{\partial \mathbf{F}}{\partial \mathbf{x}}$) and return it in the
    /// "flattened" format.
    ///
    /// \details The result is a 3-by-3-by-12 third-order tensor but flattened into a 9-by-12 matrix.
    ///
    /// Reference: [2, Appendix D]
    template <typename Derived>
    Eigen::Matrix<typename Derived::Scalar, 9, 12>
    calcVecTetrahedronPartDeformGradPartPos(const Eigen::MatrixBase<Derived>& rest_shape_mat_inv)
    {
        using Scalar = typename Derived::Scalar;

        // Analytics partial derivatives $\frac{\partial \mathbf{D}_\text{s}{\partial x_{i}}$
        Eigen::Matrix<Scalar, 3, 3> PDsPx[12];

        PDsPx[0] << -1.0, -1.0, -1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
        PDsPx[1] << 0.0, 0.0, 0.0, -1.0, -1.0, -1.0, 0.0, 0.0, 0.0;
        PDsPx[2] << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.0, -1.0, -1.0;

        PDsPx[3] << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
        PDsPx[4] << 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0;
        PDsPx[5] << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0;

        PDsPx[6] << 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
        PDsPx[7] << 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0;
        PDsPx[8] << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0;

        PDsPx[9] << 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
        PDsPx[10] << 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0;
        PDsPx[11] << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0;

        Eigen::Matrix<Scalar, 9, 12> vec_PFPx;
        for (std::size_t i = 0; i < 12; ++i)
        {
            const Eigen::Matrix<Scalar, 3, 3> PFPx_i = PDsPx[i] * rest_shape_mat_inv;

            vec_PFPx.col(i) = Eigen::Map<const Eigen::Matrix<Scalar, 9, 1>>(PFPx_i.data(), PFPx_i.size());
        }

        return vec_PFPx;
    }
} // namespace elasty::fem

#endif // ELASTY_FEM_HPP


#include <d3dx9.h>
#include <d3d9.h>

#include <iostream>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>

typedef boost::numeric::ublas::vector<float>  Vector3;
typedef boost::numeric::ublas::matrix<float>  Matrix44;

int main()
{
	Vector3  vector(4);
	Matrix44 matrix(4, 4);
	D3DXVECTOR3 dVector3;
	D3DXMATRIX dMatrix44;

	for (int i = 0; i < vector.size(); ++i) {
		vector(i) = i;
	}

	for (int i = 0; i < 4; ++i) {
		for (int j = 0; j < 4; ++j) {
			matrix(i, j) = 4 * i + j;
			dMatrix44.m[i][j] = matrix(i, j);
		}
	}

	std::cout << vector << std::endl;
	std::cout << matrix << std::endl;
	//std::cout << dMatrix44 << std::endl;
	for (int i = 0; i < 4; ++i) {
		for (int j = 0; j < 4; ++j) {
			std::cout << dMatrix44.m[i][j] << " ";
		}
	}
	std::cout << std::endl;

	Vector3 multipul = boost::numeric::ublas::prod(matrix, vector);
	//Vector3 multipul = boost::numeric::ublas::prod(vector, matrix);

	std::cout << multipul << std::endl;

	return 0;
}


/*
Copyright (c) 2018 Inverse Palindrome
Apophis - Conversions.hpp
InversePalindrome.com
*/

#pragma once

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

namespace Conversions
{
    template<typename T>
    T degreesToRadians(T degrees)
    {
        return degrees * boost::math::constants::pi<T>() / (T)180;
    }

    template<typename T>
    T radiansToDegrees(T radians)
    {
        return radians * (T)180 / boost::math::constants::pi<T>();
    }
}


//
//  GrowthHelper.hpp
//  Elasticity
//
//  Created by Wim van Rees on 8/17/16.
//  Copyright © 2016 Wim van Rees. All rights reserved.
//

#ifndef GrowthHelper_hpp
#define GrowthHelper_hpp

#include "common.hpp"
#include "TriangleInfo.hpp"

#include <Eigen/Eigenvalues>
#include <unsupported/Eigen/MatrixFunctions>

class DecomposedGrowthState
{
protected:
    Real s1, s2;
    Eigen::Vector3d v1, v2;

    void decompose_metric(const Eigen::MatrixXd & rxy, const Eigen::Matrix2d & metric)
    {
        // decompute metric
        const Eigen::Matrix2d a_i = rxy.transpose() * rxy;

        // compute delta_metric
        const Eigen::Matrix2d delta_a = a_i.inverse() * metric;

        // eigensolver
        Eigen::EigenSolver<Eigen::Matrix2d> es(delta_a);
        const Eigen::Vector2d evals = es.eigenvalues().real();
        const Eigen::Matrix2d evecs = es.eigenvectors().real();

        // evecs is the V-matrix (generalized eigen-vectors) but not appropriately normalized yet -- normalize
        // const Eigen::Matrix2d shouldBeI = evecs.transpose() * a_i * evecs;
        // const Eigen::Matrix2d V = evecs * shouldBeI.inverse().sqrt();

        const Eigen::MatrixXd v12_evec = rxy * evecs;
        Eigen::Vector3d v1_evec = v12_evec.col(0).normalized();
        Eigen::Vector3d v2_evec = v12_evec.col(1).normalized();

        // note: for everything else here we assume that v1 and v2 are orthogonal
        // however if s1==s2 --> the values of v1 and v2 are arbitrary and so from whatever we do above, they might turn out to not be orthogonal
        // so, in this case we orthogonalize them by hand (they are already normalized)
        // but since we can still have them equal, instead we set them equal to rxy_1 and rxy_2 first and orthogonalize second
        if(std::abs(evals(0) - evals(1)) < 1e-12)
        {
            v1_evec = rxy.col(0);
            v2_evec = rxy.col(1);

            v1_evec.normalize();
            v2_evec = v2_evec - v1_evec.dot(v2_evec)*v1_evec;
            v2_evec.normalize();
        }

        // finally we assign
        s1 = std::sqrt(evals(0));
        s2 = std::sqrt(evals(1));
        v1 = v1_evec;
        v2 = v2_evec;

        if(s2 > s1) // make sure that s1 is the biggest (does not matter really, just convention -- will still give the same matrix)
        {
            std::swap(s1,s2);
            v1.swap(v2);
        }
#ifndef NDEBUG
        if(not checkVectorValidity())
        {
            std::cout << "decomposition is not valid : the eigenvectors are not unit length and/or not orthogonal" << std::endl;
            print();
            printf("computed eigenvalues are %10.10e \t %10.10e\n", evals(0), evals(1));
            printf("computed evec1 is %10.10e \t %10.10e\n", evecs(0,0), evecs(1,0));
            printf("computed evec2 is %10.10e \t %10.10e\n", evecs(0,1), evecs(1,1));
            printf("input rxy_1 is %10.10e \t %10.10e \t %10.10e\n", rxy(0,0), rxy(1,0), rxy(2,0));
            printf("input rxy_2 is %10.10e \t %10.10e \t %10.10e\n", rxy(0,1), rxy(1,1), rxy(2,1));
            printf("input metric is %10.10e \t %10.10e \t %10.10e\n", metric(0,0), metric(0,1), metric(1,1));

            assert(checkVectorValidity());
        }
#endif
    }

public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW // https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html

    DecomposedGrowthState(const Real s1_in, const Real s2_in, const Eigen::Vector3d & v1_in, const Eigen::Vector3d & v2_in):
    s1(s1_in),
    s2(s2_in),
    v1(v1_in),
    v2(v2_in)
    {}

    DecomposedGrowthState(const Eigen::MatrixXd & rxy, const Eigen::Matrix2d & metric)
    {
        decompose_metric(rxy, metric);
    }

    void changeMetric(const Eigen::MatrixXd & rxy, const Eigen::Matrix2d & metric)
    {
        decompose_metric(rxy, metric);
    }

    bool checkVectorValidity(const Real tol = 1e-9) const
    {
        const bool unit_v1 = std::abs(v1.dot(v1) - 1) < tol;
        const bool unit_v2 = std::abs(v2.dot(v2) - 1) < tol;
        const bool ortho = std::abs(v1.dot(v2)) < tol;
        return (unit_v1 and unit_v2 and ortho);
    }

    void print() const
    {
        printf("s1/s2 : %10.10e \t %10.10e \t\t v1 : %10.10e, %10.10e, %10.10e \t v2 : %10.10e, %10.10e, %10.10e\n",s1,s2,v1(0),v1(1),v1(2),v2(0),v2(1),v2(2));
    }

    Eigen::Matrix2d computeMetric(const Eigen::MatrixXd & rxy) const
    {
        const Eigen::Matrix2d a_i = rxy.transpose() * rxy;
        const Eigen::Matrix2d Lsq = (Eigen::Matrix2d() << s1*s1,0,0,s2*s2).finished();
        Eigen::MatrixXd v12(3,2);
        v12 << v1,v2;
        const Eigen::Matrix2d V = (v12.transpose() * rxy).inverse();
        const Eigen::Matrix2d a_f = a_i * V * Lsq * V.transpose() * a_i;
        return a_f;
    }

    Real get_s1() const {return s1;}
    Real get_s2() const {return s2;}
    Eigen::Vector3d get_v1() const {return v1;}
    Eigen::Vector3d get_v2() const {return v2;}
};

class GrowthState
{
protected:
    const Eigen::MatrixXd rxy_base;
    const Eigen::Matrix2d a_final;
    const DecomposedGrowthState decomposed_final;
    Eigen::Matrix2d a_init;
    DecomposedGrowthState decomposed_init;

public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW // https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html

    GrowthState(const Eigen::MatrixXd & rxy_base_in, const Eigen::Matrix2d & a_final_in):
    rxy_base(rxy_base_in),
    a_final(a_final_in),
    decomposed_final(rxy_base, a_final),
    a_init(rxy_base.transpose() * rxy_base),
    decomposed_init(rxy_base, a_init)
    {
    }

    GrowthState(const Eigen::MatrixXd & rxy_base_in, const Eigen::Matrix2d & a_init_in, const Eigen::Matrix2d & a_final_in):
    rxy_base(rxy_base_in),
    a_final(a_final_in),
    decomposed_final(rxy_base, a_final),
    a_init(a_init_in),
    decomposed_init(rxy_base, a_init)
    {
    }

    void changeInitialState(const Eigen::Matrix2d & a_init_in)
    {
        a_init = a_init_in;
        decomposed_init.changeMetric(rxy_base, a_init);
    }

    Eigen::Matrix2d interpolate_from_iso(const Real t) const
    {
        // we start from isotropic : just interpolate the growth factors
        const Real s1_interp = (1.0 - t) * decomposed_init.get_s1() + t*decomposed_final.get_s1();
        const Real s2_interp = (1.0 - t) * decomposed_init.get_s2() + t*decomposed_final.get_s2();

        const DecomposedGrowthState decomposed_intermediate(s1_interp, s2_interp, decomposed_final.get_v1(), decomposed_final.get_v2());
        return decomposed_intermediate.computeMetric(rxy_base);
    }

    Eigen::Matrix2d interpolate_from_iso_logeucl(const Real t) const
    {
        // we start from isotropic : just interpolate the growth factors
        const Real s1_interp = std::exp( (1.0 - t) * std::log(decomposed_init.get_s1()) + t*std::log(decomposed_final.get_s1()) );
        const Real s2_interp = std::exp( (1.0 - t) * std::log(decomposed_init.get_s2()) + t*std::log(decomposed_final.get_s2()) );

        const DecomposedGrowthState decomposed_intermediate(s1_interp, s2_interp, decomposed_final.get_v1(), decomposed_final.get_v2());
        return decomposed_intermediate.computeMetric(rxy_base);
    }

    Eigen::Matrix2d interpolate(const Real t) const
    {
        if(std::abs(decomposed_init.get_s1() - decomposed_init.get_s2()) < 1e-12) return interpolate_from_iso(t);

        // t between 0 and 1
        return (1.0 - t)*a_init + t*a_final;
    }

    Eigen::Matrix2d interpolateLogEucl(const Real t) const
    {
        if(std::abs(decomposed_init.get_s1() - decomposed_init.get_s2()) < 1e-12) return interpolate_from_iso_logeucl(t);

        // t between 0 and 1
        return ((1.0 - t)*a_init.log() + t * a_final.log()).exp();
    }

    Eigen::Matrix2d getInitGrowthMetric() const
    {
        return a_init;
    }

    Eigen::Matrix2d getFinalGrowthMetric() const
    {
        return a_final;
    }

    const DecomposedGrowthState & getDecomposedInitState() const
    {
        return decomposed_init;
    }

    const DecomposedGrowthState & getDecomposedFinalState() const
    {
        return decomposed_final;
    }
};

template<typename tMesh>
struct GrowthHelper
{
    static void anglesToVectors(const Eigen::Ref<const Eigen::VectorXd> angles, Eigen::Ref<Eigen::MatrixXd> vectors, const Real phase = 0)
    {
        const int nFaces = angles.rows();
        for(int i=0;i<nFaces;++i)
        {
            const Real phi = angles(i);
            const Eigen::Vector3d dir = (Eigen::Vector3d() <<  std::cos(phi + phase), std::sin(phi + phase), 0).finished();
            for(int d=0;d<3;++d)
                vectors(i,d) = dir(d);
        }
    }

    static void anglesToVectors(const Eigen::Ref<const Eigen::VectorXd> angles, const Eigen::Ref<const Eigen::VectorXd> lengths, Eigen::Ref<Eigen::MatrixXd> vectors, const Real phase = 0)
    {
        const int nFaces = angles.rows();
        for(int i=0;i<nFaces;++i)
        {
            const Real phi = angles(i);
            const Eigen::Vector3d dir = (Eigen::Vector3d() <<  std::cos(phi + phase), std::sin(phi + phase), 0).finished();
            for(int d=0;d<3;++d)
                vectors(i,d) = lengths(i)*dir(d);
        }
    }

    static void vectorsToAngles(const Eigen::Ref<const Eigen::MatrixXd> dir1, const Eigen::Ref<const Eigen::MatrixXd> dir2, Eigen::Ref<Eigen::VectorXd> angles, Eigen::Ref<Eigen::VectorXd> growthRates1, Eigen::Ref<Eigen::VectorXd> growthRates2)
    {
        const int nFaces = dir1.rows();
        for(int i=0;i<nFaces;++i)
        {
            const Eigen::Vector3d & dir1_i = dir1.row(i);
            const Eigen::Vector3d & dir2_i = dir2.row(i);

            const Real mag_dir1_i = dir1_i.norm();
            const Real mag_dir2_i = dir2_i.norm();

            const Real angle_i = std::atan2(dir1_i(1), dir1_i(0));

            // fill the arrays
            angles(i) = angle_i;
            growthRates1(i) = mag_dir1_i;
            growthRates2(i) = mag_dir2_i;

        }
    }
    static void computeAbarsIsoGrowth(const tMesh & mesh, const Eigen::Ref<const Eigen::VectorXd> growthRates, tVecMat2d & aforms)
    {
        const int nFaces = mesh.getNumberOfFaces();
        const auto & topo = mesh.getTopology();
        const auto & reststate = mesh.getRestConfiguration(); // does not matter which one at this point

        aforms.resize(nFaces);

        for(int i=0;i<nFaces;++i)
        {
            const TriangleInfo info = reststate.getTriangleInfoLite(topo, i);
            const Real growth = std::pow(1 + growthRates(i),2);

            const Eigen::Vector3d dir_p = (Eigen::Vector3d() << 1, 0, 0).finished();
            const Eigen::Vector3d dir_o = (Eigen::Vector3d() << 0, 1, 0).finished();

            const Real e1_p = (info.e1).dot(dir_p);
            const Real e1_o = (info.e1).dot(dir_o);

            const Real e2_p = (info.e2).dot(dir_p);
            const Real e2_o = (info.e2).dot(dir_o);

            const Real a11 = (e1_p*e1_p + e1_o*e1_o)*growth;
            const Real a12 = (e1_p*e2_p + e1_o*e2_o)*growth;
            const Real a22 = (e2_p*e2_p + e2_o*e2_o)*growth;

            aforms[i](0,0) = a11;
            aforms[i](0,1) = aforms[i](1,0) = a12;
            aforms[i](1,1) = a22;
        }
    }

    static void computeAbarsIsoGrowth_Gradient(const tMesh & mesh, const Eigen::Ref<const Eigen::VectorXd> growthRates, const Eigen::Ref<const Eigen::MatrixXd> gradEng_abar, Eigen::Ref<Eigen::VectorXd> gradEng_rate)
    {
        // compute gradient abar wrt growth
        const int nFaces = mesh.getNumberOfFaces();
        const auto & topo = mesh.getTopology();
        const auto & reststate = mesh.getRestConfiguration(); // does not matter which one at this point

        for(int i=0;i<nFaces;++i)
        {
            const TriangleInfo info = reststate.getTriangleInfoLite(topo, i);
            const Real dgrowth = 2.0*(1 + growthRates(i));

            const Eigen::Vector3d dir_p = (Eigen::Vector3d() << 1, 0, 0).finished();
            const Eigen::Vector3d dir_o = (Eigen::Vector3d() << 0, 1, 0).finished();

            const Real e1_p = (info.e1).dot(dir_p);
            const Real e1_o = (info.e1).dot(dir_o);

            const Real e2_p = (info.e2).dot(dir_p);
            const Real e2_o = (info.e2).dot(dir_o);

            const Real da11 = (e1_p*e1_p + e1_o*e1_o)*dgrowth;
            const Real da12 = (e1_p*e2_p + e1_o*e2_o)*dgrowth;
            const Real da22 = (e2_p*e2_p + e2_o*e2_o)*dgrowth;

            gradEng_rate(i) = gradEng_abar(i,0)*da11 + gradEng_abar(i,1)*da12 + gradEng_abar(i,2)*da22;
        }
    }

    static void computeAbarsOrthoGrowth(const tMesh & mesh, const Eigen::Ref<const Eigen::VectorXd> growthAngles, const Eigen::Ref<const Eigen::VectorXd> growthRates_p, const Eigen::Ref<const Eigen::VectorXd> growthRates_o, tVecMat2d & aforms)
    {
        const int nFaces = mesh.getNumberOfFaces();
        const auto & topo = mesh.getTopology();
        const auto & reststate = mesh.getRestConfiguration(); // does not matter which one at this point

        aforms.resize(nFaces);

        for(int i=0;i<nFaces;++i)
        {
            const TriangleInfo info = reststate.getTriangleInfoLite(topo, i);

            const Real growth_p = std::pow(1 + growthRates_p(i),2);
            const Real growth_o = std::pow(1 + growthRates_o(i),2);
            const Real phi = growthAngles(i);

            const Eigen::Vector3d dir_p = (Eigen::Vector3d() <<  std::cos(phi), std::sin(phi), 0).finished();
            const Eigen::Vector3d dir_o = (Eigen::Vector3d() << -std::sin(phi), std::cos(phi), 0).finished();

//          new edges are the following
//          const Eigen::Vector3d e0_new = (1.0 + growthRate_p)*(e0.dot(dir_p))*dir_p + (1.0 + growthRate_o)*(e0.dot(dir_o))*dir_o;
//          const Eigen::Vector3d e1_new = (1.0 + growthRate_p)*(e1.dot(dir_p))*dir_p + (1.0 + growthRate_o)*(e1.dot(dir_o))*dir_o;
//          const Eigen::Vector3d e2_new = (1.0 + growthRate_p)*(e2.dot(dir_p))*dir_p + (1.0 + growthRate_o)*(e2.dot(dir_o))*dir_o;
//          a11 = e1_new.dot(e1_new), a12 = e1_new.dot(e2_new), a22 = e2_new.dot(e2_new)
//          also : e0_new + e1_new + e2_new = 0

            const Real e1_p = (info.e1).dot(dir_p);
            const Real e1_o = (info.e1).dot(dir_o);

            const Real e2_p = (info.e2).dot(dir_p);
            const Real e2_o = (info.e2).dot(dir_o);

            const Real a11 = e1_p*e1_p*growth_p + e1_o*e1_o*growth_o;
            const Real a12 = e1_p*e2_p*growth_p + e1_o*e2_o*growth_o;
            const Real a22 = e2_p*e2_p*growth_p + e2_o*e2_o*growth_o;

            aforms[i](0,0) = a11;
            aforms[i](0,1) = aforms[i](1,0) = a12;
            aforms[i](1,1) = a22;
        }
    }

    static void computeAbarsOrthoGrowth_Gradient(const tMesh & mesh, const Eigen::Ref<const Eigen::VectorXd> growthAngles, const Eigen::Ref<const Eigen::VectorXd> growthRates_p, const Eigen::Ref<const Eigen::VectorXd> growthRates_o, const Eigen::Ref<const Eigen::MatrixXd> gradEng_abar, Eigen::Ref<Eigen::VectorXd> gradEng_angle, Eigen::Ref<Eigen::VectorXd> gradEng_rate_p, Eigen::Ref<Eigen::VectorXd> gradEng_rate_o)
    {
        // compute gradient abar wrt growth
        const int nFaces = mesh.getNumberOfFaces();
        const auto & topo = mesh.getTopology();
        const auto & reststate = mesh.getRestConfiguration(); // does not matter which one at this point

        for(int i=0;i<nFaces;++i)
        {
            const TriangleInfo info = reststate.getTriangleInfoLite(topo, i);

            const Real growth_p = std::pow(1 + growthRates_p(i),2);
            const Real growth_o = std::pow(1 + growthRates_o(i),2);
            const Real phi = growthAngles(i);

            const Eigen::Vector3d dir_p = (Eigen::Vector3d() <<  std::cos(phi), std::sin(phi), 0).finished();
            const Eigen::Vector3d dir_o = (Eigen::Vector3d() << -std::sin(phi), std::cos(phi), 0).finished();

            // derivatives
            const Real dgrowth_p = 2.0*(1 + growthRates_p(i));
            const Real dgrowth_o = 2.0*(1 + growthRates_o(i));

            const Eigen::Vector3d ddir_p = (Eigen::Vector3d() << -std::sin(phi),  std::cos(phi), 0).finished();
            const Eigen::Vector3d ddir_o = (Eigen::Vector3d() << -std::cos(phi), -std::sin(phi), 0).finished();

            const Real e1_p = (info.e1).dot(dir_p);
            const Real e1_o = (info.e1).dot(dir_o);

            const Real e2_p = (info.e2).dot(dir_p);
            const Real e2_o = (info.e2).dot(dir_o);

            const Real e1_dp = (info.e1).dot(ddir_p);
            const Real e1_do = (info.e1).dot(ddir_o);

            const Real e2_dp = (info.e2).dot(ddir_p);
            const Real e2_do = (info.e2).dot(ddir_o);

            const Real da11_dphi = 2.0*e1_p*e1_dp*growth_p + 2.0*e1_o*e1_do*growth_o;
            const Real da12_dphi = (e1_p*e2_dp + e1_dp*e2_p)*growth_p + (e1_o*e2_do + e1_do*e2_o)*growth_o;
            const Real da22_dphi = 2.0*e2_p*e2_dp*growth_p + 2.0*e2_o*e2_do*growth_o;

            const Real da11_dgrowth_p = e1_p*e1_p*dgrowth_p;
            const Real da12_dgrowth_p = e1_p*e2_p*dgrowth_p;
            const Real da22_dgrowth_p = e2_p*e2_p*dgrowth_p;

            const Real da11_dgrowth_o = e1_o*e1_o*dgrowth_o;
            const Real da12_dgrowth_o = e1_o*e2_o*dgrowth_o;
            const Real da22_dgrowth_o = e2_o*e2_o*dgrowth_o;

            gradEng_angle(i)  = gradEng_abar(i,0)*da11_dphi      + gradEng_abar(i,1)*da12_dphi      + gradEng_abar(i,2)*da22_dphi;
            gradEng_rate_p(i) = gradEng_abar(i,0)*da11_dgrowth_p + gradEng_abar(i,1)*da12_dgrowth_p + gradEng_abar(i,2)*da22_dgrowth_p;
            gradEng_rate_o(i) = gradEng_abar(i,0)*da11_dgrowth_o + gradEng_abar(i,1)*da12_dgrowth_o + gradEng_abar(i,2)*da22_dgrowth_o;
        }
    }

    static void computeAbarsOrthoGrowth_GradientVertices(const tMesh & mesh, const Eigen::Ref<const Eigen::VectorXd> growthAngles, const Eigen::Ref<const Eigen::VectorXd> growthRates_p, const Eigen::Ref<const Eigen::VectorXd> growthRates_o, const Eigen::Ref<const Eigen::MatrixXd> gradEng_abar, Eigen::Ref<Eigen::MatrixXd> gradEng_verts)
    {
        // compute gradient abar wrt growth
        const int nFaces = mesh.getNumberOfFaces();
        const auto & topo = mesh.getTopology();
        const auto & reststate = mesh.getRestConfiguration(); // does not matter which one at this point

        for(int i=0;i<nFaces;++i)
        {
            const TriangleInfo info = reststate.getTriangleInfoLite(topo, i);

            const Real growth_p = std::pow(1 + growthRates_p(i),2);
            const Real growth_o = std::pow(1 + growthRates_o(i),2);
            const Real phi = growthAngles(i);

            const Eigen::Vector3d dir_p = (Eigen::Vector3d() <<  std::cos(phi), std::sin(phi), 0).finished();
            const Eigen::Vector3d dir_o = (Eigen::Vector3d() << -std::sin(phi), std::cos(phi), 0).finished();

            // e1 = v2 - v1
            // e2 = v0 - v2
            const Real e1_p = (info.e1).dot(dir_p);
            const Real e1_o = (info.e1).dot(dir_o);

            const Real e2_p = (info.e2).dot(dir_p);
            const Real e2_o = (info.e2).dot(dir_o);

//            const Real a11 = e1_p*e1_p*growth_p + e1_o*e1_o*growth_o;
//            const Real a12 = e1_p*e2_p*growth_p + e1_o*e2_o*growth_o;
//            const Real a22 = e2_p*e2_p*growth_p + e2_o*e2_o*growth_o;

            const Eigen::Vector3d gradv0_a12 = e1_p*growth_p*dir_p + e1_o*growth_o*dir_o;
            const Eigen::Vector3d gradv0_a22 = 2.0*(e2_p*growth_p*dir_p + e2_o*growth_o*dir_o);

            const Eigen::Vector3d gradv1_a11 = -2.0*(e1_p*growth_p*dir_p + e1_o*growth_o*dir_o);
            const Eigen::Vector3d gradv1_a12 = -e2_p*growth_p*dir_p - e2_o*growth_o*dir_o;

            const Eigen::Vector3d gradv2_a11 = 2.0*(e1_p*growth_p*dir_p + e1_o*growth_o*dir_o);
            const Eigen::Vector3d gradv2_a12 = growth_p*(e2_p - e1_p)*dir_p + growth_o*(e2_o - e1_o)*dir_o;
            const Eigen::Vector3d gradv2_a22 = -2.0*(e2_p*growth_p*dir_p + e2_o*growth_o*dir_o);

            for(int d=0;d<2;++d)
            {
                gradEng_verts(info.idx_v0,d) += gradEng_abar(i,1)*gradv0_a12(d) + gradEng_abar(i,2)*gradv0_a22(d);
                gradEng_verts(info.idx_v1,d) += gradEng_abar(i,0)*gradv1_a11(d) + gradEng_abar(i,1)*gradv1_a12(d);
                gradEng_verts(info.idx_v2,d) += gradEng_abar(i,0)*gradv2_a11(d) + gradEng_abar(i,1)*gradv2_a12(d) + gradEng_abar(i,2)*gradv2_a22(d);
            }

        }
    }

    static void computeAbarsOrthoGrowthShell(const tMesh & mesh, const Eigen::Ref<const Eigen::MatrixXd> growthdirs_1, const Eigen::Ref<const Eigen::VectorXd> growthRates_1, const Eigen::Ref<const Eigen::VectorXd> growthRates_2, tVecMat2d & aforms)
    {
        const int nFaces = mesh.getNumberOfFaces();
        const auto & topo = mesh.getTopology();
        const auto & reststate = mesh.getRestConfiguration(); // does not matter which one at this point

        aforms.resize(nFaces);

        for(int i=0;i<nFaces;++i)
        {
            const TriangleInfo info = reststate.getTriangleInfoLite(topo, i);

            const Real growth_1 = std::pow(1 + growthRates_1(i),2);
            const Real growth_2 = std::pow(1 + growthRates_2(i),2);
            const Eigen::Vector3d dir_1 = growthdirs_1.row(i).normalized(); // should be normalized already but do it again just to be sure

            // get the face normal to compute dir_2
            const Eigen::Vector3d facenormal = (info.e2).cross(info.e0).normalized();
            const Eigen::Vector3d dir_2 = (facenormal.cross(dir_1)).normalized(); // so that dir_1 cross dir_2 = facenormal

            const Real e1_1 = (info.e1).dot(dir_1);
            const Real e1_2 = (info.e1).dot(dir_2);

            const Real e2_1 = (info.e2).dot(dir_1);
            const Real e2_2 = (info.e2).dot(dir_2);

            const Real a11 = e1_1*e1_1*growth_1 + e1_2*e1_2*growth_2;
            const Real a12 = e1_1*e2_1*growth_1 + e1_2*e2_2*growth_2;
            const Real a22 = e2_1*e2_1*growth_1 + e2_2*e2_2*growth_2;

            aforms[i](0,0) = a11;
            aforms[i](0,1) = aforms[i](1,0) = a12;
            aforms[i](1,1) = a22;
        }
    }

    static void computeAbarsIsoGrowthPerEdge(const tMesh & mesh, const Eigen::Ref<const Eigen::VectorXd> growthRates, tVecMat2d & aforms)
    {
        // now the growth rates are prescribed per-edge
        const int nFaces = mesh.getNumberOfFaces();
        const auto & topo = mesh.getTopology();
        const auto & reststate = mesh.getRestConfiguration(); // does not matter which one at this point

        aforms.resize(nFaces);

        for(int i=0;i<nFaces;++i)
        {
            const TriangleInfo info = reststate.getTriangleInfoLite(topo, i);
            const Real growth_e0 = std::pow(1 + growthRates(info.idx_e0), 2);
            const Real growth_e1 = std::pow(1 + growthRates(info.idx_e1), 2);
            const Real growth_e2 = std::pow(1 + growthRates(info.idx_e2), 2);

            const Real e0_magsq = (info.e0).dot(info.e0);
            const Real e1_magsq = (info.e1).dot(info.e1);
            const Real e2_magsq = (info.e2).dot(info.e2);

            const Real a11 = growth_e1 * e1_magsq;
            const Real a12 = -0.5*( growth_e1 * e1_magsq + growth_e2 * e2_magsq - growth_e0 * e0_magsq ); // checked : this is correct
            const Real a22 = growth_e2 * e2_magsq;

            aforms[i](0,0) = a11;
            aforms[i](0,1) = aforms[i](1,0) = a12;
            aforms[i](1,1) = a22;
        }
    }

    static void computeAbarsIsoGrowthPerEdge_Gradient(const tMesh & mesh, const Eigen::Ref<const Eigen::VectorXd> growthRates, const Eigen::Ref<const Eigen::MatrixXd> gradEng_abar, Eigen::Ref<Eigen::VectorXd> gradEng_rate)
    {
        // compute gradient abar wrt growth
        const int nFaces = mesh.getNumberOfFaces();
        const auto & topo = mesh.getTopology();
        const auto & reststate = mesh.getRestConfiguration(); // does not matter which one at this point

        for(int i=0;i<nFaces;++i)
        {
            const TriangleInfo info = reststate.getTriangleInfoLite(topo, i);

            const Real dgrowth_e0 = 2 * (1 + growthRates(info.idx_e0));
            const Real dgrowth_e1 = 2 * (1 + growthRates(info.idx_e1));
            const Real dgrowth_e2 = 2 * (1 + growthRates(info.idx_e2));

            const Real e0_magsq = (info.e0).dot(info.e0);
            const Real e1_magsq = (info.e1).dot(info.e1);
            const Real e2_magsq = (info.e2).dot(info.e2);

            const Real da11_e1 = dgrowth_e1 * e1_magsq;

            const Real da12_e0 =  0.5 * dgrowth_e0 * e0_magsq;
            const Real da12_e1 = -0.5 * dgrowth_e1 * e1_magsq;
            const Real da12_e2 = -0.5 * dgrowth_e2 * e2_magsq;

            const Real da22_e2 = dgrowth_e2 * e2_magsq;

            gradEng_rate(info.idx_e0) += gradEng_abar(i,1)*da12_e0;
            gradEng_rate(info.idx_e1) += gradEng_abar(i,0)*da11_e1 + gradEng_abar(i,1)*da12_e1;
            gradEng_rate(info.idx_e2) += gradEng_abar(i,1)*da12_e2 + gradEng_abar(i,2)*da22_e2;
        }
    }

    static void computeAbarsIsoGrowthPerVertex(const tMesh & mesh, const Eigen::Ref<const Eigen::VectorXd> growthRates, tVecMat2d & aforms)
    {
        // now the growth rates are prescribed per-edge
        const int nFaces = mesh.getNumberOfFaces();
        const auto & topo = mesh.getTopology();
        const auto & reststate = mesh.getRestConfiguration(); // does not matter which one at this point

        aforms.resize(nFaces);

        for(int i=0;i<nFaces;++i)
        {
            const TriangleInfo info = reststate.getTriangleInfoLite(topo, i);
            const Real growth_v0 = 1 + growthRates(info.idx_v0);
            const Real growth_v1 = 1 + growthRates(info.idx_v1);
            const Real growth_v2 = 1 + growthRates(info.idx_v2);

            const Real growth_e1 = 0.5*(growth_v2 + growth_v1);
            const Real growth_e2 = 0.5*(growth_v0 + growth_v2);

            const Eigen::Vector3d dir_p = (Eigen::Vector3d() << 1, 0, 0).finished();
            const Eigen::Vector3d dir_o = (Eigen::Vector3d() << 0, 1, 0).finished();

            const Real e1_p = (info.e1).dot(dir_p);
            const Real e1_o = (info.e1).dot(dir_o);

            const Real e2_p = (info.e2).dot(dir_p);
            const Real e2_o = (info.e2).dot(dir_o);

            const Real e1_p_growth = e1_p * growth_e1;
            const Real e1_o_growth = e1_o * growth_e1;
            const Real e2_p_growth = e2_p * growth_e2;
            const Real e2_o_growth = e2_o * growth_e2;

            const Real a11 = e1_p_growth*e1_p_growth + e1_o_growth*e1_o_growth;
            const Real a12 = e1_p_growth*e2_p_growth + e1_o_growth*e2_o_growth;
            const Real a22 = e2_p_growth*e2_p_growth + e2_o_growth*e2_o_growth;

            aforms[i](0,0) = a11;
            aforms[i](0,1) = aforms[i](1,0) = a12;
            aforms[i](1,1) = a22;
        }
    }

    static void computeAbarsIsoGrowthPerVertex_Gradient(const tMesh & mesh, const Eigen::Ref<const Eigen::VectorXd> growthRates, const Eigen::Ref<const Eigen::MatrixXd> gradEng_abar, Eigen::Ref<Eigen::VectorXd> gradEng_rate)
    {
        // compute gradient abar wrt growth
        const int nFaces = mesh.getNumberOfFaces();
        const auto & topo = mesh.getTopology();
        const auto & reststate = mesh.getRestConfiguration(); // does not matter which one at this point

        for(int i=0;i<nFaces;++i)
        {
            const TriangleInfo info = reststate.getTriangleInfoLite(topo, i);
            const Real growth_v0 = 1 + growthRates(info.idx_v0);
            const Real growth_v1 = 1 + growthRates(info.idx_v1);
            const Real growth_v2 = 1 + growthRates(info.idx_v2);

            const Real growth_e1 = 0.5*(growth_v2 + growth_v1);
            const Real growth_e2 = 0.5*(growth_v0 + growth_v2);

            const Eigen::Vector3d dir_p = (Eigen::Vector3d() << 1, 0, 0).finished();
            const Eigen::Vector3d dir_o = (Eigen::Vector3d() << 0, 1, 0).finished();

            const Real e1_p = (info.e1).dot(dir_p);
            const Real e1_o = (info.e1).dot(dir_o);

            const Real e2_p = (info.e2).dot(dir_p);
            const Real e2_o = (info.e2).dot(dir_o);

            const Real e1_p_growth = e1_p * growth_e1;
            const Real e1_o_growth = e1_o * growth_e1;
            const Real e2_p_growth = e2_p * growth_e2;
            const Real e2_o_growth = e2_o * growth_e2;

            const Real da11_e1 = 2.0*(e1_p_growth * e1_p + e1_o_growth * e1_o);
            const Real da12_e1 = e1_p * e2_p_growth + e1_o * e2_o_growth;
            const Real da12_e2 = e1_p_growth * e2_p + e1_o_growth * e2_o;
            const Real da22_e2 = 2.0*(e2_p_growth * e2_p + e2_o_growth * e2_o);

            const Real da12_v0 = 0.5*da12_e2;
            const Real da22_v0 = 0.5*da22_e2;

            const Real da11_v1 = 0.5*da11_e1;
            const Real da12_v1 = 0.5*da12_e1;

            const Real da11_v2 = 0.5*da11_e1;
            const Real da12_v2 = 0.5*(da12_e1 + da12_e2);
            const Real da22_v2 = 0.5*da22_e2;

            gradEng_rate(info.idx_v0) += gradEng_abar(i,1)*da12_v0 + gradEng_abar(i,2)*da22_v0;
            gradEng_rate(info.idx_v1) += gradEng_abar(i,0)*da11_v1 + gradEng_abar(i,1)*da12_v1;
            gradEng_rate(info.idx_v2) += gradEng_abar(i,0)*da11_v2 + gradEng_abar(i,1)*da12_v2 + gradEng_abar(i,2)*da22_v2;
        }
    }
};

#endif /* GrowthHelper_hpp */


#include <nobody/energy.hpp>

#include <Eigen/Dense>

#include <nobody/euler_integrator.hpp>

namespace nobody {

float kinetic_energy(const std::vector<particle>& particles) {
  float energy = 0.0f;
  for (const auto& p : particles) energy += p.mass * p.velocity.squaredNorm();
  return 0.5f * energy;
}

float potential_energy(const std::vector<particle>& particles) {
  float energy = 0.0f;
  for (std::size_t i = 0; i < particles.size(); ++i) {
    for (std::size_t j = 0; j < i; ++j) {
      energy += particles[i].mass * particles[j].mass /
                (particles[i].position - particles[j].position).norm();
    }
  }
  return -gravitation_const * energy;
}

Eigen::Vector3f angular_momentum(const std::vector<particle>& particles) {
  Eigen::Vector3f result(0, 0, 0);
  for (const auto& particle : particles) {
    result += particle.mass * particle.position.cross(particle.velocity);
  }
  return result;
}

}  // namespace nobody


//
//  ExtraCirProcesses.cpp
//  Master Thesis
//
//  Created by Magnus Mencke on 25/05/2021.
//  Copyright © 2021 Magnus Mencke. All rights reserved.
//

#include "ExtraCirProcesses.hpp"

#include <boost/math/distributions/non_central_chi_squared.hpp>
#include <ql/math/distributions/normaldistribution.hpp>

namespace QuantLib {
    Real CirProcess::x0() const {
        return x0_;
    }

    Real CirProcess::speed() const {
        return speed_;
    }

    Real CirProcess::volatility() const {
        return volatility_;
    }

    Real CirProcess::level() const {
        return level_;
    }

    Real CirProcess::drift(Time, Real x) const {
        return speed_ * (level_ - x);
    }

    Real CirProcess::diffusion(Time, Real) const {
        return volatility_;
    }

    Real CirProcess::expectation(Time, Real x0,
                                        Time dt) const {
        return level_ + (x0 - level_) * std::exp(-speed_*dt);
    }

    Real CirProcess::stdDeviation(Time t, Real x0,
                                         Time dt) const {
        return std::sqrt(variance(t,x0,dt));
    }

    //Full truncation scheme (as in Brigo, Morini and Pallavicini)
    //Inspired by Heston implementation
    //  see Lord, R., R. Koekkoek and D. van Dijk (2006),
    // "A Comparison of biased simulation schemes for
    //  stochastic volatility models",
    // Working Paper, Tinbergen Institute
    Real CirProcess::evolve (Time t0,
                                    Real x0,
                                    Time dt,
                                    Real dw) const {
        Real resultTrunc;
        switch (discretization_) {
            case None: {
                resultTrunc=apply(expectation(t0,x0,dt),stdDeviation(t0,x0,dt)*dw);
                break;
            }
            case FullTruncation: {
                Real x0_trunc = x0>0.0 ? x0 : 0.0;

                Real result = apply( expectation(t0, x0_trunc, dt),stdDeviation(t0,x0_trunc,dt)*dw);

                resultTrunc = result>0.0 ? result : 0.0;

                break;
            }
            case QuadraticExponential: {
                // for details of the quadratic exponential discretization scheme
                // see Leif Andersen,
                // Efficient Simulation of the Heston Stochastic Volatility Model
                const Real ex = std::exp(-speed_*dt);

                const Real m  =  level_+(x0-level_)*ex;
                const Real s2 =  x0*volatility_*volatility_*ex/speed_*(1-ex)
                + level_*volatility_*volatility_/(2*speed_)*(1-ex)*(1-ex);
                const Real psi = s2/(m*m);

                if (psi <= 1.5) {
                    const Real b2 = 2/psi-1+std::sqrt(2/psi*(2/psi-1));
                    const Real b  = std::sqrt(b2);
                    const Real a  = m/(1+b2);

                    resultTrunc = a*(b+dw)*(b+dw);
                }
                else {
                    const Real p = (psi-1)/(psi+1);
                    const Real beta = (1-p)/m;

                    const Real u = CumulativeNormalDistribution()(dw);

                    resultTrunc = ((u <= p) ? 0.0 : std::log((1-p)/(1-u))/beta);
                }
                break;
            }
            case Exact: {
                CumulativeNormalDistribution dwDist; //despite the name, dw is standard normal
                Real uniform= dwDist(dw); //transforming normal to uniform

                Real c=(4*speed_)/(volatility_*volatility_*(1-std::exp(-speed_*dt)));
                Real nu=(4*speed_*level_)/(volatility_*volatility_);
                Real eta=c*x0*std::exp(-speed_*dt);

                //we had some strange errors using QuantLib's Chi2 distribution, so we use the one from boost
                boost::math::non_central_chi_squared_distribution<double,  boost::math::policies::policy<>> chi2(nu, eta);
                //InverseNonCentralCumulativeChiSquareDistribution chi2(nu, eta);

                resultTrunc = quantile(chi2,uniform)/c;
            }
        }

        return resultTrunc;
    }

    CirProcess::CirProcess(Real speed,
                                                     Volatility vol,
                                                     Real x0,
                                                     Real level,
                                                     Discretization d)
    : x0_(x0), speed_(speed), level_(level), volatility_(vol), discretization_(d) {
        QL_REQUIRE(volatility_ >= 0.0, "negative volatility given");
    }

    Real CirProcess::variance(Time, Real, Time dt) const {
        Real exponent1 = std::exp(-speed_ * dt);
        Real exponent2 = std::exp(-2 * speed_ * dt);
        Real fraction = (volatility_ * volatility_) / speed_;

        return x0_ * fraction * (exponent1 - exponent2) + level_ * fraction * (1 - exponent1) * (1 - exponent1);
    }

}


#include <socialgraph.h>
#include <math.h>
#include <boost/graph/johnson_all_pairs_shortest.hpp>
#include <boost/graph/bc_clustering.hpp>
#include <boost/graph/kruskal_min_spanning_tree.hpp>

Graph createGraph(std::ifstream &fin){
	facebook::Name name;
	facebook::Data data = facebook::readFile(fin, name);

	facebook::Map map = facebook::createMap(data);

	Graph g = facebook::createGraph(data, map);

	return g;
}

WeightedGraph createWeightedGraph(Graph g){
	WeightedGraph wg(num_vertices(g));

	property_map<WeightedGraph, edge_weight_t>::type weightmap = get(edge_weight, wg);
	EdgeIterator e, e_end;
	for(tie(e, e_end) = edges(g); e != e_end; e++){
		WeightedEdgeDescriptor e1; bool inserted;
		tie(e1, inserted) = add_edge(source(*e, g), target(*e, g), wg);
		weightmap[e1] = 1;
	}

	return wg;
}

Matrix createAllPairsShortestPaths(WeightedGraph wg){
	int V = num_vertices(wg);
	Matrix D(V, std::vector<int>(V));
	johnson_all_pairs_shortest_paths(wg, D);

	return D;
}

//have to corresponding vertices
Edges newEdgeList(Graph g, Graph g2){
	Edges edges;
	for(int i=0; i<num_vertices(g); i++){
		for(int j=i+1; j<num_vertices(g); j++){
			if(edge(i, j, g).second == 0){
				if(edge(i, j, g2).second == 1){
					Edge e(i, j);
					edges.push_back(e);
				}
			}
		}
	}
	return edges;
}

Matrix newList2Matrix(Graph g, Edges edges){
	Matrix matrix(num_vertices(g));
	for(int i=0; i<num_vertices(g); i++){
		std::vector<int> temp(num_vertices(g), 0);
		matrix[i] = temp;
	}
	for(int i=0; i<edges.size(); i++){
		matrix[edges[i].first][edges[i].second] = 1;
		matrix[edges[i].second][edges[i].first] = 1;
	}
	return matrix;
}

MatrixScore initializeScore(Graph g){
	MatrixScore scores(num_vertices(g));

	for(int i=0; i<num_vertices(g); i++){
		std::vector<double> score(num_vertices(g), 0.0);
		scores[i] = score;
	}
	return scores;
}

std::vector<double> sortDouble(std::vector<double> scores){
	std::vector<double> temps;
	for(int i=0; i<scores.size(); i++){
		if(scores[i] > 0)
			temps.push_back(scores[i]);
	}
	std::sort(temps.begin(), temps.end());
	return temps;
}

bool comparisonSort(Score i, Score j){
	if(i.first < j.first) return false;
	if(j.first < i.first) return true;
	return j.second.first < i.second.first;
}

Scores SortScores(MatrixScore mscores){
	Scores scores;

	for(int i=0; i<mscores.size(); i++){
		for(int j=i+1; j<mscores.size(); j++){
			scores.push_back(Score(mscores[i][j], Edge(i,j)));
		}
	}
	std::sort(scores.begin(), scores.end(), comparisonSort);

	return scores;
}

Scores SortIndividualScores(MatrixScore mscores, int node){
	Scores scores;

	for(int i=0; i<mscores.size(); i++){
		scores.push_back(Score(mscores[node][i], Edge(node, i)));
	}

	std::sort(scores.begin(), scores.end(), comparisonSort);

	return scores;
}

float AvgDegree(Graph g){
	float avg=0.0;

	for(int i=0; i<num_vertices(g); i++)
		avg += out_degree(i,g);

	avg = avg / num_vertices(g);

	return avg;
}

MatrixScore ExtAdamicAdar(Graph g){
	Graph temp_g = g;
	WeightedGraph wg = createWeightedGraph(temp_g);
	Matrix distance = createAllPairsShortestPaths(wg);
	MatrixScore scores = initializeScore(g);

	for(int i=0; i<num_vertices(g); i++){
		for(int j=0; j<num_vertices(g); j++){
			if(edge(i,j,g).second == 1)
				scores[i][j] = 1;
		}
	}

	float avg = AvgDegree(g);
	float thresh = (float)avg/log(2);

	for(int k=0; k<2; k++){

	for(int x=0; x<num_vertices(temp_g); x++){
		for(int y=0; y<num_vertices(temp_g); y++){
			if(distance[x][y] == 2){
				for(int i=0; i<num_vertices(g); i++){
					if(distance[x][i] == 1 && distance[i][y] == 1){
						scores[x][y] += (double)(scores[x][i] * scores[y][i])/log(out_degree(i,g));
					}
				}
				if(scores[x][y] > thresh)
					scores[x][y] = 1.0;
				else
					scores[x][y] = scores[x][y] / thresh;
				add_edge(x,y,temp_g);
			}
		}
	}
	wg = createWeightedGraph(temp_g);
	distance = createAllPairsShortestPaths(wg);

	}

	return scores;
}

MatrixScore AdamicAdar(Graph g){
	Matrix distance = createAllPairsShortestPaths(createWeightedGraph(g));
	MatrixScore scores = initializeScore(g);

	for(int x=0; x<num_vertices(g); x++){
		#pragma omp parallel for
		for(int y=x+1; y<num_vertices(g); y++){
			if(distance[x][y] == 2){
				for(int i=0; i<num_vertices(g); i++){
					if(x!=i && y!=i && out_degree(i,g) > 1){
						int d = distance[x][i] + distance[y][i];
						if(d == distance[x][y])
							scores[x][y] += (double)1/log(out_degree(i,g));
					}
				}
			}
		}
	}

	return scores;
}

void normalize(MatrixScore scores, Graph g){
	//mean
	int count=0;
	double avg=0.0;
	for(int x=0; x<num_vertices(g); x++){
		for(int y=x+1; y<num_vertices(g); y++){
			if(edge(x,y,g).second == 0){
				avg += scores[x][y];
				count++;
			}
		}
	}
	avg = avg/count;

	double dev=0.0;
	for(int x=0; x<num_vertices(g); x++){
		for(int y=x+1; y<num_vertices(g); y++){
			if(edge(x,y,g).second == 0){
				dev += pow(scores[x][y] - avg, 2);
			}
		}
	}
	dev = sqrt(dev/count);
	std::cout << avg << " " << dev << std::endl;
}

MatrixScore MultiTimeAdamicAdar(Graph g, Matrix distance, Matrix new_matrix){
	MatrixScore scores(num_vertices(g));
	for(int i=0; i<num_vertices(g); i++){
		std::vector<double> score(num_vertices(g), 0.0);
		scores[i] = score;
	}

	std::vector<int> is_new(num_vertices(g), 0);
	for(int i=0; i<num_vertices(g); i++){
		for(int j=0; j<num_vertices(g); j++){
			if(new_matrix[i][j] == 1){
				is_new[i] = 1;
				break;
			}
		}
	}

	for(int s=0; s<num_vertices(g); s++){
		for(int t=s+1; t<num_vertices(g); t++){
			if(distance[s][t] == 2){
				for(int i=0; i<num_vertices(g); i++){
					if(distance[s][i] == 1 && distance[t][i] == 1){
						float si = (new_matrix[s][i] == 1)?1.5:1;
						float ti = (new_matrix[t][i] == 1)?1.5:1;
						scores[s][t] += (si*ti)/log10(out_degree(i,g));
					}
				}
			}
		}
	}

	return scores;
}

float AvgDegreeAA(Graph g, Edges new_edges, Matrix distance){
	int count=0; float temp_avg=0.0;
	for(int i=0; i<new_edges.size(); i++){
		int s = new_edges[i].first;
		int t = new_edges[i].second;

		if(distance[s][t] == 2){
			for(int j=0; j<num_vertices(g); j++){
				if(distance[s][j] + distance[j][t] == distance[s][t]){
					temp_avg += out_degree(j,g);
					count++;
				}
			}
		}
	}

	return temp_avg/count;
}

Matrix createMatrix(Graph g){
	Matrix matrix(num_vertices(g));

	#pragma omp parallel for
	for(int i=0; i<num_vertices(g); i++){
		std::vector<int> list(num_vertices(g));
		for(int j=0; j<num_vertices(g); j++){
			if(edge(i,j,g).second == 1)
				list[j] = 1;
			else
				list[j] = 0;
		}
		matrix[i] = list;
	}

	return matrix;
}

Matrix multiplyMatrix(Matrix A, Matrix B){
	Matrix matrix(A.size());

	#pragma omp parallel for
	for(int i=0; i<A.size(); i++){
		std::vector<int> list(A.size(), 0);
		for(int j=0; j<A.size(); j++){
			for(int k=0; k<A.size(); k++){
				list[j] += A[i][k] * B[k][j];
			}
		}
		matrix[i] = list;
	}

	return matrix;
}

std::vector<double> VertexBetweennessCentrality(Graph g, std::vector<double> &v_centrality_vec, std::vector<double> &e_centrality_vec){
	StdEdgeIndexMap my_e_index;
	EdgeIndexMap e_index(my_e_index);
	int i=0;
	BGL_FORALL_EDGES(edge, g, Graph){
		my_e_index.insert(std::pair<EdgeDescriptor, int>(edge,i));
		++i;
	}

	//std::vector<double>e_centrality_vec(num_edges(g), 0.0);
	iterator_property_map<std::vector<double>::iterator, EdgeIndexMap> e_centrality_map(e_centrality_vec.begin(), e_index);

	VertexIndexMap v_index = get(vertex_index, g);
	//std::vector<double> v_centrality_vec(num_vertices(g), 0.0);
	iterator_property_map<std::vector<double>::iterator, VertexIndexMap> v_centrality_map(v_centrality_vec.begin(), v_index);
	brandes_betweenness_centrality(g, v_centrality_map, e_centrality_map);
	//relative_betweenness_centrality(g, v_centrality_map);

	return v_centrality_vec;
}

MatrixScore BCBasedLP(Graph g, Matrix distance, std::vector<double> v_centrality_vec, float beta){
	MatrixScore score = initializeScore(g);
	//Matrix distance = createAllPairsShortestPaths(createWeightedGraph(g));

	#pragma omp parallel for
	for(int x=0; x<num_vertices(g); x++){
		for(int y=x+1; y<num_vertices(g); y++){
		/*
			if(distance[x][y] == 3){

			std::vector<float> z;
			double avg=0.0;
			for(int i=0; i<num_vertices(g); i++){
				if((distance[x][i] + distance[i][y] == distance[x][y]) && x!=i && y!=i){
					z.push_back(v_centrality_vec[i]);
					avg += v_centrality_vec[i];
				}
			}

			avg = avg/z.size();

			for(int i=0; i<z.size(); i++){
				score[x][y] += pow(avg - z[i],2);
			}

			if(score[x][y] != 0)
				score[x][y] = sqrt(score[x][y]);
			else{
				double tavg = (v_centrality_vec[x] + v_centrality_vec[y])/2;
				tavg = pow(v_centrality_vec[x]-tavg,2) + pow(v_centrality_vec[y]-tavg,2);
				score[x][y] = sqrt(tavg);
			}

			}
		*/

			for(int i=0; i<num_vertices(g); i++){
				if((distance[x][i] + distance[i][y] == distance[x][y])
				&& x!=i && y!=i){
					//score[x][y] += 1/v_centrality_vec[i];
					//score[x][y] += 1/pow(v_centrality_vec[i],2);
					//score[x][y] += 1/log(v_centrality_vec[i]);
					//score[x][y] += 1/log(1+v_centrality_vec[i]);
					score[x][y] += 1/(pow(log(1+v_centrality_vec[i]),2));
				}
			}

			if(distance[x][y] >= 2)
				score[x][y] *= pow(beta, distance[x][y]-2);
		}
	}

	return score;
}

float BCBasedLPst(Graph g, Matrix distance, std::vector<double> v_centrality_vec, float beta, int s, int t){

	float score=0.0;
	int count=0;
	std::vector<float> z;
	double avg=0.0;
	for(int i=0; i<num_vertices(g); i++){
		if((distance[s][i] + distance[i][t] == distance[s][t]) && s!=i && t!=i){
			//score += v_centrality_vec[i];
			z.push_back(v_centrality_vec[i]);
			avg += v_centrality_vec[i];
			//count++;
		}
	}

	avg = avg/z.size();

	return avg;

	/*
	for(int i=0; i<z.size(); i++){
		score += pow(avg - z[i],2);
	}

	score = sqrt(score/z.size());

	return score;
	*/
}

/*
	Matrix distance = createAllPairsShortestPaths(createWeightedGraph(g));

	MatrixScore score = initializeScore(g);

	MatrixList ml;
	ml.push_back(createMatrix(g));

	for(int i=0; i<num_vertices(g); i++){
		for(int j=i+1; j<num_vertices(g); j++){
			if(distance[i][j] > 1 && distance[i][j] < num_vertices(g)){
				while(distance[i][j] > ml.size())
					ml.push_back(multiplyMatrix(ml[0], ml[ml.size()-1]));

				int num = ml[distance[i][j]-1][i][j];

				for(int k=0; k<num_vertices(g); k++){
					if(distance[i][k] + distance[k][j] == distance[i][j] && k!=i && k!=j){
						score[i][j] += (float)(ml[distance[i][k]-1][i][k] * ml[distance[k][j]-1][k][j])/num;
					}
				}
			}
		}
	}

	return score;
*/

Graph ClusterEdgeCentrality(Graph g, double max_centrality){
	Graph cg = g;
	StdEdgeIndexMap my_e_index;
	EdgeIndexMap e_index(my_e_index);
	int i=0;
	BGL_FORALL_EDGES(edge, cg, Graph){
		my_e_index.insert(std::pair<EdgeDescriptor, int>(edge, i));
		++i;
	}

	std::vector<double>e_centrality_vec(num_edges(cg), 0.0);
	iterator_property_map<std::vector<double>::iterator, EdgeIndexMap> e_centrality_map(e_centrality_vec.begin(), e_index);

	bc_clustering_threshold<double>terminate(max_centrality, cg, false);
	betweenness_centrality_clustering(cg, terminate, e_centrality_map);

	for(int i=0; i<num_vertices(cg); i++){
		for(int j=0; j<num_vertices(cg); j++){
			if(edge(i, j, cg).second == 1){
			}
		}
	}

	return cg;
}

Graph EdgesMST(Graph g, std::vector<double> ebc){
	MatrixScore weights = initializeScore(g);

	int k=0;
	BGL_FORALL_EDGES(e, g, Graph){
		int s = source(e, g);
		int t = target(e, g);
		if(s < t)
			weights[s][t] += ebc[k];
		else if(t < s)
			weights[t][s] += ebc[k];
		k++;
	}

	for(int i=0; i<num_vertices(g); i++){
		for(int j=i+1; j<num_vertices(g); j++){
			weights[i][j] = weights[i][j]/2;
			weights[j][i] = weights[i][j];
		}
	}

	UndirectedGraph ug(num_vertices(g));

	property_map<UndirectedGraph, edge_weight_t>::type weightmap = get(edge_weight,ug);

	BGL_FORALL_EDGES(e, g, Graph){
		int s = source(e, g);
		int t = target(e, g);
		if(edge(s,t,ug).second != 1){
			UndirectedEdgeDescriptor ue;
			bool inserted;
			tie(ue, inserted) = add_edge(s,t,ug);
			weightmap[ue] = (size_t)weights[s][t];
		}
	}

	std::vector<UndirectedEdgeDescriptor> spanning_tree;
	kruskal_minimum_spanning_tree(ug, std::back_inserter(spanning_tree));

	Graph tempg(num_vertices(g));
	for(int i=0; i<spanning_tree.size(); i++){
		int s = source(spanning_tree[i], ug);
		int t = target(spanning_tree[i], ug);

		add_edge(s,t,tempg);
		add_edge(t,s,tempg);
	}

	return tempg;
}

MatrixScore EdgeCentrality(Graph g, std::vector<double> ebc){
	MatrixScore score = initializeScore(g);

	int k=0;
	BGL_FORALL_EDGES(e, g, Graph){
		int s = source(e, g);
		int t = target(e, g);
		score[s][t] = ebc[k];
		score[t][s] = ebc[k];
		k++;
	}

	return score;
}

void NormalizePr(MatrixScore &scores, Matrix distance){
	//mean
	int count=0;
	double avg=0.0;
	for(int x=0; x<scores.size(); x++){
		for(int y=x+1; y<scores[x].size(); y++){
			if(distance[x][y] == 2){
				avg += scores[x][y];
				count++;
			}
		}
	}

	avg = avg/count;

	double dev=0.0;
	for(int x=0; x<scores.size(); x++){
		for(int y=x+1; y<scores[x].size(); y++){
			if(distance[x][y] == 2){
				dev += pow(scores[x][y] - avg, 2);
			}
		}
	}
	dev = sqrt(dev/count);

	count =0;

	for(int x=0; x<scores.size(); x++){
		for(int y=x+1; y<scores[x].size(); y++){
			if(distance[x][y] == 2){
				scores[x][y] = (scores[x][y] - avg)/dev;
				scores[y][x] = scores[x][y];
				if(scores[x][y] >= 1.0)
					count++;
			}
		}
	}
	std::cout << "number of scores above 1: " << count << std::endl;
}


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

class Kalman {
    public:

        Kalman(){
            is_init = false;
        }

        ~Kalman(){
            ;
        }

        bool isInit(){
            return is_init;
        }

        // 初始化状态向量
        void init(Eigen::VectorXd x_in){
            x_ = x_in;
            is_init = true;
        }

        // 状态转移矩阵
        void setF(Eigen::MatrixXd F_in){
            F_ = F_in;
        }

        // 系统的不确定度
        void setP(Eigen::MatrixXd P_in){
            P_ = P_in;
        }

        // 过程噪声 （一般为单位矩阵）
        void setQ(Eigen::MatrixXd Q_in){
            Q_ = Q_in;
        }

        // 测量矩阵
        void setH(Eigen::MatrixXd H_in){
            H_ = H_in;
        }

         // 测量噪声矩阵（一般由传感器厂家提供）
        void setR(Eigen::MatrixXd R_in){
            R_ = R_in;
        }

        // 计算预测值
        void predict(){
            x_ = F_ * x_;
            // std::cout << x_ << std::endl;
            //printf("predict ok, x_ shape is %d %d\n", x_.rows(), x_.cols());
            P_ = F_*P_*F_.transpose();
            //printf("predict ok, P_ shape is %d %d\n", P_.rows(), P_.cols());
        }

        // 观测
        void measurement_update(const Eigen::VectorXd &z){
            // 观测值z与预测值x的差值y
            Eigen::VectorXd y = z - H_ * x_;
            //printf("update ok, y_ shape is %d %d\n", y.rows(), y.cols());
            // 求解卡尔曼增益k，差值y的权重
            Eigen::MatrixXd S_ = H_ * P_ * H_.transpose() + R_;
            //printf("update ok, S_ shape is %d %d\n", S_.rows(), S_.cols());
            Eigen::MatrixXd K_ = P_ * H_.transpose() * S_.inverse();
            //printf("update ok, K_ shape is %d %d\n", K_.rows(), K_.cols());
            // 更新状态向量，考虑了测量值，预测值，整个系统的噪声
            x_ = x_ + K_ * y;
            int size = x_.size();
            Eigen::MatrixXd I = Eigen::MatrixXd::Identity(size, size);
            P_ = (I - K_ * H_) * P_;
        }

        Eigen::VectorXd getX(){
            return x_;
        }

    private:
        bool is_init;
        Eigen::VectorXd x_;
        Eigen::MatrixXd F_, P_, Q_, H_, R_;
};

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

    Kalman kalman;

    int iter=0;
    double mx = 1.0, my = 1.0, dt=0.1;
    while (iter++ < 1070)
    {

        if (!kalman.isInit())
        {
            Eigen::VectorXd x_in(4, 1);
            x_in << mx, my, 0.0, 0.0;
            kalman.init(x_in);

            Eigen::MatrixXd P_in(4, 4);
            P_in << 1.0, 0.0, 0.0, 0.0,
                    0.0, 1.0, 0.0, 0.0,
                    0.0, 0.0, 100.0, 0.0,
                    0.0, 0.0, 0.0, 100.0;
            kalman.setP(P_in);

            Eigen::MatrixXd Q_in(4, 4);
            Q_in << 1.0, 0.0, 0.0, 0.0,
                    0.0, 1.0, 0.0, 0.0,
                    0.0, 0.0, 1.0, 0.0,
                    0.0, 0.0, 0.0, 1.0;

            kalman.setQ(Q_in);

            Eigen::MatrixXd H_in(2, 4);
            H_in << 1.0, 0.0, 0.0, 0.0,
                    0.0, 1.0, 0.0, 0.0;

            kalman.setH(H_in);

            Eigen::MatrixXd R_in(2, 2);
            R_in << 0.0225, 0.0,
                    0.0, 0.0225;

            kalman.setR(R_in);
            continue;
        }

        Eigen::MatrixXd F_in(4, 4);
        F_in << 1.0, 0.0, dt, 0.0,
                0.0, 1.0, 0.0, dt,
                0.0, 0.0, 1.0, 0.0,
                0.0, 0.0, 0.0, 1.0;

        kalman.setF(F_in);
        kalman.predict();

        Eigen::VectorXd z(2, 1);
        z << mx, my;
        kalman.measurement_update(z);

        Eigen::VectorXd x_out = kalman.getX();
        std::cout << "x: " << x_out(0) << ", y:" << x_out(1) << std::endl;
        // std::cout << "vx: " << x_out(2) << ", vy:" << x_out(3) << std::endl;

        std::cout << "mx: " << mx << ", my:" << my << std::endl;

        // dt = 0.1;
        mx += 0.3;
        my += 0.4;

        if(iter == 998) mx = 1, my = 1;
        if(iter == 999) mx = 10, my = 10;
    }

    return 0;
}


#include <fmt/core.h>

#include <Eigen/Sparse>
#include <Eigen/SparseLU>
#include <cassert>
#include <iostream>
#include <vector>

#include "NumpySaver.hpp"
#include "Timer.hpp"
#include "YAMLUtils.hpp"
#include "bsplines.hpp"

using namespace Eigen;
using std::cout, std::endl, std::cerr;

void test_bsplines(double x = .1, Index order = 3, Index nknots = 11) {
  std::vector<double> std_knots(nknots);
  Map<ArrayXd> knots(std_knots.data(), std_knots.size());
  knots = ArrayXd::LinSpaced(nknots, 0, 1);

  auto [values, index] = bsplines::bsplines(x, std_knots, order);

  Map<VectorXd> values_eigen(values.data(), values.size());
  auto sum = values_eigen.sum();
  if (std::abs(sum - 1) > 1e-15) {
    cerr << "The knots are: " << knots.transpose() << endl;
    cerr << "The spline value at " << x << " is: " << values_eigen.transpose()
         << endl;
    cerr << "Summing all spline values (should be 1): " << sum << endl;
  }
}

SparseMatrix<double> build_matrix(const std::vector<double>& knots,
                                  std::size_t order = 3) {
  auto n = knots.size();

  SparseMatrix<double> matrix(n + 1, n + order - 2);
  matrix.reserve(order * (n + 1) - 2);

  auto [deriv_0, _index] = bsplines::ndx_bsplines(knots[0], knots, order, 2);
  for (int col = 0; col < int(order) - 1; col++)
    matrix.insert(0, col) = deriv_0[col + 1];

  for (int row = 1; row < int(n); row++) {
    auto [deriv, index] = bsplines::ndx_bsplines(knots[row], knots, order, 2);
    for (int col = row - 1; col < row - 1 + int(order); col++)
      matrix.insert(row, col) = deriv[col - row + 2];
  }

  auto [deriv_last, __index] =
      bsplines::ndx_bsplines(knots[n - 1], knots, order, 1);
  matrix.insert(n, n + order - 3) = deriv_last[order];
  matrix.insert(n, n + order - 4) = deriv_last[order - 1];

  return matrix;
}

double rho_uniform_sphere(double r, double R, double q) {
  if (r <= R) return q / (4. * M_PI / 3. * R * R * R);

  return 0;
}

double rho_uniform_shell(double r, double R1, double R2, double q) {
  if (r < R1) return 0;
  if (r <= R2) return q / (4. * M_PI / 3. * (R2 * R2 * R2 - R1 * R1 * R1));

  return 0;
}

double rho_hydrogen_ground_state(double r, double e, double r_bohr = 1) {
  return e / M_PI / (r_bohr * r_bohr * r_bohr) * std::exp(-2 * r / r_bohr);
}

VectorXd build_rhs(const std::vector<double>& knots,
                   std::function<double(double)>& rho) {
  auto n = knots.size();
  VectorXd rhs(n + 1);
  for (std::size_t i = 0; i < n; i++) {
    rhs[i] = -knots[i] * 4 * M_PI * rho(knots[i]);
  }

  rhs[n] = 0;

  return rhs;
}

double evaluate_spline(const double& x, const std::vector<double>& knots,
                       const VectorXd& weights, std::size_t order = 3) {
  auto n = knots.size();
  assert(int(n + order) - 1 == weights.size());

  auto [values, index] = bsplines::bsplines(x, knots, order);

  double sum = 0;
  for (std::size_t i = 0; i <= order; i++)
    sum += values[i] * weights[index + i];

  return sum;
}

VectorXd evaluate_spline(const VectorXd& x, const std::vector<double>& knots,
                         const VectorXd& weights, std::size_t order = 3) {
  auto n = x.size();
  VectorXd ret(n);
  for (Index i = 0; i < n; i++) {
    ret[i] = evaluate_spline(x[i], knots, weights, order);
  }
  return ret;
}

void solve_problem(YAML::Node node) {
  using namespace std::placeholders;

  auto name = node["name"].as<std::string>();
  auto R = node["R"].as<double>();
  auto R1 = node["R1"].as<double>();
  auto R2 = node["R2"].as<double>();
  auto q = node["q"].as<double>();
  auto e = node["e"].as<double>();
  auto r_bohr = node["r_bohr"].as<double>();

  YAML::Node knot_node = node["knots"];
  auto knots = get_yaml_values<double>(knot_node);

  auto matrix = build_matrix(knots);

  std::vector<std::tuple<const char*, std::function<double(double)>>> rhos = {
      {"uniform_sphere", std::bind(rho_uniform_sphere, _1, R, q)},
      {"uniform_shell", std::bind(rho_uniform_shell, _1, R1, R2, q)},
      {"hydrogen", std::bind(rho_hydrogen_ground_state, _1, e, r_bohr)},
  };

  if (knots.size() < 30) {
    cout << "Knots = ";
    for (std::size_t i = 0; i < knots.size(); i++) cout << knots[i] << ", ";
    cout << "\n";
  }

  // first do the (sparse) LU decomposition
  Eigen::SparseLU<Eigen::SparseMatrix<double>, Eigen::COLAMDOrdering<int>>
      solver;
  solver.analyzePattern(matrix);
  solver.factorize(matrix);

  VectorXd x = VectorXd::LinSpaced(1000, knots[0], knots[knots.size() - 1]);

  for (std::size_t i = 0; i < rhos.size(); i++) {
    auto rho_name = std::get<0>(rhos[i]);
    auto rho = std::get<1>(rhos[i]);

    cout << "\nProblem = " << rho_name << "\n";

    auto rhs = build_rhs(knots, rho);
    auto solution = solver.solve(rhs);
    if (knots.size() < 30) {
      cout << "rhs vector: " << rhs.transpose() << "\n";
      cout << "solution vector: " << solution.transpose() << "\n";
    }

    VectorXd weights(solution.size() + 1);
    weights << 0, solution;

    VectorXd sol = evaluate_spline(x, knots, weights);

    NumpySaver(fmt::format("build/output/solution_{}_{}.npy", name, rho_name))
        << x << sol;
  }
  cout << "\n";
}

void plot_bsplines(std::size_t order = 3) {
  std::vector<double> std_knots(11);
  Map<ArrayXd> knots(std_knots.data(), std_knots.size());
  knots = ArrayXd::LinSpaced(11, 0, 1);
  VectorXd xs = VectorXd::LinSpaced(1000, knots[0], knots[knots.size() - 1]);
  MatrixXd spline(xs.size(), order + 1);
  MatrixXd dspline(xs.size(), order + 1);
  MatrixXd ddspline(xs.size(), order + 1);

  for (Index i = 0; i < xs.size(); i++) {
    auto [x_spline, index_0] = bsplines::bsplines(xs[i], std_knots, order);
    auto [x_dspline, index_1] =
        bsplines::ndx_bsplines(xs[i], std_knots, order, 1);
    auto [x_ddspline, index_2] =
        bsplines::ndx_bsplines(xs[i], std_knots, order, 2);
    for (std::size_t j = 0; j < order + 1; j++) {
      spline(i, j) = x_spline[j];
      dspline(i, j) = x_dspline[j];
      ddspline(i, j) = x_ddspline[j];
    }
  }
  NumpySaver saver("build/output/plot_splines.npy");
  saver << xs;
  for (std::size_t j = 0; j < order + 1; j++) saver << spline.col(j);
  for (std::size_t j = 0; j < order + 1; j++) saver << dspline.col(j);
  for (std::size_t j = 0; j < order + 1; j++) saver << ddspline.col(j);
}

int main() {
  cout << "Testing if all spline values at each point sum to 1, no error means "
          "it worked."
       << endl;
  for (double x = 0; x <= 1; x += .001) {
    test_bsplines(x, 3, 11);
  }

  std::vector<double> std_knots(11);
  Map<ArrayXd> knots(std_knots.data(), std_knots.size());
  knots = ArrayXd::LinSpaced(11, 0, 1);

  cout << "Testing Matrix creation..." << endl;
  auto matrix = build_matrix(std_knots);
  cout << "Matrix size is " << matrix.rows() << "x" << matrix.cols() << endl;
  cout << matrix << endl;

  YAML::Node config = YAML::LoadFile("config.yaml");

  for (std::size_t i = 0; i < config.size(); i++) {
    cout << "\n\n*****************************************\n"
         << fmt::format("Running simulation {}/{}", i + 1, config.size())
         << endl;
    solve_problem(config[i]);
  }

  plot_bsplines();
  return 0;
}


//  (C) Copyright Nick Thompson 2021.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
#ifndef BOOST_MATH_TOOLS_QUARTIC_ROOTS_HPP
#define BOOST_MATH_TOOLS_QUARTIC_ROOTS_HPP
#include <array>
#include <cmath>
#include <boost/math/tools/cubic_roots.hpp>

namespace boost::math::tools {

namespace detail {

// Make sure the nans are always at the back of the array:
template<typename Real>
bool comparator(Real r1, Real r2) {
   using std::isnan;
   if (isnan(r2)) { return true; }
   return r1 < r2;
}

template<typename Real>
std::array<Real, 4> polish_and_sort(Real a, Real b, Real c, Real d, Real e, std::array<Real, 4>& roots) {
    // Polish the roots with a Halley iterate.
    using std::fma;
    using std::abs;
    for (auto &r : roots) {
        Real df = fma(4*a, r, 3*b);
        df = fma(df, r, 2*c);
        df = fma(df, r, d);
        Real d2f = fma(12*a, r, 6*b);
        d2f = fma(d2f, r, 2*c);
        Real f = fma(a, r, b);
        f = fma(f,r,c);
        f = fma(f,r,d);
        f = fma(f,r,e);
        Real denom = 2*df*df - f*d2f;
        if (abs(denom) > (std::numeric_limits<Real>::min)())
        {
            r -= 2*f*df/denom;
        }
    }
    std::sort(roots.begin(), roots.end(), detail::comparator<Real>);
    return roots;
}

}
// Solves ax^4 + bx^3 + cx^2 + dx + e = 0.
// Only returns the real roots, as these are the only roots of interest in ray intersection problems.
// Follows Graphics Gems V: https://github.com/erich666/GraphicsGems/blob/master/gems/Roots3And4.c
template<typename Real>
std::array<Real, 4> quartic_roots(Real a, Real b, Real c, Real d, Real e) {
    using std::abs;
    using std::sqrt;
    auto nan = std::numeric_limits<Real>::quiet_NaN();
    std::array<Real, 4> roots{nan, nan, nan, nan};
    if (abs(a) <= (std::numeric_limits<Real>::min)()) {
        auto cbrts = cubic_roots(b, c, d, e);
        roots[0] = cbrts[0];
        roots[1] = cbrts[1];
        roots[2] = cbrts[2];
        if (b == 0 && c == 0 && d == 0 && e == 0) {
           roots[3] = 0;
        }
        return detail::polish_and_sort(a, b, c, d, e, roots);
    }
    if (abs(e) <= (std::numeric_limits<Real>::min)()) {
        auto v = cubic_roots(a, b, c, d);
        roots[0] = v[0];
        roots[1] = v[1];
        roots[2] = v[2];
        roots[3] = 0;
        return detail::polish_and_sort(a, b, c, d, e, roots);
    }
    // Now solve x^4 + Ax^3 + Bx^2 + Cx + D = 0.
    Real A = b/a;
    Real B = c/a;
    Real C = d/a;
    Real D = e/a;
    Real Asq = A*A;
    // Let x = y - A/4:
    // Mathematica: Expand[(y - A/4)^4 + A*(y - A/4)^3 + B*(y - A/4)^2 + C*(y - A/4) + D]
    // We now solve the depressed quartic y^4 + py^2 + qy + r = 0.
    Real p = B - 3*Asq/8;
    Real q = C - A*B/2 + Asq*A/8;
    Real r = D - A*C/4 + Asq*B/16 - 3*Asq*Asq/256;
    if (abs(r) <= (std::numeric_limits<Real>::min)()) {
        auto [r1, r2, r3] = cubic_roots(Real(1), Real(0), p, q);
        r1 -= A/4;
        r2 -= A/4;
        r3 -= A/4;
        roots[0] = r1;
        roots[1] = r2;
        roots[2] = r3;
        roots[3] = -A/4;
        return detail::polish_and_sort(a, b, c, d, e, roots);
    }
    // Biquadratic case:
    if (abs(q) <= (std::numeric_limits<Real>::min)()) {
        auto [r1, r2] = quadratic_roots(Real(1), p, r);
        if (r1 >= 0) {
           Real rtr = sqrt(r1);
           roots[0] = rtr - A/4;
           roots[1] = -rtr - A/4;
        }
        if (r2 >= 0) {
           Real rtr = sqrt(r2);
           roots[2] = rtr - A/4;
           roots[3] = -rtr - A/4;
        }
        return detail::polish_and_sort(a, b, c, d, e, roots);
    }

    // Now split the depressed quartic into two quadratics:
    // y^4 + py^2 + qy + r = (y^2 + sy + u)(y^2 - sy + v) = y^4 + (v+u-s^2)y^2 + s(v - u)y + uv
    // So p = v+u-s^2, q = s(v - u), r = uv.
    // Then (v+u)^2 - (v-u)^2 = 4uv = 4r = (p+s^2)^2 - q^2/s^2.
    // Multiply through by s^2 to get s^2(p+s^2)^2 - q^2 - 4rs^2 = 0, which is a cubic in s^2.
    // Then we let z = s^2, to get
    // z^3 + 2pz^2 + (p^2 - 4r)z - q^2 = 0.
    auto z_roots = cubic_roots(Real(1), 2*p, p*p - 4*r, -q*q);
    // z = s^2, so s = sqrt(z).
    // No real roots:
    if (z_roots.back() <= 0) {
      return roots;
    }
    Real s = sqrt(z_roots.back());

    // s is nonzero, because we took care of the biquadratic case.
    Real v = (p + s*s + q/s)/2;
    Real u = v - q/s;
    // Now solve y^2 + sy + u = 0:
    auto [root0, root1] = quadratic_roots(Real(1), s, u);

    // Now solve y^2 - sy + v = 0:
    auto [root2, root3] = quadratic_roots(Real(1), -s, v);
    roots[0] = root0;
    roots[1] = root1;
    roots[2] = root2;
    roots[3] = root3;

    for (auto& r : roots) {
        r -= A/4;
    }
    return detail::polish_and_sort(a, b, c, d, e, roots);
}

}
#endif


#include <iostream>
#include <Eigen/Dense>
#include "uranus/Matrix.hpp"
#include "uranus/un-constrained.hpp"

int main()
{
	problem<2> f;	  // f = x_1^2 + 4x_2^2

	f.matirx_Jacobian_ << 2, 0, 0, 8;

    uranus::Vector<2> x0;
	x0 << 1, 1;                   // 初始点

	uranus::Vector<2> y = BFGS<2>(f,x0,0.001,true); // 用BFGS求解

    std::cout <<"zuiyoujie:\n" << y <<"\n";

    // dasds
    problem<2> f2;
    f2.matirx_Jacobian_ << 2, 0, 0, 2;
    uranus::Vector<2> x2;
    x2 << 5,5;

    double M_k = 0.1;                          // init M_k > 0
    uranus::Vector<2> x3;
    double a;
	do{
		M_k = 10 * M_k;                         // update M_k
		f2.matirx_Jacobian_(1,1) = 2 + M_k;     // update J mat
        cout << "M_k=============================\n"<<M_k<<"\n";
		x3 = BFGS(f2, x2, 0.001, true);       // min f

        a = M_k * pow(x3(1)-1, 2);
        std::cout <<"x3: \n" << x3 <<"\n";
	}
	while(a > 0.01);

    std::cout <<"x3: \n" << x3 <<"\n";

    return 0;
}


#pragma once

#include <polyfem/Types.hpp>

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

namespace polyfem
{

	// Show some stats about the matrix M: det, singular values, condition number, etc
	void show_matrix_stats(const Eigen::MatrixXd &M);

	template <typename T>
	T determinant(const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, 0, 3, 3> &mat)
	{
		assert(mat.rows() == mat.cols());

		if (mat.rows() == 1)
			return mat(0);
		else if (mat.rows() == 2)
			return mat(0, 0) * mat(1, 1) - mat(0, 1) * mat(1, 0);
		else if (mat.rows() == 3)
			return mat(0, 0) * (mat(1, 1) * mat(2, 2) - mat(1, 2) * mat(2, 1)) - mat(0, 1) * (mat(1, 0) * mat(2, 2) - mat(1, 2) * mat(2, 0)) + mat(0, 2) * (mat(1, 0) * mat(2, 1) - mat(1, 1) * mat(2, 0));

		assert(false);
		return T(0);
	}

	template <typename T>
	bool read_matrix(const std::string &path, Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> &mat);

	template <typename T>
	bool read_matrix_binary(const std::string &path, Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> &mat);

	template <typename Mat>
	bool write_matrix_binary(const std::string &path, const Mat &mat);

	class SpareMatrixCache
	{
	public:
		SpareMatrixCache() {}
		SpareMatrixCache(const size_t size);
		SpareMatrixCache(const size_t rows, const size_t cols);
		SpareMatrixCache(const SpareMatrixCache &other);

		void init(const size_t size);
		void init(const size_t rows, const size_t cols);
		void init(const SpareMatrixCache &other);

		void set_zero();

		inline void reserve(const size_t size) { entries_.reserve(size); }
		inline size_t entries_size() const { return entries_.size(); }
		inline size_t capacity() const { return entries_.capacity(); }
		inline size_t non_zeros() const { return mapping_.empty() ? mat_.nonZeros() : values_.size(); }

		void add_value(const int i, const int j, const double value);
		StiffnessMatrix get_matrix(const bool compute_mapping = true);
		void prune();

		SpareMatrixCache operator+(const SpareMatrixCache &a) const;
		void operator+=(const SpareMatrixCache &o);

	private:
		size_t size_;
		StiffnessMatrix tmp_, mat_;
		std::vector<Eigen::Triplet<double>> entries_;
		std::vector<std::vector<std::pair<int, size_t>>> mapping_;
		std::vector<int> inner_index_, outer_index_;
		std::vector<double> values_;
	};

} // namespace polyfem


/**
 * File: random_var.cpp
 * Date: Mon Nov  9 10:31:35 CET 2020
 * Author: Open Risk  (www.openriskmanagement.com)
 *
 * Examples of using the tailRisk library to compute tail risk measures
 *
 */

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

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

    // Reading in some data for a type 0 representation (discrete distribution)
    int LossGrid = 1000;
    int DataType = 0;
    RandomVar L(LossGrid, DataType);
    L.ReadFromJSON("../../data/example5.json");
    L.Print();

    // Calculate various measures
    double alpha = 0.8;
    int threshold = 0;

    std::cout << "Mean Value: " << L.Mean() << std::endl;
    std::cout << "Median Value: " << L.Median() << std::endl;
    std::cout << "STD Value: " << L.StandardDeviation() << std::endl;
    std::cout << "Kurtosis: " << L.Kurtosis() << std::endl;
    std::cout << "Skeweness: " << L.Skeweness() << std::endl;
    std::cout << "Quantile @ " << alpha << ": " << L.Quantile(alpha) << std::endl;
    std::cout << "Quantile Index @ " << alpha << ": " << L.Quantile_Index(alpha) << std::endl;
    std::cout << "VaR @ " << alpha << ": " << L.VaR(alpha) << std::endl;
    std::cout << "Expected Shortfall @ " << alpha << ": " << L.ExpectedShortFall(alpha) << std::endl;
    std::cout << "Exceedance Probability: " << L.ExceedanceProbability(threshold) << std::endl;
    std::cout << "Mean Excess: " << L.MeanExcess(threshold ) << std::endl;

    return 0;
}


//=======================================================================
// Copyright (c) 2014 Andrzej Pacuk
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
/**
 * @file performance_measures.hpp
 * @brief
 * @author Andrzej Pacuk
 * @version 1.0
 * @date 2014-10-22
 */
#ifndef PALL_PERFORMANCE_MEASURES_HPP
#define PALL_PERFORMANCE_MEASURES_HPP

#include <boost/range/combine.hpp>
#include <boost/range/empty.hpp>
#include <boost/range/size.hpp>
#include <boost/tuple/tuple.hpp>

#include <algorithm>
#include <cmath>
#include <utility>

namespace paal {

/**
 * @brief
 *
 * @tparam FloatType
 * @tparam Probs
 * @tparam TestResults
 *
 * @param probs
 * @param test_results
 */
template<typename FloatType = double,
         typename Probs, typename TestResults>
FloatType log_loss(Probs &&probs, TestResults &&test_results) {
    assert(boost::size(probs) == boost::size(test_results));
    assert(!boost::empty(probs));

    FloatType loss{};
    static FloatType EPSILON{1e-6};
    for(auto prob_result : boost::combine(probs, test_results)) {
        FloatType prob, result;
        boost::tie(prob, result) = prob_result;
        loss -= std::log(std::max(result ? prob : 1 - prob,
                                  EPSILON));
    }

    return loss / boost::size(probs);
}

/**
 * @brief
 *
 * @tparam FloatType
 * @param log_loss
 *
 * @return
 */
template<typename FloatType>
FloatType likelihood_from_log_loss(FloatType log_loss) {
    return std::exp(-log_loss);
}

/**
 * @brief
 *
 * @tparam FloatType
 * @tparam Probs
 * @tparam TestResults
 *
 * @param probs
 * @param test_results
 */
template<typename FloatType = double,
         typename Probs, typename TestResults>
FloatType likelihood(Probs &&probs, TestResults &&test_results) {
    return likelihood_from_log_loss(log_loss<FloatType>(std::forward<Probs>(probs),
                                    std::forward<TestResults>(test_results)));
}

/**
 * @brief
 *
 * @tparam FloatType
 * @tparam Probs
 * @tparam TestResults
 *
 * @param probs
 * @param test_results
 */
template<typename FloatType = double,
typename Probs, typename TestResults>
FloatType mean_absolute_error(Probs &&probs, TestResults &&test_results) {
    assert(boost::size(probs) == boost::size(test_results));
    assert(!boost::empty(probs));

    FloatType loss{};
    for(auto prob_result : boost::combine(probs, test_results)) {
        FloatType prob, result;
        boost::tie(prob, result) = prob_result;
        loss += std::abs(prob - result);
    }

    return loss / boost::size(probs);
}

} //! paal

#endif /* PALL_PERFORMANCE_MEASURES_HPP */


#pragma once

#include <vector>
#include <cassert>

#include <iostream>
#include <iomanip>

#include <Eigen/Dense>

//! \file rkintegrator.hpp Solution for Problem 1, implementing RkIntegrator class

//! \brief Implements a Runge-Kutta explicit solver for a given Butcher tableau for autonomous ODEs
//! \tparam State a type representing the space in which the solution lies, e.g. R^d, represented by e.g. Eigen::VectorXd.
template <class State>
class RKIntegrator {
public:
    //! \brief Constructor for the RK method.
    //! Performs size checks and copies A and b into internal storage
    //! \param[in] A matrix containing coefficents of Butcher tableau, must be (strictly) lower triangular (no check)
    //! \param[in] b vector containing coefficients of lower part of Butcher tableau
    RKIntegrator(const Eigen::MatrixXd & A, const Eigen::VectorXd & b)
        : A(A), b(b), s(b.size()) {
        assert( A.cols() == A.rows() && "Matrix must be square.");
        assert( A.cols() == b.size() && "Incompatible matrix/vector size.");
    }

    //! \brief Perform the solution of the ODE
    //! Solve an autonomous ODE y' = f(y), y(0) = y0, using a RK scheme given in the Butcher tableau provided in the
    //! constructor. Performs N equidistant steps upto time T with initial data y0
    //! \tparam Function type for function implementing the rhs function. Must have State operator()(State x)
    //! \param[in] f function handle for rhs in y' = f(y), e.g. implemented using lambda funciton
    //! \param[in] T final time T
    //! \param[in] y0 initial data y(0) = y0 for y' = f(y)
    //! \param[in] N number of steps to perform. Step size is h = T / N. Steps are equidistant.
    //! \return vector containing all steps y^n (for each n) including initial and final value
    template <class Function>
    std::vector<State> solve(const Function &f, double T, const State & y0, unsigned int N) const {
        // Iniz step size
        double h = T / N;

        // Will contain all steps, reserve memory for efficiency
        std::vector<State> res;
        res.reserve(N+1);

        // Store initial data
        res.push_back(y0);

        // Initialize some memory to store temporary values
        State ytemp1 = y0;
        State ytemp2 = y0;
        // Pointers to swap previous value
        State * yold = &ytemp1;
        State * ynew = &ytemp2;

        // Loop over all fixed steps
        for(unsigned int k = 0; k < N; ++k) {
            // Compute, save and swap next step
            step(f, h, *yold, *ynew);
            res.push_back(*ynew);
            std::swap(yold, ynew);
        }

        return res;
    }

private:

    //! \brief Perform a single step of the RK method for the solution of the autonomous ODE
    //! Compute a single explicit RK step y^{n+1} = y_n + \sum ... starting from value y0 and storing next value in y1
    //! \tparam Function type for function implementing the rhs. Must have State operator()(State x)
    //! \param[in] f function handle for ths f, s.t. y' = f(y)
    //! \param[in] h step size
    //! \param[in] y0 initial state
    //! \param[out] y1 next step y^{n+1} = y^n + ...
    template <class Function>
    void step(const Function &f, double h, const State & y0, State & y1) const {
        // create vector holding next value
        y1 = y0;

        // Reserve space for increments
        std::vector<State> k;
        k.reserve(s);

        // Loop over the size of RK
        for(unsigned int i = 0; i < s; ++i) {
            // Compute increments and save them to k
            State incr = y0;
            for(unsigned int j = 0; j < i; ++j) {
                incr += h*A(i,j)*k.at(j);
            }
            k.push_back( f( incr ) );
            y1 += h * b(i) * k.back();
        }
    }

    //! Matrix A in Butcher scheme
    const Eigen::MatrixXd A;
    //! Vector b in Butcher scheme
    const Eigen::VectorXd b;
    //! Size of Butcher matrix and vector A and b
    unsigned int s;
};

Nummerical Methods for CSE/PS13/solutions_ps13/rkintegrator.hpp