/*
 *  (C) Copyright Nick Thompson 2018.
 *  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_INTEGER_MOD_INVERSE_HPP
#define BOOST_INTEGER_MOD_INVERSE_HPP
#include <stdexcept>
#include <boost/throw_exception.hpp>
#include <boost/integer/extended_euclidean.hpp>

namespace boost { namespace integer {

// From "The Joy of Factoring", Algorithm 2.7.
// Here's some others names I've found for this function:
// PowerMod[a, -1, m] (Mathematica)
// mpz_invert (gmplib)
// modinv (some dude on stackoverflow)
// Would mod_inverse be sometimes mistaken as the modular *additive* inverse?
// In any case, I think this is the best name we can get for this function without agonizing.
template<class Z>
Z mod_inverse(Z a, Z modulus)
{
    if (modulus < Z(2))
    {
        BOOST_THROW_EXCEPTION(std::domain_error("mod_inverse: modulus must be > 1"));
    }
    // make sure a < modulus:
    a = a % modulus;
    if (a == Z(0))
    {
        // a doesn't have a modular multiplicative inverse:
        return Z(0);
    }
    boost::integer::euclidean_result_t<Z> u = boost::integer::extended_euclidean(a, modulus);
    if (u.gcd > Z(1))
    {
        return Z(0);
    }
    // x might not be in the range 0 < x < m, let's fix that:
    while (u.x <= Z(0))
    {
        u.x += modulus;
    }
    // While indeed this is an inexpensive and comforting check,
    // the multiplication overflows and hence makes the check itself buggy.
    //BOOST_ASSERT(u.x*a % modulus == 1);
    return u.x;
}

}}
#endif


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

#include <iostream>
#include <fstream>
#include <utility>
#include <cmath>
#include <experimental/random>

#include <cstdlib>
#include <Eigen/unsupported/Eigen/FFT>

using namespace Eigen;

#include "kshape.h"

typedef Matrix<long double,Dynamic,Dynamic> MatrixXld;
typedef Matrix< std::complex< long double >, Dynamic, Dynamic > MatrixXcld;
typedef Matrix< long double, Dynamic, 1 > VectorXld;
typedef Matrix< std::complex< long double >, Dynamic, 1 > VectorXcld;

VectorXld coefWiseDivision(VectorXld x, VectorXld y){
    VectorXld ans(x.size());
    for (int i=0;i<x.size();++i){
          ans(i) = x(i)/y(i);
    }
    return ans;
}

VectorXld circshift(VectorXld vec,int shift)
{
  if (shift==0)
	{
		return vec;
	}

	int n = vec.size();
	VectorXld y(n);
  y.setZero();

	if (shift > 0) // shift right
	{
		y.head(shift) = vec.tail(shift);
		y.tail(n - shift) = vec.head(n - shift);
	}

  if(shift<0) // shift left
	{
		y.head(n + shift) = vec.tail(n + shift);
		y.tail(abs(shift)) = vec.head(abs(shift));

	}

	return y;
}

VectorXld shiftWithZeors(VectorXld vec, int shift){
  int n = vec.size();
  VectorXld newvec(n);
  newvec.setZero();
  if (shift==0)
	{
    newvec.array() = vec.array();
		return newvec;
	}

  if (std::abs(shift)>n){
    return newvec;
  }

	 if (shift > 0) // shift right
	{
		newvec.tail(n - shift) = vec.head(n - shift);
    //vec.head(shift).setZero();
	}

  if(shift<0) // shift left
	{
		newvec.head(n + shift) = vec.tail(n + shift);
		//vec.tail(abs(shift)).setZero();
	}

	return newvec;
}

VectorXld NCC(VectorXld x, VectorXld y){
    long double normed =  x.norm()*y.norm();
    if (normed == 0){
        normed =  1000000000000.0 ;
    }

    int x_len = x.size();

    int fft_size = 1 << (int)ceil(log2((2*x_len - 1)));

    FFT<long double> fft;
    VectorXcld x_out;

    VectorXld transformed_x(fft_size);
    transformed_x.setZero();

    transformed_x.head(x_len) = x;
    fft.fwd(x_out, transformed_x);

    VectorXcld y_out;
    VectorXld transformed_y(fft_size);
    transformed_y.setZero();
    transformed_y.head(y.size()) = y;
    fft.fwd(y_out, transformed_y);

    VectorXld cc_out;
    VectorXcld inter =  x_out.cwiseProduct( y_out.conjugate() );
    fft.inv(cc_out, inter);

    VectorXld cc(2*x_len - 1);
    cc.setZero();
    cc.head(x_len-1) = cc_out.tail(x_len-1);
    cc.tail(x_len) = cc_out.head(x_len);

    return cc.real() / normed;
}

std::vector<MatrixXld> NCC3D(MatrixXld x, MatrixXld y){
    VectorXld x_norm = x.rowwise().norm();
    VectorXld y_norm = y.rowwise().norm();
    MatrixXld den(x_norm.size(),y_norm.size());
    for (int i = 0;i<x_norm.size();++i){
          den.row(i) = x_norm(i)*(y_norm);
    }
    den = den.unaryExpr([](long double v) { return v==0.0 ? 1000000000.0 : v; });
    int x_len = x.cols();
    int y_len = y.cols();

    int fft_size = 1 << (int)ceil(log2((2*x_len - 1)));

    FFT<long double> fft;

    MatrixXcld x_out;
    MatrixXcld y_out;

    std::vector<MatrixXld> cc(y.rows());

    for (int k=0;k<y.rows();++k){

      VectorXld y_in(fft_size);
      y_in.setZero();
      VectorXcld fft_y_out;

      y_in.head(y_len) = y.row(k);
      fft.fwd(fft_y_out, y_in);

      MatrixXld cc_row(x.rows(), 2*x_len-1);
      for (int j=0;j<x.rows();++j){
          VectorXld x_in(fft_size);
          x_in.setZero();
          VectorXcld fft_x_out;

          x_in.head(x_len) = x.row(j);
          fft.fwd(fft_x_out, x_in);
          VectorXld inverted_product;
          VectorXcld t1 = fft_x_out.cwiseProduct(fft_y_out.conjugate());
          fft.inv(inverted_product, t1);

          cc_row.row(j).head(x_len-1) = inverted_product.tail(x_len-1);
          cc_row.row(j).tail(x_len) = inverted_product.head(x_len);
          cc_row.row(j) = cc_row.row(j).array() / den.coeff(j,k);
      }
      cc[k]=cc_row;
    }

    return cc;
}

std::pair<long double,VectorXld> SBD (VectorXld x, VectorXld y){
      VectorXld ncc = NCC(x, y);
      int idx;
      long double dist = 1.0 - ncc.maxCoeff(&idx);

      VectorXld yshift = shiftWithZeors(y, (idx+1)-std::max(x.size(),y.size()));

     return std::make_pair(dist, yshift);
}

MatrixXld z_norm(MatrixXld a, int axis = 0,int ddof=0){
      VectorXld means;
      VectorXld sstd;
      if (axis == 0){
        means = a.colwise().mean();
        int n = means.size();

        VectorXld sstd = VectorXld(n);
        for (int i=0;i<n;++i){
          sstd(i)=((a.col(i).array()-means(i)).pow(2).colwise().sum().sum()/(a.rows()-ddof));
        }

        sstd = sstd.array().pow(0.5);

        MatrixXld temp = (a.rowwise()-means.transpose());
        for (int i=0;i<n;i++){

          temp.col(i) = (temp.col(i)/(sstd(i)));
        }

        return temp;
      }
      else {
        means = a.rowwise().mean();
        int n = means.size();
        VectorXld sstd = VectorXld(n);
        for (int i=0;i<n;++i){
          sstd(i)=((a.row(i).array()-means(i)).pow(2).rowwise().sum().sum()/(a.cols()-ddof));
        }

        sstd = sstd.array().pow(0.5);

        MatrixXld temp = (a.colwise()-means);
        for (int i=0;i<n;i++){
          temp.row(i) = (temp.row(i)/(sstd(i)));
        }

        return temp;
      }
}

VectorXld extractShape(MatrixXld cluster, VectorXld cur_center){
    if (cluster.rows()==1){
      return cluster.row(0);
    }

      for (int i=0;i<cluster.rows();++i){
          if (cur_center.sum()!=0.0){
              cluster.row(i) = SBD(cur_center,cluster.row(i)).second;
          }
    }
    MatrixXld y = z_norm(cluster, 1,1);
    MatrixXld s = y.transpose()*y;

    int columns = cluster.cols();

    MatrixXld p(columns, columns);
    p.array() = 1.0/(long double)columns;

    p = MatrixXld::Identity(columns, columns) - p;

    MatrixXld m = (p.transpose()*s)*p;
    SelfAdjointEigenSolver<MatrixXld> eigensolver(m);
    MatrixXcld eigenVectors = eigensolver.eigenvectors();
    int cols = eigenVectors.cols();
    VectorXcld eigenvalues = eigensolver.eigenvalues();

    VectorXld centroid = eigenVectors.col(cols-1).real();

    long double dist1 = (cluster.row(0)-centroid.transpose()).array().pow(2).sum();
    long double dist2 = (cluster.row(0)+centroid.transpose()).array().pow(2).sum();

    if (dist1>=dist2){
      centroid.array() *= -1.0;
    }

    return z_norm(centroid,0,1);

  }

std::pair<std::vector<int>, MatrixXld> kshape(MatrixXld x, int k, int runs_to_average_over){
    int m = x.rows();
    int n = x.cols();

    std::vector<int> vidx(m);
    std::vector<int> bestidx(m);

    MatrixXld bestcentroids(k,n);
    bestcentroids.setZero();

    int number_in_center[k];
    long double bestdist=0.0;

    for (int run=0;run<runs_to_average_over;++run){
          MatrixXld centroids(k,n);
          centroids.setZero();

          bool changed;
          long double dist;
          memset(number_in_center,0,sizeof(number_in_center));

          for (int i=0;i<m;i++){
            int rint = std::experimental::randint(0, k-1);
            vidx[i] = rint;
            number_in_center[rint] += 1;
          }

          for (int iter=0;iter<100;++iter){
            std::cout << "Commence Iteration " << iter;
            for (int j=0;j<k;++j){
                int center_j_size = number_in_center[j];
                MatrixXld center_j = MatrixXld(center_j_size, n);
                int row = 0;
                for (int l=0;l<m;++l){
                    if (vidx[l]==j){
                        center_j.row(row) = x.row(l);
                        ++row;
                    }
                }

                if (row==0){
                  centroids.row(j).array() = 0;
                }
                else {
                  centroids.row(j) = extractShape(center_j, centroids.row(j)).transpose();
                }

            }

            std::vector<MatrixXld> x_corr = NCC3D(x, centroids);

            MatrixXld distances(m, k);

            for (int i=0;i<k;++i){
              distances.col(i) = 1 - x_corr[i].rowwise().maxCoeff().array();
            }
            memset(number_in_center,0,sizeof(number_in_center));
            changed = false;
            dist = 0.0;
            for (int i=0;i<x.rows();++i){
              int tempi;
              dist+=std::abs(distances.row(i).minCoeff(&tempi));
              if (vidx[i]!=tempi){
                vidx[i]=tempi;
                changed = true;
              }
              number_in_center[tempi]++;
            }

            std::cout << "\r";
            if (!changed){
              if (dist>bestdist){
                bestdist=dist;
                bestidx=vidx;
                bestcentroids = centroids;
              }
              break;
            }
          }

          if (changed){
            if (dist>bestdist){
              bestdist=dist;
              bestidx=vidx;
              bestcentroids = centroids;
            }
          }
    }

    return std::make_pair(bestidx, bestcentroids);
}

MatrixXld read_csv(std::string filename, int rows, int cols){
      std::ifstream file(filename);
      MatrixXld mat(rows,cols);

      std::string line;
      long double val; int row=0;

      while(std::getline(file, line))
      {
          std::stringstream ss(line);
          // Keep track of the current column index
          int col = 0;
          // Extract each integer
          while(ss >> val){
              // Add the current integer to the 'colIdx' column's values vector
              mat(row,col) = val;
              // If the next token is a comma, ignore it and move on
              if(ss.peek() == ',') ss.ignore();
              // Increment the column index
              col++;
          }
        row++;
      }

      file.close();

      return z_norm(mat,1,1);
}

void write_csv(std::string filename, MatrixXld mat){
      std::ofstream file(filename);

      for(int i = 0; i < mat.rows(); ++i)
      {
          for(int j = 0; j < mat.cols(); ++j)
          {
              file << mat(i,j);
              if(j != mat.cols() - 1) file << ",";
          }
          file << "\n";
      }
      file.close();

}

void write_csv(std::string filename, std::vector<int> v){
      std::ofstream file(filename);

      for(int j = 0; j < v.size(); ++j){
            file << v[j];
            if(j != v.size() - 1) file << ",";
      }
      file << "\n";
      file.close();
}

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

  if (argc < 6){
    std::cerr << "Need to speficy a csv, and its number of rows and columns, as well as K and a number of runs" << std::endl;
    return 1;
  }

  std::string csv_filename = std::string(argv[1]);
  int rows = atoi(argv[2]), cols = atoi(argv[3]), k = atoi(argv[4]), runs=atoi(argv[5]);

  MatrixXld mat = read_csv(csv_filename, rows, cols);
  mat = z_norm(mat, 1,1);
  std::pair<std::vector<int>, MatrixXld> result = kshape(mat, k, runs);
  write_csv("out_centroids.csv", result.second);
  write_csv("out_indices.csv", result.first);
  std::vector<int> cluster_counts(k);
  for (auto i: result.first) cluster_counts[i]++;
  std::cout << std::endl;
  for (int i=0;i<k;++i){
    std::cout << i << ": " << cluster_counts[i] << std::endl;
  }

  return 0;
}


/*
    Boost Competency Test - GSoC 2020
    digu_J - Digvijay Janartha
    NIT Hamirpur - INDIA
*/

#include <algorithm>
#include <iostream>
#include <iterator>
#include <utility>

#include <boost/geometry/geometry.hpp>

namespace bg = boost::geometry;

namespace algo1
{
// Solves 2D convex hull in O(n * h) complexity for Multipoint concept
// Where n is the total input points and h is the total points in convex hull
template <typename MultiPoint, typename Size>
class ConvexHull {

    private:
        MultiPoint hull;
        Size n;

    public:
        ConvexHull(MultiPoint mp, Size sz);
        void print_hull();
};

template <typename MultiPoint, typename Size>
ConvexHull<MultiPoint, Size>::ConvexHull(MultiPoint mp, Size sz)
{
    hull = mp;
    n = sz;
}

// To print the convex hull
template <typename MultiPoint, typename Size>
void ConvexHull<MultiPoint, Size>::print_hull()
{
    std::cout << "Resulting points of Convex Hull are: " << std::endl;
    for (Size i = 0; i < n; ++i)
    {
        std::cout << bg::get<0>(hull[i]) << " " << bg::get<1>(hull[i]) << std::endl;
    }
}

// This method returns true if the points are oriented in counter-clockwise manner
template <typename Point>
static inline bool check(Point a, Point b, Point c)
{
    typedef typename bg::coordinate_type<Point>::type c_type;

    c_type value = (bg::get<1>(b) - bg::get<1>(a)) * (bg::get<0>(c) - bg::get<0>(b))
                   - (bg::get<0>(b) - bg::get<0>(a)) * (bg::get<1>(c) - bg::get<1>(b));

    return (value < 0);
}

// Sorts input in increasing order of x values and in case of ties, increasing y values
template <typename Range>
static inline void sort(Range& range)
{
    typedef typename boost::range_value<Range>::type point_type;
    typedef boost::geometry::less<point_type> comparator;

    std::sort(boost::begin(range), boost::end(range), comparator());
}

// Driver code
template <typename MultiPoint>
inline void GiftWrapping(MultiPoint input, MultiPoint& hull)
{
    typedef typename boost::range_size<MultiPoint>::type size_type;

    size_type n = boost::size(input);

    sort(input);

    // Algorithm starts from leftmost point, which is the first point in input
    size_type p = 0;
    size_type q = 0;
    while (true)
    {
        bg::append(hull, input[p]);
        q = (p + 1) % n;
        for (size_type i = 0; i < n; ++i)
        {
            if (check(input[p], input[i], input[q]))
            {
                q = i;
            }
        }
        if (q == 0)
        {
            bg::append(hull, input[q]);
            // Ends when counter reaches leftmost point again
            break;
        }
        p = q;
    }

    // Uncomment lines below to print the current convex hull
    // size_type h = boost::size(hull);
    // ConvexHull<MultiPoint, size_type> debug(hull, h);
    // debug.print_hull();
}

} // namespace algo1


#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;
}


#include <cmath>
#include <iostream>
#include <random>
#include <vector>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics.hpp>
#include <boost/accumulators/statistics/weighted_mean.hpp>
#include <boost/accumulators/statistics/weighted_variance.hpp>
#include <gtest/gtest.h>

// Assumes discrete distributions
using Value = int;
using Probability = double;
using Mean = double;
using ProbabilityDensity = std::vector<Probability>;
constexpr std::mt19937::result_type GlobalSeed = 123;

template <typename T>
ProbabilityDensity MakeDistribution(Value maxNumber, Value trialSize, T& dist) {
    std::mt19937 gen{GlobalSeed};
    ProbabilityDensity prob(maxNumber + 1);
    const Probability unit = 1.0 / trialSize;
    for(auto i = decltype(trialSize){0}; i < trialSize; ++i) {
        Value num = std::round(dist(gen));
        if ((num >= 0) && (num <= maxNumber)) {
            prob.at(num) += unit;
        }
    }

    return prob;
}

ProbabilityDensity MakeNormalDistribution(Mean mean, Mean variance, Value maxNumber, Value trialSize) {
    std::mt19937 gen{GlobalSeed};
    std::normal_distribution<Probability> dist(mean, std::sqrt(variance));
    return MakeDistribution(maxNumber, trialSize, dist);
}

ProbabilityDensity MakePoissonDistribution(Mean mean, Value maxNumber, Value trialSize) {
    std::mt19937 gen{GlobalSeed};
    std::poisson_distribution<Value> dist(mean);
    return MakeDistribution(maxNumber, trialSize, dist);
}

ProbabilityDensity MakeNegativeBinomialDistribution(Value size, Mean prob, Value maxNumber, Value trialSize) {
    std::mt19937 gen{GlobalSeed};
    std::negative_binomial_distribution<Value> dist(size, prob);
    return MakeDistribution(maxNumber, trialSize, dist);
}

// Probability density from 0 to n (n+1 elements)
ProbabilityDensity MergeProbabilityDensity(const ProbabilityDensity& lhs, const ProbabilityDensity& rhs) {
    if (lhs.empty()) {
        return rhs;
    }

    if (rhs.empty()) {
        return lhs;
    }

    const auto maxLeft = lhs.size() - 1;
    const auto maxRight = rhs.size() - 1;
    const auto maxJoint = maxLeft + maxRight;
    ProbabilityDensity jointProb(maxJoint + 1);

    for(auto iRight = decltype(maxRight){0}; iRight <= maxRight; ++iRight) {
        const auto probRight = rhs.at(iRight);
        for(auto iLeft = decltype(maxLeft){0}; iLeft <= maxLeft; ++iLeft) {
            jointProb.at(iLeft + iRight) += lhs.at(iLeft) * probRight;
        }
    }

    return jointProb;
}

struct Moments {
    Mean mean {0};
    Mean variance {0};
};

Moments GetMeanVariance(const ProbabilityDensity& dist, Probability epsilon) {
    using namespace boost::accumulators;
    EXPECT_NEAR(1.0, std::accumulate(dist.begin(), dist.end(), 0.0), epsilon);

    Mean value = 0;
    Mean actualMeanBasic = 0.0;
    for (const auto& prob : dist) {
        actualMeanBasic += value * prob;
        value += 1.0;
    }

    value = 0;
    Mean actualVarianceBasic = 0.0;
    for (const auto& prob : dist) {
        actualVarianceBasic += (value - actualMeanBasic) * (value - actualMeanBasic) * prob;
        value += 1.0;
    }

    // Not weighted
    accumulator_set<Mean, stats<tag::mean, tag::variance>> accBase;
    value = 0;
    for (const auto& prob : dist) {
        Value count = static_cast<decltype(count)>(prob * 10000000);
        for(Value i=0; i<count; ++i) {
            accBase(value);
        }
        value += 1.0;
    }
    constexpr Mean EpsilonAccBase = 0.01;
    EXPECT_NEAR(actualMeanBasic, mean(accBase), EpsilonAccBase);
    EXPECT_NEAR(actualVarianceBasic, variance(accBase), EpsilonAccBase);

    // Must set the third type and have weighted_variance take (lazy)
    accumulator_set<Mean, stats<tag::weighted_mean, tag::weighted_variance(lazy)>, Mean> acc;
    value = 0;
    for (const auto& prob : dist) {
        // Do not omit the weight below
        acc(value, weight = prob);
//      acc(value, prob);
        value += 1.0;
    }
    const Mean actualMean = weighted_mean(acc);
    const Mean actualVariance = weighted_variance(acc);
    constexpr Mean EpsilonAcc = 0.000001;
    EXPECT_NEAR(actualMeanBasic, actualMean, EpsilonAcc);
    EXPECT_NEAR(actualVarianceBasic, actualVariance, EpsilonAcc);

    // weighted_variance without lazy does not work
    accumulator_set<Mean, stats<tag::weighted_mean, tag::weighted_variance>, Mean> accNonLazy;
    value = 0;
    for (const auto& prob : dist) {
        accNonLazy(value, weight = prob);
        value += 1.0;
    }
    EXPECT_NEAR(actualMeanBasic, weighted_mean(accNonLazy), EpsilonAcc);
//  EXPECT_NEAR(actualVarianceBasic, weighted_variance(accNonLazy), EpsilonAcc);

    // If we forget weight = ...
    accumulator_set<Mean, stats<tag::mean>> accWrong;
    value = 0;
    for (const auto& prob : dist) {
        // Do not omit the weight below!
        accWrong(value, prob);
        value += 1.0;
    }
//  EXPECT_NEAR(actualMeanBasic, mean(accWrong), EpsilonAcc);

    // Replacing tag::weighted_mean to tag::mean works
    accumulator_set<Mean, stats<tag::mean>, Mean> accPlain;
    value = 0;
    for (const auto& prob : dist) {
        accPlain(value, weight = prob);
        value += 1.0;
    }
    EXPECT_NEAR(actualMeanBasic, mean(accPlain), EpsilonAcc);

    // But if we omit the third type argument...
    accumulator_set<Mean, stats<tag::mean>> accPlainWrong;
    value = 0;
    for (const auto& prob : dist) {
        accPlainWrong(value, weight = prob);
        value += 1.0;
    }
//  EXPECT_NEAR(actualMeanBasic, mean(accPlainWrong), EpsilonAcc);

    Moments result {actualMeanBasic, actualVarianceBasic};
    return result;
}

class TestMergeProbabilityDensity : public ::testing::Test {};

TEST_F(TestMergeProbabilityDensity, Normal) {
    constexpr Mean expectedMean = 100;
    constexpr Mean expectedVariance = 25;
    constexpr Value maxNumber = 200;
    constexpr Value trialSize = 1000000;

    const auto dist = MakeNormalDistribution(expectedMean, expectedVariance, maxNumber, trialSize);
    const auto actual = GetMeanVariance(dist, 0.001);
    EXPECT_EQ(expectedMean, std::round(actual.mean));
    EXPECT_EQ(std::round(expectedVariance), std::round(actual.variance));
}

TEST_F(TestMergeProbabilityDensity, Poisson) {
    constexpr Mean expectedMean = 10;
    constexpr Mean expectedVariance = expectedMean;
    constexpr Value maxNumber = 200;
    constexpr Value trialSize = 1000000;

    const auto dist = MakePoissonDistribution(expectedMean, maxNumber, trialSize);
    const auto actual = GetMeanVariance(dist, 0.001);
    EXPECT_EQ(expectedMean, std::round(actual.mean));
    EXPECT_EQ(std::round(expectedVariance), std::round(actual.variance));
}

TEST_F(TestMergeProbabilityDensity, NegativeBinomial) {
    constexpr Value size = 10;
    constexpr Mean probToSuccess = 0.4;
    constexpr Mean expectedMean = static_cast<Mean>(size) * (1.0 - probToSuccess) / probToSuccess;
    constexpr Mean expectedVariance = expectedMean / probToSuccess;
    constexpr Value maxNumber = 200;
    constexpr Value trialSize = 1000000;

    const auto dist = MakeNegativeBinomialDistribution(size, probToSuccess, maxNumber, trialSize);
    const auto actual = GetMeanVariance(dist, 0.001);
    EXPECT_EQ(expectedMean, std::round(actual.mean));
    EXPECT_EQ(std::round(expectedVariance), std::round(actual.variance));
}

TEST_F(TestMergeProbabilityDensity, SumNormals) {
    const std::vector<Moments> testCases {{410, 16}, {450, 25}, {460, 36}, {470, 49}};
    const std::vector<Moments>::size_type testCaseSize = testCases.size();
    constexpr Value maxNumber = 900;
    constexpr Value trialSize = 1000000;

    for(auto size = decltype(testCaseSize){1}; size <= testCaseSize; ++size) {
        ProbabilityDensity accumDist;
        Mean expectedMean = 0.0;
        Mean expectedVariance = 0.0;
        for(auto i = decltype(size){0}; i < size; ++i) {
            auto mean = testCases.at(i).mean;
            auto variance = testCases.at(i).variance;
            expectedMean += mean;
            expectedVariance += variance;
            const auto dist = MakeNormalDistribution(mean, variance, maxNumber, trialSize);
            accumDist = MergeProbabilityDensity(accumDist, dist);
        }

        const auto actual = GetMeanVariance(accumDist, 0.1);
        EXPECT_TRUE(((expectedMean - 1) < actual.mean) && (actual.mean < (expectedMean + 1)));
        EXPECT_TRUE(((expectedVariance - 1) < actual.variance) && (actual.variance < (expectedVariance + 1)));
    }
}

TEST_F(TestMergeProbabilityDensity, SumPoisson) {
    const std::vector<Mean> testCases {4, 8, 12, 16};
    const std::vector<Moments>::size_type testCaseSize = testCases.size();
    constexpr Value maxNumber = 255;
    constexpr Value trialSize = 100000;

    for(auto size = decltype(testCaseSize){1}; size <= testCaseSize; ++size) {
        ProbabilityDensity accumDist;
        Mean expected = 0.0;
        for(auto i = decltype(size){0}; i < size; ++i) {
            auto mean = testCases.at(i);
            expected += mean;
            const auto dist = MakePoissonDistribution(mean, maxNumber, trialSize);
            accumDist = MergeProbabilityDensity(accumDist, dist);
        }

        const auto actual = GetMeanVariance(accumDist, 0.1);
        EXPECT_TRUE(((expected - 1) < actual.mean) && (actual.mean < (expected + 1)));
        EXPECT_TRUE(((expected - 1) < actual.variance) && (actual.variance < (expected + 1)));
    }
}

TEST_F(TestMergeProbabilityDensity, SumNegativeBinomial) {
    struct Params {
        Value size {0};
        Mean probToSuccess {0.0};
    };

    const std::vector<Params> testCases {{3, 0.8}, {8, 0.6}, {10, 0.45}, {20, 0.7}};
    const std::vector<Params>::size_type testCaseSize = testCases.size();
    constexpr Value maxNumber = 200;
    constexpr Value trialSize = 100000;

    for(auto size = decltype(testCaseSize){1}; size <= testCaseSize; ++size) {
        ProbabilityDensity accumDist;
        Mean expectedMean = 0.0;
        Mean expectedVariance = 0.0;
        for(auto i = decltype(size){0}; i < size; ++i) {
            const auto& testCase = testCases[i];
            auto mean = static_cast<Mean>(testCase.size) * (1.0 - testCase.probToSuccess) / testCase.probToSuccess;
            auto variance = mean / testCase.probToSuccess;
            expectedMean += mean;
            expectedVariance += variance;
            const auto dist = MakeNegativeBinomialDistribution(testCase.size, testCase.probToSuccess, maxNumber, trialSize);
            accumDist = MergeProbabilityDensity(accumDist, dist);
        }

        const auto actual = GetMeanVariance(accumDist, 0.1);
        EXPECT_TRUE(((expectedMean - 1) < actual.mean) && (actual.mean < (expectedMean + 1)));
        EXPECT_TRUE(((expectedVariance - 1) < actual.variance) && (actual.variance < (expectedVariance + 1)));
    }
}

class TestBoostAccumulators : public ::testing::Test {};

TEST_F(TestBoostAccumulators, Weighted) {
    using namespace boost::accumulators;
    constexpr Mean EpsilonAcc = 0.001;

    accumulator_set<Mean, stats<tag::weighted_mean, tag::weighted_variance(lazy)>, Mean> accLazy;
    accLazy(-3.0, weight = 1.0);
    accLazy(-1.0, weight = 3.0);
    accLazy(1.0, weight = 6.0);
    accLazy(5.0, weight = 0.0);
    EXPECT_NEAR(0.0, mean(accLazy), EpsilonAcc);
    EXPECT_NEAR(1.8, variance(accLazy), EpsilonAcc);

    accumulator_set<Mean, stats<tag::weighted_mean, tag::weighted_variance>, Mean> accNonLazy;
    accNonLazy(4.0, weight = 0.1);
    accNonLazy(6.0, weight = 0.3);
    accNonLazy(8.0, weight = 0.6);
    accNonLazy(12.0, weight = 0.0);
    EXPECT_NEAR(7.0, mean(accNonLazy), EpsilonAcc);
    EXPECT_NEAR(1.8, variance(accNonLazy), EpsilonAcc);
}

int main(int argc, char* argv[]) {
    ::testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}

/*
Local Variables:
mode: c++
coding: utf-8-dos
tab-width: nil
c-file-style: "stroustrup"
End:
*/


/*
 * CNN demo for MNIST dataset
 * Author: Kai Han (kaihana@163.com)
 * Details in https://github.com/iamhankai/mini-dnn-cpp
 * Copyright 2018 Kai Han
 */
#include <Eigen/Dense>
#include <algorithm>
#include <iostream>

#include "src/layer.h"
#include "src/layer/conv.h"
#include "src/layer/fully_connected.h"
#include "src/layer/ave_pooling.h"
#include "src/layer/max_pooling.h"
#include "src/layer/relu.h"
#include "src/layer/sigmoid.h"
#include "src/layer/softmax.h"
#include "src/loss.h"
#include "src/loss/mse_loss.h"
#include "src/loss/cross_entropy_loss.h"
#include "src/mnist.h"
#include "src/network.h"
#include "src/optimizer.h"
#include "src/optimizer/sgd.h"

int main() {
  // data
  MNIST dataset("../data/mnist/");
  dataset.read();
  int n_train = dataset.train_data.cols();
  int dim_in = dataset.train_data.rows();
  std::cout << "mnist train number: " << n_train << std::endl;
  std::cout << "mnist test number: " << dataset.test_labels.cols() << std::endl;
  // dnn
  Network dnn;
  Layer* conv1 = new Conv(1, 28, 28, 4, 5, 5, 2, 2, 2);
  Layer* pool1 = new MaxPooling(4, 14, 14, 2, 2, 2);
  Layer* conv2 = new Conv(4, 7, 7, 16, 5, 5, 1, 2, 2);
  Layer* pool2 = new MaxPooling(16, 7, 7, 2, 2, 2);
  Layer* fc3 = new FullyConnected(pool2->output_dim(), 32);
  Layer* fc4 = new FullyConnected(32, 10);
  Layer* relu1 = new ReLU;
  Layer* relu2 = new ReLU;
  Layer* relu3 = new ReLU;
  Layer* softmax = new Softmax;
  dnn.add_layer(conv1);
  dnn.add_layer(relu1);
  dnn.add_layer(pool1);
  dnn.add_layer(conv2);
  dnn.add_layer(relu2);
  dnn.add_layer(pool2);
  dnn.add_layer(fc3);
  dnn.add_layer(relu3);
  dnn.add_layer(fc4);
  dnn.add_layer(softmax);
  // loss
  Loss* loss = new CrossEntropy;
  dnn.add_loss(loss);
  // train & test
  SGD opt(0.001, 5e-4, 0.9, true);
  // SGD opt(0.001);
  const int n_epoch = 5;
  const int batch_size = 128;
  for (int epoch = 0; epoch < n_epoch; epoch ++) {
    shuffle_data(dataset.train_data, dataset.train_labels);
    for (int start_idx = 0; start_idx < n_train; start_idx += batch_size) {
      int ith_batch = start_idx / batch_size;
      Matrix x_batch = dataset.train_data.block(0, start_idx, dim_in,
                                    std::min(batch_size, n_train - start_idx));
      Matrix label_batch = dataset.train_labels.block(0, start_idx, 1,
                                    std::min(batch_size, n_train - start_idx));
      Matrix target_batch = one_hot_encode(label_batch, 10);
      if (false && ith_batch % 10 == 1) {
        std::cout << ith_batch << "-th grad: " << std::endl;
        dnn.check_gradient(x_batch, target_batch, 10);
      }
      dnn.forward(x_batch);
      dnn.backward(x_batch, target_batch);
      // display
      if (ith_batch % 50 == 0) {
        std::cout << ith_batch << "-th batch, loss: " << dnn.get_loss()
        << std::endl;
      }
      // optimize
      dnn.update(opt);
    }
    // test
    dnn.forward(dataset.test_data);
    float acc = compute_accuracy(dnn.output(), dataset.test_labels);
    std::cout << std::endl;
    std::cout << epoch + 1 << "-th epoch, test acc: " << acc << std::endl;
    std::cout << 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>();
    }
}


#include <iostream>
#include <typeinfo>
#define BOOST_TEST_MODULE DenseMIASolveTests

#include "MIAConfig.h"

#ifdef MIA_USE_HEADER_ONLY_TESTS
#include <boost/test/included/unit_test.hpp>
#else
#include <boost/test/unit_test.hpp>
#endif

#include "DenseMIA.h"
#include "Index.h"

template<class _data_type>
void solve_work(size_t dim1,size_t dim2){

    LibMIA::MIAINDEX i;
    LibMIA::MIAINDEX j;
    LibMIA::MIAINDEX k;
    LibMIA::MIAINDEX l;
    LibMIA::MIAINDEX m;
    LibMIA::MIAINDEX n;
    LibMIA::MIAINDEX o;
    LibMIA::MIAINDEX p;

    LibMIA::DenseMIA<_data_type,4> a(dim1,dim1,dim1,dim1);
    LibMIA::DenseMIA<_data_type,4> a2(dim2,dim2,dim1,dim1);
    LibMIA::DenseMIA<_data_type,4> b(dim1,dim1,dim1,dim1);
    LibMIA::DenseMIA<_data_type,4> b2(dim2,dim2,dim1,dim1);
    LibMIA::DenseMIA<_data_type,4> c(dim1,dim1,dim1,dim1);
    LibMIA::DenseMIA<_data_type,4> d(dim1,dim1,dim1,dim1);
    LibMIA::DenseMIA<_data_type,4> d2(dim1,dim1,dim1,dim1);

    a.randu(-30,30); //we can invert these, b/c random matrices are non-singular with probability one (almost surely)
    b.randu(-30,30);
    a2.randu(-30,30);
    b2.randu(-30,30);

    c(i,j,m,n)=a(i,j,k,l)|b(k,l,m,n);
    d(k,l,m,n)=a(i,j,k,l)*c(i,j,m,n);
    BOOST_CHECK_MESSAGE(d.fuzzy_equals(b,test_precision<_data_type>()),std::string("Inner/Outer Product Inverse 1 for ")+typeid(_data_type).name() );

    c(i,j,m,n)=a(i,k,j,l)|b(k,l,m,n);
    d(k,l,m,n)=a(i,k,j,l)*c(i,j,m,n);
    BOOST_CHECK_MESSAGE(d.fuzzy_equals(b,test_precision<_data_type>()),std::string("Inner/Outer Product Inverse 2 for ")+typeid(_data_type).name() );

    c(i,j,l,m)=a(i,!j,k,!l)|b(k,!j,!l,m);
    d(k,j,l,m)=a(i,!j,k,!l)*c(i,!j,!l,m);
    BOOST_CHECK_MESSAGE(d.fuzzy_equals(b,test_precision<_data_type>()),std::string("Inner/Outer/Inter Product Inverse 1 for ")+typeid(_data_type).name() );

    c(i,j,l,m)=a(!i,!j,k,l)|b(m,!j,k,!i);
    d(m,j,k,i)=a(!i,!j,k,l)*c(!i,!j,l,m);
    BOOST_CHECK_MESSAGE(d.fuzzy_equals(b,test_precision<_data_type>()),std::string("Inner/Outer/Inter Product Inverse 2 for ")+typeid(_data_type).name() );

    c(i,j,m,n)=a2(k,l,i,j)|b2(k,l,m,n);
    //test with normal equations
    d(o,p,m,n)=a2(k,l,o,p)*a2(k,l,i,j)*c(i,j,m,n);
    d2(i,j,m,n)=a2(k,l,i,j)*b2(k,l,m,n);
    BOOST_CHECK_MESSAGE(d.fuzzy_equals(d2,test_precision<_data_type>()),std::string("Inner/Outer Product Least Squares 1 for ")+typeid(_data_type).name() );

    c(i,j,k,l)=a(!i,!j,!k,!l)|b(!k,!j,!l,!i);
    d(k,j,l,i)=a(!i,!j,!k,!l)*c(!i,!j,!k,!l);
    BOOST_CHECK_MESSAGE(d.fuzzy_equals(b,test_precision<_data_type>()),std::string("Pure Inter-product Inverse 1 for ")+typeid(_data_type).name() );

}

BOOST_AUTO_TEST_CASE( DenseMIASolveTests )
{

    solve_work<double>(8,10);

    solve_work<float>(8,10);

}


//
//  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);
    }

}