// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2013 Alec Jacobson <alecjacobson@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
#include "grad.h"
#include <Eigen/Geometry>
#include <vector>

#include "per_face_normals.h"
#include "volume.h"
#include "doublearea.h"

template <typename DerivedV, typename DerivedF>
IGL_INLINE void grad_tet(const Eigen::PlainObjectBase<DerivedV>&V,
                     const Eigen::PlainObjectBase<DerivedF>&T,
                            Eigen::SparseMatrix<typename DerivedV::Scalar> &G,
                            bool uniform) {
  using namespace Eigen;
  assert(T.cols() == 4);
  const int n = V.rows(); int m = T.rows();

  /*
      F = [ ...
      T(:,1) T(:,2) T(:,3); ...
      T(:,1) T(:,3) T(:,4); ...
      T(:,1) T(:,4) T(:,2); ...
      T(:,2) T(:,4) T(:,3)]; */
  MatrixXi F(4*m,3);
  for (int i = 0; i < m; i++) {
    F.row(0*m + i) << T(i,0), T(i,1), T(i,2);
    F.row(1*m + i) << T(i,0), T(i,2), T(i,3);
    F.row(2*m + i) << T(i,0), T(i,3), T(i,1);
    F.row(3*m + i) << T(i,1), T(i,3), T(i,2);
  }
  // compute volume of each tet
  VectorXd vol; igl::volume(V,T,vol);

  VectorXd A(F.rows());
  MatrixXd N(F.rows(),3);
  if (!uniform) {
    // compute tetrahedron face normals
    igl::per_face_normals(V,F,N); int norm_rows = N.rows();
    for (int i = 0; i < norm_rows; i++)
      N.row(i) /= N.row(i).norm();
    igl::doublearea(V,F,A); A/=2.;
  } else {
    // Use a uniform tetrahedra as a reference, with the same volume as the original one:
    //
    // Use normals of the uniform tet (V = h*[0,0,0;1,0,0;0.5,sqrt(3)/2.,0;0.5,sqrt(3)/6.,sqrt(2)/sqrt(3)])
    //         0         0    1.0000
    //         0.8165   -0.4714   -0.3333
    //         0          0.9428   -0.3333
    //         -0.8165   -0.4714   -0.3333
    for (int i = 0; i < m; i++) {
      N.row(0*m+i) << 0,0,1;
      double a = sqrt(2)*std::cbrt(3*vol(i)); // area of a face in a uniform tet with volume = vol(i)
      A(0*m+i) = (pow(a,2)*sqrt(3))/4.;
    }
    for (int i = 0; i < m; i++) {
      N.row(1*m+i) << 0.8165,-0.4714,-0.3333;
      double a = sqrt(2)*std::cbrt(3*vol(i));
      A(1*m+i) = (pow(a,2)*sqrt(3))/4.;
    }
    for (int i = 0; i < m; i++) {
      N.row(2*m+i) << 0,0.9428,-0.3333;
      double a = sqrt(2)*std::cbrt(3*vol(i));
      A(2*m+i) = (pow(a,2)*sqrt(3))/4.;
    }
    for (int i = 0; i < m; i++) {
      N.row(3*m+i) << -0.8165,-0.4714,-0.3333;
      double a = sqrt(2)*std::cbrt(3*vol(i));
      A(3*m+i) = (pow(a,2)*sqrt(3))/4.;
    }

  }

  /*  G = sparse( ...
      [0*m + repmat(1:m,1,4) ...
       1*m + repmat(1:m,1,4) ...
       2*m + repmat(1:m,1,4)], ...
      repmat([T(:,4);T(:,2);T(:,3);T(:,1)],3,1), ...
      repmat(A./(3*repmat(vol,4,1)),3,1).*N(:), ...
      3*m,n);*/
  std::vector<Triplet<double> > G_t;
  for (int i = 0; i < 4*m; i++) {
    int T_j; // j indexes : repmat([T(:,4);T(:,2);T(:,3);T(:,1)],3,1)
    switch (i/m) {
      case 0:
        T_j = 3;
        break;
      case 1:
        T_j = 1;
        break;
      case 2:
        T_j = 2;
        break;
      case 3:
        T_j = 0;
        break;
    }
    int i_idx = i%m;
    int j_idx = T(i_idx,T_j);

    double val_before_n = A(i)/(3*vol(i_idx));
    G_t.push_back(Triplet<double>(0*m+i_idx, j_idx, val_before_n * N(i,0)));
    G_t.push_back(Triplet<double>(1*m+i_idx, j_idx, val_before_n * N(i,1)));
    G_t.push_back(Triplet<double>(2*m+i_idx, j_idx, val_before_n * N(i,2)));
  }
  G.resize(3*m,n);
  G.setFromTriplets(G_t.begin(), G_t.end());
}

template <typename DerivedV, typename DerivedF>
IGL_INLINE void grad_tri(const Eigen::PlainObjectBase<DerivedV>&V,
                     const Eigen::PlainObjectBase<DerivedF>&F,
                    Eigen::SparseMatrix<typename DerivedV::Scalar> &G,
                    bool uniform)
{
  Eigen::Matrix<typename DerivedV::Scalar,Eigen::Dynamic,3>
    eperp21(F.rows(),3), eperp13(F.rows(),3);

  for (int i=0;i<F.rows();++i)
  {
    // renaming indices of vertices of triangles for convenience
    int i1 = F(i,0);
    int i2 = F(i,1);
    int i3 = F(i,2);

    // #F x 3 matrices of triangle edge vectors, named after opposite vertices
    Eigen::Matrix<typename DerivedV::Scalar, 1, 3> v32 = V.row(i3) - V.row(i2);
    Eigen::Matrix<typename DerivedV::Scalar, 1, 3> v13 = V.row(i1) - V.row(i3);
    Eigen::Matrix<typename DerivedV::Scalar, 1, 3> v21 = V.row(i2) - V.row(i1);
    Eigen::Matrix<typename DerivedV::Scalar, 1, 3> n = v32.cross(v13);
    // area of parallelogram is twice area of triangle
    // area of parallelogram is || v1 x v2 ||
    // This does correct l2 norm of rows, so that it contains #F list of twice
    // triangle areas
    double dblA = std::sqrt(n.dot(n));
    Eigen::Matrix<typename DerivedV::Scalar, 1, 3> u;
    if (!uniform) {
      // now normalize normals to get unit normals
      u = n / dblA;
    } else {
      // Abstract equilateral triangle v1=(0,0), v2=(h,0), v3=(h/2, (sqrt(3)/2)*h)

      // get h (by the area of the triangle)
      double h = sqrt( (dblA)/sin(M_PI / 3.0)); // (h^2*sin(60))/2. = Area => h = sqrt(2*Area/sin_60)

      Eigen::VectorXd v1,v2,v3;
      v1 << 0,0,0;
      v2 << h,0,0;
      v3 << h/2.,(sqrt(3)/2.)*h,0;

      // now fix v32,v13,v21 and the normal
      v32 = v3-v2;
      v13 = v1-v3;
      v21 = v2-v1;
      n = v32.cross(v13);
    }

    // rotate each vector 90 degrees around normal
    double norm21 = std::sqrt(v21.dot(v21));
    double norm13 = std::sqrt(v13.dot(v13));
    eperp21.row(i) = u.cross(v21);
    eperp21.row(i) = eperp21.row(i) / std::sqrt(eperp21.row(i).dot(eperp21.row(i)));
    eperp21.row(i) *= norm21 / dblA;
    eperp13.row(i) = u.cross(v13);
    eperp13.row(i) = eperp13.row(i) / std::sqrt(eperp13.row(i).dot(eperp13.row(i)));
    eperp13.row(i) *= norm13 / dblA;
  }

  std::vector<int> rs;
  rs.reserve(F.rows()*4*3);
  std::vector<int> cs;
  cs.reserve(F.rows()*4*3);
  std::vector<double> vs;
  vs.reserve(F.rows()*4*3);

  // row indices
  for(int r=0;r<3;r++)
  {
    for(int j=0;j<4;j++)
    {
      for(int i=r*F.rows();i<(r+1)*F.rows();i++) rs.push_back(i);
    }
  }

  // column indices
  for(int r=0;r<3;r++)
  {
    for(int i=0;i<F.rows();i++) cs.push_back(F(i,1));
    for(int i=0;i<F.rows();i++) cs.push_back(F(i,0));
    for(int i=0;i<F.rows();i++) cs.push_back(F(i,2));
    for(int i=0;i<F.rows();i++) cs.push_back(F(i,0));
  }

  // values
  for(int i=0;i<F.rows();i++) vs.push_back(eperp13(i,0));
  for(int i=0;i<F.rows();i++) vs.push_back(-eperp13(i,0));
  for(int i=0;i<F.rows();i++) vs.push_back(eperp21(i,0));
  for(int i=0;i<F.rows();i++) vs.push_back(-eperp21(i,0));
  for(int i=0;i<F.rows();i++) vs.push_back(eperp13(i,1));
  for(int i=0;i<F.rows();i++) vs.push_back(-eperp13(i,1));
  for(int i=0;i<F.rows();i++) vs.push_back(eperp21(i,1));
  for(int i=0;i<F.rows();i++) vs.push_back(-eperp21(i,1));
  for(int i=0;i<F.rows();i++) vs.push_back(eperp13(i,2));
  for(int i=0;i<F.rows();i++) vs.push_back(-eperp13(i,2));
  for(int i=0;i<F.rows();i++) vs.push_back(eperp21(i,2));
  for(int i=0;i<F.rows();i++) vs.push_back(-eperp21(i,2));

  // create sparse gradient operator matrix
  G.resize(3*F.rows(),V.rows());
  std::vector<Eigen::Triplet<typename DerivedV::Scalar> > triplets;
  for (int i=0;i<(int)vs.size();++i)
  {
    triplets.push_back(Eigen::Triplet<typename DerivedV::Scalar>(rs[i],cs[i],vs[i]));
  }
  G.setFromTriplets(triplets.begin(), triplets.end());
}

template <typename DerivedV, typename DerivedF>
IGL_INLINE void igl::grad(const Eigen::PlainObjectBase<DerivedV>&V,
                     const Eigen::PlainObjectBase<DerivedF>&F,
                    Eigen::SparseMatrix<typename DerivedV::Scalar> &G,
                    bool uniform)
{
  assert(F.cols() == 3 || F.cols() == 4);
  if (F.cols() == 3)
    return grad_tri(V,F,G,uniform);
  if (F.cols() == 4)
    return grad_tet(V,F,G,uniform);
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template instantiation
template void igl::grad<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::SparseMatrix<Eigen::Matrix<double, -1, -1, 0, -1, -1>::Scalar, 0, int>&, bool);
template void igl::grad<Eigen::Matrix<double, -1, 3, 0, -1, 3>, Eigen::Matrix<int, -1, 3, 0, -1, 3> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 3, 0, -1, 3> > const&, Eigen::SparseMatrix<Eigen::Matrix<double, -1, 3, 0, -1, 3>::Scalar, 0, int>&, bool);
#endif


////////////////////////////////////////////////////////////////////////////////
// NeoHookeanEnergy.hh
////////////////////////////////////////////////////////////////////////////////
/*! @file
//  Volumetric and plane stress Neo-Hookean energy.
*///////////////////////////////////////////////////////////////////////////////
#ifndef NEOHOOKEANENERGY_HH
#define NEOHOOKEANENERGY_HH

#include <Eigen/Dense>
#include <cstdlib>

#include <MeshFEM/EnergyDensities/Tensor.hh>
#include <MeshFEM/EnergyDensities/EnergyTraits.hh>
#include <MeshFEM/EnergyDensities/EDensityAdaptors.hh>

/**
 *  Implements the Neo-Hookean Energy described in the Neo-Hookean Energy section of doc/doc.pdf
 */
template<typename _Real, size_t _Dim, template<typename, size_t> class _Derived_T>
struct NeoHookeanEnergyBase : public Concepts::NeoHookeanEnergy
{
    static constexpr size_t Dimension = _Dim;
    static constexpr size_t N         = _Dim;
    static constexpr EDensityType EDType = EDensityType::FBased;
    using Real = _Real;
    using Derived = _Derived_T<Real, N>;
    using Matrix = Eigen::Matrix<Real, N, N>;

    NeoHookeanEnergyBase(const NeoHookeanEnergyBase& other) = default;

    // Constructor copying material properties only, not the current deformation
    NeoHookeanEnergyBase(const NeoHookeanEnergyBase& other, UninitializedDeformationTag &&)
        : m_lambda(other.m_lambda), m_mu(other.m_mu), m_finite_continuation_start(other.m_finite_continuation_start)
    { }

    // Construct from Lame's first parameter (lambda) and shear modulus (mu).
    NeoHookeanEnergyBase(Real lambda, Real mu, Real finite_continuation_start = -1)
        : m_lambda(lambda), m_mu(mu), m_finite_continuation_start(finite_continuation_start)
    {
        setDeformationGradient(Matrix::Identity());
    }

    void setDeformationGradient(const Matrix& deformation_gradient, const EvalLevel /* elevel */ = EvalLevel::Full) {
        m_F = deformation_gradient;
        m_detF = deformation_gradient.determinant();
        m_Finv = m_F.inverse();
    }

    const Matrix &getDeformationGradient() const { return m_F; }

    Real energy() const {
        // Standard behavior: return inf for inverted elements
        if (m_finite_continuation_start <= 0 && m_detF < 0) {
            return std::numeric_limits<Real>::max();
        }

        const Real I3 = getI3();
        const Real I1 = getI1();

        // Modified behavior to support inverted elements:
        // if det F < eps, we replace the log(I3) term by a constant + exp(- (det (F) - eps) )
        // where the constant is chosen such that the energy remains continuous
        if (m_finite_continuation_start > 0 && m_detF < m_finite_continuation_start) {
            Derived tmp(m_lambda, m_mu, m_finite_continuation_start);
            Matrix tmp_F = Matrix::Identity();
            tmp_F(0, 0) = m_finite_continuation_start;
            tmp.setDeformationGradient(tmp_F);
            Real continuation_constant = - std::log(tmp.getI3()) * (m_lambda / 2 + m_mu) / 2;

            return m_lambda * (I3 - 1) / 4 + m_mu * (I1 - 3) / 2
                + continuation_constant + std::exp(-(m_detF - m_finite_continuation_start)) - 1;
        }

        return (m_mu / 2) * (I1 - 3) + (m_lambda / 4) * (I3 - 1) - std::log(I3) * (m_mu / 2 + m_lambda / 4);
    }

    Matrix denergy() const {
        if (m_finite_continuation_start > 0 && m_detF < m_finite_continuation_start) {
            Real dPsi3 = m_lambda / 4;
            return (-std::exp(-(m_detF - m_finite_continuation_start))) * m_detF * m_Finv.transpose()
                + dPsi3 * d_I3_d_F()
                + d_psi_d_I1() * d_I1_d_F();
        }

        return d_psi_d_I1() * d_I1_d_F() + d_psi_d_I3() * d_I3_d_F();
    }

    Real denergy(const Matrix& dF) const { return doubleContract(dF, denergy()); }

    Real d2energy(const Matrix& dF_a, const Matrix& dF_b) const {
        return doubleContract(dF_a, delta_denergy(dF_b));
    }

    // Directional derivative of "denergy" along dF:
    //      (d^2 psi / dF^2) : dF
    template<class Mat_>
    Matrix delta_denergy(const Mat_ &dF) const {
        if (m_finite_continuation_start > 0 && m_detF < m_finite_continuation_start) {
            // ln I3 term is constant, but exp(-(detF)) got added
            Real dPsi3 = m_lambda / 4;
            Real exp_term = -std::exp(-(m_detF - m_finite_continuation_start));

            Matrix d_det_dF = m_detF * m_Finv.transpose();
            Matrix delta_d_det_dF = doubleContract(d_det_dF, dF) * m_Finv.transpose()
                                  - m_detF * (m_Finv * dF * m_Finv).transpose();

            return exp_term * d_det_dF * d_det_dF
                + exp_term * delta_d_det_dF
                + dPsi3 * delta_d_I3_d_F(dF)
                + d_psi_d_I1() * delta_d_I1_d_F(dF);
        }

        Matrix dI3 = d_I3_d_F();
        Real delta_I3 = doubleContract(dI3, dF);
        return d_psi_d_I1() * delta_d_I1_d_F(dF) + (d2_psi_d2_I3() * delta_I3) * dI3 + d_psi_d_I3() * delta_d_I3_d_F(dF);
    }

    // (d^3 psi / dF^3) :: (dF_a \otimes dF_b)
    // Second variation of "denergy" along (dF_a, dF_b)
    template<class Mat_, class Mat2_>
    Matrix delta2_denergy(const Mat_ &dF_a, const Mat2_ &dF_b) const {
        if (m_finite_continuation_start > 0) throw std::runtime_error("Finite continuation energy variant is not supported");

        Matrix dI3 = d_I3_d_F();
        Real delta_I3_a = doubleContract(dI3, dF_a),
             delta_I3_b = doubleContract(dI3, dF_b);
        Matrix delta_dI3_a = delta_d_I3_d_F(dF_a),
               delta_dI3_b = delta_d_I3_d_F(dF_b);
        return // Derivative of (d_psi_d_I1() * delta_d_I1_d_F(dF):                      (Note d2_psi_d_I1 = 0)
               d_psi_d_I1() * delta2_d_I1_d_F(dF_a, dF_b)                                // Symmetric
               // Derivative of (d2_psi_d2_I3() * delta_I3) * dI3:
             + (d3_psi_d3_I3() * delta_I3_b * delta_I3_a) * dI3                          // Symmetric
             + (d2_psi_d2_I3() *              doubleContract(delta_dI3_b, dF_a)) * dI3   // Symmetric
             + (d2_psi_d2_I3() *              delta_I3_a) * delta_dI3_b                  // Symmetric pair (*)
               // Derivative of d_psi_d_I3() * delta_d_I3_d_F(dF):
             + (d2_psi_d2_I3() *              delta_I3_b) * delta_dI3_a                  // Symmetric pair (*)
             + (  d_psi_d_I3()                          ) * delta2_d_I3_d_F(dF_a, dF_b); // Symmetric
    }

    ////////////////////////////////////////////////////////////////////////////
    // Invariants of the Cauchy-Green deformation tensor and their derivatives
    // appearing in the energy density expressions.
    // These must be provided by the derived class since they differ for the
    // 2D plane stress and 3D volumetric cases.
    ////////////////////////////////////////////////////////////////////////////
    // Trace of Cauchy-Green deformation tensor.
    Real getI1() const { return derived().getI1(); }

    // Determinant of Cauchy-Green deformation tensor.
    Real getI3() const { return derived().getI3(); }

    // dI1/dF
    Matrix d_I1_d_F() const { return derived().d_I1_d_F(); }

    // dI3/dF
    Matrix d_I3_d_F() const { return derived().d_I3_d_F(); }

    // (d^2 I1 / dF^2) : dF

    template<class Mat_>
    Matrix delta_d_I1_d_F(const Mat_ &dF) const { return derived().delta_d_I1_d_F(dF); }

    // (d^2 I1 / dF^2) : dF
    template<class Mat_>
    Matrix delta_d_I3_d_F(const Mat_ &dF) const { return derived().delta_d_I3_d_F(dF); }

    // (d^3 I1 / dF^3) :: (dF_a \otimes dF_b)
    Matrix delta2_d_I1_d_F(const Matrix &dF_a, const Matrix &dF_b) const { return derived().delta2_d_I1_d_F(dF_a, dF_b); }

    // (d^3 I3 / dF^3) :: (dF_a \otimes dF_b)
    Matrix delta2_d_I3_d_F(const Matrix &dF_a, const Matrix &dF_b) const { return derived().delta2_d_I3_d_F(dF_a, dF_b); }

    Matrix PK2Stress() const { return m_Finv * denergy(); }

    const Derived &derived() const { return *static_cast<const Derived *>(this); }
protected:
    ////////////////////////////////////////////////////////////////////////////
    // Derivatives of the energy density with respect to the tensor invariants.
    ////////////////////////////////////////////////////////////////////////////
    // Derivative of the energy density with respect to I1
    Real d_psi_d_I1() const { return m_mu / 2; }

    // Derivative of the energy density with respect to I3
    Real d_psi_d_I3() const { return (m_lambda - (2 * m_mu + m_lambda) / getI3()) / 4; }

    // Second derivative of the energy density with respect to I3
    Real d2_psi_d2_I3() const {
        Real I3 = getI3();
        return (2 * m_mu + m_lambda) / (4 * I3 * I3);
    }

    // Third derivative of the energy density with respect to I3
    Real d3_psi_d3_I3() const {
        Real I3 = getI3();
        return - (m_mu + 0.5 * m_lambda) / (I3 * I3 * I3);
    }

    ////////////////////////////////////////////////////////////////////////////
    // Derivatives of the "unpadded" invariants
    // (i.e., the 2x2 invariants for 2D, not including the C33 component)
    ////////////////////////////////////////////////////////////////////////////
    Real           unpaddedI3()     const { return m_detF * m_detF; }
    Matrix       d_unpaddedI3_d_F() const { return (2 * unpaddedI3()) * m_Finv.transpose(); }
    template<typename Mat_>
    Real delta_unpaddedI3(const Mat_ &dF) const { return (2 * unpaddedI3()) * doubleContract(m_Finv.transpose(), dF); }
    template<typename Mat_>
    Matrix delta_d_unpaddedI3_d_F(const Mat_ &dF) const {
        return (2 * delta_unpaddedI3(dF)) *  m_Finv.transpose()
             - (2 *       unpaddedI3()  ) * (m_Finv * dF * m_Finv).transpose();
    }

    Matrix delta2_d_unpaddedI3_d_F(const Matrix &dF_a, const Matrix &dF_b) const {
        Real delta2_unpaddedI3_ab = doubleContract(delta_d_unpaddedI3_d_F(dF_a), dF_b);
        Matrix delta_Finv_a = -(m_Finv * dF_a * m_Finv),
               delta_Finv_b = -(m_Finv * dF_b * m_Finv);

        return (2 * delta2_unpaddedI3_ab)   *       m_Finv.transpose()
             + (2 * delta_unpaddedI3(dF_a)) * delta_Finv_b.transpose()
             + (2 * delta_unpaddedI3(dF_b)) * delta_Finv_a.transpose()
             - (2 *       unpaddedI3()  ) * (delta_Finv_b * dF_a * m_Finv).transpose()
             - (2 *       unpaddedI3()  ) * (m_Finv * dF_a * delta_Finv_b).transpose();
    }

    Real m_lambda = 0.0; // Lame's first parameter
    Real m_mu = 0.0;     // Shear modulus
    Real m_finite_continuation_start = -1;

    // Cached deformation quantities.
    Matrix m_F = Matrix::Identity(), m_Finv = Matrix::Identity();
    Real m_detF = 1.0;
};

template<typename _Real, size_t _Dim>
struct NeoHookeanEnergy;

template<typename _Real>
struct NeoHookeanEnergy<_Real, 2> : public NeoHookeanEnergyBase<_Real, 2, NeoHookeanEnergy>
{
    using Base = NeoHookeanEnergyBase<_Real, 2, ::NeoHookeanEnergy>;
    using Real = _Real;
    using Matrix = typename Base::Matrix;

    using Base::Base;

    NeoHookeanEnergy(const NeoHookeanEnergy &other)
        : Base(other), m_C33(other.m_C33) { }

    NeoHookeanEnergy &operator=(const NeoHookeanEnergy &other) = default; // Silence deprecation warning.

    void setDeformationGradient(const Matrix &F, const EvalLevel elevel = EvalLevel::Full) {
        Base::setDeformationGradient(F, elevel);
        m_C33 = (m_lambda + 2 * m_mu) / (m_lambda * unpaddedI3() + 2 * m_mu);
    }

    // Trace of full (padded) Cauchy-Green deformation tensor.
    Real getI1() const { return m_F.squaredNorm() + m_C33; }

    // Determinant of full (padded) Cauchy-Green deformation tensor.
    Real getI3() const { return unpaddedI3() * m_C33; }

    // dI1/dF
    Matrix d_I1_d_F() const { return 2 * m_F + d_C33_d_F(); }

    // dI3/dF
    Matrix d_I3_d_F() const { return d_unpaddedI3_d_F() * m_C33 + unpaddedI3() * d_C33_d_F(); }

    // (d^2 I1 / dF^2) : dF
    template<class Mat_>
    Matrix delta_d_I1_d_F(const Mat_ &dF) const { return 2 * dF.matrix() + delta_d_C33_d_F(dF); }

    // (d^2 I&1 / dF^2) : dF
    template<typename Mat_>
    Matrix delta_d_I3_d_F(const Mat_ &dF) const {
        Matrix dC33 = d_C33_d_F();
        Matrix d_unpaddedI3 = d_unpaddedI3_d_F();
        Real delta_unpaddedI3_val = doubleContract(d_unpaddedI3, dF);

        return delta_d_unpaddedI3_d_F(dF) * m_C33                    +
               d_unpaddedI3               * doubleContract(dC33, dF) +
               delta_unpaddedI3_val       * dC33                     +
               unpaddedI3()               * delta_d_C33_d_F(dF);
    }

    // (d^3 I1 / dF^3) :: (dF_a \otimes dF_b)
    Matrix delta2_d_I1_d_F(const Matrix &dF_a, const Matrix &dF_b) const { return delta2_d_C33_d_F(dF_a, dF_b); }

    // (d^3 I3 / dF^3) :: (dF_a \otimes dF_b)
    Matrix delta2_d_I3_d_F(const Matrix &dF_a, const Matrix &dF_b) const {
        Matrix dC33 = d_C33_d_F();
        Real delta_C33_a   = doubleContract(dC33, dF_a),
             delta_C33_b   = doubleContract(dC33, dF_b);
        Real delta2_C33_ab = doubleContract(delta_d_C33_d_F(dF_a), dF_b);
        Matrix delta_d_unpaddedI3_a  = delta_d_unpaddedI3_d_F(dF_a),
               delta_d_unpaddedI3_b  = delta_d_unpaddedI3_d_F(dF_b);
        Real    delta2_unpaddedI3_ab = doubleContract(delta_d_unpaddedI3_a, dF_b);

        Matrix d_unpaddedI3 = d_unpaddedI3_d_F();
        return // Derivative of delta_d_unpaddedI3_d_F(dF) * m_C33:
               delta2_d_unpaddedI3_d_F(dF_a, dF_b) * m_C33 + delta_d_unpaddedI3_a * delta_C33_b
               // Derivative of d_unpaddedI3 * doubleContract(dC33, dF):
             + delta_d_unpaddedI3_b * delta_C33_a + d_unpaddedI3 * delta2_C33_ab
               // Derivative of delta_unpaddedI3 * dC33:
             + delta2_unpaddedI3_ab * dC33 + doubleContract(d_unpaddedI3, dF_a) * delta_d_C33_d_F(dF_b)
               // Derivative of unpaddedI3() * delta_d_C33_d_F(dF):
             + doubleContract(d_unpaddedI3, dF_b) * delta_d_C33_d_F(dF_a) + unpaddedI3() * delta2_d_C33_d_F(dF_a, dF_b);
    }

protected:
    // Derivative of normal component C33 with respect to the 2D deformation gradient.
    Matrix d_C33_d_F() const { return d_C33_d_unpaddedI3() * d_unpaddedI3_d_F(); }

    template<class Mat_>
    Matrix delta_d_C33_d_F(const Mat_ &dF) const {
        return delta_d_C33_d_unpaddedI3(dF) * d_unpaddedI3_d_F() +
               d_C33_d_unpaddedI3()         * delta_d_unpaddedI3_d_F(dF);
    }

    Matrix delta2_d_C33_d_F(const Matrix &dF_a, const Matrix &dF_b) const {
        Matrix dC33 = d_C33_d_F();
        Real delta_C33_a   = doubleContract(dC33, dF_a),
             delta_C33_b   = doubleContract(dC33, dF_b);
        Real delta2_C33_ab = doubleContract(delta_d_C33_d_F(dF_a), dF_b);

        Real coeff = -2 * m_lambda / (m_lambda + 2 * m_mu);
        // Second variation of d_C33_d_unpaddedI3 along (dF_a, dF_b)
        Real delta2_d_C33_d_unpaddedI3_ab = coeff * (delta_C33_a * delta_C33_b + m_C33 * delta2_C33_ab);
        Real delta_d_C33_d_unpaddedI3_a   = coeff * m_C33 * delta_C33_a;
        Real delta_d_C33_d_unpaddedI3_b   = coeff * m_C33 * delta_C33_b;

        return delta2_d_C33_d_unpaddedI3_ab *        d_unpaddedI3_d_F()
             + delta_d_C33_d_unpaddedI3_a   *  delta_d_unpaddedI3_d_F(dF_b)
             + delta_d_C33_d_unpaddedI3_b   *  delta_d_unpaddedI3_d_F(dF_a)
             + d_C33_d_unpaddedI3()         * delta2_d_unpaddedI3_d_F(dF_a, dF_b);
    }

    // Derivative of normal component C33 with respect to the unpadded I3 invariant.
    Real d_C33_d_unpaddedI3() const {
        return -m_C33 * m_C33 * (m_lambda / (m_lambda + 2 * m_mu));
    }

    // Directional derivative of d_C33_d_unpaddedI3 along dF
    template<class Mat_>
    Real delta_d_C33_d_unpaddedI3(const Mat_ &dF) const {
        Real delta_C33 = doubleContract(d_C33_d_F(), dF);
        return -2 * m_C33 * delta_C33 * (m_lambda / (m_lambda + 2 * m_mu));
    }

private:
    using Base::m_F;
    using Base::m_detF;
    using Base::m_lambda;
    using Base::m_mu;
    using Base::unpaddedI3;
    using Base::d_unpaddedI3_d_F;
    using Base::delta_d_unpaddedI3_d_F;
    using Base::delta2_d_unpaddedI3_d_F;
    Real m_C33 = 1.0;
};

template<typename _Real>
struct NeoHookeanEnergy<_Real, 3> : public NeoHookeanEnergyBase<_Real, 3, NeoHookeanEnergy>
{
    using Base = NeoHookeanEnergyBase<_Real, 3, ::NeoHookeanEnergy>;
    using Real = _Real;
    using Matrix = typename Base::Matrix;
    using Base::Base;

    Real getI3() const { return m_detF * m_detF; }
    Real getI1() const { return m_F.squaredNorm(); }

    Matrix d_I1_d_F() const { return 2 * m_F; }
    Matrix d_I3_d_F() const { return this->d_unpaddedI3_d_F(); }

    template<typename Mat_> Matrix delta_d_I1_d_F(const Mat_ &dF) const { return 2 * dF.matrix(); }
    template<typename Mat_> Matrix delta_d_I3_d_F(const Mat_ &dF) const { return this->delta_d_unpaddedI3_d_F(dF); }

    Matrix delta2_d_I1_d_F(const Matrix &/* dF_a */, const Matrix /* &dF_b */) const { return Matrix::Zero(); }
    Matrix delta2_d_I3_d_F(const Matrix    &dF_a   , const Matrix    &dF_b   ) const { return this->delta2_d_unpaddedI3_d_F(dF_a, dF_b); }
private:
    using Base::m_F;
    using Base::m_detF;
    using Base::m_lambda;
    using Base::m_mu;
};

// Simulate a Neo-Hookean sheet made of a material with Poisson's ratio 0.5.
// This would cause the volumetric NeoHookeanEnergy above to blow up, but we
// can obtain a membrane energy by imposing incompressibility as a hard
// constraint.
template<typename _Real>
struct IncompressibleNeoHookeanEnergyCBased {
    static constexpr EDensityType EDType = EDensityType::CBased;
    static constexpr size_t Dimension = 2;
    static constexpr size_t N         = 2;
    using Real   = _Real;
    using Matrix = Eigen::Matrix<_Real, 2, 2>;
    using M2d    = Matrix;

    static constexpr const char *name() { return "IncompressibleNeoHookean"; }

    IncompressibleNeoHookeanEnergyCBased(const IncompressibleNeoHookeanEnergyCBased &other, UninitializedDeformationTag &&) {
        setYoungModulus(other.youngModulus());
    }

    IncompressibleNeoHookeanEnergyCBased(Real E = 6 /* corresponds to "stiffness"  of 1 */) {
        setYoungModulus(E);
    }

    void setC(const M2d &C) {
        Real a = C(0, 0),
             b = C(0, 1),
             c = C(1, 1);
        if (std::abs(b - C(1, 0)) > 1e-15) throw std::runtime_error("Asymmetric matrix");

        m_C = C;
        m_trace_C = C.trace();
        m_det_C = a * c - b * b;
        m_grad_det_C <<  c, -b,
                        -b,  a;
    }

    Real energy() const {
        return stiffness * (m_trace_C + 1.0 / m_det_C - 3.0);
    }

    // dpsi / dE = 2 * dpsi/dC
    M2d PK2Stress() const {
        return 2 * stiffness * (M2d::Identity() - (1.0 / (m_det_C * m_det_C)) * m_grad_det_C);
    }

    template<class Mat_>
    M2d delta_PK2Stress(const Mat_ &dC) const {
        M2d adj_dC;
        adj_dC << dC(1, 1), -dC(0, 1),
                 -dC(1, 0),  dC(0, 0);
        return 2 * (((2.0 * stiffness / (m_det_C * m_det_C * m_det_C)) * doubleContract(m_grad_det_C, dC)) * m_grad_det_C
                         - (stiffness / (m_det_C * m_det_C))                                               * adj_dC);
    }

    template<class Mat_, class Mat2_>
    Matrix delta2_PK2Stress(const Mat_ &/* dC_a */, const Mat2_ &/* dC_b */) const {
        throw std::runtime_error("Unimplemented");
    }

    // The stiffness parameter is mu / 2 = E / (4 * (1 + nu)) = E / 6
    void setYoungModulus(Real E) {
        stiffness = E / 6;
    }

    Real youngModulus() const {
        return stiffness * 6;
    }

    void copyMaterialProperties(const IncompressibleNeoHookeanEnergyCBased &other) { setYoungModulus(other.youngModulus()); }

    Real stiffness = 1.0; // mu / 2

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
    Real m_trace_C, m_det_C;
    M2d m_grad_det_C;
    M2d m_C;
};

template <typename _Real>
using IncompressibleNeoHookeanEnergy = EnergyDensityFBasedFromCBased<IncompressibleNeoHookeanEnergyCBased<_Real>>;

#endif


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

#ifndef _MATRIX_FREE_OP_
#define _MATRIX_FREE_OP_

class MatrixFreeOperator;

namespace Eigen { namespace internal {
		template<>
		struct traits<MatrixFreeOperator> : public Eigen::internal::traits<Eigen::MatrixXd> {};
	}
}

class MatrixFreeOperator : public Eigen::EigenBase<Eigen::MatrixXd>
{
	public:

		typedef double Scalar;
		typedef double RealScalar;
		typedef int StorageIndex;

		enum {
			ColsAtCompileTime = Eigen::Dynamic,
			MaxColsAtCompileTime = Eigen::Dynamic,
			IsRowMajor = false
		};

		Index rows() const {return this-> _size;}
		Index cols() const {return this-> _size;}

		template<typename Vtype>
		Eigen::Product<MatrixFreeOperator,Vtype,Eigen::AliasFreeProduct> operator*(const Eigen::MatrixBase<Vtype>& x) const {
			return Eigen::Product<MatrixFreeOperator,Vtype,Eigen::AliasFreeProduct>(*this, x.derived());
		}

		// custom API
		MatrixFreeOperator();

		// convenience function
		Eigen::MatrixXd get_full_mat() const;
		Eigen::VectorXd diagonal() const;
		int get_size() const {return this->_size;}
		void set_size(int N) {this->_size = N;}

		// extract row/col of the operator
		virtual Eigen::VectorXd col(int index) const = 0;
		Eigen::VectorXd diag_el;

	protected:

		int _size;
};

namespace Eigen{

	namespace internal{

		// replacement of the mat*vect operation
		template<typename Vtype>
		struct generic_product_impl<MatrixFreeOperator, Vtype, DenseShape, DenseShape, GemvProduct>
		: generic_product_impl_base<MatrixFreeOperator,Vtype,generic_product_impl<MatrixFreeOperator,Vtype>>
		{

			typedef typename Product<MatrixFreeOperator,Vtype>::Scalar Scalar;

			template<typename Dest>
			static void scaleAndAddTo(Dest& dst, const MatrixFreeOperator& op, const Vtype &v, const Scalar& alpha)
			{
				//returns dst = alpha * op * v
				// alpha must be 1 here
				assert(alpha==Scalar(1) && "scaling is not implemented");
				EIGEN_ONLY_USED_FOR_DEBUG(alpha);

				// make the mat vect product
				for (int i=0; i<op.cols(); i++)
					dst += v(i) * op.col(i);
			}
		};

		// replacement of the operator*matrix operation
		template<typename Mtype>
		struct generic_product_impl<MatrixFreeOperator, Mtype, DenseShape, DenseShape, GemmProduct>
		: generic_product_impl_base<MatrixFreeOperator, Mtype, generic_product_impl<MatrixFreeOperator,Mtype>>
		{

			typedef typename Product<MatrixFreeOperator,Mtype>::Scalar Scalar;

			template<typename Dest>
			static void scaleAndAddTo(Dest& dst, const MatrixFreeOperator& op, const Mtype &m, const Scalar& alpha)
			{
				//returns dst = alpha * op * v
				// alpha must be 1 here
				assert(alpha==Scalar(1) && "scaling is not implemented");
				EIGEN_ONLY_USED_FOR_DEBUG(alpha);

				// make the mat vect product
				for (int i=0; i<op.cols();i++)
				{
					for (int j=0; j<m.cols(); j++)
						dst.col(j) += m(i,j) * op.col(i);
				}

			}
		};
	}
}

#endif


#include <iostream>
#include <chrono>
#include <Eigen/Dense>
#include <Eigen/src/Core/util/DisableStupidWarnings.h>

#include <unsupported/Eigen/NonLinearOptimization>
#include <unsupported/Eigen/NumericalDiff>

using namespace std;
using namespace Eigen;

#ifdef __cplusplus
extern "C"
{
#endif
#include "calc_cnv_res_equilibrium_NaHCO3_CaCl2.h"
#ifdef __cplusplus
}
#endif
#include "eq_exported_constanst.h"

// Generic functor
template <typename _Scalar, int NX = Eigen::Dynamic, int NY = Eigen::Dynamic>
struct Functor
{
    typedef _Scalar Scalar;
    enum
    {
        InputsAtCompileTime = NX,
        ValuesAtCompileTime = NY
    };
    typedef Eigen::Matrix<Scalar, InputsAtCompileTime, 1> InputType;
    typedef Eigen::Matrix<Scalar, ValuesAtCompileTime, 1> ValueType;
    typedef Eigen::Matrix<Scalar, ValuesAtCompileTime, InputsAtCompileTime> JacobianType;

    int m_inputs, m_values;

    Functor() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {}
    Functor(int inputs, int values) : m_inputs(inputs), m_values(values) {}

    int inputs() const { return m_inputs; }
    int values() const { return m_values; }
};

struct my_functor : Functor<double>
{
    my_functor(void) : Functor<double>(2, 2) {}
    int operator()(const Eigen::VectorXd &x, Eigen::VectorXd &fvec) const
    {
        // Implement y = 10*(x0+3)^2 + (x1-5)^2
        fvec(0) = 10.0 * pow(x(0) + 3.0, 2) + pow(x(1) - 5.0, 2);
        fvec(1) = 0;

        return 0;
    }
};

void testStackOverflowSampleOpt()
{

    int8_t N = 15;
    double state_vals[15] = {-4.41410428, -5.986602, -7.88445742, -2.00194266, -8.30714557,
                             -2.03389701, -4.23809309, -5.21713241, -7.94810423, -3.68749322,
                             -2.34133052, -7.36356081, -3.85258587, -3.51907119, -2.001};

    Eigen::VectorXd x;
    x << -4.41410428, -5.986602, -7.88445742, -2.00194266, -8.30714557,
        -2.03389701, -4.23809309, -5.21713241, -7.94810423, -3.68749322,
        -2.34133052, -7.36356081, -3.85258587, -3.51907119, -2.001;
    // x(0) = 2.0;
    // x(1) = 3.0;
    std::cout << "x: " << x << std::endl;

    my_functor functor;
    Eigen::NumericalDiff<my_functor> numDiff(functor);
    Eigen::LevenbergMarquardt<Eigen::NumericalDiff<my_functor>, double> lm(numDiff);
    lm.parameters.maxfev = 2000;
    lm.parameters.xtol = 1.0e-10;
    std::cout << lm.parameters.maxfev << std::endl;

    int ret = lm.minimize(x);
    std::cout << lm.iter << std::endl;
    std::cout << ret << std::endl;

    std::cout << "x that minimizes the function: " << x << std::endl;

    std::cout << "press [ENTER] to continue " << std::endl;
    std::cin.get();
    return;
}

struct hybrd_functor : Functor<double>
{
    hybrd_functor(int N, const VectorXd &concs_in, double TK_in) :
        Functor<double>(N, N),
        concs(concs_in),
        TK(TK_in) {}

    int operator()(const VectorXd &x, VectorXd &fvec) const
    {
        double temp, temp1, temp2;
        const VectorXd::Index n = x.size();

        assert(fvec.size() == n);
        double *x_ptr = (double *)x.data();
        double *fvec_ptr = fvec.data();
        calc_cnv_res_equilibrium_NaHCO3_CaCl2(TK, (double *)concs.data(), x_ptr, fvec_ptr);

        return 0;
    }
    int df(const VectorXd &x, MatrixXd &fjac)
    {
        const VectorXd::Index n = x.size();
        assert(fjac.rows() == n);
        assert(fjac.cols() == n);

        double *x_ptr = (double *)x.data();
        double *J_ptr = fjac.data();
        calc_jac(TK, x_ptr, J_ptr);
        fjac.transposeInPlace();
        return 0;
    }

    private:
        VectorXd concs;
        double TK;
};

std::chrono::duration<double> getElapsedTime(const std::chrono::time_point<std::chrono::system_clock> &start)
{
    std::chrono::time_point<std::chrono::system_clock> end = std::chrono::system_clock::now();
    std::chrono::duration<double> elapsed_seconds = end - start;
    return elapsed_seconds;
}

void testHybrj1()
{
    const int n = 15;
    int info;
    VectorXd x(n);
    // x << -1, -5.986602  , -7.88445742, -2.00194266, -8.30714557,
    //     -2.03389701, -4.23809309, -5.21713241, -7.94810423, -3.68749322,
    //     -2.34133052, -7.36356081, -3.85258587, -3.51907119, -2.1;
    VectorXd concs(2);
    concs << 10e-3, 5e-3;
    double TK = 25.0 + 273.15;

    /* the following starting values provide a rough fit. */
    x.setConstant(n, -1.);

    std::cout << "Hybr: w/o Jacobian" << std::endl;
    std::chrono::time_point<std::chrono::system_clock> start = std::chrono::system_clock::now();

    hybrd_functor functor(n, concs, TK);
    HybridNonLinearSolver<hybrd_functor> solver(functor);
    info = solver.solveNumericalDiff(x);

    std::cout << x << std::endl;
    std::cout << "Solution Code: " << info << std::endl;
    std::cout << "Elapsed: " << getElapsedTime(start).count() << " s" << std::endl;

    //-----------------------------------------------------------------------
    x.setConstant(n, -1.);
    std::cout << "Hybr: w/ Jacobian" << std::endl;
    start = std::chrono::system_clock::now();

    hybrd_functor functorJac(n, concs, TK);
    HybridNonLinearSolver<hybrd_functor> solverJac(functorJac);
    info = solverJac.hybrj1(x);

    std::cout << x << std::endl;
    std::cout << "Solution Code: " << info << std::endl;
    std::cout << "Elapsed: " << getElapsedTime(start).count() << " s" << std::endl;

    //-----------------------------------------------------------------------
    std::cout << "Calculating Calcite Saturation Index:" << info << std::endl;
    double SI_calcite = calc_phase_SI_Calcite(TK, x.data());
    std::cout << "SI Calcite = " << SI_calcite << std::endl;

    VectorXd gammas(n);
    calc_gammas(TK, x.data(), gammas.data());
    std::cout << "Gammas: \n " << gammas << std::endl;
    double pH = - log10(pow(10, x[IDX_SPECIES["H+"]]) * gammas[IDX_SPECIES["H+"]]);
    std::cout << "pH = " << pH << std::endl;

    return;
}

int main(int argc, char *argv[])
{
    testHybrj1();
    return 0;
}


//    Copyright 2017 Rainer Gemulla
//
//    Licensed under the Apache License, Version 2.0 (the "License");
//    you may not use this file except in compliance with the License.
//    You may obtain a copy of the License at
//
//        http://www.apache.org/licenses/LICENSE-2.0
//
//    Unless required by applicable law or agreed to in writing, software
//    distributed under the License is distributed on an "AS IS" BASIS,
//    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//    See the License for the specific language governing permissions and
//    limitations under the License.
/** \file
 *
 * Illustrates matrix factorization with DSGD. We first creates factors and then a data matrix
 * from these factors. This process ensures that we know the best factorization of the input.
 * These matrices are distributed across a cluster. We then try to reconstruct the factors
 * using DSGD.
 *
 * Run with: mpirun --hosts localhost,localhost dsgd
 * (make sure to use a production build, otherwise it will be slow)
 */
#include <iostream>

#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/math/distributions/normal.hpp>
#include <boost/random/uniform_real.hpp>

#include <util/evaluation.h>

#include <mpi2/mpi2.h>
#include <mf/mf.h>

log4cxx::LoggerPtr logger(log4cxx::Logger::getLogger("main"));

using namespace std;
using namespace mf;
using namespace mpi2;
using namespace rg;
using namespace boost::numeric::ublas;

// type of SGD
typedef UpdateTruncate<UpdateNzslL2> Update;
//typedef UpdateTruncate<UpdateNzsl> Update;
typedef RegularizeNone Regularize;
//typedef RegularizeL2 Regularize;
//typedef RegularizeNzl2 Regularize;
typedef SumLoss<NzslLoss, L2Loss> Loss;
//typedef SumLoss<NzslLoss, Nzl2Loss> Loss;
typedef NzslLoss TestLoss;

int main(int argc, char* argv[]) {
	// initialize mf library and mpi2
	boost::mpi::communicator& world = mfInit(argc, argv);

	// parameters for the factorization
	mf_size_type size1 = 10000;
	mf_size_type size2 = 10000;
	mf_size_type nnz = 1000000;
	double sigma = sqrt(10); // standard deviation
	double lambda = 1/sigma/sigma;
	mf_size_type r = 10;

	// parameters for distribution
	int tasksPerRank = 4;
	mf_size_type blocks1 = world.size() * tasksPerRank;
	mf_size_type blocks2 = world.size() * tasksPerRank;

	// parameters for SGD
	double eps0 = 0.01;
	mf_size_type epochs = 10;
	SgdOrder order = SGD_ORDER_WOR;
	StratumOrder stratumOrder = STRATUM_ORDER_RSEQ;
	Update update = Update(UpdateNzslL2(lambda), -10*sigma, 10*sigma); // truncate for numerical stability
//	Update update = Update(UpdateNzsl(), -10*sigma, 10*sigma); // truncate for numerical stability
	Regularize regularize;
//	Regularize regularize = Regularize(lambda);
	Loss loss((NzslLoss()), L2Loss(lambda));
//	Loss loss((NzslLoss()), Nzl2Loss(lambda));
	TestLoss testLoss;
	mf_size_type testNnz = nnz/100;
	BalanceType balanceType = BALANCE_NONE;
	BalanceMethod balanceMethod = BALANCE_OPTIMAL;
	bool mapReduce = false;

	// start mf library
	mfStart();

	if (world.rank() == 0)
	{
#ifndef NDEBUG
		LOG4CXX_WARN(logger, "Warning: Debug mode activated (runtimes may be slow).");
#endif

		// TODO: distribute matrix generation
		// generate original factors by sampling from a normal(0,sigma) distribution
		Random32 random; // note: this takes a default seed (not randomized!)
		DenseMatrix wIn(size1, r);
		DenseMatrixCM hIn(r, size2);
		generateRandom(wIn, random, boost::normal_distribution<>(0, sigma));
		generateRandom(hIn, random, boost::normal_distribution<>(0, sigma));

		// generate a sparse matrix by selecting random entries from the generated factors
		// and add small Gaussian noise
		SparseMatrix v;
		generateRandom(v, nnz, wIn, hIn, random);
		addRandom(v, random, boost::normal_distribution<>(0, 0.1));
		LOG4CXX_INFO(logger, "Data matrix: "
			<< v.size1() << " x " << v.size2() << ", " << v.nnz() << " nonzeros");
		LOG4CXX_INFO(logger, "Loss with original factors: " << loss((FactorizationData<>(v, wIn, hIn))));

		// create a test matrix (without noise)
		SparseMatrix vTest;
		generateRandom(vTest, testNnz, wIn, hIn, random);
		LOG4CXX_INFO(logger, "Test matrix: "
			<< v.size1() << " x " << v.size2() << ", " << vTest.nnz() << " nonzeros");

		// take a small sample and remove empty rows/columns
		ProjectedSparseMatrix Vsample;
		projectRandomSubmatrix(random, v, Vsample, v.size1()/5, v.size2()/5);
		projectFrequent(Vsample, 0);
		LOG4CXX_INFO(logger, "Sample matrix: "
			<< Vsample.data.size1() << " x " << Vsample.data.size2()
			<< ", " << Vsample.data.nnz() << " nonzeros");

		// generate initial factors by sampling from a uniform[-0.5,0.5] distribution
		DenseMatrix w(size1, r);
		DenseMatrixCM h(r, size2);
		generateRandom(w, random, boost::uniform_real<>(-0.5, 0.5));
		generateRandom(h, random, boost::uniform_real<>(-0.5, 0.5));

		// distribute the input matrices and test matrix (cheaper than fetching W/H)
		DistributedSparseMatrix dv = distributeMatrix("V", blocks1, blocks2, true, v);
		LOG4CXX_INFO(logger, "Distributed data matrix: "
					<< dv.blocks1() << " x " << dv.blocks2() << " blocks");
		DistributedSparseMatrix dvTest = distributeMatrix("VTest", blocks1, blocks2, true, vTest);
		DistributedDenseMatrix dw = distributeMatrix("W", blocks1, 1, true, w);
		DistributedDenseMatrixCM dh = distributeMatrix("H", 1, blocks2, false, h);
		LOG4CXX_INFO(logger, "Distributed factor matrices");

		// initialize the DSGD
		Timer t;
		DsgdRunner dsgdRunner(random);
		DsgdJob<Update,Regularize> dsgdJob(dv, dw, dh, update, regularize, order,
				stratumOrder, mapReduce, tasksPerRank);
		//BoldDriver decay(eps0);
		DistributedDecayAuto<Update,Regularize,Loss> decay(dsgdJob, loss, Vsample, "decay", eps0,
				world.size()*tasksPerRank);
		Trace trace;

		//std::string s1("Loss"),s2("nzsl"),s3("Regularize"),s4("L2"),s5("nodes"),s6("threads");

		trace.addField("Loss","nzsl");
		trace.addField("Regularize","L2");

		trace.addField("nodes",3);
		trace.addField("threads",4);

		// print the test loss
		DsgdFactorizationData<> testData(dvTest,dw,dh,tasksPerRank);
		LOG4CXX_INFO(logger, "Initial test loss: " << testLoss(testData));

		// run DSGD to try to reconstruct the original factors
		t.start();
		dsgdRunner.run(dsgdJob, loss, epochs, decay, trace, balanceType, balanceMethod, &testData, &testLoss);
		//dsgdRunner.run(dsgdJob, loss, epochs, decay, trace);

		t.stop();
		LOG4CXX_INFO(logger, "Total time: " << t);

		// print the test loss
		LOG4CXX_INFO(logger, "Final test loss: " << testLoss(testData));

		// write trace to an R file
		LOG4CXX_INFO(logger, "Writing trace to " << "/tmp/dsgd-trace.R");
		trace.toRfile("/tmp/dsgd-trace.R", "dsgd");

//		cout << squaredSums2(dsgdJob.dw, tasksPerRank) << endl;
//		cout << squaredSums1(dsgdJob.dh, tasksPerRank) << endl;
	}

	mfStop();
	mfFinalize();

	return 0;
}


#include <Eigen/Core>
#include <Eigen/LU>
#include <elasty/alembic-manager.hpp>
#include <elasty/fem.hpp>
#include <timer.hpp>
#include <vector>

namespace
{
    constexpr size_t k_num_dims = 2;

    constexpr double k_youngs_modulus = 800.0;
    constexpr double k_poisson_ratio  = 0.40;

    constexpr double k_first_lame  = elasty::fem::calcFirstLame(k_youngs_modulus, k_poisson_ratio);
    constexpr double k_second_lame = elasty::fem::calcSecondLame(k_youngs_modulus, k_poisson_ratio);

    constexpr unsigned k_num_substeps = 10;
    constexpr double   k_delta_time   = 1.0 / 60.0;

    constexpr double k_damping_factor = 0.1;

    enum class Model
    {
        CoRotational,
        StVenantKirchhoff
    };

    constexpr Model k_model = Model::CoRotational;
} // namespace

template <int N> struct Mesh
{
    using ElemList = Eigen::Matrix<std::int32_t, N + 1, Eigen::Dynamic>;

    ElemList elems;

    Eigen::VectorXd x_rest;
    Eigen::VectorXd x;
    Eigen::VectorXd v;
    Eigen::VectorXd f;

    double mass = 1.0;

    /// \details Should be precomputed
    Eigen::VectorXd lumped_mass;

    /// \details Should be precomputed
    std::vector<double> volume_array;

    /// \details Should be precomputed
    std::vector<Eigen::Matrix<double, N, N>> rest_shape_mat_inv_array;

    /// \details Should be precomputed
    std::vector<Eigen::Matrix<double, N * N, N*(N + 1)>> vec_PFPx_array;
};

using TriangleMesh = Mesh<2>;

template <typename Derived> typename Derived::Scalar calcEnergyDensity(const Eigen::MatrixBase<Derived>& deform_grad)
{
    switch (k_model)
    {
        case Model::StVenantKirchhoff:
            return elasty::fem::calcStVenantKirchhoffEnergyDensity(deform_grad, k_first_lame, k_second_lame);
        case Model::CoRotational:
            return elasty::fem::calcCoRotationalEnergyDensity(deform_grad, k_first_lame, k_second_lame);
    }
}

template <typename Derived>
Eigen::Matrix<typename Derived::Scalar, 2, 2> calcPiolaStress(const Eigen::MatrixBase<Derived>& deform_grad)
{
    switch (k_model)
    {
        case Model::StVenantKirchhoff:
            return elasty::fem::calcStVenantKirchhoffPiolaStress(deform_grad, k_first_lame, k_second_lame);
        case Model::CoRotational:
            return elasty::fem::calcCoRotationalPiolaStress(deform_grad, k_first_lame, k_second_lame);
    }
}

class Explicit2dEngine
{
public:
    Explicit2dEngine() {}

    void proceedFrame()
    {
        const std::size_t              num_verts         = m_mesh.x_rest.size() / k_num_dims;
        const std::vector<std::size_t> constrained_verts = {0, 1, 2, 3, 4};

        // Reset forces
        m_mesh.f = Eigen::VectorXd::Zero(2 * num_verts);

        // Calculate forces
        for (std::size_t i = 0; i < m_mesh.elems.cols(); ++i)
        {
            const auto& indices = m_mesh.elems.col(i);

            // Retrieve precomputed values
            const auto& D_m_inv  = m_mesh.rest_shape_mat_inv_array[i];
            const auto& area     = m_mesh.volume_array[i];
            const auto& vec_PFPx = m_mesh.vec_PFPx_array[i];

            // Calculate the deformation gradient $\mathbf{F}$
            const auto F = elasty::fem::calc2dTriangleDeformGrad(m_mesh.x.segment<2>(2 * indices[0]),
                                                                 m_mesh.x.segment<2>(2 * indices[1]),
                                                                 m_mesh.x.segment<2>(2 * indices[2]),
                                                                 D_m_inv);

            // Calculate $\frac{\partial \Phi}{\partial \mathbf{x}}$ and related values
            const auto P      = calcPiolaStress(F);
            const auto vec_P  = Eigen::Map<const Eigen::Vector4d>(P.data(), P.size());
            const auto PPsiPx = vec_PFPx.transpose() * vec_P;

            // Calculate the internal force
            const auto force = -area * PPsiPx;

            assert(indices.size() == 3);
            assert(F.rows() == 2 && F.cols() == 2);
            assert(vec_PFPx.rows() == 4 && vec_PFPx.cols() == 6);
            assert(PPsiPx.rows() == 6 && PPsiPx.cols() == 1);

            // Accumulate forces
            m_mesh.f.segment(2 * indices[0], 2) += force.segment(2 * 0, 2);
            m_mesh.f.segment(2 * indices[1], 2) += force.segment(2 * 1, 2);
            m_mesh.f.segment(2 * indices[2], 2) += force.segment(2 * 2, 2);
        }

        // Apply gravity force
        for (size_t i = 0; i < num_verts; ++i)
        {
            m_mesh.f[i * 2 + 1] += m_mesh.lumped_mass(i * 2 + 1) * (-9.80665);
        }

        // Calculate the "modified" inverse lumped mass matrix
        // Note: This "mass modification" is described in "Large Steps in Cloth Simulation" (SIGGRAPH '98)
        Eigen::VectorXd W_diags = m_mesh.lumped_mass.cwiseInverse();
        for (size_t i : constrained_verts)
        {
            W_diags.segment(i * 2, 2) = Eigen::Vector2d::Zero();
        }
        const auto W = W_diags.asDiagonal();

        // Note: Explicit Euler integration (for velocities)
        m_mesh.v = m_mesh.v + m_delta_physics_time * W * m_mesh.f;

        // Note: Explicit Euler integration (for positions)
        m_mesh.x = m_mesh.x + m_delta_physics_time * m_mesh.v;

        // Apply naive damping
        m_mesh.v *= std::exp(-k_damping_factor * m_delta_physics_time);
    }

    void initializeScene()
    {
        // A simple cantilever

        constexpr std::size_t num_cols  = 20;
        constexpr std::size_t num_rows  = 4;
        constexpr std::size_t num_verts = (num_cols + 1) * (num_rows + 1);
        constexpr std::size_t num_elems = (num_cols * num_rows) * 2;
        constexpr double      size      = 1.0;

        m_mesh.elems.resize(3, num_elems);
        m_mesh.x_rest.resize(num_verts * k_num_dims);

        // Generate a triangle mesh
        for (std::size_t col = 0; col < num_cols; ++col)
        {
            for (std::size_t row = 0; row < num_rows; ++row)
            {
                const auto base = col * (num_rows + 1) + row;

                m_mesh.elems.col(2 * num_rows * col + 2 * row + 0) << 0 + base, 1 + base, (num_rows + 1) + 1 + base;
                m_mesh.elems.col(2 * num_rows * col + 2 * row + 1) << 0 + base, (num_rows + 1) + 1 + base,
                    (num_rows + 1) + base;

                m_mesh.x_rest.segment(k_num_dims * ((num_rows + 1) * col + row), k_num_dims) =
                    Eigen::Vector2d{col * 1.0, -1.0 * row};
            }
            m_mesh.x_rest.segment(k_num_dims * ((num_rows + 1) * col + num_rows), k_num_dims) =
                Eigen::Vector2d{col * 1.0, -1.0 * num_rows};
        }
        for (std::size_t row = 0; row < num_rows; ++row)
        {
            m_mesh.x_rest.segment(k_num_dims * ((num_rows + 1) * num_cols + row), k_num_dims) =
                Eigen::Vector2d{num_cols * 1.0, -1.0 * row};
        }
        m_mesh.x_rest.segment(k_num_dims * ((num_rows + 1) * num_cols + num_rows), k_num_dims) =
            Eigen::Vector2d{num_cols * 1.0, -1.0 * num_rows};

        // Set transform
        m_mesh.x_rest *= 1.0 / static_cast<double>(num_rows);
        for (std::size_t vert = 0; vert < num_verts; ++vert)
        {
            m_mesh.x_rest[2 * vert + 1] += 0.5;
        }
        m_mesh.x_rest *= size;

        // Initialize other values
        m_mesh.x = m_mesh.x_rest;
        m_mesh.v = Eigen::VectorXd::Zero(k_num_dims * num_verts);
        m_mesh.f = Eigen::VectorXd::Zero(k_num_dims * num_verts);

        // Perform precomputation
        m_mesh.volume_array.resize(num_elems);
        m_mesh.rest_shape_mat_inv_array.resize(num_elems);
        m_mesh.vec_PFPx_array.resize(num_elems);
        for (std::size_t elem_index = 0; elem_index < num_elems; ++elem_index)
        {
            const auto& indices = m_mesh.elems.col(elem_index);

            m_mesh.volume_array[elem_index] = elasty::fem::calc2dTriangleArea(m_mesh.x_rest.segment<2>(2 * indices[0]),
                                                                              m_mesh.x_rest.segment<2>(2 * indices[1]),
                                                                              m_mesh.x_rest.segment<2>(2 * indices[2]));
            m_mesh.rest_shape_mat_inv_array[elem_index] =
                elasty::fem::calc2dShapeMatrix(m_mesh.x_rest.segment<2>(2 * indices[0]),
                                               m_mesh.x_rest.segment<2>(2 * indices[1]),
                                               m_mesh.x_rest.segment<2>(2 * indices[2]))
                    .inverse();
            m_mesh.vec_PFPx_array[elem_index] =
                elasty::fem::calcVecTrianglePartDeformGradPartPos(m_mesh.rest_shape_mat_inv_array[elem_index]);
        }
        m_mesh.lumped_mass = elasty::fem::calcTriangleMeshLumpedMass(m_mesh.x_rest, m_mesh.elems, m_mesh.mass);
    }

    /// \brief Getter of the delta physics time.
    ///
    /// \details The value equals to the delta frame time devided by the number of substeps.
    double getDeltaPhysicsTime() const { return m_delta_physics_time; }

    void setDeltaPhysicsTime(const double delta_physics_time) { m_delta_physics_time = delta_physics_time; }

    const TriangleMesh* getMesh() const { return &m_mesh; }

private:
    double m_delta_physics_time = 1.0 / 60.0;

    TriangleMesh m_mesh;
};

int main(int argc, char** argv)
{
    Explicit2dEngine engine;

    engine.initializeScene();
    engine.setDeltaPhysicsTime(k_delta_time / static_cast<double>(k_num_substeps));

    const auto        mesh      = engine.getMesh();
    const std::size_t num_verts = mesh->x_rest.size() / 2;
    const std::size_t num_elems = mesh->elems.cols();

    auto alembic_manager = elasty::createTriangleMesh2dAlembicManager(
        "./out.abc", k_delta_time, num_verts, num_elems, mesh->x.data(), mesh->elems.data());

    for (unsigned int frame = 0; frame < 240; ++frame)
    {
        timer::Timer t(std::to_string(frame));

        alembic_manager->submitCurrentStatus();

        for (int i = 0; i < k_num_substeps; ++i)
        {
            engine.proceedFrame();
        }
    }

    return 0;
}


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2008 Steven Watanabe
//
// 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)

// $Id: lambda.cpp 27 2008-06-16 14:50:58Z maehne $

////////////////////////////////////////////////////////////////////////
///
/// \file lambda.cpp
///
/// \brief Example demonstrating the usage of Boost.Units' quantity,
///        unit, and absolute types in functors created with the
///        Boost.Lambda library and stored in Boost.Function objects.
///
/// \author Torsten Maehne
/// \date   2008-06-04
///
/// A mechanical, electrical, geometrical, and thermal example
/// demonstrate how to use Boost.Units' quantity, unit, and absolute
/// types in lambda expressions. The resulting functors can be stored
/// in boost::function objects. It is also shown how to work around a
/// limitation of Boost.Lambda's bind() to help it to find the correct
/// overloaded function by specifying its signature with a
/// static_cast.
///
////////////////////////////////////////////////////////////////////////

#include <iostream>
#include <boost/function.hpp>
#include <boost/units/io.hpp>
#include <boost/units/cmath.hpp>
#include <boost/units/pow.hpp>
#include <boost/units/systems/si.hpp>
#include <boost/units/absolute.hpp>

// Include boost/units/lambda.hpp instead of boost/lambda/lambda.hpp
// for a convenient usage of Boost.Units' quantity, unit, and absolute
// types in lambda expressions. The header augments Boost.Lambda's
// return type detuction system to recognize the new types so that not
// for each arithmetic operation the return type needs to be
// explicitely specified.
#include <boost/units/lambda.hpp>

#include <boost/lambda/bind.hpp>

static const double pi = 3.14159265358979323846;

//[lambda_snippet_1

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

   using namespace std;
   namespace bl = boost::lambda;
   namespace bu = boost::units;
   namespace si = boost::units::si;

   ////////////////////////////////////////////////////////////////////////
   // Mechanical example: linear accelerated movement
   ////////////////////////////////////////////////////////////////////////

   // Initial condition variables for acceleration, speed, and displacement
   bu::quantity<si::acceleration> a = 2.0 * si::meters_per_second_squared;
   bu::quantity<si::velocity> v = 1.0 * si::meters_per_second;
   bu::quantity<si::length> s0 = 0.5 * si::meter;

   // Displacement over time
   boost::function<bu::quantity<si::length> (bu::quantity<si::time>) >
       s = 0.5 * bl::var(a) * bl::_1 * bl::_1
           + bl::var(v) * bl::_1
           + bl::var(s0);

   cout << "Linear accelerated movement:" << endl
        << "a = " << a << ", v = " << v << ", s0 = " << s0 << endl
        << "s(1.0 * si::second) = " << s(1.0 * si::second) << endl
        << endl;

   // Change initial conditions
   a = 1.0 * si::meters_per_second_squared;
   v = 2.0 * si::meters_per_second;
   s0 = -1.5 * si::meter;

   cout << "a = " << a << ", v = " << v << ", s0 = " << s0 << endl
        << "s(1.0 * si::second) = " << s(1.0 * si::second) << endl
        << endl;

   ////////////////////////////////////////////////////////////////////////
   // Electrical example: oscillating current
   ////////////////////////////////////////////////////////////////////////

   // Constants for the current amplitude, frequency, and offset current
   const bu::quantity<si::current> iamp = 1.5 * si::ampere;
   const bu::quantity<si::frequency> f = 1.0e3 * si::hertz;
   const bu::quantity<si::current> i0 = 0.5 * si::ampere;

   // The invocation of the sin function needs to be postponed using
   // bind to specify the oscillation function. A lengthy static_cast
   // to the function pointer referencing boost::units::sin() is needed
   // to avoid an "unresolved overloaded function type" error.
   boost::function<bu::quantity<si::current> (bu::quantity<si::time>) >
       i = iamp
           * bl::bind(static_cast<bu::dimensionless_quantity<si::system, double>::type (*)(const bu::quantity<si::plane_angle>&)>(bu::sin),
                      2.0 * pi * si::radian * f * bl::_1)
           + i0;

   cout << "Oscillating current:" << endl
        << "iamp = " << iamp << ", f = " << f << ", i0 = " << i0 << endl
        << "i(1.25e-3 * si::second) = " << i(1.25e-3 * si::second) << endl
        << endl;

   ////////////////////////////////////////////////////////////////////////
   // Geometric example: area calculation for a square
   ////////////////////////////////////////////////////////////////////////

   // Length constant
   const bu::quantity<si::length> l = 1.5 * si::meter;

   // Again an ugly static_cast is needed to bind pow<2> to the first
   // function argument.
   boost::function<bu::quantity<si::area> (bu::quantity<si::length>) >
       A = bl::bind(static_cast<bu::quantity<si::area> (*)(const bu::quantity<si::length>&)>(bu::pow<2>),
                    bl::_1);

   cout << "Area of a square:" << endl
        << "A(" << l <<") = " << A(l) << endl << endl;

   ////////////////////////////////////////////////////////////////////////
   // Thermal example: temperature difference of two absolute temperatures
   ////////////////////////////////////////////////////////////////////////

   // Absolute temperature constants
   const bu::quantity<bu::absolute<si::temperature> >
       Tref = 273.15 * bu::absolute<si::temperature>();
   const bu::quantity<bu::absolute<si::temperature> >
       Tamb = 300.00 * bu::absolute<si::temperature>();

   boost::function<bu::quantity<si::temperature> (bu::quantity<bu::absolute<si::temperature> >,
                                                  bu::quantity<bu::absolute<si::temperature> >)>
       dT = bl::_2 - bl::_1;

   cout << "Temperature difference of two absolute temperatures:" << endl
        << "dT(" << Tref << ", " << Tamb << ") = " << dT(Tref, Tamb) << endl
        << endl;

   return 0;
}
//]


#include <NTL/mat_RR.h>

#include <NTL/new.h>

NTL_START_IMPL

void add(mat_RR& X, const mat_RR& A, const mat_RR& B)
{
   long n = A.NumRows();
   long m = A.NumCols();

   if (B.NumRows() != n || B.NumCols() != m)
      Error("matrix add: dimension mismatch");

   X.SetDims(n, m);

   long i, j;
   for (i = 1; i <= n; i++)
      for (j = 1; j <= m; j++)
         add(X(i,j), A(i,j), B(i,j));
}

void sub(mat_RR& X, const mat_RR& A, const mat_RR& B)
{
   long n = A.NumRows();
   long m = A.NumCols();

   if (B.NumRows() != n || B.NumCols() != m)
      Error("matrix sub: dimension mismatch");

   X.SetDims(n, m);

   long i, j;
   for (i = 1; i <= n; i++)
      for (j = 1; j <= m; j++)
         sub(X(i,j), A(i,j), B(i,j));
}

void mul_aux(mat_RR& X, const mat_RR& A, const mat_RR& B)
{
   long n = A.NumRows();
   long l = A.NumCols();
   long m = B.NumCols();

   if (l != B.NumRows())
      Error("matrix mul: dimension mismatch");

   X.SetDims(n, m);

   long i, j, k;
   RR acc, tmp;

   for (i = 1; i <= n; i++) {
      for (j = 1; j <= m; j++) {
         clear(acc);
         for(k = 1; k <= l; k++) {
            mul(tmp, A(i,k), B(k,j));
            add(acc, acc, tmp);
         }
         X(i,j) = acc;
      }
   }
}

void mul(mat_RR& X, const mat_RR& A, const mat_RR& B)
{
   if (&X == &A || &X == &B) {
      mat_RR tmp;
      mul_aux(tmp, A, B);
      X = tmp;
   }
   else
      mul_aux(X, A, B);
}

static
void mul_aux(vec_RR& x, const mat_RR& A, const vec_RR& b)
{
   long n = A.NumRows();
   long l = A.NumCols();

   if (l != b.length())
      Error("matrix mul: dimension mismatch");

   x.SetLength(n);

   long i, k;
   RR acc, tmp;

   for (i = 1; i <= n; i++) {
      clear(acc);
      for (k = 1; k <= l; k++) {
         mul(tmp, A(i,k), b(k));
         add(acc, acc, tmp);
      }
      x(i) = acc;
   }
}

void mul(vec_RR& x, const mat_RR& A, const vec_RR& b)
{
   if (&b == &x || A.position1(x) != -1) {
      vec_RR tmp;
      mul_aux(tmp, A, b);
      x = tmp;
   }
   else
      mul_aux(x, A, b);
}

static
void mul_aux(vec_RR& x, const vec_RR& a, const mat_RR& B)
{
   long n = B.NumRows();
   long l = B.NumCols();

   if (n != a.length())
      Error("matrix mul: dimension mismatch");

   x.SetLength(l);

   long i, k;
   RR acc, tmp;

   for (i = 1; i <= l; i++) {
      clear(acc);
      for (k = 1; k <= n; k++) {
         mul(tmp, a(k), B(k,i));
         add(acc, acc, tmp);
      }
      x(i) = acc;
   }
}

void mul(vec_RR& x, const vec_RR& a, const mat_RR& B)
{
   if (&a == &x) {
      vec_RR tmp;
      mul_aux(tmp, a, B);
      x = tmp;
   }
   else
      mul_aux(x, a, B);
}

void ident(mat_RR& X, long n)
{
   X.SetDims(n, n);
   long i, j;

   for (i = 1; i <= n; i++)
      for (j = 1; j <= n; j++)
         if (i == j)
            set(X(i, j));
         else
            clear(X(i, j));
}

void determinant(RR& d, const mat_RR& M_in)
{
   long k, n;
   long i, j;
   long pos;
   RR t1, t2;
   RR *x, *y;

   n = M_in.NumRows();

   if (M_in.NumCols() != n)
      Error("determinant: nonsquare matrix");

   if (n == 0) {
      set(d);
      return;
   }

   mat_RR M;

   M = M_in;

   RR det;
   set(det);

   RR maxval;

   for (k = 0; k < n; k++) {
      pos = -1;
      clear(maxval);
      for (i = k; i < n; i++) {
         abs(t1, M[i][k]);
         if (t1 > maxval) {
            pos = i;
            maxval = t1;
         }
      }

      if (pos != -1) {
         if (k != pos) {
            swap(M[pos], M[k]);
            negate(det, det);
         }

         mul(det, det, M[k][k]);

         // make M[k, k] == -1

         inv(t1, M[k][k]);
         negate(t1, t1);
         for (j = k+1; j < n; j++) {
            mul(M[k][j], M[k][j], t1);
         }

         for (i = k+1; i < n; i++) {
            // M[i] = M[i] + M[k]*M[i,k]

            t1 = M[i][k];

            x = M[i].elts() + (k+1);
            y = M[k].elts() + (k+1);

            for (j = k+1; j < n; j++, x++, y++) {
               // *x = *x + (*y)*t1

               mul(t2, *y, t1);
               add(*x, *x, t2);
            }
         }
      }
      else {
         clear(d);
         return;
      }
   }

   d = det;
}

RR determinant(const mat_RR& a)
   { RR x; determinant(x, a); NTL_OPT_RETURN(RR, x); }

long IsIdent(const mat_RR& A, long n)
{
   if (A.NumRows() != n || A.NumCols() != n)
      return 0;

   long i, j;

   for (i = 1; i <= n; i++)
      for (j = 1; j <= n; j++)
         if (i != j) {
            if (!IsZero(A(i, j))) return 0;
         }
         else {
            if (!IsOne(A(i, j))) return 0;
         }

   return 1;
}

void transpose(mat_RR& X, const mat_RR& A)
{
   long n = A.NumRows();
   long m = A.NumCols();

   long i, j;

   if (&X == & A) {
      if (n == m)
         for (i = 1; i <= n; i++)
            for (j = i+1; j <= n; j++)
               swap(X(i, j), X(j, i));
      else {
         mat_RR tmp;
         tmp.SetDims(m, n);
         for (i = 1; i <= n; i++)
            for (j = 1; j <= m; j++)
               tmp(j, i) = A(i, j);
         X.kill();
         X = tmp;
      }
   }
   else {
      X.SetDims(m, n);
      for (i = 1; i <= n; i++)
         for (j = 1; j <= m; j++)
            X(j, i) = A(i, j);
   }
}

void solve(RR& d, vec_RR& X,
           const mat_RR& A, const vec_RR& b)

{
   long n = A.NumRows();
   if (A.NumCols() != n)
      Error("solve: nonsquare matrix");

   if (b.length() != n)
      Error("solve: dimension mismatch");

   if (n == 0) {
      set(d);
      X.SetLength(0);
      return;
   }

   long i, j, k, pos;
   RR t1, t2;
   RR *x, *y;

   mat_RR M;
   M.SetDims(n, n+1);

   for (i = 0; i < n; i++) {
      for (j = 0; j < n; j++)
         M[i][j] = A[j][i];
      M[i][n] = b[i];
   }

   RR det;
   set(det);

   RR maxval;

   for (k = 0; k < n; k++) {
      pos = -1;
      clear(maxval);
      for (i = k; i < n; i++) {
         abs(t1, M[i][k]);
         if (t1 > maxval) {
            pos = i;
            maxval = t1;
         }
      }

      if (pos != -1) {
         if (k != pos) {
            swap(M[pos], M[k]);
            negate(det, det);
         }

         mul(det, det, M[k][k]);

         // make M[k, k] == -1

         inv(t1, M[k][k]);
         negate(t1, t1);
         for (j = k+1; j <= n; j++) {
            mul(M[k][j], M[k][j], t1);
         }

         for (i = k+1; i < n; i++) {
            // M[i] = M[i] + M[k]*M[i,k]

            t1 = M[i][k];

            x = M[i].elts() + (k+1);
            y = M[k].elts() + (k+1);

            for (j = k+1; j <= n; j++, x++, y++) {
               // *x = *x + (*y)*t1

               mul(t2, *y, t1);
               add(*x, *x, t2);
            }
         }
      }
      else {
         clear(d);
         return;
      }
   }

   X.SetLength(n);
   for (i = n-1; i >= 0; i--) {
      clear(t1);
      for (j = i+1; j < n; j++) {
         mul(t2, X[j], M[i][j]);
         add(t1, t1, t2);
      }
      sub(t1, t1, M[i][n]);
      X[i] = t1;
   }

   d = det;
}

void inv(RR& d, mat_RR& X, const mat_RR& A)
{
   long n = A.NumRows();
   if (A.NumCols() != n)
      Error("inv: nonsquare matrix");

   if (n == 0) {
      set(d);
      X.SetDims(0, 0);
      return;
   }

   long i, j, k, pos;
   RR t1, t2;
   RR *x, *y;

   mat_RR M;
   M.SetDims(n, 2*n);

   for (i = 0; i < n; i++) {
      for (j = 0; j < n; j++) {
         M[i][j] = A[i][j];
         clear(M[i][n+j]);
      }
      set(M[i][n+i]);
   }

   RR det;
   set(det);

   RR maxval;

   for (k = 0; k < n; k++) {
      pos = -1;
      clear(maxval);
      for (i = k; i < n; i++) {
         abs(t1, M[i][k]);
         if (t1 > maxval) {
            pos = i;
            maxval = t1;
         }
      }

      if (pos != -1) {
         if (k != pos) {
            swap(M[pos], M[k]);
            negate(det, det);
         }

         mul(det, det, M[k][k]);

         // make M[k, k] == -1

         inv(t1, M[k][k]);
         negate(t1, t1);
         for (j = k+1; j < 2*n; j++) {
            mul(M[k][j], M[k][j], t1);
         }

         for (i = k+1; i < n; i++) {
            // M[i] = M[i] + M[k]*M[i,k]

            t1 = M[i][k];

            x = M[i].elts() + (k+1);
            y = M[k].elts() + (k+1);

            for (j = k+1; j < 2*n; j++, x++, y++) {
               // *x = *x + (*y)*t1

               mul(t2, *y, t1);
               add(*x, *x, t2);
            }
         }
      }
      else {
         clear(d);
         return;
      }
   }

   X.SetDims(n, n);
   for (k = 0; k < n; k++) {
      for (i = n-1; i >= 0; i--) {
         clear(t1);
         for (j = i+1; j < n; j++) {
            mul(t2, X[j][k], M[i][j]);
            add(t1, t1, t2);
         }
         sub(t1, t1, M[i][n+k]);
         X[i][k] = t1;
      }
   }

   d = det;
}

void mul(mat_RR& X, const mat_RR& A, const RR& b_in)
{
   RR b = b_in;
   long n = A.NumRows();
   long m = A.NumCols();

   X.SetDims(n, m);

   long i, j;
   for (i = 0; i < n; i++)
      for (j = 0; j < m; j++)
         mul(X[i][j], A[i][j], b);
}

void mul(mat_RR& X, const mat_RR& A, double b_in)
{
   static RR b;
   b = b_in;
   long n = A.NumRows();
   long m = A.NumCols();

   X.SetDims(n, m);

   long i, j;
   for (i = 0; i < n; i++)
      for (j = 0; j < m; j++)
         mul(X[i][j], A[i][j], b);
}

void diag(mat_RR& X, long n, const RR& d_in)
{
   RR d = d_in;
   X.SetDims(n, n);
   long i, j;

   for (i = 1; i <= n; i++)
      for (j = 1; j <= n; j++)
         if (i == j)
            X(i, j) = d;
         else
            clear(X(i, j));
}

long IsDiag(const mat_RR& A, long n, const RR& d)
{
   if (A.NumRows() != n || A.NumCols() != n)
      return 0;

   long i, j;

   for (i = 1; i <= n; i++)
      for (j = 1; j <= n; j++)
         if (i != j) {
            if (!IsZero(A(i, j))) return 0;
         }
         else {
            if (A(i, j) != d) return 0;
         }

   return 1;
}

void negate(mat_RR& X, const mat_RR& A)
{
   long n = A.NumRows();
   long m = A.NumCols();

   X.SetDims(n, m);

   long i, j;
   for (i = 1; i <= n; i++)
      for (j = 1; j <= m; j++)
         negate(X(i,j), A(i,j));
}

long IsZero(const mat_RR& a)
{
   long n = a.NumRows();
   long i;

   for (i = 0; i < n; i++)
      if (!IsZero(a[i]))
         return 0;

   return 1;
}

void clear(mat_RR& x)
{
   long n = x.NumRows();
   long i;
   for (i = 0; i < n; i++)
      clear(x[i]);
}

mat_RR operator+(const mat_RR& a, const mat_RR& b)
{
   mat_RR res;
   add(res, a, b);
   NTL_OPT_RETURN(mat_RR, res);
}

mat_RR operator*(const mat_RR& a, const mat_RR& b)
{
   mat_RR res;
   mul_aux(res, a, b);
   NTL_OPT_RETURN(mat_RR, res);
}

mat_RR operator-(const mat_RR& a, const mat_RR& b)
{
   mat_RR res;
   sub(res, a, b);
   NTL_OPT_RETURN(mat_RR, res);
}

mat_RR operator-(const mat_RR& a)
{
   mat_RR res;
   negate(res, a);
   NTL_OPT_RETURN(mat_RR, res);
}

vec_RR operator*(const mat_RR& a, const vec_RR& b)
{
   vec_RR res;
   mul_aux(res, a, b);
   NTL_OPT_RETURN(vec_RR, res);
}

vec_RR operator*(const vec_RR& a, const mat_RR& b)
{
   vec_RR res;
   mul_aux(res, a, b);
   NTL_OPT_RETURN(vec_RR, res);
}

void inv(mat_RR& X, const mat_RR& A)
{
   RR d;
   inv(d, X, A);
   if (d == 0) Error("inv: non-invertible matrix");
}

void power(mat_RR& X, const mat_RR& A, const ZZ& e)
{
   if (A.NumRows() != A.NumCols()) Error("power: non-square matrix");

   if (e == 0) {
      ident(X, A.NumRows());
      return;
   }

   mat_RR T1, T2;
   long i, k;

   k = NumBits(e);
   T1 = A;

   for (i = k-2; i >= 0; i--) {
      sqr(T2, T1);
      if (bit(e, i))
         mul(T1, T2, A);
      else
         T1 = T2;
   }

   if (e < 0)
      inv(X, T1);
   else
      X = T1;
}

NTL_END_IMPL


#include <random>
#include <cmath>
#include <cassert>

#include <omp.h>

#include <NTL/ZZ.h>
#include <NTL/tools.h>

#include "gsw.hpp"

using namespace std;
using namespace NTL;

GSW::GSW() {
    GSW(80, 1);
}

GSW::GSW(const int kappa, const int L) {
    // Search for suitable parameters:
    // n >= log(q/sigma)(k+110)/7.2
    // q/sigma6 > 8(N + 1)^L
    BigInt lower_bound;
    n = (kappa+110)/7.2;
    quotient = 4;
    l = floor(log(quotient)/log(2)) + 1;
    N = (n + 1) * l;
    while (true) {
        power(lower_bound, N+1,L);
        lower_bound *= 8 * sigma6;
        if (quotient <= lower_bound) {
            NextPrime(quotient, lower_bound);
        } else {
            break;
        }
        n = log(quotient/ceil(sigma))*(kappa+110)/(7.2*log(2));
        l = floor(log(quotient)/log(2)) + 1;
        N = (n + 1) * l;
    }

    n_1 = n+1;
    m = ceil(n * log(quotient)/log(2));

    gaussSampler = new GaussSampler(sigma);

    omp_set_num_threads(4);
}

GSW::~GSW() {
    delete gaussSampler;
}

BIVector GSW::secret_key_gen() const {
    BIVector secret_key(n+1);
    // sample uniformly
    for (size_t i = 1; i < secret_key.size(); i++) {
        secret_key[i] = RandomBnd(quotient);
    }
    secret_key[0] = 1;
    return secret_key;
}

BIMatrix GSW::public_key_gen(const BIVector& sk) const {
    // recovering t from sk, defined as t = (-s_2,...,-s_n) in Z_q
    BIVector t(n);
    for (unsigned int i = 0; i < n; i++) {
        t[i] = quotient - sk[i+1];
    }

    // Uniformaly generated matrix (part of pk)
    BIMatrix B(m * n);
    for (unsigned int i = 0; i < m*n; i++)
        B[i] = RandomBnd(quotient);

    // First column of public key  b = B*t + e
    BIVector b(m);
    //BIVector e(m); // Error vector, optimized out
    BigInt temp;
    for (unsigned int i = 0; i < m; i++) {
        for (unsigned int j = 0; j < n; j++) {
            MulMod(temp, B[i*n+j], t[j], quotient);
            AddMod(b[i], b[i], temp, quotient);
            //b[i] += B[i*n + j] * t[j];
            //b[i] = b[i] % quotient;
        }
        int bit = gaussSampler->sample() % sigma6;
        //e[i] = bit;
        b[i] += bit;
    }

    // Observe that pk * sk = e
    BIMatrix pk(m * n_1);
    for (unsigned int i = 0; i < m; i++) {
        pk[i*n_1] = b[i];
    }
    for (unsigned int i = 0; i < m*n; i++) {
        pk[1+i+(i/n)] = B[i];
    }

    // this is satisfied, the check proves it
#define DEBUG
#ifdef DEBUG
    BIVector e_1(m);
    for (unsigned int i = 0; i < m; i++) {
        e_1[i] = 0;
        for (unsigned int j = 0; j < n_1; j++) {
            e_1[i] += pk[i*n_1 + j] * sk[j];
            e_1[i] = e_1[i] % quotient;
        }
        //assert(e_1[i] == e[i]);
    }
#endif

    return pk;
}

BitMatrix GSW::encrypt(const BIMatrix& public_key, const BigInt& message) const {
    bernoulli_distribution bernoulli(0.5);

    BitMatrix R(N * m);
    for (size_t i = 0; i < R.size(); i++) {
        R[i] = bernoulli(generator);
    }
    BIMatrix RA(N * n_1);
    BigInt temp;
# pragma omp parallel for shared (R, public_key, RA) schedule(guided)
    for (unsigned int i = 0; i < N; i++) {
        if (omp_get_thread_num() == 0)
            cerr << "Calc RA matrix " << i << " out of " << N << "\r";
        for (unsigned int j = 0; j < n_1; j++) {
            RA[i*n_1 + j] = 0;
            for (unsigned int k = 0; k < m; k++) {
                MulMod(temp, R[i*m + k], public_key[k*n_1 + j], quotient);
                AddMod(RA[i*n_1 + j], RA[i*n_1 + j], temp, quotient);
                //RA[i*n_1 + j] += R[i*m + k] * public_key[k*n_1 + j];
                //RA[i*n_1 + j] = RA[i*n_1 + j] % quotient;
            }
        }
    }
    cerr << endl;
    const BitMatrix RAbits = bit_decomp(RA);
    BIMatrix C(N * N);
# pragma omp parallel for shared (C, message) schedule(guided)
    for (unsigned int i = 0; i < N; i++) {
        if (omp_get_thread_num() == 0)
            cerr << "Calc ciphertext matrix " << i << " out of " << N << "\r";
        for (unsigned int j = 0; j < N; j++) {
            C[i*N + j] = RAbits[i*N + j];
            // message * identity
            if (i == j) {
                C[i*N + j] += message;
            }
            //C[i*N + j] = C[i*N + j] % quotient;
        }
    }

    cerr << endl << "Now to flatten" << endl;

    return flatten(C);
}

BigInt GSW::decrypt(const BIVector& sk, const BitMatrix& C) const {
    BigInt m, it, fract;
    const auto v = powers_of_2(sk);
    BIVector powered_m_bits(l-1);
    for (unsigned int i = 0; i < l-1; i++) {
        powered_m_bits[i] = 0;
        for (unsigned int j = 0; j < N; j++) {
            powered_m_bits[i] += C[i*(l-1) + j] * v[j];
            powered_m_bits[i] = powered_m_bits[i] % quotient;
        }
    }
    m = 0;
    for (int i = l-2; i >= 0; i--) {
        it = (powered_m_bits[i] - pow(2, i)*m);
        fract = it % quotient/2;
        bool bit = fract >= quotient/4;

        m += bit << (l-2 - i);
    }

    return m;
}

bool GSW::decrypt_bit(const BIVector& sk, const BitMatrix& C) const {
    unsigned int i;
    const auto v = powers_of_2(sk);
    BigInt q_4, q_2; q_4 = quotient/4; q_2 = quotient/2;

    for(i = 0; i < l; i++) {
        if(v[i] > q_4 && v[i] <= q_2) break;
    }

    BigInt xi, temp;
    xi = 0;
    for (unsigned int j = 0; j < N; j++) {
        MulMod(temp, C[i*N + j], v[j], quotient);
        AddMod(xi, xi, temp, quotient);
        //xi += (C[i*N + j] * v[j]);
        //xi = xi % quotient;
    }

    return xi >= v[i]/2;
}

BitMatrix GSW::nand(const BitMatrix& a, const BitMatrix& b) const {
    BIMatrix res(a.size());

    for (unsigned int i = 0; i < N; i++) {
        res[i*N + i] = 1;
    }

    BigInt temp;
# pragma omp parallel for shared (a, b, res) schedule(guided)
    for (unsigned int i = 0; i < N; i++) {
        if (omp_get_thread_num() == 0)
            cerr << "Performing a NAND, hold on " << i << " out of " << N << "\r";
        for (unsigned int j = 0; j < N; j++) {
            for (unsigned int k = 0; k < N; k++) {
                res[i*N + j] = res[i*N + j] + (a[i*N + k] & b[k*N + j]);
            }
            res[i*N + j] = res[i*N + j] % quotient;
        }
    }
    cerr << endl;

    return flatten(res);
}

//////////////////////////////////////////////
// Utility Functions
//////////////////////////////////////////////

BIVector GSW::powers_of_2(const BIVector& a) const {
    BIVector result(N);
# pragma omp parallel for shared (result, a) schedule(guided)
    for (unsigned int i = 0; i < n+1; i++) {
        for (unsigned int j = 0; j < l; j++) {
            MulMod(result[i*l + j], power2_ZZ(j), a[i], quotient);
            //result[i*l + j] = (BigInt) pow(2, j)*a[i] % quotient;
        }
    }

    return result;
}

BitVector GSW::bit_decomp(const BIVector& a) const {
    unsigned int num_rows = a.size() / n_1;
    BitVector result(n_1*l*num_rows);
    BigInt mask;
    mask = 1;
# pragma omp parallel for shared (result, a) schedule(guided)
    for (unsigned int j = 0; j < l; j++) {
        for (unsigned int k = 0; k < num_rows; k++) {
            for (unsigned int i = 0; i < n_1; i++) {
                result[k*l*n_1 + i*l + j] = (a[k*n_1 + i] & mask) > 0;
            }
        }
        mask <<= 1;
    }

    return result;
}

BIVector GSW::inverse_bit_decomp(const BitVector& a) const {
    BIVector result(n_1 * N);
    BigInt multiplier;
    multiplier = 1;

# pragma omp parallel for shared (result, a) schedule(guided)
    for (unsigned int j = 0; j < l; j++) {
        for (unsigned int row = 0; row < N; row++) {
            for (unsigned int i = 0; i < n_1; i++) {
                result[row * n_1 + i] += a[row*n_1*l + i*l + j] * multiplier;
            }
        }
        multiplier <<= 1;
    }

    return result;
}

BIVector GSW::inverse_bit_decomp(const BIVector& a) const {
    BIVector result(n_1 * N);
    BigInt multiplier;
    multiplier = 1;

# pragma omp parallel for shared (result, a) schedule(guided)
    for (unsigned int j = 0; j < l; j++) {
        for (unsigned int row = 0; row < N; row++) {
            for (unsigned int i = 0; i < n_1; i++) {
                result[row * n_1 + i] += a[row*n_1*l + i*l + j] * multiplier;
            }
        }
        multiplier <<= 1;
    }

    return result;
}

BitVector GSW::flatten(const BitVector& a) const {
    return bit_decomp(inverse_bit_decomp(a));
}

BitVector GSW::flatten(const BIVector& a) const {
    return bit_decomp(inverse_bit_decomp(a));
}


#ifndef IMAGE_REPROJECTION_PLUGINS_SPHERE_SURFACE_MODEL_HPP
#define IMAGE_REPROJECTION_PLUGINS_SPHERE_SURFACE_MODEL_HPP

#include <cmath>
#include <string>

#include <image_reprojection/surface_model.hpp>
#include <image_reprojection_plugins/SphereStamped.h>
#include <topic_tools/shape_shifter.h>

#include <boost/thread/locks.hpp>
#include <boost/thread/shared_mutex.hpp>

#include <opencv2/core/core.hpp>

namespace image_reprojection_plugins {

class SphereSurfaceModel : public image_reprojection::SurfaceModel {
public:
  SphereSurfaceModel() {}

  virtual ~SphereSurfaceModel() {}

  virtual void update(const topic_tools::ShapeShifter &surface) {
    const SphereStampedConstPtr sphere(surface.instantiate< SphereStamped >());
    CV_Assert(sphere);
    update(*sphere);
  }

  void update(const SphereStamped &sphere) {
    CV_Assert(sphere.radius > 0.);

    boost::unique_lock< boost::shared_mutex > write_lock(mutex_);

    frame_id_ = sphere.header.frame_id;
    center_ = cv::Vec3f(sphere.center.x, sphere.center.y, sphere.center.z);
    radius_ = sphere.radius;
  }

  virtual std::string getFrameId() const {
    boost::shared_lock< boost::shared_mutex > read_lock(mutex_);
    return frame_id_;
  }

private:
  virtual void onInit() {}

  virtual void onIntersection(const cv::Vec3f &src_origin, const cv::Mat &src_direction,
                              cv::Mat &dst, cv::Mat &mask) const {
    boost::shared_lock< boost::shared_mutex > read_lock(mutex_);
    multiraySphereIntersection(src_origin, src_direction, dst, mask);
  }

  void multiraySphereIntersection(const cv::Vec3f &src_origin, const cv::Mat &src_direction,
                                  cv::Mat &dst, cv::Mat &mask) const {
    dst.create(src_direction.size(), CV_32FC3);
    for (int x = 0; x < src_direction.size().width; ++x) {
      for (int y = 0; y < src_direction.size().height; ++y) {
        unsigned char &m(mask.at< unsigned char >(y, x));
        const cv::Vec3f &sd(src_direction.at< cv::Vec3f >(y, x));
        cv::Vec3f &d(dst.at< cv::Vec3f >(y, x));
        m = (m != 0 && raySphereIntersection(src_origin, sd, d)) ? 1 : 0;
      }
    }
  }

  bool raySphereIntersection(const cv::Vec3f &src_origin, const cv::Vec3f &src_direction,
                             cv::Vec3f &dst) const {
    // intersection point (x) can be described as
    //   x = p + t * d  (t >= 0)
    //   |x - c| = r
    // where p: ray origin, d: ray direction, c: center of sphere, r: radius of sphere
    //   |d|^2 * t^2 + 2 * dot(d, p - c) * t + |p - c|^2 - r^2 = 0

    // position of ray origin with respect to center of sphere
    const cv::Vec3f o(src_origin - center_);

    // coefficients
    const double a(src_direction.dot(src_direction));
    const double b(src_direction.dot(o));
    const double c(o.dot(o) - radius_ * radius_);
    const double D(b * b - a * c); // discriminant
    if (/* no intersection */ D < 0.) {
      return false;
    }

    // intersection point
    const double sD(std::sqrt(D));
    const double t0((-b + sD) / a), t1((-b - sD) / a);
    if (t0 >= 0. && t1 >= 0.) {
      dst = src_origin + std::min(t0, t1) * src_direction;
      return true;
    } else if (t0 >= 0. && t1 < 0.) {
      dst = src_origin + t0 * src_direction;
      return true;
    } else if (t0 < 0. && t1 >= 0.) {
      dst = src_origin + t1 * src_direction;
      return true;
    }

    return false;
  }

private:
  mutable boost::shared_mutex mutex_;
  std::string frame_id_;
  cv::Vec3f center_;
  double radius_;
};

} // namespace image_reprojection_plugins

#endif


// Filename: matrix_free_pcg.cpp (part of MTL4)

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/itl/itl.hpp>

struct poisson2D_diagonal_pc
{
    template <typename VectorIn, typename VectorOut>
    void solve(const VectorIn& x, VectorOut& y) const
    {
	for (std::size_t i= 0; i < size(x); i++)
	    y[i]= x[i] * 0.25;
    }

    template <typename VectorIn, typename VectorOut>
    void adjoint_solve(const VectorIn& x, VectorOut& y) const
    {
	for (std::size_t i= 0; i < size(x); i++)
	    y[i]= x[i] * 0.25;
    }
};

template <typename Vector>
itl::pc::solver<poisson2D_diagonal_pc, Vector, false>
inline solve(const poisson2D_diagonal_pc& P, const Vector& x)
{
    return itl::pc::solver<poisson2D_diagonal_pc, Vector, false>(P, x);
}

template <typename Vector>
itl::pc::solver<poisson2D_diagonal_pc, Vector, true>
inline adjoint_solve(const poisson2D_diagonal_pc& P, const Vector& x)
{
    return itl::pc::solver<poisson2D_diagonal_pc, Vector, true>(P, x);
}

int main(int, char**)
{
    // For a more realistic example set size to 1000 or larger
    const int size = 10, N = size * size;

    typedef mtl::mat::poisson2D_dirichlet  matrix_type;
    matrix_type                               A(size, size);
    poisson2D_diagonal_pc                     P;

    mtl::dense_vector<double>                 x(N, 1.0), b(N);

    b = A * x;
    x= 0;
    itl::cyclic_iteration<double>             iter(b, 100, 1.e-11, 0.0, 5);
    cg(A, x, b, P, P, iter);

    return 0;
}


#include <engine/Eigenmodes.hpp>
#include <engine/Vectormath.hpp>
#include <engine/Manifoldmath.hpp>

#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <SymEigsSolver.h>  // Also includes <MatOp/DenseSymMatProd.h>

#include <fmt/format.h>
#include <fmt/ostream.h>

namespace Engine
{
    namespace Eigenmodes
    {
        using Utility::Log_Level;
        using Utility::Log_Sender;

        void Check_Eigenmode_Parameters(std::shared_ptr<Data::Spin_System> system)
        {
            int nos = system->nos;
            auto& n_modes = system->ema_parameters->n_modes;
            if (n_modes > 2*nos-2)
            {
                n_modes = 2*nos-2;
                system->modes.resize(n_modes);
                system->eigenvalues.resize(n_modes);

                Log(Log_Level::Warning, Log_Sender::EMA, fmt::format("Number of eigenmodes declared in "
                    "EMA Parameters is too large. The number is set to {}", n_modes));
            }
            if (n_modes != system->modes.size())
                system->modes.resize(n_modes);

            // Initial check of selected_mode
            auto& n_mode_follow = system->ema_parameters->n_mode_follow;
            if (n_mode_follow > n_modes-1)
            {
                Log(Log_Level::Warning, Log_Sender::EMA, fmt::format("Eigenmode number {} is not "
                    "available. The largest eigenmode ({}) is used instead", n_mode_follow, n_modes-1));
                n_mode_follow = n_modes-1;
            }
        }

        void Calculate_Eigenmodes(std::shared_ptr<Data::Spin_System> system, int idx_img, int idx_chain)
        {
            int nos = system->nos;

            Check_Eigenmode_Parameters(system);

            auto& n_modes = system->ema_parameters->n_modes;

            // vectorfield mode(nos, Vector3{1, 0, 0});
            vectorfield spins_initial = *system->spins;

            Log( Log_Level::Info, Log_Sender::EMA, fmt::format("Started calculation of {} Eigenmodes ", n_modes ),
                idx_img, idx_chain );

            // Calculate the Eigenmodes
            vectorfield gradient(nos);
            MatrixX hessian(3*nos, 3*nos);

            // The gradient (unprojected)
            system->hamiltonian->Gradient(spins_initial, gradient);
            Vectormath::set_c_a(1, gradient, gradient, system->geometry->mask_unpinned);

            // The Hessian (unprojected)
            system->hamiltonian->Hessian(spins_initial, hessian);

            // Get the eigenspectrum
            MatrixX hessian_constrained = MatrixX::Zero(2*nos, 2*nos);
            MatrixX tangent_basis = MatrixX::Zero(3*nos, 2*nos);
            VectorX eigenvalues;
            MatrixX eigenvectors;
            bool successful = Eigenmodes::Hessian_Partial_Spectrum(system->ema_parameters, spins_initial, gradient, hessian,
                n_modes, tangent_basis, hessian_constrained, eigenvalues, eigenvectors);

            if (successful)
            {
                // get every mode and save it to system->modes
                for (int i=0; i<n_modes; i++)
                {
                    // Extract the minimum mode (transform evec_lowest_2N back to 3N)
                    VectorX evec_3N = tangent_basis * eigenvectors.col(i);

                    // dynamically allocate the system->modes
                    system->modes[i] = std::shared_ptr<vectorfield>(new vectorfield(nos, Vector3{1,0,0}));

                    // Set the modes
                    for (int j=0; j<nos; j++)
                        (*system->modes[i])[j] = {evec_3N[3*j], evec_3N[3*j+1], evec_3N[3*j+2]};

                    // get the eigenvalues
                    system->eigenvalues[i] = eigenvalues(i);
                }

                Log( Log_Level::Info, Log_Sender::All, fmt::format("Finished calculation of {} Eigenmodes ", n_modes ),
                    idx_img, idx_chain );

                int ev_print = std::min(n_modes, 100);
                Log( Log_Level::Info, Log_Sender::EMA, fmt::format("Eigenvalues: {}",
                    eigenvalues.head( ev_print ).transpose() ), idx_img, idx_chain );
            }
            else
            {
                //// TODO: What to do then?
                Log( Log_Level::Warning, Log_Sender::All, "Something went wrong in eigenmode calculation...",
                    idx_img, idx_chain );
            }
        }

        bool Hessian_Full_Spectrum(const std::shared_ptr<Data::Parameters_Method> parameters,
            const vectorfield & spins, const vectorfield & gradient, const MatrixX & hessian,
            MatrixX & tangent_basis, MatrixX & hessian_constrained, VectorX & eigenvalues, MatrixX & eigenvectors)
        {
            int nos = spins.size();

            // Calculate the final Hessian to use for the minimum mode
            // TODO: add option to choose different Hessian calculation
            hessian_constrained = MatrixX::Zero(2*nos, 2*nos);
            tangent_basis       = MatrixX::Zero(3*nos, 2*nos);
            Manifoldmath::hessian_bordered(spins, gradient, hessian, tangent_basis, hessian_constrained);
            // Manifoldmath::hessian_projected(spins, gradient, hessian, tangent_basis, hessian_constrained);
            // Manifoldmath::hessian_weingarten(spins, gradient, hessian, tangent_basis, hessian_constrained);
            // Manifoldmath::hessian_spherical(spins, gradient, hessian, tangent_basis, hessian_constrained);
            // Manifoldmath::hessian_covariant(spins, gradient, hessian, tangent_basis, hessian_constrained);

            // Create and initialize a Eigen solver. Note: the hessian matrix should be symmetric!
            Eigen::SelfAdjointEigenSolver<MatrixX> hessian_spectrum(hessian_constrained);

            // Extract real eigenvalues
            eigenvalues = hessian_spectrum.eigenvalues().real();
            // Retrieve the real eigenvectors
            eigenvectors = hessian_spectrum.eigenvectors().real();

            // Return whether the calculation was successful
            return true;
        }

        bool Hessian_Partial_Spectrum(const std::shared_ptr<Data::Parameters_Method> parameters,
            const vectorfield & spins, const vectorfield & gradient, const MatrixX & hessian, int n_modes,
            MatrixX & tangent_basis, MatrixX & hessian_constrained, VectorX & eigenvalues, MatrixX & eigenvectors)
        {
            int nos = spins.size();

            // Restrict number of calculated modes to [1,2N)
            n_modes = std::max(1, std::min(2*nos-2, n_modes));

            // If we have only one spin, we can only calculate the full spectrum
            if (n_modes == nos)
                return Hessian_Full_Spectrum(parameters, spins, gradient, hessian, tangent_basis, hessian_constrained, eigenvalues, eigenvectors);

            // Calculate the final Hessian to use for the minimum mode
            // TODO: add option to choose different Hessian calculation
            hessian_constrained = MatrixX::Zero(2*nos, 2*nos);
            tangent_basis       = MatrixX::Zero(3*nos, 2*nos);
            Manifoldmath::hessian_bordered(spins, gradient, hessian, tangent_basis, hessian_constrained);
            // Manifoldmath::hessian_projected(spins, gradient, hessian, tangent_basis, hessian_constrained);
            // Manifoldmath::hessian_weingarten(spins, gradient, hessian, tangent_basis, hessian_constrained);
            // Manifoldmath::hessian_spherical(spins, gradient, hessian, tangent_basis, hessian_constrained);
            // Manifoldmath::hessian_covariant(spins, gradient, hessian, tangent_basis, hessian_constrained);

            // Remove degrees of freedom of pinned spins
            #ifdef SPIRIT_ENABLE_PINNING
                for (int i=0; i<nos; ++i)
                {
                    // TODO: pinning is now in Data::Geometry
                    // if (!parameters->pinning->mask_unpinned[i])
                    // {
                    //     // Remove interaction block
                    //     for (int j=0; j<nos; ++j)
                    //     {
                    //         hessian_constrained.block<2,2>(2*i,2*j).setZero();
                    //         hessian_constrained.block<2,2>(2*j,2*i).setZero();
                    //     }
                    //     // Set diagonal matrix entries of pinned spins to a large value
                    //     hessian_constrained.block<2,2>(2*i,2*i).setZero();
                    //     hessian_constrained.block<2,2>(2*i,2*i).diagonal().setConstant(nos*1e5);
                    // }
                }
            #endif // SPIRIT_ENABLE_PINNING

            // Create the Spectra Matrix product operation
            Spectra::DenseSymMatProd<scalar> op(hessian_constrained);
            // Create and initialize a Spectra solver
            Spectra::SymEigsSolver< scalar, Spectra::SMALLEST_ALGE, Spectra::DenseSymMatProd<scalar> > hessian_spectrum(&op, n_modes, 2*nos);
            hessian_spectrum.init();

            // Compute the specified spectrum, sorted by smallest real eigenvalue
            int nconv = hessian_spectrum.compute(1000, 1e-10, int(Spectra::SMALLEST_ALGE));

            // Extract real eigenvalues
            eigenvalues = hessian_spectrum.eigenvalues().real();

            // Retrieve the real eigenvectors
            eigenvectors = hessian_spectrum.eigenvectors().real();

            // Return whether the calculation was successful
            return (hessian_spectrum.info() == Spectra::SUCCESSFUL) && (nconv > 0);
        }
    }
}