// ====================================================================
// This file is part of FlexibleSUSY.
//
// FlexibleSUSY is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published
// by the Free Software Foundation, either version 3 of the License,
// or (at your option) any later version.
//
// FlexibleSUSY is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with FlexibleSUSY.  If not, see
// <http://www.gnu.org/licenses/>.
// ====================================================================

#ifndef linalg2_hpp
#define linalg2_hpp

#include <cstdint>
#include <limits>
#include <cctype>
#include <cmath>
#include <complex>
#include <algorithm>
#include <Eigen/Core>
#include <Eigen/SVD>
#include <Eigen/Eigenvalues>
#include "config.h"

namespace flexiblesusy {

#define MAX_(i, j) (((i) > (j)) ? (i) : (j))
#define MIN_(i, j) (((i) < (j)) ? (i) : (j))

template<class Real, class Scalar, int M, int N>
void svd_eigen
(const Eigen::Matrix<Scalar, M, N>& m,
 Eigen::Array<Real, MIN_(M, N), 1>& s,
 Eigen::Matrix<Scalar, M, M> *u,
 Eigen::Matrix<Scalar, N, N> *vh)
{
    Eigen::JacobiSVD<Eigen::Matrix<Scalar, M, N> >
	svd(m, (u ? Eigen::ComputeFullU : 0) | (vh ? Eigen::ComputeFullV : 0));
    s = svd.singularValues();
    if (u)  *u  = svd.matrixU();
    if (vh) *vh = svd.matrixV().adjoint();
}

template<class Real, class Scalar, int N>
void hermitian_eigen
(const Eigen::Matrix<Scalar, N, N>& m,
 Eigen::Array<Real, N, 1>& w,
 Eigen::Matrix<Scalar, N, N> *z)
{
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix<Scalar,N,N> >
	es(m, z ? Eigen::ComputeEigenvectors : Eigen::EigenvaluesOnly);
    w = es.eigenvalues();
    if (z) *z = es.eigenvectors();
}

#ifdef ENABLE_LAPACK

#   ifdef ENABLE_ILP64MKL_WORKAROUND
using lapack_int = int64_t;
#   else
using lapack_int = int;
#   endif

extern "C" void zgesvd_
(const char& JOBU, const char& JOBVT, const lapack_int& M, const lapack_int& N,
 std::complex<double> *A, const lapack_int& LDA, double *S,
 std::complex<double> *U, const lapack_int& LDU,
 std::complex<double> *VT, const lapack_int& LDVT,
 std::complex<double> *WORK, const lapack_int& LWORK, double *RWORK,
 lapack_int& INFO);

extern "C" void dgesvd_
(const char& JOBU, const char& JOBVT, const lapack_int& M, const lapack_int& N,
 double *A, const lapack_int& LDA, double *S,
 double *U, const lapack_int& LDU,
 double *VT, const lapack_int& LDVT,
 double *WORK, const lapack_int& LWORK,
 lapack_int& INFO);

extern "C" void zheev_
(const char& JOBZ, const char& UPLO, const lapack_int& N,
 std::complex<double> *A, const lapack_int& LDA, double *W,
 std::complex<double> *WORK, const lapack_int& LWORK, double *RWORK,
 lapack_int& INFO);

extern "C" void dsyev_
(const char& JOBZ, const char& UPLO, const lapack_int& N,
 double *A, const lapack_int& LDA, double *W,
 double *WORK, const lapack_int& LWORK,
 lapack_int& INFO);

#define def_svd_lapack(t, f, ...)					\
template<int M, int N>							\
void svd_lapack								\
(const Eigen::Matrix<t, M, N>& m,					\
 Eigen::Array<double, MIN_(M, N), 1>& s,				\
 Eigen::Matrix<t, M, M> *u  = 0,					\
 Eigen::Matrix<t, N, N> *vh = 0)					\
{									\
    const     char JOBU  = u  ? 'A' : 'N';				\
    const     char JOBVT = vh ? 'A' : 'N';				\
    Eigen::Matrix<t, M, N> A = m;					\
    const     lapack_int LDA   = M;					\
              t   *U    = u ? u->data() : 0;				\
    const     lapack_int LDU   = M;					\
              t   *VT   = vh ? vh->data() : 0;				\
    const     lapack_int LDVT  = N;					\
    const     lapack_int LWORK = get_lwork(__VA_ARGS__,);		\
    Eigen::Array<t, LWORK, 1> WORK;					\
    decl_rwork(__VA_ARGS__);						\
    lapack_int INFO;							\
    f(JOBU, JOBVT, M, N, A.data(), LDA, s.data(), U, LDU, VT, LDVT,	\
      WORK.data(), LWORK, put_rwork(__VA_ARGS__) INFO);			\
}

#define def_hermitian_lapack(s, f, ...)					\
template<int N>								\
void hermitian_lapack							\
(const Eigen::Matrix<s, N, N>& m,					\
 Eigen::Array<double, N, 1>& w,						\
 Eigen::Matrix<s, N, N> *z = 0)						\
{									\
    const     char JOBZ = z ? 'V' : 'N';				\
    const     char UPLO = 'L';						\
    Eigen::Matrix<s, N, N> A = m;					\
    const     lapack_int LDA   = N;					\
    const     lapack_int LWORK = get_lwork(__VA_ARGS__,);		\
    Eigen::Array<s, LWORK, 1> WORK;					\
    decl_rwork(__VA_ARGS__);						\
    lapack_int INFO;							\
    f(JOBZ, UPLO, N, A.data(), LDA, w.data(), WORK.data(), LWORK,	\
      put_rwork(__VA_ARGS__) INFO);					\
    if (z) *z = A;							\
}

#define get_lwork(lwork, ...) (lwork)

#define get_rwork_macro(_1, _2, name, ...) name

#define nop_(_1)

#define do_decl_rwork(_1, lrwork) Eigen::Array<double, (lrwork), 1> RWORK

#define decl_rwork(...) \
    get_rwork_macro(__VA_ARGS__, do_decl_rwork, nop_,)(__VA_ARGS__)

#define do_put_rwork(_1, _2) RWORK.data(),

#define put_rwork(...) \
    get_rwork_macro(__VA_ARGS__, do_put_rwork, nop_,)(__VA_ARGS__)

def_svd_lapack(std::complex<double>, zgesvd_, 3*MAX_(M,N), 5*MIN_(M,N))
def_svd_lapack(double, dgesvd_, MAX_(3*MIN_(M,N)+MAX_(M,N),5*MIN_(M,N)))

def_hermitian_lapack(std::complex<double>, zheev_, 2*N-1, 3*N-2)
def_hermitian_lapack(double, dsyev_, 3*N-1)

#endif // ENABLE_LAPACK

/**
 * Template version of DDISNA from LAPACK.
 */
template<int M, int N, class Real>
void disna(const char& JOB, const Eigen::Array<Real, MIN_(M, N), 1>& D,
	   Eigen::Array<Real, MIN_(M, N), 1>& SEP, int& INFO)
{
//  -- LAPACK computational routine (version 3.4.0) --
//  -- LAPACK is a software package provided by Univ. of Tennessee,    --
//  -- Univ. of California Berkeley, Univ. of Colorado Denver and NAG Ltd..--
//     November 2011
//
//  =====================================================================

//     .. Parameters ..
      const Real         ZERO = 0;
//     .. Local Scalars ..
      bool               DECR, EIGEN, INCR, LEFT, RIGHT, SINGUL;
      int                I, K;
      Real               ANORM, EPS, NEWGAP, OLDGAP, SAFMIN, THRESH;
//
//     Test the input arguments
//
      INFO = 0;
      EIGEN = std::toupper(JOB) == 'E';
      LEFT  = std::toupper(JOB) == 'L';
      RIGHT = std::toupper(JOB) == 'R';
      SINGUL = LEFT || RIGHT;
      if (EIGEN)
	 K = M;
      else if (SINGUL)
         K = MIN_(M, N);
      if (!EIGEN && !SINGUL)
         INFO = -1;
      else if (M < 0)
         INFO = -2;
      else if (K < 0)
         INFO = -3;
      else {
         INCR = true;
         DECR = true;
         for (I = 0; I < K - 1; I++) {
            if (INCR)
               INCR = INCR && D(I) <= D(I+1);
            if (DECR)
	       DECR = DECR && D(I) >= D(I+1);
	 }
         if (SINGUL && K > 0) {
            if (INCR)
               INCR = INCR && ZERO <= D(0);
            if (DECR)
               DECR = DECR && D(K-1) >= ZERO;
         }
         if (!(INCR || DECR))
            INFO = -4;
      }
      if (INFO != 0) {
         // CALL XERBLA( 'DDISNA', -INFO )
         return;
      }
//
//     Quick return if possible
//
      if (K == 0)
         return;
//
//     Compute reciprocal condition numbers
//
      if (K == 1)
         SEP(0) = std::numeric_limits<Real>::max();
      else {
         OLDGAP = std::fabs(D(1) - D(0));
         SEP(0) = OLDGAP;
         for (I = 1; I < K - 1; I++) {
            NEWGAP = std::fabs(D(I+1) - D(I));
            SEP(I) = std::min(OLDGAP, NEWGAP);
            OLDGAP = NEWGAP;
	 }
         SEP(K-1) = OLDGAP;
      }
      if (SINGUL)
         if ((LEFT && M > N) || (RIGHT && M < N)) {
            if (INCR)
               SEP( 0 ) = std::min(SEP( 0 ), D( 0 ));
            if (DECR)
               SEP(K-1) = std::min(SEP(K-1), D(K-1));
         }
//
//     Ensure that reciprocal condition numbers are not less than
//     threshold, in order to limit the size of the error bound
//
      // Note std::numeric_limits<double>::epsilon() == 2 * DLAMCH('E')
      // since  the former is the smallest eps such that 1.0 + eps > 1.0
      // while DLAMCH('E') is the smallest eps such that 1.0 - eps < 1.0
      EPS = std::numeric_limits<Real>::epsilon();
      SAFMIN = std::numeric_limits<Real>::min();
      ANORM = std::max(std::fabs(D(0)), std::fabs(D(K-1)));
      if (ANORM == ZERO)
         THRESH = EPS;
      else
         THRESH = std::max(EPS*ANORM, SAFMIN);
      for (I = 0; I < K; I++)
	 SEP(I) = std::max(SEP(I), THRESH);
}

template<class Real, class Scalar, int M, int N>
void svd_internal
(const Eigen::Matrix<Scalar, M, N>& m,
 Eigen::Array<Real, MIN_(M, N), 1>& s,
 Eigen::Matrix<Scalar, M, M> *u,
 Eigen::Matrix<Scalar, N, N> *vh)
{
    svd_eigen(m, s, u, vh);
}

#ifdef ENABLE_LAPACK

// ZGESVD of ATLAS seems to be faster than Eigen::JacobiSVD for M, N >= 4

template<class Scalar, int M, int N>
void svd_internal
(const Eigen::Matrix<Scalar, M, N>& m,
 Eigen::Array<double, MIN_(M, N), 1>& s,
 Eigen::Matrix<Scalar, M, M> *u,
 Eigen::Matrix<Scalar, N, N> *vh)
{
    svd_lapack(m, s, u, vh);
}

template<class Scalar>
void svd_internal
(const Eigen::Matrix<Scalar, 3, 3>& m,
 Eigen::Array<double, 3, 1>& s,
 Eigen::Matrix<Scalar, 3, 3> *u,
 Eigen::Matrix<Scalar, 3, 3> *vh)
{
    svd_eigen(m, s, u, vh);
}

template<class Scalar>
void svd_internal
(const Eigen::Matrix<Scalar, 2, 2>& m,
 Eigen::Array<double, 2, 1>& s,
 Eigen::Matrix<Scalar, 2, 2> *u,
 Eigen::Matrix<Scalar, 2, 2> *vh)
{
    svd_eigen(m, s, u, vh);
}

template<class Scalar>
void svd_internal
(const Eigen::Matrix<Scalar, 1, 1>& m,
 Eigen::Array<double, 1, 1>& s,
 Eigen::Matrix<Scalar, 1, 1> *u,
 Eigen::Matrix<Scalar, 1, 1> *vh)
{
    svd_eigen(m, s, u, vh);
}

#endif // ENABLE_LAPACK

template<class Real, class Scalar, int M, int N>
void svd_errbd
(const Eigen::Matrix<Scalar, M, N>& m,
 Eigen::Array<Real, MIN_(M, N), 1>& s,
 Eigen::Matrix<Scalar, M, M> *u  = 0,
 Eigen::Matrix<Scalar, N, N> *vh = 0,
 Real *s_errbd = 0,
 Eigen::Array<Real, MIN_(M, N), 1> *u_errbd = 0,
 Eigen::Array<Real, MIN_(M, N), 1> *v_errbd = 0)
{
    svd_internal(m, s, u, vh);

    // see http://www.netlib.org/lapack/lug/node96.html
    if (!s_errbd) return;
    const Real EPSMCH = std::numeric_limits<Real>::epsilon();
    *s_errbd = EPSMCH * s[0];

    Eigen::Array<Real, MIN_(M, N), 1> RCOND;
    int INFO;
    if (u_errbd) {
	disna<M, N>('L', s, RCOND, INFO);
	u_errbd->fill(*s_errbd);
	*u_errbd /= RCOND;
    }
    if (v_errbd) {
	disna<M, N>('R', s, RCOND, INFO);
	v_errbd->fill(*s_errbd);
	*v_errbd /= RCOND;
    }
}

/**
 * Singular value decomposition of M-by-N matrix m such that
 *
 *     sigma.setZero(); sigma.diagonal() = s;
 *     m == u * sigma * vh    // LAPACK convention
 *
 * and `(s >= 0).all()`.  Elements of s are in descending order.  The
 * above decomposition can be put in the form
 *
 *     m == u * s.matrix().asDiagonal() * vh
 *
 * if `M == N`.
 *
 * @tparam     Real   type of real and imaginary parts of Scalar
 * @tparam     Scalar type of elements of m, u, and vh
 * @tparam     M      number of rows in m
 * @tparam     N      number of columns in m
 * @param[in]  m      M-by-N matrix to be decomposed
 * @param[out] s      array of length min(M,N) to contain singular values
 * @param[out] u      M-by-M unitary matrix
 * @param[out] vh     N-by-N unitary matrix
 */
template<class Real, class Scalar, int M, int N>
void svd
(const Eigen::Matrix<Scalar, M, N>& m,
 Eigen::Array<Real, MIN_(M, N), 1>& s,
 Eigen::Matrix<Scalar, M, M>& u,
 Eigen::Matrix<Scalar, N, N>& vh)
{
    svd_errbd(m, s, &u, &vh);
}

/**
 * Same as svd(m, s, u, vh) except that an approximate error bound for
 * the singular values is returned as well.  The error bound is
 * estimated following the method presented at
 * http://www.netlib.org/lapack/lug/node96.html.
 *
 * @param[out] s_errbd approximate error bound for the elements of s
 *
 * See the documentation of svd(m, s, u, vh) for the other parameters.
 */
template<class Real, class Scalar, int M, int N>
void svd
(const Eigen::Matrix<Scalar, M, N>& m,
 Eigen::Array<Real, MIN_(M, N), 1>& s,
 Eigen::Matrix<Scalar, M, M>& u,
 Eigen::Matrix<Scalar, N, N>& vh,
 Real& s_errbd)
{
    svd_errbd(m, s, &u, &vh, &s_errbd);
}

/**
 * Same as svd(m, s, u, vh, s_errbd) except that approximate error
 * bounds for the singular vectors are returned as well.  The error
 * bounds are estimated following the method presented at
 * http://www.netlib.org/lapack/lug/node96.html.
 *
 * @param[out] u_errbd array of approximate error bounds for u
 * @param[out] v_errbd array of approximate error bounds for vh
 *
 * See the documentation of svd(m, s, u, vh, s_errbd) for the other
 * parameters.
 */
template<class Real, class Scalar, int M, int N>
void svd
(const Eigen::Matrix<Scalar, M, N>& m,
 Eigen::Array<Real, MIN_(M, N), 1>& s,
 Eigen::Matrix<Scalar, M, M>& u,
 Eigen::Matrix<Scalar, N, N>& vh,
 Real& s_errbd,
 Eigen::Array<Real, MIN_(M, N), 1>& u_errbd,
 Eigen::Array<Real, MIN_(M, N), 1>& v_errbd)
{
    svd_errbd(m, s, &u, &vh, &s_errbd, &u_errbd, &v_errbd);
}

/**
 * Returns singular values of M-by-N matrix m via s such that
 * `(s >= 0).all()`.  Elements of s are in descending order.
 *
 * @tparam     Real   type of real and imaginary parts of Scalar
 * @tparam     Scalar type of elements of m, u, and vh
 * @tparam     M      number of rows in m
 * @tparam     N      number of columns in m
 * @param[in]  m      M-by-N matrix to be decomposed
 * @param[out] s      array of length min(M,N) to contain singular values
 */
template<class Real, class Scalar, int M, int N>
void svd
(const Eigen::Matrix<Scalar, M, N>& m,
 Eigen::Array<Real, MIN_(M, N), 1>& s)
{
    svd_errbd(m, s);
}

/**
 * Same as svd(m, s) except that an approximate error bound for the
 * singular values is returned as well.  The error bound is estimated
 * following the method presented at
 * http://www.netlib.org/lapack/lug/node96.html.
 *
 * @param[out] s_errbd approximate error bound for the elements of s
 *
 * See the documentation of svd(m, s) for the other parameters.
 */
template<class Real, class Scalar, int M, int N>
void svd
(const Eigen::Matrix<Scalar, M, N>& m,
 Eigen::Array<Real, MIN_(M, N), 1>& s,
 Real& s_errbd)
{
    svd_errbd(m, s, 0, 0, &s_errbd);
}

// Eigen::SelfAdjointEigenSolver seems to be faster than ZHEEV of ATLAS

template<class Real, class Scalar, int N>
void diagonalize_hermitian_internal
(const Eigen::Matrix<Scalar, N, N>& m,
 Eigen::Array<Real, N, 1>& w,
 Eigen::Matrix<Scalar, N, N> *z)
{
    hermitian_eigen(m, w, z);
}

template<class Real, class Scalar, int N>
void diagonalize_hermitian_errbd
(const Eigen::Matrix<Scalar, N, N>& m,
 Eigen::Array<Real, N, 1>& w,
 Eigen::Matrix<Scalar, N, N> *z = 0,
 Real *w_errbd = 0,
 Eigen::Array<Real, N, 1> *z_errbd = 0)
{
    diagonalize_hermitian_internal(m, w, z);

    // see http://www.netlib.org/lapack/lug/node89.html
    if (!w_errbd) return;
    const Real EPSMCH = std::numeric_limits<Real>::epsilon();
    Real mnorm = std::max(std::abs(w[0]), std::abs(w[N-1]));
    *w_errbd = EPSMCH * mnorm;

    if (!z_errbd) return;
    Eigen::Array<Real, N, 1> RCONDZ;
    int INFO;
    disna<N, N>('E', w, RCONDZ, INFO);
    z_errbd->fill(*w_errbd);
    *z_errbd /= RCONDZ;
}

/**
 * Diagonalizes N-by-N hermitian matrix m so that
 *
 *     m == z * w.matrix().asDiagonal() * z.adjoint()
 *
 * Elements of w are in ascending order.
 *
 * @tparam     Real   type of real and imaginary parts of Scalar
 * @tparam     Scalar type of elements of m and z
 * @tparam     N      number of rows and columns in m and z
 * @param[in]  m      N-by-N matrix to be diagonalized
 * @param[out] w      array of length N to contain eigenvalues
 * @param[out] z      N-by-N unitary matrix
 */
template<class Real, class Scalar, int N>
void diagonalize_hermitian
(const Eigen::Matrix<Scalar, N, N>& m,
 Eigen::Array<Real, N, 1>& w,
 Eigen::Matrix<Scalar, N, N>& z)
{
    diagonalize_hermitian_errbd(m, w, &z);
}

/**
 * Same as diagonalize_hermitian(m, w, z) except that an approximate
 * error bound for the eigenvalues is returned as well.  The error
 * bound is estimated following the method presented at
 * http://www.netlib.org/lapack/lug/node89.html.
 *
 * @param[out] w_errbd approximate error bound for the elements of w
 *
 * See the documentation of diagonalize_hermitian(m, w, z) for the
 * other parameters.
 */
template<class Real, class Scalar, int N>
void diagonalize_hermitian
(const Eigen::Matrix<Scalar, N, N>& m,
 Eigen::Array<Real, N, 1>& w,
 Eigen::Matrix<Scalar, N, N>& z,
 Real& w_errbd)
{
    diagonalize_hermitian_errbd(m, w, &z, &w_errbd);
}

/**
 * Same as diagonalize_hermitian(m, w, z, w_errbd) except that
 * approximate error bounds for the eigenvectors are returned as well.
 * The error bounds are estimated following the method presented at
 * http://www.netlib.org/lapack/lug/node89.html.
 *
 * @param[out] z_errbd array of approximate error bounds for z
 *
 * See the documentation of diagonalize_hermitian(m, w, z, w_errbd)
 * for the other parameters.
 */
template<class Real, class Scalar, int N>
void diagonalize_hermitian
(const Eigen::Matrix<Scalar, N, N>& m,
 Eigen::Array<Real, N, 1>& w,
 Eigen::Matrix<Scalar, N, N>& z,
 Real& w_errbd,
 Eigen::Array<Real, N, 1>& z_errbd)
{
    diagonalize_hermitian_errbd(m, w, &z, &w_errbd, &z_errbd);
}

/**
 * Returns eigenvalues of N-by-N hermitian matrix m via w.
 * Elements of w are in ascending order.
 *
 * @tparam     Real   type of real and imaginary parts of Scalar
 * @tparam     Scalar type of elements of m and z
 * @tparam     N      number of rows and columns in m and z
 * @param[in]  m      N-by-N matrix to be diagonalized
 * @param[out] w      array of length N to contain eigenvalues
 */
template<class Real, class Scalar, int N>
void diagonalize_hermitian
(const Eigen::Matrix<Scalar, N, N>& m,
 Eigen::Array<Real, N, 1>& w)
{
    diagonalize_hermitian_errbd(m, w);
}

/**
 * Same as diagonalize_hermitian(m, w) except that an approximate
 * error bound for the eigenvalues is returned as well.  The error
 * bound is estimated following the method presented at
 * http://www.netlib.org/lapack/lug/node89.html.
 *
 * @param[out] w_errbd approximate error bound for the elements of w
 *
 * See the documentation of diagonalize_hermitian(m, w) for the other
 * parameters.
 */
template<class Real, class Scalar, int N>
void diagonalize_hermitian
(const Eigen::Matrix<Scalar, N, N>& m,
 Eigen::Array<Real, N, 1>& w,
 Real& w_errbd)
{
    diagonalize_hermitian_errbd(m, w, 0, &w_errbd);
}

template<class Real>
struct RephaseOp {
    std::complex<Real> operator() (const std::complex<Real>& z) const
	{ return std::polar(Real(1), std::arg(z)/2); }
};

template<class Real, int N>
void diagonalize_symmetric_errbd
(const Eigen::Matrix<std::complex<Real>, N, N>& m,
 Eigen::Array<Real, N, 1>& s,
 Eigen::Matrix<std::complex<Real>, N, N> *u = 0,
 Real *s_errbd = 0,
 Eigen::Array<Real, N, 1> *u_errbd = 0)
{
    svd_errbd(m, s, u, (Eigen::Matrix<std::complex<Real>, N, N> *)0,
	      s_errbd, u_errbd);
    if (!u) return;
    Eigen::Array<std::complex<Real>, N, 1> diag =
	(u->adjoint() * m * u->conjugate()).diagonal();
    *u *= diag.unaryExpr(RephaseOp<Real>()).matrix().asDiagonal();
}

/**
 * Diagonalizes N-by-N complex symmetric matrix m so that
 *
 *     m == u * s.matrix().asDiagonal() * u.transpose()
 *
 * and `(s >= 0).all()`.  Elements of s are in descending order.
 *
 * @tparam     Real type of real and imaginary parts
 * @tparam     N    number of rows and columns in m and u
 * @param[in]  m    N-by-N complex symmetric matrix to be decomposed
 * @param[out] s    array of length N to contain singular values
 * @param[out] u    N-by-N complex unitary matrix
 */
template<class Real, int N>
void diagonalize_symmetric
(const Eigen::Matrix<std::complex<Real>, N, N>& m,
 Eigen::Array<Real, N, 1>& s,
 Eigen::Matrix<std::complex<Real>, N, N>& u)
{
    diagonalize_symmetric_errbd(m, s, &u);
}

/**
 * Same as diagonalize_symmetric(m, s, u) except that an approximate
 * error bound for the singular values is returned as well.  The error
 * bound is estimated following the method presented at
 * http://www.netlib.org/lapack/lug/node96.html.
 *
 * @param[out] s_errbd approximate error bound for the elements of s
 *
 * See the documentation of diagonalize_symmetric(m, s, u) for the
 * other parameters.
 */
template<class Real, int N>
void diagonalize_symmetric
(const Eigen::Matrix<std::complex<Real>, N, N>& m,
 Eigen::Array<Real, N, 1>& s,
 Eigen::Matrix<std::complex<Real>, N, N>& u,
 Real& s_errbd)
{
    diagonalize_symmetric_errbd(m, s, &u, &s_errbd);
}

/**
 * Same as diagonalize_symmetric(m, s, u, s_errbd) except that
 * approximate error bounds for the singular vectors are returned as
 * well.  The error bounds are estimated following the method
 * presented at http://www.netlib.org/lapack/lug/node96.html.
 *
 * @param[out] u_errbd array of approximate error bounds for u
 *
 * See the documentation of diagonalize_symmetric(m, s, u, s_errbd)
 * for the other parameters.
 */
template<class Real, int N>
void diagonalize_symmetric
(const Eigen::Matrix<std::complex<Real>, N, N>& m,
 Eigen::Array<Real, N, 1>& s,
 Eigen::Matrix<std::complex<Real>, N, N>& u,
 Real& s_errbd,
 Eigen::Array<Real, N, 1>& u_errbd)
{
    diagonalize_symmetric_errbd(m, s, &u, &s_errbd, &u_errbd);
}

/**
 * Returns singular values of N-by-N complex symmetric matrix m via s
 * such that `(s >= 0).all()`.  Elements of s are in descending order.
 *
 * @tparam     Real type of real and imaginary parts
 * @tparam     N    number of rows and columns in m and u
 * @param[in]  m    N-by-N complex symmetric matrix to be decomposed
 * @param[out] s    array of length N to contain singular values
 */
template<class Real, int N>
void diagonalize_symmetric
(const Eigen::Matrix<std::complex<Real>, N, N>& m,
 Eigen::Array<Real, N, 1>& s)
{
    diagonalize_symmetric_errbd(m, s);
}

/**
 * Same as diagonalize_symmetric(m, s) except that an approximate
 * error bound for the singular values is returned as well.  The error
 * bound is estimated following the method presented at
 * http://www.netlib.org/lapack/lug/node96.html.
 *
 * @param[out] s_errbd approximate error bound for the elements of s
 *
 * See the documentation of diagonalize_symmetric(m, s) for the other
 * parameters.
 */
template<class Real, int N>
void diagonalize_symmetric
(const Eigen::Matrix<std::complex<Real>, N, N>& m,
 Eigen::Array<Real, N, 1>& s,
 Real& s_errbd)
{
    diagonalize_symmetric_errbd(m, s, 0, &s_errbd);
}

template<class Real>
struct FlipSignOp {
    std::complex<Real> operator() (const std::complex<Real>& z) const {
	return z.real() < 0 ? std::complex<Real>(0,1) :
	    std::complex<Real>(1,0);
    }
};

template<class Real, int N>
void diagonalize_symmetric_errbd
(const Eigen::Matrix<Real, N, N>& m,
 Eigen::Array<Real, N, 1>& s,
 Eigen::Matrix<std::complex<Real>, N, N> *u = 0,
 Real *s_errbd = 0,
 Eigen::Array<Real, N, 1> *u_errbd = 0)
{
    Eigen::Matrix<Real, N, N> z;
    diagonalize_hermitian_errbd(m, s, u ? &z : 0, s_errbd, u_errbd);
    // see http://forum.kde.org/viewtopic.php?f=74&t=62606
    if (u) *u = z * s.template cast<std::complex<Real> >().
		unaryExpr(FlipSignOp<Real>()).matrix().asDiagonal();
    s = s.abs();
}

/**
 * Diagonalizes N-by-N real symmetric matrix m so that
 *
 *     m == u * s.matrix().asDiagonal() * u.transpose()
 *
 * and `(s >= 0).all()`.  Order of elements of s is *unspecified*.
 *
 * @tparam     Real type of real and imaginary parts
 * @tparam     N    number of rows and columns of m
 * @param[in]  m    N-by-N real symmetric matrix to be decomposed
 * @param[out] s    array of length N to contain singular values
 * @param[out] u    N-by-N complex unitary matrix
 *
 * @note Use diagonalize_hermitian() unless sign of `s[i]` matters.
 */
template<class Real, int N>
void diagonalize_symmetric
(const Eigen::Matrix<Real, N, N>& m,
 Eigen::Array<Real, N, 1>& s,
 Eigen::Matrix<std::complex<Real>, N, N>& u)
{
    diagonalize_symmetric_errbd(m, s, &u);
}

/**
 * Same as diagonalize_symmetric(m, s, u) except that an approximate
 * error bound for the singular values is returned as well.  The error
 * bound is estimated following the method presented at
 * http://www.netlib.org/lapack/lug/node89.html.
 *
 * @param[out] s_errbd approximate error bound for the elements of s
 *
 * See the documentation of diagonalize_symmetric(m, s, u) for the
 * other parameters.
 */
template<class Real, int N>
void diagonalize_symmetric
(const Eigen::Matrix<Real, N, N>& m,
 Eigen::Array<Real, N, 1>& s,
 Eigen::Matrix<std::complex<Real>, N, N>& u,
 Real& s_errbd)
{
    diagonalize_symmetric_errbd(m, s, &u, &s_errbd);
}

/**
 * Same as diagonalize_symmetric(m, s, u, s_errbd) except that
 * approximate error bounds for the singular vectors are returned as
 * well.  The error bounds are estimated following the method
 * presented at http://www.netlib.org/lapack/lug/node89.html.
 *
 * @param[out] u_errbd array of approximate error bounds for u
 *
 * See the documentation of diagonalize_symmetric(m, s, u, s_errbd)
 * for the other parameters.
 */
template<class Real, int N>
void diagonalize_symmetric
(const Eigen::Matrix<Real, N, N>& m,
 Eigen::Array<Real, N, 1>& s,
 Eigen::Matrix<std::complex<Real>, N, N>& u,
 Real& s_errbd,
 Eigen::Array<Real, N, 1>& u_errbd)
{
    diagonalize_symmetric_errbd(m, s, &u, &s_errbd, &u_errbd);
}

/**
 * Returns singular values of N-by-N real symmetric matrix m via s
 * such that `(s >= 0).all()`.  Order of elements of s is
 * *unspecified*.
 *
 * @tparam     Real type of elements of m and s
 * @tparam     N    number of rows and columns of m
 * @param[in]  m    N-by-N real symmetric matrix to be decomposed
 * @param[out] s    array of length N to contain singular values
 *
 * @note Use diagonalize_hermitian() unless sign of `s[i]` matters.
 */
template<class Real, int N>
void diagonalize_symmetric
(const Eigen::Matrix<Real, N, N>& m,
 Eigen::Array<Real, N, 1>& s)
{
    diagonalize_symmetric_errbd(m, s);
}

/**
 * Same as diagonalize_symmetric(m, s) except that an approximate
 * error bound for the singular values is returned as well.  The error
 * bound is estimated following the method presented at
 * http://www.netlib.org/lapack/lug/node89.html.
 *
 * @param[out] s_errbd approximate error bound for the elements of s
 *
 * See the documentation of diagonalize_symmetric(m, s) for the other
 * parameters.
 */
template<class Real, int N>
void diagonalize_symmetric
(const Eigen::Matrix<Real, N, N>& m,
 Eigen::Array<Real, N, 1>& s,
 Real& s_errbd)
{
    diagonalize_symmetric_errbd(m, s, 0, &s_errbd);
}

template<class Real, class Scalar, int M, int N>
void reorder_svd_errbd
(const Eigen::Matrix<Scalar, M, N>& m,
 Eigen::Array<Real, MIN_(M, N), 1>& s,
 Eigen::Matrix<Scalar, M, M> *u  = 0,
 Eigen::Matrix<Scalar, N, N> *vh = 0,
 Real *s_errbd = 0,
 Eigen::Array<Real, MIN_(M, N), 1> *u_errbd = 0,
 Eigen::Array<Real, MIN_(M, N), 1> *v_errbd = 0)
{
    svd_errbd(m, s, u, vh, s_errbd, u_errbd, v_errbd);
    s.reverseInPlace();
    if (u) {
	Eigen::PermutationMatrix<M> p;
	p.setIdentity();
	p.indices().template segment<MIN_(M, N)>(0).reverseInPlace();
	*u *= p;
    }
    if (vh) {
	Eigen::PermutationMatrix<N> p;
	p.setIdentity();
	p.indices().template segment<MIN_(M, N)>(0).reverseInPlace();
	vh->transpose() *= p;
    }
    if (u_errbd) u_errbd->reverseInPlace();
    if (v_errbd) v_errbd->reverseInPlace();
}

/**
 * Singular value decomposition of M-by-N matrix m such that
 *
 *     sigma.setZero(); sigma.diagonal() = s;
 *     m == u * sigma * vh    // LAPACK convention
 *
 * and `(s >= 0).all()`.  Elements of s are in ascending order.  The
 * above decomposition can be put in the form
 *
 *     m == u * s.matrix().asDiagonal() * vh
 *
 * if `M == N`.
 *
 * @tparam     Real   type of real and imaginary parts of Scalar
 * @tparam     Scalar type of elements of m, u, and vh
 * @tparam     M      number of rows in m
 * @tparam     N      number of columns in m
 * @param[in]  m      M-by-N matrix to be decomposed
 * @param[out] s      array of length min(M,N) to contain singular values
 * @param[out] u      M-by-M unitary matrix
 * @param[out] vh     N-by-N unitary matrix
 */
template<class Real, class Scalar, int M, int N>
void reorder_svd
(const Eigen::Matrix<Scalar, M, N>& m,
 Eigen::Array<Real, MIN_(M, N), 1>& s,
 Eigen::Matrix<Scalar, M, M>& u,
 Eigen::Matrix<Scalar, N, N>& vh)
{
    reorder_svd_errbd(m, s, &u, &vh);
}

/**
 * Same as reorder_svd(m, s, u, vh) except that an approximate error
 * bound for the singular values is returned as well.  The error bound
 * is estimated following the method presented at
 * http://www.netlib.org/lapack/lug/node96.html.
 *
 * @param[out] s_errbd approximate error bound for the elements of s
 *
 * See the documentation of reorder_svd(m, s, u, vh) for the other
 * parameters.
 */
template<class Real, class Scalar, int M, int N>
void reorder_svd
(const Eigen::Matrix<Scalar, M, N>& m,
 Eigen::Array<Real, MIN_(M, N), 1>& s,
 Eigen::Matrix<Scalar, M, M>& u,
 Eigen::Matrix<Scalar, N, N>& vh,
 Real& s_errbd)
{
    reorder_svd_errbd(m, s, &u, &vh, &s_errbd);
}

/**
 * Same as reorder_svd(m, s, u, vh, s_errbd) except that approximate
 * error bounds for the singular vectors are returned as well.  The
 * error bounds are estimated following the method presented at
 * http://www.netlib.org/lapack/lug/node96.html.
 *
 * @param[out] u_errbd array of approximate error bounds for u
 * @param[out] v_errbd array of approximate error bounds for vh
 *
 * See the documentation of reorder_svd(m, s, u, vh, s_errbd) for the
 * other parameters.
 */
template<class Real, class Scalar, int M, int N>
void reorder_svd
(const Eigen::Matrix<Scalar, M, N>& m,
 Eigen::Array<Real, MIN_(M, N), 1>& s,
 Eigen::Matrix<Scalar, M, M>& u,
 Eigen::Matrix<Scalar, N, N>& vh,
 Real& s_errbd,
 Eigen::Array<Real, MIN_(M, N), 1>& u_errbd,
 Eigen::Array<Real, MIN_(M, N), 1>& v_errbd)
{
    reorder_svd_errbd(m, s, &u, &vh, &s_errbd, &u_errbd, &v_errbd);
}

/**
 * Returns singular values of M-by-N matrix m via s such that
 * `(s >= 0).all()`.  Elements of s are in ascending order.
 *
 * @tparam     Real   type of real and imaginary parts of Scalar
 * @tparam     Scalar type of elements of m, u, and vh
 * @tparam     M      number of rows in m
 * @tparam     N      number of columns in m
 * @param[in]  m      M-by-N matrix to be decomposed
 * @param[out] s      array of length min(M,N) to contain singular values
 */
template<class Real, class Scalar, int M, int N>
void reorder_svd
(const Eigen::Matrix<Scalar, M, N>& m,
 Eigen::Array<Real, MIN_(M, N), 1>& s)
{
    reorder_svd_errbd(m, s);
}

/**
 * Same as reorder_svd(m, s) except that an approximate error bound
 * for the singular values is returned as well.  The error bound is
 * estimated following the method presented at
 * http://www.netlib.org/lapack/lug/node96.html.
 *
 * @param[out] s_errbd approximate error bound for the elements of s
 *
 * See the documentation of reorder_svd(m, s) for the other
 * parameters.
 */
template<class Real, class Scalar, int M, int N>
void reorder_svd
(const Eigen::Matrix<Scalar, M, N>& m,
 Eigen::Array<Real, MIN_(M, N), 1>& s,
 Real& s_errbd)
{
    reorder_svd_errbd(m, s, 0, 0, &s_errbd);
}

template<class Real, int N>
void reorder_diagonalize_symmetric_errbd
(const Eigen::Matrix<std::complex<Real>, N, N>& m,
 Eigen::Array<Real, N, 1>& s,
 Eigen::Matrix<std::complex<Real>, N, N> *u = 0,
 Real *s_errbd = 0,
 Eigen::Array<Real, N, 1> *u_errbd = 0)
{
    diagonalize_symmetric_errbd(m, s, u, s_errbd, u_errbd);
    s.reverseInPlace();
    if (u) *u = u->rowwise().reverse().eval();
    if (u_errbd) u_errbd->reverseInPlace();
}

template<class Real, int N>
void reorder_diagonalize_symmetric_errbd
(const Eigen::Matrix<Real, N, N>& m,
 Eigen::Array<Real, N, 1>& s,
 Eigen::Matrix<std::complex<Real>, N, N> *u = 0,
 Real *s_errbd = 0,
 Eigen::Array<Real, N, 1> *u_errbd = 0)
{
    diagonalize_symmetric_errbd(m, s, u, s_errbd, u_errbd);
    Eigen::PermutationMatrix<N> p;
    p.setIdentity();
    std::sort(p.indices().data(), p.indices().data() + p.indices().size(),
              [&s] (int i, int j) { return s[i] < s[j]; });
#if EIGEN_VERSION_AT_LEAST(3,1,4)
    s.matrix().transpose() *= p;
    if (u_errbd) u_errbd->matrix().transpose() *= p;
#else
    Eigen::Map<Eigen::Matrix<Real, N, 1> >(s.data()).transpose() *= p;
    if (u_errbd)
	Eigen::Map<Eigen::Matrix<Real, N, 1> >(u_errbd->data()).transpose()
	    *= p;
#endif
    if (u) *u *= p;
}

/**
 * Diagonalizes N-by-N symmetric matrix m so that
 *
 *     m == u * s.matrix().asDiagonal() * u.transpose()
 *
 * and `(s >= 0).all()`.  Elements of s are in ascending order.
 *
 * @tparam     Real   type of real and imaginary parts of Scalar
 * @tparam     Scalar type of elements of m
 * @tparam     N      number of rows and columns in m and u
 * @param[in]  m      N-by-N symmetric matrix to be decomposed
 * @param[out] s      array of length N to contain singular values
 * @param[out] u      N-by-N complex unitary matrix
 */
template<class Real, class Scalar, int N>
void reorder_diagonalize_symmetric
(const Eigen::Matrix<Scalar, N, N>& m,
 Eigen::Array<Real, N, 1>& s,
 Eigen::Matrix<std::complex<Real>, N, N>& u)
{
    reorder_diagonalize_symmetric_errbd(m, s, &u);
}

/**
 * Same as reorder_diagonalize_symmetric(m, s, u) except that an
 * approximate error bound for the singular values is returned as
 * well.  The error bound is estimated following the method presented
 * at http://www.netlib.org/lapack/lug/node96.html.
 *
 * @param[out] s_errbd approximate error bound for the elements of s
 *
 * See the documentation of reorder_diagonalize_symmetric(m, s, u) for
 * the other parameters.
 */
template<class Real, class Scalar, int N>
void reorder_diagonalize_symmetric
(const Eigen::Matrix<Scalar, N, N>& m,
 Eigen::Array<Real, N, 1>& s,
 Eigen::Matrix<std::complex<Real>, N, N>& u,
 Real& s_errbd)
{
    reorder_diagonalize_symmetric_errbd(m, s, &u, &s_errbd);
}

/**
 * Same as reorder_diagonalize_symmetric(m, s, u, s_errbd) except that
 * approximate error bounds for the singular vectors are returned as
 * well.  The error bounds are estimated following the method
 * presented at http://www.netlib.org/lapack/lug/node96.html.
 *
 * @param[out] u_errbd array of approximate error bounds for u
 *
 * See the documentation of reorder_diagonalize_symmetric(m, s, u,
 * s_errbd) for the other parameters.
 */
template<class Real, class Scalar, int N>
void reorder_diagonalize_symmetric
(const Eigen::Matrix<Scalar, N, N>& m,
 Eigen::Array<Real, N, 1>& s,
 Eigen::Matrix<std::complex<Real>, N, N>& u,
 Real& s_errbd,
 Eigen::Array<Real, N, 1>& u_errbd)
{
    reorder_diagonalize_symmetric_errbd(m, s, &u, &s_errbd, &u_errbd);
}

/**
 * Returns singular values of N-by-N symmetric matrix m via s such
 * that `(s >= 0).all()`.  Elements of s are in ascending order.
 *
 * @tparam     Real   type of real and imaginary parts of Scalar
 * @tparam     Scalar type of elements of m
 * @tparam     N      number of rows and columns in m and u
 * @param[in]  m      N-by-N symmetric matrix to be decomposed
 * @param[out] s      array of length N to contain singular values
 */
template<class Real, class Scalar, int N>
void reorder_diagonalize_symmetric
(const Eigen::Matrix<Scalar, N, N>& m,
 Eigen::Array<Real, N, 1>& s)
{
    reorder_diagonalize_symmetric_errbd(m, s);
}

/**
 * Same as reorder_diagonalize_symmetric(m, s) except that an
 * approximate error bound for the singular values is returned as
 * well.  The error bound is estimated following the method presented
 * at http://www.netlib.org/lapack/lug/node96.html.
 *
 * @param[out] s_errbd approximate error bound for the elements of s
 *
 * See the documentation of reorder_diagonalize_symmetric(m, s) for
 * the other parameters.
 */
template<class Real, class Scalar, int N>
void reorder_diagonalize_symmetric
(const Eigen::Matrix<Scalar, N, N>& m,
 Eigen::Array<Real, N, 1>& s,
 Real& s_errbd)
{
    reorder_diagonalize_symmetric_errbd(m, s, 0, &s_errbd);
}

template<class Real, class Scalar, int M, int N>
void fs_svd_errbd
(const Eigen::Matrix<Scalar, M, N>& m,
 Eigen::Array<Real, MIN_(M, N), 1>& s,
 Eigen::Matrix<Scalar, M, M> *u = 0,
 Eigen::Matrix<Scalar, N, N> *v = 0,
 Real *s_errbd = 0,
 Eigen::Array<Real, MIN_(M, N), 1> *u_errbd = 0,
 Eigen::Array<Real, MIN_(M, N), 1> *v_errbd = 0)
{
    reorder_svd_errbd(m, s, u, v, s_errbd, u_errbd, v_errbd);
    if (u) u->transposeInPlace();
}

/**
 * Singular value decomposition of M-by-N matrix m such that
 *
 *     sigma.setZero(); sigma.diagonal() = s;
 *     m == u.transpose() * sigma * v
 *     // convention of Haber and Kane, Phys. Rept. 117 (1985) 75-263
 *
 * and `(s >= 0).all()`.  Elements of s are in ascending order.  The
 * above decomposition can be put in the form
 *
 *     m == u.transpose() * s.matrix().asDiagonal() * v
 *
 * if `M == N`.
 *
 * @tparam     Real   type of real and imaginary parts of Scalar
 * @tparam     Scalar type of elements of m, u, and v
 * @tparam     M      number of rows in m
 * @tparam     N      number of columns in m
 * @param[in]  m      M-by-N matrix to be decomposed
 * @param[out] s      array of length min(M,N) to contain singular values
 * @param[out] u      M-by-M unitary matrix
 * @param[out] v      N-by-N unitary matrix
 */
template<class Real, class Scalar, int M, int N>
void fs_svd
(const Eigen::Matrix<Scalar, M, N>& m,
 Eigen::Array<Real, MIN_(M, N), 1>& s,
 Eigen::Matrix<Scalar, M, M>& u,
 Eigen::Matrix<Scalar, N, N>& v)
{
    fs_svd_errbd(m, s, &u, &v);
}

/**
 * Same as fs_svd(m, s, u, v) except that an approximate error bound
 * for the singular values is returned as well.  The error bound is
 * estimated following the method presented at
 * http://www.netlib.org/lapack/lug/node96.html.
 *
 * @param[out] s_errbd approximate error bound for the elements of s
 *
 * See the documentation of fs_svd(m, s, u, v) for the other
 * parameters.
 */
template<class Real, class Scalar, int M, int N>
void fs_svd
(const Eigen::Matrix<Scalar, M, N>& m,
 Eigen::Array<Real, MIN_(M, N), 1>& s,
 Eigen::Matrix<Scalar, M, M>& u,
 Eigen::Matrix<Scalar, N, N>& v,
 Real& s_errbd)
{
    fs_svd_errbd(m, s, &u, &v, &s_errbd);
}

/**
 * Same as fs_svd(m, s, u, v, s_errbd) except that approximate error
 * bounds for the singular vectors are returned as well.  The error
 * bounds are estimated following the method presented at
 * http://www.netlib.org/lapack/lug/node96.html.
 *
 * @param[out] u_errbd array of approximate error bounds for u
 * @param[out] v_errbd array of approximate error bounds for vh
 *
 * See the documentation of fs_svd(m, s, u, v, s_errbd) for the other
 * parameters.
 */
template<class Real, class Scalar, int M, int N>
void fs_svd
(const Eigen::Matrix<Scalar, M, N>& m,
 Eigen::Array<Real, MIN_(M, N), 1>& s,
 Eigen::Matrix<Scalar, M, M>& u,
 Eigen::Matrix<Scalar, N, N>& v,
 Real& s_errbd,
 Eigen::Array<Real, MIN_(M, N), 1>& u_errbd,
 Eigen::Array<Real, MIN_(M, N), 1>& v_errbd)
{
    fs_svd_errbd(m, s, &u, &v, &s_errbd, &u_errbd, &v_errbd);
}

/**
 * Returns singular values of M-by-N matrix m via s such that
 * `(s >= 0).all()`.  Elements of s are in ascending order.
 *
 * @tparam     Real   type of real and imaginary parts of Scalar
 * @tparam     Scalar type of elements of m, u, and v
 * @tparam     M      number of rows in m
 * @tparam     N      number of columns in m
 * @param[in]  m      M-by-N matrix to be decomposed
 * @param[out] s      array of length min(M,N) to contain singular values
 */
template<class Real, class Scalar, int M, int N>
void fs_svd
(const Eigen::Matrix<Scalar, M, N>& m,
 Eigen::Array<Real, MIN_(M, N), 1>& s)
{
    fs_svd_errbd(m, s);
}

/**
 * Same as fs_svd(m, s) except that an approximate error bound for the
 * singular values is returned as well.  The error bound is estimated
 * following the method presented at
 * http://www.netlib.org/lapack/lug/node96.html.
 *
 * @param[out] s_errbd approximate error bound for the elements of s
 *
 * See the documentation of fs_svd(m, s) for the other parameters.
 */
template<class Real, class Scalar, int M, int N>
void fs_svd
(const Eigen::Matrix<Scalar, M, N>& m,
 Eigen::Array<Real, MIN_(M, N), 1>& s,
 Real& s_errbd)
{
    fs_svd_errbd(m, s, 0, 0, &s_errbd);
}

/**
 * Singular value decomposition of M-by-N *real* matrix m such that
 *
 *     sigma.setZero(); sigma.diagonal() = s;
 *     m == u.transpose() * sigma * v
 *     // convention of Haber and Kane, Phys. Rept. 117 (1985) 75-263
 *
 * and `(s >= 0).all()`.  Elements of s are in ascending order.  The
 * above decomposition can be put in the form
 *
 *     m == u.transpose() * s.matrix().asDiagonal() * v
 *
 * if `M == N`.
 *
 * @tparam     Real   type of real and imaginary parts
 * @tparam     M      number of rows in m
 * @tparam     N      number of columns in m
 * @param[in]  m      M-by-N *real* matrix to be decomposed
 * @param[out] s      array of length min(M,N) to contain singular values
 * @param[out] u      M-by-M *complex* unitary matrix
 * @param[out] v      N-by-N *complex* unitary matrix
 *
 * @note This is a convenience overload for the case where the type of
 * u and v (complex) differs from that of m (real).  Mathematically,
 * real u and v are enough to accommodate SVD of any real m.
 */
template<class Real, int M, int N>
void fs_svd
(const Eigen::Matrix<Real, M, N>& m,
 Eigen::Array<Real, MIN_(M, N), 1>& s,
 Eigen::Matrix<std::complex<Real>, M, M>& u,
 Eigen::Matrix<std::complex<Real>, N, N>& v)
{
    fs_svd(m.template cast<std::complex<Real> >().eval(), s, u, v);
}

/**
 * Same as fs_svd(m, s, u, v) except that an approximate error bound
 * for the singular values is returned as well.  The error bound is
 * estimated following the method presented at
 * http://www.netlib.org/lapack/lug/node96.html.
 *
 * @param[out] s_errbd approximate error bound for the elements of s
 *
 * See the documentation of fs_svd(m, s, u, v) for the other
 * parameters.
 */
template<class Real, int M, int N>
void fs_svd
(const Eigen::Matrix<Real, M, N>& m,
 Eigen::Array<Real, MIN_(M, N), 1>& s,
 Eigen::Matrix<std::complex<Real>, M, M>& u,
 Eigen::Matrix<std::complex<Real>, N, N>& v,
 Real& s_errbd)
{
    fs_svd(m.template cast<std::complex<Real> >().eval(), s, u, v, s_errbd);
}

/**
 * Same as fs_svd(m, s, u, v, s_errbd) except that approximate error
 * bounds for the singular vectors are returned as well.  The error
 * bounds are estimated following the method presented at
 * http://www.netlib.org/lapack/lug/node96.html.
 *
 * @param[out] u_errbd array of approximate error bounds for u
 * @param[out] v_errbd array of approximate error bounds for vh
 *
 * See the documentation of fs_svd(m, s, u, v, s_errbd) for the other
 * parameters.
 */
template<class Real, int M, int N>
void fs_svd
(const Eigen::Matrix<Real, M, N>& m,
 Eigen::Array<Real, MIN_(M, N), 1>& s,
 Eigen::Matrix<std::complex<Real>, M, M>& u,
 Eigen::Matrix<std::complex<Real>, N, N>& v,
 Real& s_errbd,
 Eigen::Array<Real, MIN_(M, N), 1>& u_errbd,
 Eigen::Array<Real, MIN_(M, N), 1>& v_errbd)
{
    fs_svd(m.template cast<std::complex<Real> >().eval(), s, u, v,
	   s_errbd, u_errbd, v_errbd);
}

template<class Real, class Scalar, int N>
void fs_diagonalize_symmetric_errbd
(const Eigen::Matrix<Scalar, N, N>& m,
 Eigen::Array<Real, N, 1>& s,
 Eigen::Matrix<std::complex<Real>, N, N> *u = 0,
 Real *s_errbd = 0,
 Eigen::Array<Real, N, 1> *u_errbd = 0)
{
    reorder_diagonalize_symmetric_errbd(m, s, u, s_errbd, u_errbd);
    if (u) u->transposeInPlace();
}

/**
 * Diagonalizes N-by-N symmetric matrix m so that
 *
 *     m == u.transpose() * s.matrix().asDiagonal() * u
 *     // convention of Haber and Kane, Phys. Rept. 117 (1985) 75-263
 *
 * and `(s >= 0).all()`.  Elements of s are in ascending order.
 *
 * @tparam     Real   type of real and imaginary parts of Scalar
 * @tparam     Scalar type of elements of m
 * @tparam     N      number of rows and columns in m and u
 * @param[in]  m      N-by-N symmetric matrix to be decomposed
 * @param[out] s      array of length N to contain singular values
 * @param[out] u      N-by-N complex unitary matrix
 */
template<class Real, class Scalar, int N>
void fs_diagonalize_symmetric
(const Eigen::Matrix<Scalar, N, N>& m,
 Eigen::Array<Real, N, 1>& s,
 Eigen::Matrix<std::complex<Real>, N, N>& u)
{
    fs_diagonalize_symmetric_errbd(m, s, &u);
}

/**
 * Same as fs_diagonalize_symmetric(m, s, u) except that an
 * approximate error bound for the singular values is returned as
 * well.  The error bound is estimated following the method presented
 * at http://www.netlib.org/lapack/lug/node96.html.
 *
 * @param[out] s_errbd approximate error bound for the elements of s
 *
 * See the documentation of fs_diagonalize_symmetric(m, s, u) for the
 * other parameters.
 */
template<class Real, class Scalar, int N>
void fs_diagonalize_symmetric
(const Eigen::Matrix<Scalar, N, N>& m,
 Eigen::Array<Real, N, 1>& s,
 Eigen::Matrix<std::complex<Real>, N, N>& u,
 Real& s_errbd)
{
    fs_diagonalize_symmetric_errbd(m, s, &u, &s_errbd);
}

/**
 * Same as fs_diagonalize_symmetric(m, s, u, s_errbd) except that
 * approximate error bounds for the singular vectors are returned as
 * well.  The error bounds are estimated following the method
 * presented at http://www.netlib.org/lapack/lug/node96.html.
 *
 * @param[out] u_errbd array of approximate error bounds for u
 *
 * See the documentation of fs_diagonalize_symmetric(m, s, u, s_errbd)
 * for the other parameters.
 */
template<class Real, class Scalar, int N>
void fs_diagonalize_symmetric
(const Eigen::Matrix<Scalar, N, N>& m,
 Eigen::Array<Real, N, 1>& s,
 Eigen::Matrix<std::complex<Real>, N, N>& u,
 Real& s_errbd,
 Eigen::Array<Real, N, 1>& u_errbd)
{
    fs_diagonalize_symmetric_errbd(m, s, &u, &s_errbd, &u_errbd);
}

/**
 * Returns singular values of N-by-N symmetric matrix m via s such
 * that `(s >= 0).all()`.  Elements of s are in ascending order.
 *
 * @tparam     Real   type of real and imaginary parts of Scalar
 * @tparam     Scalar type of elements of m
 * @tparam     N      number of rows and columns in m and u
 * @param[in]  m      N-by-N symmetric matrix to be decomposed
 * @param[out] s      array of length N to contain singular values
 */
template<class Real, class Scalar, int N>
void fs_diagonalize_symmetric
(const Eigen::Matrix<Scalar, N, N>& m,
 Eigen::Array<Real, N, 1>& s)
{
    fs_diagonalize_symmetric_errbd(m, s);
}

/**
 * Same as fs_diagonalize_symmetric(m, s) except that an approximate
 * error bound for the singular values is returned as well.  The error
 * bound is estimated following the method presented at
 * http://www.netlib.org/lapack/lug/node96.html.
 *
 * @param[out] s_errbd approximate error bound for the elements of s
 *
 * See the documentation of fs_diagonalize_symmetric(m, s) for the
 * other parameters.
 */
template<class Real, class Scalar, int N>
void fs_diagonalize_symmetric
(const Eigen::Matrix<Scalar, N, N>& m,
 Eigen::Array<Real, N, 1>& s,
 Real& s_errbd)
{
    fs_diagonalize_symmetric_errbd(m, s, 0, &s_errbd);
}

template<class Real, class Scalar, int N>
void fs_diagonalize_hermitian_errbd
(const Eigen::Matrix<Scalar, N, N>& m,
 Eigen::Array<Real, N, 1>& w,
 Eigen::Matrix<Scalar, N, N> *z = 0,
 Real *w_errbd = 0,
 Eigen::Array<Real, N, 1> *z_errbd = 0)
{
    diagonalize_hermitian_errbd(m, w, z, w_errbd, z_errbd);
    Eigen::PermutationMatrix<N> p;
    p.setIdentity();
    std::sort(p.indices().data(), p.indices().data() + p.indices().size(),
              [&w] (int i, int j) { return std::abs(w[i]) < std::abs(w[j]); });
#if EIGEN_VERSION_AT_LEAST(3,1,4)
    w.matrix().transpose() *= p;
    if (z_errbd) z_errbd->matrix().transpose() *= p;
#else
    Eigen::Map<Eigen::Matrix<Real, N, 1> >(w.data()).transpose() *= p;
    if (z_errbd)
	Eigen::Map<Eigen::Matrix<Real, N, 1> >(z_errbd->data()).transpose()
	    *= p;
#endif
    if (z) *z = (*z * p).adjoint().eval();
}

/**
 * Diagonalizes N-by-N hermitian matrix m so that
 *
 *     m == z.adjoint() * w.matrix().asDiagonal() * z    // convention of SARAH
 *
 * w is arranged so that `abs(w[i])` are in ascending order.
 *
 * @tparam     Real   type of real and imaginary parts of Scalar
 * @tparam     Scalar type of elements of m and z
 * @tparam     N      number of rows and columns in m and z
 * @param[in]  m      N-by-N matrix to be diagonalized
 * @param[out] w      array of length N to contain eigenvalues
 * @param[out] z      N-by-N unitary matrix
 */
template<class Real, class Scalar, int N>
void fs_diagonalize_hermitian
(const Eigen::Matrix<Scalar, N, N>& m,
 Eigen::Array<Real, N, 1>& w,
 Eigen::Matrix<Scalar, N, N>& z)
{
    fs_diagonalize_hermitian_errbd(m, w, &z);
}

/**
 * Same as fs_diagonalize_hermitian(m, w, z) except that an
 * approximate error bound for the eigenvalues is returned as well.
 * The error bound is estimated following the method presented at
 * http://www.netlib.org/lapack/lug/node89.html.
 *
 * @param[out] w_errbd approximate error bound for the elements of w
 *
 * See the documentation of fs_diagonalize_hermitian(m, w, z) for the
 * other parameters.
 */
template<class Real, class Scalar, int N>
void fs_diagonalize_hermitian
(const Eigen::Matrix<Scalar, N, N>& m,
 Eigen::Array<Real, N, 1>& w,
 Eigen::Matrix<Scalar, N, N>& z,
 Real& w_errbd)
{
    fs_diagonalize_hermitian_errbd(m, w, &z, &w_errbd);
}

/**
 * Same as fs_diagonalize_hermitian(m, w, z, w_errbd) except that
 * approximate error bounds for the eigenvectors are returned as well.
 * The error bounds are estimated following the method presented at
 * http://www.netlib.org/lapack/lug/node89.html.
 *
 * @param[out] z_errbd array of approximate error bounds for z
 *
 * See the documentation of fs_diagonalize_hermitian(m, w, z, w_errbd)
 * for the other parameters.
 */
template<class Real, class Scalar, int N>
void fs_diagonalize_hermitian
(const Eigen::Matrix<Scalar, N, N>& m,
 Eigen::Array<Real, N, 1>& w,
 Eigen::Matrix<Scalar, N, N>& z,
 Real& w_errbd,
 Eigen::Array<Real, N, 1>& z_errbd)
{
    fs_diagonalize_hermitian_errbd(m, w, &z, &w_errbd, &z_errbd);
}

/**
 * Returns eigenvalues of N-by-N hermitian matrix m via w.
 * w is arranged so that `abs(w[i])` are in ascending order.
 *
 * @tparam     Real   type of real and imaginary parts of Scalar
 * @tparam     Scalar type of elements of m and z
 * @tparam     N      number of rows and columns in m and z
 * @param[in]  m      N-by-N matrix to be diagonalized
 * @param[out] w      array of length N to contain eigenvalues
 */
template<class Real, class Scalar, int N>
void fs_diagonalize_hermitian
(const Eigen::Matrix<Scalar, N, N>& m,
 Eigen::Array<Real, N, 1>& w)
{
    fs_diagonalize_hermitian_errbd(m, w);
}

/**
 * Same as fs_diagonalize_hermitian(m, w) except that an approximate
 * error bound for the eigenvalues is returned as well.  The error
 * bound is estimated following the method presented at
 * http://www.netlib.org/lapack/lug/node89.html.
 *
 * @param[out] w_errbd approximate error bound for the elements of w
 *
 * See the documentation of fs_diagonalize_hermitian(m, w) for the
 * other parameters.
 */
template<class Real, class Scalar, int N>
void fs_diagonalize_hermitian
(const Eigen::Matrix<Scalar, N, N>& m,
 Eigen::Array<Real, N, 1>& w,
 Real& w_errbd)
{
    fs_diagonalize_hermitian_errbd(m, w, 0, &w_errbd);
}

} // namespace flexiblesusy

#endif // linalg2_hpp


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2014 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 "local_basis.h"

#include <sstream>
#include <string>
#include <fstream>

#include <vector>
#include <Eigen/Geometry>

template <typename DerivedV, typename DerivedF>
IGL_INLINE void igl::local_basis(
  const Eigen::PlainObjectBase<DerivedV>& V,
  const Eigen::PlainObjectBase<DerivedF>& F,
  Eigen::PlainObjectBase<DerivedV>& B1,
  Eigen::PlainObjectBase<DerivedV>& B2,
  Eigen::PlainObjectBase<DerivedV>& B3
  )
{
  using namespace Eigen;
  using namespace std;
  B1.resize(F.rows(),3);
  B2.resize(F.rows(),3);
  B3.resize(F.rows(),3);

  for (unsigned i=0;i<F.rows();++i)
  {
      Eigen::Matrix<typename DerivedV::Scalar, 1, 3> v1 = (V.row(F(i,1)) - V.row(F(i,0))).normalized();
      Eigen::Matrix<typename DerivedV::Scalar, 1, 3> t = V.row(F(i,2)) - V.row(F(i,0));
      Eigen::Matrix<typename DerivedV::Scalar, 1, 3> v3 = v1.cross(t).normalized();
      Eigen::Matrix<typename DerivedV::Scalar, 1, 3> v2 = v1.cross(v3).normalized();

      B1.row(i) = v1;
      B2.row(i) = -v2;
      B3.row(i) = v3;
  }
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template specialization
// generated by autoexplicit.sh
template void igl::local_basis<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::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 3, 0, -1, 3> >&);
template void igl::local_basis<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::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
#endif


//****************************************************************************
// (c) 2008, 2009 by the openOR Team
//****************************************************************************
// The contents of this file are available under the GPL v2.0 license
// or under the openOR comercial license. see
//   /Doc/openOR_free_license.txt or
//   /Doc/openOR_comercial_license.txt
// for Details.
//****************************************************************************
//! OPENOR_INTERFACE_FILE(Image_ImageData)
//****************************************************************************
/**
 * @file
 * @author Christian Winne
 * \ingroup Image_ImageData
 */

#ifndef openOR_Image_Image3DSize_hpp
#define openOR_Image_Image3DSize_hpp

#include <boost/tr1/memory.hpp>

#include <openOR/Plugin/CreateInterface.hpp>
#include <openOR/Math/vector.hpp>
#include <openOR/Math/matrix.hpp>
#include <openOR/Math/vectorfunctions.hpp>
#include <openOR/Utility/Types.hpp>

namespace openOR {
   namespace Image {
     /**
      * Interface that describes a size of a cubic voxel based volume.
      * The volume is described by its voxel count, its size in millimeter.
      * \ingroup Image_ImageData
      */
     struct Image3DSize {

         /**
          * Getter for the volume size as a Size3D object in mm
          * @return The volume size in mm
          */
         virtual Math::Vector3d sizeMM() const = 0;

         //! \brief	Setter for the volume-size as a Size3UI object in indices
         //! \param[in] sizeMM The volume size in indices
         virtual void setSizeMM(const Math::Vector3d& sizeMM) = 0;

         /**
          * Getter for the volume-size as a Size3UI object in indices
          * @return The volume size in indices
          */
         virtual Math::Vector3ui size() const = 0;

         /**
          * Setter for the volume-size as a Size3UI object in indices
          * @param size The volume size in indices
          */
         virtual void setSize(const Math::Vector3ui& size) = 0;

     };
   }

  /*
   * Getter for the voxel width in mm
   * @return The voxel width in mm
   */
   inline const double voxelWidthMM(std::tr1::shared_ptr<const Image::Image3DSize> pVolumeSize) { return pVolumeSize->sizeMM()(0) / pVolumeSize->size()(0); }

  /**
  * Getter for the voxel height in mm
  * @return The voxel height in mm
  */
  inline const double voxelHeightMM(std::tr1::shared_ptr<const Image::Image3DSize> pVolumeSize) { return pVolumeSize->sizeMM()(1) / pVolumeSize->size()(1); }

  /**
  * Getter for the voxel depth in mm
  * @return The voxel depth in mm
  */
  inline const double voxelDepthMM(std::tr1::shared_ptr<const Image::Image3DSize> pVolumeSize) { return pVolumeSize->sizeMM()(2) / pVolumeSize->size()(2); }

  inline Math::Vector3d voxelSizeMM(std::tr1::shared_ptr<const Image::Image3DSize> pVolumeSize)
  {
    Math::Vector3d size;
    size(0) = voxelWidthMM(pVolumeSize);
    size(1) = voxelHeightMM(pVolumeSize);
    size(2) = voxelDepthMM(pVolumeSize);
    return size;
  }
  inline Math::Vector3d voxelSizeMMInverted(std::tr1::shared_ptr<const Image::Image3DSize> pVolumeSize)
  {
     Math::Vector3d size;
     size(0) = 1.0/voxelWidthMM(pVolumeSize);
     size(1) = 1.0/voxelHeightMM(pVolumeSize);
     size(2) = 1.0/voxelDepthMM(pVolumeSize);
     return size;
  }

  /**
   * Getter for the width of the whole volume object in mm
   * @return The width of the volume object
   */
  inline const double widthMM(std::tr1::shared_ptr<const Image::Image3DSize> pVolumeSize) { return pVolumeSize->sizeMM()(0); }

  /**
   * Getter for the height of the whole volume object in mm
   * @return The height of the volume object
   */
  inline const double heightMM(std::tr1::shared_ptr<const Image::Image3DSize> pVolumeSize) { return pVolumeSize->sizeMM()(1); }

  /**
   * Getter for the depth of the whole volume object in mm
   * @return The depth of the volume object
   */
  inline const double depthMM(std::tr1::shared_ptr<const Image::Image3DSize> pVolumeSize) { return pVolumeSize->sizeMM()(2); }

  inline Math::Vector3d sizeMM(std::tr1::shared_ptr<const Image::Image3DSize> pVolumeSize) { return pVolumeSize->sizeMM(); }

  /**
   * Getter for the width of the whole volume object in indices
   * @return The width of the volume object
   */
  inline const uint width(std::tr1::shared_ptr<const Image::Image3DSize> pVolumeSize) { return pVolumeSize->size()(0); }

  /**
   * Getter for the height of the whole volume object in indices
   * @return The height of the volume object
   */
  inline const uint height(std::tr1::shared_ptr<const Image::Image3DSize> pVolumeSize) { return pVolumeSize->size()(1); }

  /**
   * Getter for the depth of the whole volume object in indices
   * @return The depth of the volume object
   */
  inline const uint depth(std::tr1::shared_ptr<const Image::Image3DSize> pVolumeSize) { return pVolumeSize->size()(2); }

  inline Math::Vector3ui size(std::tr1::shared_ptr<const Image::Image3DSize> pVolumeSize) { return pVolumeSize->size(); }

  /**
   * Checks whether given relative mm-based point is outside of volume
   * @param   vecPoint The point to check for
   * @return  True, if the point is outside the volume
   */
  inline const bool isOutside(std::tr1::shared_ptr<const Image::Image3DSize> pVolume, const Math::Vector3d& vecPoint) {
     Math::Vector3d sizeMM = pVolume->sizeMM();
     if (vecPoint(0) < 0 || vecPoint(0) > sizeMM(0) ||
         vecPoint(1) < 0 || vecPoint(1) > sizeMM(1) ||
         vecPoint(2) < 0 || vecPoint(2) > sizeMM(2))
     {
        return true;
     }
     return false;
  }

  /**
   * Checks whether given relative index-based point is outside of volume
   * @param   vecPoint The point to check for
   * @return  True, if the point is outside the volume
   */
  inline const bool isOutside(std::tr1::shared_ptr<const Image::Image3DSize> pVolume, const Math::Vector3i& vecPoint){
     Math::Vector3ui size = pVolume->size();
     if (vecPoint(0) < 0 || vecPoint(0) >= static_cast<int>(size(0)) ||
         vecPoint(1) < 0 || vecPoint(1) >= static_cast<int>(size(1)) ||
         vecPoint(2) < 0 || vecPoint(2) >= static_cast<int>(size(2)))
     {
        return true;
     }
     return false;
  }

  inline const bool isOutside(std::tr1::shared_ptr<const Image::Image3DSize> pVolume, const Math::Vector3ui& vecPoint){
     Math::Vector3ui size = pVolume->size();
     // code review 25.09.2012: is it necessary to check for less than zero? isn't unsigned not always zero at minimum?
     if (vecPoint(0) < 0 || (unsigned int)vecPoint(0) >= size(0) ||
         vecPoint(1) < 0 || (unsigned int)vecPoint(1) >= size(1) ||
         vecPoint(2) < 0 || (unsigned int)vecPoint(2) >= size(2))
     {
        return true;
     }
     return false;
  }

  /**
   * Returns the absolute BoundingBox around the volume.
   * @return   The absolute BoundingBox around the volume
   */
  //Math::BoundingBoxD getBoundingBox(std::tr1::shared_ptr<const Volume> pVolume) {}

  /**
   * Returns index position corresponding to the given mm position.
   * @return The index position
   */

  inline void convertToVecPosition(std::tr1::shared_ptr<const Image::Image3DSize> pVolumeSize, const unsigned long index, Math::Vector3ui& vecPosition )
  {
     vecPosition(0) = ( index % pVolumeSize->size()(0) ) ;
     vecPosition(1) = ( (index % (pVolumeSize->size()(0) * pVolumeSize->size()(1))) / pVolumeSize->size()(0) );
     vecPosition(2) = index / (pVolumeSize->size()(0) * pVolumeSize->size()(1));
  }

  inline void convertToPosition (std::tr1::shared_ptr<const Image::Image3DSize> pVolumeSize, unsigned long index, Math::Vector3d &pos){
     Math::Vector3d voxl_mm = voxelSizeMM(pVolumeSize);
     Math::Vector3ui vecPos;
     convertToVecPosition(pVolumeSize, index, vecPos);
     pos = Math::elementProd<Math::Vector3d>(vecPos, voxl_mm);
  }

  inline uint convertToIndex(std::tr1::shared_ptr<const Image::Image3DSize> pVolumeSize, const Math::Vector3ui& vecPosition)
  {
    return (vecPosition(2) * pVolumeSize->size()(1) + vecPosition(1)) * pVolumeSize->size()(0) + vecPosition(0);
  }

  inline uint convertToIndex(std::tr1::shared_ptr<const Image::Image3DSize> pVolumeSize, const Math::Vector3i& vecPosition)
  {
    assert(vecPosition(0) >= 0 && vecPosition(0) < static_cast<int>(pVolumeSize->size()(0)) &&
      vecPosition(1) >= 0 && vecPosition(1) < static_cast<int>(pVolumeSize->size()(1)) &&
      vecPosition(2) >= 0 && vecPosition(2) < static_cast<int>(pVolumeSize->size()(2)));

    Math::Vector3ui vec;
    vec(0) = static_cast<uint>(vecPosition(0));
    vec(1) = static_cast<uint>(vecPosition(1));
    vec(2) = static_cast<uint>(vecPosition(2));
    return convertToIndex(pVolumeSize, vec);
  }

  inline Math::Vector3i convertToIndexPosition(std::tr1::shared_ptr<const Image::Image3DSize> pVolumeSize, const Math::Vector3d& vecPosition)
  {
    Math::Vector3i result;
    result(0) = static_cast<int>(vecPosition(0) * pVolumeSize->size()(0) / pVolumeSize->sizeMM()(0));
    result(1) = static_cast<int>(vecPosition(1) * pVolumeSize->size()(1) / pVolumeSize->sizeMM()(1));
    result(2) = static_cast<int>(vecPosition(2) * pVolumeSize->size()(2) / pVolumeSize->sizeMM()(2));
    return result;
  }

  /**
   * Returns internal memory index corresponding to the given index/mm position.
   * @return The index
   */
  inline uint convertToIndex(std::tr1::shared_ptr<const Image::Image3DSize> pVolumeSize, const Math::Vector3d& vecPosition)
  {
    return convertToIndex(pVolumeSize, convertToIndexPosition(pVolumeSize, vecPosition));
  }

  inline Math::Matrix44d normalizedTVolume(std::tr1::shared_ptr<const Image::Image3DSize> pVolumeSize)
  {
    Math::Matrix44d mat = Math::MatrixTraits<Math::Matrix44d>::IDENTITY;
    mat(0, 0) = 1.0 / widthMM(pVolumeSize);
    mat(1, 1) = 1.0 / heightMM(pVolumeSize);
    mat(2, 2) = 1.0 / depthMM(pVolumeSize);
    return mat;
  }

  inline Math::Matrix44d volumeTNormalized(std::tr1::shared_ptr<const Image::Image3DSize> pVolumeSize)
  {
    Math::Matrix44d mat = Math::MatrixTraits<Math::Matrix44d>::IDENTITY;
    mat(0, 0) = widthMM(pVolumeSize);
    mat(1, 1) = heightMM(pVolumeSize);
    mat(2, 2) = depthMM(pVolumeSize);
    return mat;
  }

}

OPENOR_CREATE_INTERFACE(openOR::Image::Image3DSize)
   Math::Vector3d sizeMM() const { return adaptee()->sizeMM(); }
   void setSizeMM(const Math::Vector3d& size) { adaptee()->setSizeMM(size); }

   Math::Vector3ui size() const { return adaptee()->size(); }
   void setSize(const Math::Vector3ui& size) { adaptee()->setSize(size); }
OPENOR_CREATE_INTERFACE_END

#endif


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2002-2012 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#include <iostream>
#include <iomanip>
#include <cmath>

#include <boost/timer/timer.hpp>

#include "dune/grid/config.h"
#include "dune/grid/uggrid.hh"
#include "dune/istl/preconditioners.hh"

#include "fem/assemble.hh"
#include "fem/norms.hh"
#include "fem/lagrangespace.hh"
#include "fem/hierarchicspace.hh"
#include "fem/embedded_errorest.hh"
#include "fem/istlinterface.hh"
#include "fem/functional_aux.hh"
#include "fem/iterate_grid.hh"
#include "fem/hierarchicErrorEstimator.hh"
#include "linalg/trivialpreconditioner.hh"
#include "linalg/jacobiPreconditioner.hh"
#include "linalg/direct.hh"
#include "linalg/triplet.hh"
#include "linalg/mumps_solve.hh"

#include "io/vtk.hh"
#include "io/amira.hh"

#include "mg/pcg.hh"
#include "linalg/apcg.hh"

#include "utilities/enums.hh"
#include "utilities/kaskopt.hh"
#include "utilities/geometric_sequence.hh"

using namespace Kaskade;
#include "poisson.hh"
#include "createGrid.hh"

int problemNo = 1;

bool compareAbs(const double x1, const double  x2)
  {
    return fabs(x1)>fabs(x2);
  }

// Implements a test problem for the cascadic multigrid algorithm as of
// Deuflhard/Weiser chapter 7.

int main(int argc, char *argv[])
{
  int verbosity = 1;
  bool dump = false;
  std::unique_ptr<boost::property_tree::ptree> pt = getKaskadeOptions(argc, argv, verbosity, dump);

  std::cout << "Start cascadic multigrid test program" << std::endl;

  int  direct, refs, order, onlyLowerTriangle = false, maxAdaptSteps, gebiet;
  DirectType directType;
  IterateType iterateType = IterateType::PCG;
  MatrixProperties property;
  std::string empty, problem, geometry, functional;

  problem = getParameter(pt, "problem", empty);
  refs = getParameter(pt, problem+".refs", 0),
  order =  getParameter(pt, problem+".order", 1),
  maxAdaptSteps = getParameter(pt, problem+".maxAdaptSteps", 10);
  geometry = getParameter(pt, problem+".geometry", empty);
  functional = getParameter(pt, problem+".functional", empty);
  gebiet = getParameter(pt, "names.geometry."+geometry, 1);
  problemNo = getParameter(pt, "names.functional."+functional, 1);
  std::cerr << "selected problem " << problem << ", functional=" <<
  functional << "(" << problemNo <<  "), geometry=" <<
  geometry << "(" << gebiet << ")" << std::endl;

  std::string s("names.type.");
  s += getParameter(pt, "solver.type", empty);
  std::cerr << "s = " << s << std::endl;
  direct = getParameter(pt, s, 0);
  std::cerr << "s = " << s << std::endl;

  s = "names.direct." + getParameter(pt, "solver.direct", empty);
  std::cerr << "s = " << s << std::endl;
  directType = static_cast<DirectType>(getParameter(pt, s, 2));
  std::cerr << "s = " << s << std::endl;

  s = "names.iterate." + getParameter(pt, "solver.iterate", empty);
  iterateType = static_cast<IterateType>(getParameter(pt, s, 0));

  property = MatrixProperties::POSITIVEDEFINITE;

  if (((property == MatrixProperties::SYMMETRIC)||(property == MatrixProperties::POSITIVEDEFINITE))&&
    ((directType == DirectType::MUMPS)||(directType == DirectType::PARDISO)))
  {
    onlyLowerTriangle = true;
  }

  int const dim2=2;
  typedef Dune::UGGrid<dim2> Grid;
  Dune::GridFactory<Grid> factory;

  switch (gebiet)
  {
    case 0:
    {
      createUnitSquare<Grid,dim2>(factory);
    }
    break;
    case 1:
    {
      createUnitCrack<Grid,dim2>(factory);
    }
    break;
    case 2:
    {
      createOriginalBoDD<Grid,dim2>(factory);
    }
    break;
    case 3:
    {
      createAreaGrid<Grid,dim2>(factory);
    }
    break;
    default:
      std::cout << "Unknown gebiet" << std::endl;
      exit(2);
  }

  std::auto_ptr<Grid> grid( factory.createGrid() );
  // the coarse grid will be refined refs times
  grid->globalRefine(refs);

  // some information on the refined mesh
  std::cout << "Grid: " << grid->size(0) << " triangles, " << std::endl;
  std::cout << "      " << grid->size(1) << " edges, " << std::endl;
  std::cout << "      " << grid->size(2) << " points" << std::endl;

  // a gridmanager is constructed
  GridManager<Grid> gridManager(std::move(grid));

  typedef Grid::LeafGridView LeafView;
  // construction of finite element space for the scalar solution T
  typedef FEFunctionSpace<ContinuousHierarchicMapper<double,LeafView> > H1Space;

  H1Space temperatureSpace(gridManager,gridManager.grid().leafGridView(),order);
  typedef boost::fusion::vector<H1Space const*> Spaces;
  Spaces spaces(&temperatureSpace);
  // VariableDescription<int spaceId, int components, int Id>
  // spaceId: number of associated FEFunctionSpace
  // components: number of components in this variable
  // Id: number of this variable
  typedef boost::fusion::vector<VariableDescription<0,1,0> >
  VariableDescriptions;
  std::string varNames[1] = { "T" };
  typedef VariableSetDescription<Spaces,VariableDescriptions> VariableSet;
  VariableSet variableSet(spaces,varNames);

  typedef VariableSet::CoefficientVectorRepresentation<>::type CoefficientVector;

  // Define a higher order space for transfering the error estimate
  H1Space temperatureSpace2(gridManager,gridManager.grid().leafView(),order+1);

  // Define the variational functional
  typedef PoissonFunctional<double,VariableSet> Functional;
  Functional F;
  typedef VariationalFunctionalAssembler<LinearizationAt<Functional> > Assembler;
  typedef Assembler::RhsArray Rhs;
  Assembler assembler(gridManager,spaces);

  typedef FEFunctionSpace<ContinuousHierarchicExtensionMapper<double,LeafView> > H1ExSpace;
  H1ExSpace spaceEx(gridManager,gridManager.grid().leafView(), order+1);

  typedef boost::fusion::vector<H1Space const*,H1ExSpace const*> H1ExSpaces;
  H1ExSpaces exSpaces(&temperatureSpace,&spaceEx);

  typedef boost::fusion::vector<VariableDescription<1,1,0> > ExVariableDescriptions;
  typedef VariableSetDescription<H1ExSpaces,ExVariableDescriptions> ExVariableSet;
  std::string exVarNames[1] = { "e"};
  ExVariableSet exVariableSet(exSpaces, exVarNames);

  typedef ExVariableSet::CoefficientVectorRepresentation<>::type ExCoefficientVector;

  typedef HierarchicErrorEstimator<LinearizationAt<Functional>,ExVariableSet> ErrorEstimator;
  typedef VariationalFunctionalAssembler<ErrorEstimator> EstGOP;

  EstGOP estGop(gridManager,exSpaces);

  typedef VariableSet::Grid::Traits::LeafIndexSet IS ;
  IS const& is = gridManager.grid().leafIndexSet();

  VariableSet::VariableSet x(variableSet), dx(variableSet);

  double const TOL = getParameter(pt, problem+".TOL", 1.0-2),
  minRefine = getParameter(pt, problem+".minRefine", 0.0);
  std::cerr << "TOL = " << TOL << ", minRefine = " << minRefine << std::endl ;
  assert(TOL>0);

  IoOptions options;
  options.outputType = IoOptions::ascii;

  constexpr int neq = Functional::TestVars::noOfVariables;
  constexpr int nvars = Functional::AnsatzVars::noOfVariables;
  int refSteps = 0;
  bool accurate = false;
  int iteSteps = getParameter(pt, "solver.iteMax", 1000);
  int verbose = getParameter(pt, "solver.verbose", 1);
  double iteEps = getParameter(pt, "solver.iteEps", 1.0e-9);
  int lookahead = getParameter(pt,"solver.APCG.lookahead",6);
  double eefactor = getParameter(pt,"ccg.eefactor",0.0);
  Dune::InverseOperatorResult result;
  double errNorm = -1;

  size_t size = variableSet.degreesOfFreedom(0,1);

  double const gamma = iterateType==IterateType::APCG? 1.0: 0.5;   // Deuflhard/Weiser (7.60)
  int const d = 2;                                    // two dimensional problem
  assert(d*gamma>1);                                  // Deuflhard/Weiser Satz 7.36
  double const beta = 1.0/sqrt(d*gamma);              // Deuflhard/Weiser (7.67)
  double const alpha = (d*gamma-1.0)/(d*(1.0+gamma)); // Deuflhard/Weiser S. 303
  double q = 2;                                       // Deuflhard/Weiser (7.62)
  long N = size;
  double zk = 0.0;
  double const requested = sqrt(1-beta*beta)*TOL;     // Deuflhard/Weiser Algorithmus 7.37
  double yk = pow(N,alpha);
  double pcgerr = -1;
  double pcgerr_fact = -1;
  int required = 0;

  std::vector<VariableSet::VariableSet> solutions;

  do {
    // Zero coefficient vectors for initialization
    ExCoefficientVector exZero(ExVariableSet::CoefficientVectorRepresentation<>::init(exVariableSet)); exZero = 0;
    CoefficientVector zero(VariableSet::CoefficientVectorRepresentation<>::init(variableSet)); zero = 0;

    std::cerr << "----------------------------------------\nStarting round\n";
    CoefficientVector solution(zero), hilfe(zero);
    assembler.assemble(linearization(F,x));
    Rhs rhs(assembler.rhs());
    AssembledGalerkinOperator<Assembler,0,neq,0,nvars> A(assembler, onlyLowerTriangle);

    if (direct)
      directInverseOperator(A,directType,property).applyscaleadd(-1.0,rhs,solution);
    else  {
      switch (iterateType) {
	case IterateType::CG: {
	  JacobiPreconditioner<AssembledGalerkinOperator<Assembler,0,neq,0,nvars> > jacobi(A,1.0);
	  Dune::CGSolver<CoefficientVector> cg(A,jacobi,iteEps,iteSteps,verbose);
	  cg.apply(hilfe,rhs,result);
	  break;
	}
	case IterateType::PCG: {
	  JacobiPreconditioner<AssembledGalerkinOperator<Assembler,0,neq,0,nvars> > jacobi(A,1.0);
	  NMIIIPCGSolver<CoefficientVector> pcg(A,jacobi,iteEps,iteSteps,verbose);
	  pcg.apply(hilfe,rhs,result);
	  break;
	}
	case IterateType::APCG: {
	  JacobiPreconditioner<AssembledGalerkinOperator<Assembler,0,neq,0,nvars> > jacobiPCG(A,1.0);
	  DefaultDualPairing<CoefficientVector,CoefficientVector> dp;
	  PCGEnergyErrorTerminationCriterion<double> terminate(iteEps,iteSteps);
	  terminate.lookahead(lookahead);
	  Pcg<CoefficientVector,CoefficientVector> apcg(A,jacobiPCG,dp,terminate,verbose);
	  apcg.apply(hilfe,rhs,result);
	  pcgerr = terminate.error();

	  CoefficientVector hilfe2(zero);
	  PCGEnergyErrorTerminationCriterion<double> terminate2(iteEps/1000,iteSteps);
	  Pcg<CoefficientVector,CoefficientVector> apcg2(A,jacobiPCG,dp,terminate2,verbose);
          Dune::InverseOperatorResult result2;
	  apcg2.apply(hilfe2,rhs,result2);
    std::ostringstream fn;
    fn << "cmg-gamma-";
    fn.width(3);
    fn.fill('0');
    fn.setf(std::ios_base::right,std::ios_base::adjustfield);
    fn << refSteps;
    fn.flush();
    std::ofstream out(fn.str().c_str());
    terminate.clear();
    required = 0;
    for (int i=0; i<terminate2.gamma2().size(); ++i) {
      terminate.step(terminate2.gamma2()[i]);
      out << terminate2.gamma2()[i] << ' ' << terminate.error() << ' ' << std::sqrt(std::accumulate(terminate2.gamma2().begin()+i,terminate2.gamma2().end(),0.0)) << '\n';
      if (std::sqrt(std::accumulate(terminate2.gamma2().begin()+i,terminate2.gamma2().end(),0.0)) >= iteEps)
	required = i;
    }

// 	  PCGEnergyErrorTerminationCriterion<double> terminate3(0.0,required+1);
// 	  Pcg<CoefficientVector,CoefficientVector> apcg3(A,jacobiPCG,dp,terminate3,verbose);
// 	  apcg3.apply(hilfe,rhs,result);
// 	  pcgerr = terminate3.error();
// 	  //pcgstep = std::sqrt(std::accumulate(terminate3.gamma2().begin(),terminate3.gamma2().end(),0.0));

	  CoefficientVector dhilfe(hilfe2); dhilfe -= hilfe;
	  CoefficientVector Adhilfe(zero);
	  A.apply(dhilfe,Adhilfe);
	  pcgerr_fact = std::sqrt(dhilfe*Adhilfe);

	  break;
	}
// 	case IterateType::SGS: {
// 	  int addedIterations = 3;
// 	  typedef Dune::BlockVector<Dune::FieldVector<double,1> > NakedCoefficientVector;
// 	  Dune::SeqSSOR<MatrixAsTriplet<double>,NakedCoefficientVector,NakedCoefficientVector> sgs(A.getmat(),1,1.0);
// 	  sgs.pre(boost::fusion::at_c<0>(hilfe.data),boost::fusion::at_c<0>(rhs.data));
// 	  CoefficientVector y(zero), z(zero), r(rhs);
// 	  boost::circular_buffer<double> gamma(addedIterations);
// 	  for (int i=0; i<iteSteps; ++i) {
// 	    sgs.apply(boost::fusion::at_c<0>(y.data),boost::fusion::at_c<0>(r.data));
// 	    A.apply(y,z);
// 	    r -= z;
// 	    hilfe += y;
// 	    result.iterations = i+1;
// 	    gamma.push_back(std::sqrt(y*z));
// 	    if (i>=addedIterations && std::accumulate(gamma.begin(),gamma.end(),0.0)<iteEps)
// 	      break;
// 	  }
// 	  std::pair<double,double> geoest = estimateGeometricSequence(gamma.begin(),gamma.end());
// 	  std::cerr << "geometric estimate: c=" << geoest.first << " q=" << geoest.second << " delta=" << iteEps << '\n';
// 	  std::copy(gamma.begin(),gamma.end(),std::ostream_iterator<double>(std::cerr," ")); std::cerr << '\n';
// 	  break;
// 	}
	default:
	  std::cerr << "Solver " << iterateType << " not available" << std::endl;
	  throw -111;
      }
      solution.axpy(-1,hilfe);
    }
    dx.data = solution.data;

    // Do hierarchical error estimation. Remember to provide the very same underlying problem to the
    // error estimator functional as has been used to compute dx (do not modify x!).
    {

    estGop.assemble(ErrorEstimator(LinearizationAt<Functional>(F,x),dx));

      // iterative solution of error estimator
      AssembledGalerkinOperator<EstGOP> E(estGop);
      Dune::InverseOperatorResult estRes;
      ExCoefficientVector const estRhside(estGop.rhs()) ;
      ExCoefficientVector estSol(exZero) ;
      JacobiPreconditioner<EstGOP> jprec(estGop, 1.0);
      jprec.apply(estSol,estRhside); //single Jacobi iteration

      // Represent error estimator as FE function
      H1ExSpace::Element<1>::type erroresttmp(spaceEx);
      erroresttmp = boost::fusion::at_c<0>(estSol.data);
      H1Space::Element<1>::type errorest(temperatureSpace2);
      errorest = erroresttmp;

      // Transfer error indicators to cells.
      std::vector<double> errorDistribution(is.size(0));
      typedef VariableSet::GridView::Codim<0>::Iterator CellIterator ;
      double maxErr = 0.0;
      for (CellIterator ci=variableSet.gridView.begin<0>(); ci!=variableSet.gridView.end<0>(); ++ci) {
	typedef H1ExSpace::Mapper::GlobalIndexRange GIR;
	double err = 0;
	GIR gix = spaceEx.mapper().globalIndices(*ci);
	for (GIR::iterator j=gix.begin(); j!=gix.end(); ++j)
	  err += boost::fusion::at_c<0>(estSol.data)[*j] * boost::fusion::at_c<0>(estRhside.data)[*j]; // remember that what adds up here is positive (due to diagonal solve)
	assert(err>=0);
	errorDistribution[is.index(*ci)] = std::sqrt(err); // Deuflhard/Weiser (6.37)
	if (err>maxErr) maxErr = err;
      }

      // Select refinement threshold: Refine all elements which
      // - have at least an error of half of the maximum error or
      // - are among the minRefine fraction with largest error
      double errLevel = maxErr/(order+1);
      if (minRefine>0.0) {
	std::vector<double> eSort(errorDistribution);
	std::sort(eSort.begin(),eSort.end(),std::greater<double>()); // sort decreasingly
	int minRefineIndex = minRefine*(eSort.size()-1);
	errLevel = std::min(errLevel,eSort[minRefineIndex]+1.0e-14);
      }

      // Compute the error estimator norm according to Deuflhard/Weiser (6.36)
      errNorm = 0;
      for (int i=0; i<boost::fusion::at_c<0>(estSol.data).N(); ++i)
	errNorm += boost::fusion::at_c<0>(estSol.data)[i] * boost::fusion::at_c<0>(estRhside.data)[i]; // remember that what adds up here is positive (due to diagonal solve)
      errNorm = std::sqrt(errNorm);

      // Termination check
      if (errNorm<requested) {
	accurate = true;

	for (int i=0; i<solutions.size()-1; ++i) {
          solutions[i] -= solutions.back();

	  CoefficientVector du(zero), Adu(zero);
	  boost::fusion::at_c<0>(du.data) = boost::fusion::at_c<0>(solutions[i].data).coefficients();
	  A.apply(du,Adu);
	  std::cerr << "i= " << i << " err= " << std::sqrt(du*Adu) << '\n';
	}
      } else {
	// Refine mesh.
	int count = 0;
	for (CellIterator ci=variableSet.gridView.begin<0>(); ci!=variableSet.gridView.end<0>(); ++ci)
	  if (errorDistribution[is.index(*ci)] >= errLevel) {
	    gridManager.mark(1,*ci);
	    ++count;
	  }
	accurate = !gridManager.adaptAtOnce();

    std::ostringstream fn;
    fn << "graph/cmg-dx";
    fn.width(3);
    fn.fill('0');
    fn.setf(std::ios_base::right,std::ios_base::adjustfield);
    fn << refSteps;
    fn.flush();
    writeVTKFile(gridManager.grid().leafView(),variableSet,dx,fn.str(),options,order);

      }
    // apply the Newton correction here
    x += dx;

    std::ostringstream fn;
    fn << "graph/cmg-sol";
    fn.width(3);
    fn.fill('0');
    fn.setf(std::ios_base::right,std::ios_base::adjustfield);
    fn << refSteps;
    fn.flush();
    writeVTKFile(gridManager.grid().leafView(),variableSet,x,fn.str(),options,order);
    // add coarse grid error estimator to fine grid prolongation
    errorest *= eefactor;
    boost::fusion::at_c<0>(x.data) -= errorest;
    }

    // store solution for later checks
    solutions.push_back(x);

    std::cout << "step= " << refSteps << " [err]= " << errNorm << " zk= " << zk << " q= " << q << " alpha= " << alpha
              << " N= " << N << " yk= " << yk << " iter= " << result.iterations << " delta= " << iteEps << " pcgerr= " << pcgerr << " pcgerr_fact= " << pcgerr_fact
              << " required= " << required << '\n';
    std::cout.flush();

        refSteps++;

    // Modify tolerances according to Deuflhard/Weiser Algorithmus 7.37
    q = variableSet.degreesOfFreedom(0,1)/static_cast<double>(N); // DOF growth factor
    N = variableSet.degreesOfFreedom(0,1);                        // new DOF
    yk += pow(N,alpha);                                           // Deuflhard/Weiser S. 303
    zk = pow(N,alpha)*(pow(errNorm/requested,d*alpha)-pow(q,alpha))/(pow(q,alpha)-1.0); // Deuflhard/Weiser S. 304
    iteEps = beta*TOL*yk/(yk+zk);

    if (getParameter(pt, "strategy", 1) == 0)
      iteEps = beta*TOL;
    if (getParameter(pt, "strategy", 1) == 2)
      iteEps = errNorm*std::pow(q,1.0/d);

    if (refSteps>maxAdaptSteps) {
      std::cout << "number of steps " << refSteps << " > maxAdaptSteps (" << maxAdaptSteps << ")\n";
      break;
    }

    std::ostringstream fn;
    fn << "graph/cmg-grid";
    fn.width(3);
    fn.fill('0');
    fn.setf(std::ios_base::right,std::ios_base::adjustfield);
    fn << refSteps-1;
    fn.flush();
    writeVTKFile(gridManager.grid().leafView(),variableSet,x,fn.str(),options,order);

  } while (!accurate);

std::cout << "End cmgtest" << std::endl;
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_EULER_FUNCTIONS_SIMD_COMMON_ERFC_HPP_INCLUDED
#define NT2_EULER_FUNCTIONS_SIMD_COMMON_ERFC_HPP_INCLUDED

#include <nt2/euler/functions/erfc.hpp>
#include <nt2/euler/functions/details/erf_kernel.hpp>
#include <nt2/include/constants/two.hpp>
#include <nt2/include/constants/twothird.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/functions/simd/abs.hpp>
#include <nt2/include/functions/simd/divides.hpp>
#include <nt2/include/functions/simd/exp.hpp>
#include <nt2/include/functions/simd/if_else.hpp>
#include <nt2/include/functions/simd/inbtrue.hpp>
#include <nt2/include/functions/simd/is_less.hpp>
#include <nt2/include/functions/simd/is_ltz.hpp>
#include <nt2/include/functions/simd/logical_andnot.hpp>
#include <nt2/include/functions/simd/minus.hpp>
#include <nt2/include/functions/simd/multiplies.hpp>
#include <nt2/include/functions/simd/oneminus.hpp>
#include <nt2/include/functions/simd/oneplus.hpp>
#include <nt2/include/functions/simd/plus.hpp>
#include <nt2/include/functions/simd/splat.hpp>
#include <nt2/include/functions/simd/sqr.hpp>
#include <nt2/include/functions/simd/unary_minus.hpp>
#include <nt2/sdk/meta/as_logical.hpp>
#include <nt2/sdk/meta/cardinal_of.hpp>

#include <boost/simd/sdk/config.hpp>

#ifndef BOOST_SIMD_NO_INFINITIES
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/functions/simd/if_zero_else.hpp>
#include <nt2/include/functions/simd/is_equal.hpp>
#endif

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( erfc_, tag::cpu_
                              , (A0)(X)
                              , ((simd_<double_<A0>,X>))
                              )
  {
    typedef A0 result_type;
    NT2_FUNCTOR_CALL(1)
    {
      typedef typename meta::as_logical<A0>::type bA0;

      A0 x =  nt2::abs(a0);
      A0 xx =  nt2::sqr(x);
      A0 lim1 = nt2::splat<A0>(0.65);
      A0 lim2 = nt2::splat<A0>(2.2);
      bA0 test0 = nt2::is_ltz(a0);
      bA0 test1 = nt2::lt(x, lim1);
      A0 r1 = nt2::Zero<A0>();
      std::size_t nb = nt2::inbtrue(test1);
      if(nb > 0)
      {
        r1 = nt2::oneminus(x*details::erf_kernel<A0>::erf1(xx));
        if (nb >= meta::cardinal_of<A0>::value)
          return nt2::if_else(test0, nt2::Two<A0>()-r1, r1);
      }
      bA0 test2 = nt2::lt(x, lim2);
      bA0 test3 = nt2::logical_andnot(test2, test1);
      A0 ex = nt2::exp(-xx);

      std::size_t nb1 = nt2::inbtrue(test3);
      if(nb1 > 0)
      {
        A0 z = ex*details::erf_kernel<A0>::erfc2(x);
        r1 = nt2::if_else(test1, r1, z);
        nb += nb1;
        if (nb >= meta::cardinal_of<A0>::value)
          return nt2::if_else(test0, Two<A0>()-r1, r1);
      }
      A0 z =  ex*details::erf_kernel<A0>::erfc3(x);
      r1 = nt2::if_else(test2, r1, z);
      #ifndef BOOST_SIMD_NO_INFINITIES
      r1 = if_zero_else( eq(x, Inf<A0>()), r1);
      #endif
      return  nt2::if_else(test0, nt2::Two<A0>()-r1, r1);
      }
  };

  BOOST_DISPATCH_IMPLEMENT  ( erfc_, tag::cpu_
                              , (A0)(X)
                              , ((simd_<single_<A0>,X>))
                              )
  {
    typedef A0 result_type;
    NT2_FUNCTOR_CALL(1)
    {
      typedef typename meta::as_logical<A0>::type bA0;

      A0 x =  nt2::abs(a0);
      bA0 test0 = nt2::is_ltz(a0);
      A0 r1 = nt2::Zero<A0>();
      bA0 test1 = nt2::lt(x, Twothird<A0>());
      A0 z = x/oneplus(x);

      std::size_t nb = nt2::inbtrue(test1);
      if(nb > 0)
      {
        r1 = details::erf_kernel<A0>::erfc3(z);
        if (nb >= meta::cardinal_of<A0>::value)
          return nt2::if_else(test0, nt2::Two<A0>()-r1, r1);
      }
      z -= nt2::splat<A0>(0.4);
      A0 r2 = exp(-sqr(x))*details::erf_kernel<A0>::erfc2(z);
      r1 = if_else(test1, r1, r2);
      #ifndef BOOST_SIMD_NO_INFINITIES
      r1 = if_zero_else( eq(x, Inf<A0>()), r1);
      #endif
      return nt2::if_else(test0, nt2::Two<A0>()-r1, r1);
    }
  };
} }

#endif


#include <iostream>
#include <fstream>
#include <armadillo>
#include <cmath>
#include <string>
#include <unordered_map>
#include "scf.h"
using namespace std;
using namespace arma;
SCF::SCF(){
  //Read in the object name like H2O
  std::string path_data = "/home/xiuyiqin/GF3ES/Test_SCF/data/";
  ifstream iptcoor(path_data+"inputfile.com");
  //In the input file never try to turn on the ERI symmetry, it will cause errors
  std::string data;
  string judge1 = "Obj=";
  string judge2 = "OCC=";
  while(!iptcoor.eof()){
      iptcoor >> data;
        if (data.compare(judge1)==0){
            iptcoor>>data;
            objname = data;
        }
        if (data.compare(judge2)==0){
            iptcoor>>data;
            _numoccp = std::stoi(data,nullptr,10);
        }
  }
  if (_numoccp == 0){
      printf("Please put number of occupied orbits in inputfile.com");
      exit(EXIT_FAILURE);
  }
  //Read in the 3 input files
  std::string overlapfile = "_overlap.dat";
  std::string coreHfile = "_coreH.dat";
  std::string erifile = "_eri.dat";
  std::string enucfile = "_enuc_Nb.dat";
  ifstream iptovlap(path_data+objname+overlapfile);
  ifstream iptcoreH(path_data+objname+coreHfile);
  ifstream ipteri(path_data+objname+erifile);
  ifstream iptenuc(path_data+objname+enucfile);

  while(!iptenuc.eof()){
  iptenuc>>_nucrepul;
  iptenuc>>_numorbit;
  }
  //to set up ioff:
  _ioff.resize(pow(_numorbit,4));
  _ioff[0] = 0;
  for(int i=1; i < pow(_numorbit,4); i++)
  _ioff[i] = _ioff[i-1] + i;
  //set up matrix
  _ovlap = new arma::mat(_numorbit,_numorbit);
  _coreHam = new arma::mat(_numorbit,_numorbit);
  _Pini = new arma::mat(_numorbit,_numorbit);
  _Pnext = new arma::mat(_numorbit,_numorbit);
  _Fini = new arma::mat(_numorbit,_numorbit);
  _Fnext = new arma::mat(_numorbit,_numorbit);
  int i = 0, j = 0,k = 0, l = 0;
  double temp,temp2;
  printf("T3");
  /*
  to input overlap integral
  and core Hamiltonian
  and nuclear repulsive energy and No of orbits
  */
   // overlap and core Hamiltonian is same size so I put them in same while
  while(!iptovlap.eof()){
  iptovlap >> i >> j >> temp;
  iptcoreH >> i >> j >> temp2;
  (*_ovlap)(i -1, j -1) = temp;
  (*_ovlap)(j -1, i -1 ) = temp;
  (*_coreHam)(i -1, j -1) = temp2;
  (*_coreHam)(j -1, i -1 ) = temp2;
  }
  /*two electron integral */
  double temp4;
  while (!ipteri.eof()) {
    ipteri >> i >> j >> k >> l >> temp4;
    _twoelec[getijkl(i, j, k, l)] = temp4;
  }
  iptcoor.close();iptovlap.close();iptcoreH.close();
  ipteri.close();iptenuc.close();
}

int SCF::getijkl(int i, int j, int k, int l){
  int ij = 0, kl = 0, ijkl = 0;
  ij = (i > j) ? _ioff[i] + j : _ioff[j] + i;
  kl = (k > l) ? _ioff[k] + l : _ioff[l] + k;
  ijkl = (ij > kl) ? _ioff[ij] + kl : _ioff[kl] + ij;
  return ijkl;
}

SCF::~SCF(){
  delete _ovlap;
  delete _coreHam;
}

void SCF::print(arma::mat ipt){
  for (size_t i = 0; i < _numorbit; i++) {
    printf("%s\n", " ");
    for (size_t j = 0; j < _numorbit; j++) {
    printf("%20.12f, %s", (ipt)(i, j), " ");
    }
  }
}

void SCF::calculation(){
  /*diagonlize overlap Matrix*/
  printf("T9");
  arma::vec Seigval;
  arma::mat Seigvec;
  arma::eig_sym(Seigval, Seigvec, *_ovlap);
  /*to get diagonlized eigenvalue matrix
  *arma::mat eigvalmat = eigvec.t() * (*_ovlap) * eigvec;//works good
  but the following one would be easier to use*/
  arma::mat Seigvalmat = arma::diagmat(Seigval);
  // to get Orthogonalization Matrix
  //So element-wise inverse and square-root ! except ij term in matrix!

  /*arma::mat et = 1/ sqrt(abs(eigvalmat));
  *arma::mat S_ihalf = arma::eye(_numorbit,_numorbit);
  for (size_t i = 0; i < _numorbit; i++) {
      S_ihalf(i, i) = sqrt(1.0 /eigvalmat(i,i));
  } this one works but the following one would be more neat
  */
  arma::mat lmd_sqrti = arma::sqrt(arma::inv(Seigvalmat));

  _Ssqrtinv = Seigvec * lmd_sqrti * Seigvec.t();//different from website But I believe it caused by different order of eigenvalue? No it is caused by wrong input file
  /*temporaly moving on
  * get the Fock matrix for inital guess from hailtonian
  * then transfer to orthogonal basis to get Fock prime
  * then diagonlize Fock! in the orthogonal basis!
  * then transfer back to original basis which is the atomic orbitals
  */
  *_Fini = *_coreHam;
  _Finiprime = _Ssqrtinv.t() * (*_coreHam) * _Ssqrtinv;
  //digonalize Fock Matrix
  arma::vec Feigval;
  arma::mat Feigvec;
  arma::eig_sym(Feigval, Feigvec, _Finiprime);
  arma::mat Coe_orig = _Ssqrtinv * Feigvec;
  //density matrix equals to C * C.t()
  *_Pini = Coe_orig.cols(0, _numoccp -1) *  Coe_orig.cols(0, _numoccp -1).t();
  //then get the Energy
  arma::mat Eini = (*_Pini) % ((*_coreHam) + *_Fini);
  _Eini = arma::accu(Eini);
  _Eini += _nucrepul;
  printf("%f\n", _Eini);

  /*iteration!
  */
  while(true){
  /*build new Fock ! by old density matrix */
  for (size_t i = 0; i < _numorbit; i++) {
      for (size_t j = 0; j < _numorbit; j++) {
        (*_Fnext)(i,j) = (*_coreHam)(i,j);
        for (size_t k = 0; k < _numorbit; k++) {
          for (size_t l = 0; l < _numorbit; l++) {
            (*_Fnext)(i,j) += (*_Pini)(k,l) * (2 * _twoelec[getijkl(i+1,j+1,k+1,l+1)] - _twoelec[getijkl(i+1,k+1,j+1,l+1)]);
          }
        }
      }
    }
    _Finiprime = _Ssqrtinv.t() * (*_Fnext) * _Ssqrtinv;
    /* to get density matrix and get Energy again..
    */
    arma::vec eigval;
    arma::mat eigvec;
    arma::eig_sym(eigval, eigvec, _Finiprime);
    arma::mat Coe = _Ssqrtinv * eigvec;
    //density matrix equals to C * C.t()
    *_Pnext = Coe.cols(0, _numoccp -1) *  Coe.cols(0, _numoccp -1).t();
    //print(*_Pnext);
    _Enext = arma::accu((*_Pnext) % ((*_coreHam) + *_Fnext)) + _nucrepul;
    printf("%20.12f\n", _Enext);
    if(abs(_Enext - _Eini) < 0.00000000001)
    break;
    _Eini = _Enext;
    *_Fini = *_Fnext;
    *_Pini = *_Pnext;
  }
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef ITL_PC_ILUT_INCLUDE
#define ITL_PC_ILUT_INCLUDE

#include <boost/numeric/mtl/vector/sparse_vector.hpp>
#include <boost/numeric/mtl/operation/invert_diagonal.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

namespace itl { namespace pc {

struct ilut_factorizer
{

    template <typename Matrix, typename Para, typename L_type, typename U_type>
    ilut_factorizer(const Matrix &A, const Para& p, L_type& L, U_type& U)
    { factorize(A, p, L, U, mtl::traits::is_row_major<Matrix>()); }

    // column-major matrices are copied first
    template <typename Matrix, typename Para, typename L_type, typename U_type, bool B>
    void factorize(const Matrix &A, const Para& p, L_type& L, U_type& U, boost::mpl::bool_<B>)
    {
	typedef typename mtl::Collection<Matrix>::value_type      value_type;
	typedef typename mtl::Collection<Matrix>::size_type       size_type;
	typedef mtl::matrix::parameters<mtl::row_major, mtl::index::c_index, mtl::non_fixed::dimensions, false, size_type> para;
	typedef mtl::matrix::compressed2D<value_type, para>  LU_type;
	LU_type LU(A);
	factorize(LU, p, L, U, boost::mpl::true_());
    }

    // According Yousef Saad: ILUT, NLAA, Vol 1(4), 387-402 (1994)
#if 0
    template <typename Value, typename MPara, typename Para, typename L_type, typename U_type>
    factorize(const mtl::matrix::compressed2D<Value, MPara>& A, const Para& p, L_type& L, U_type& U, boost::mpl::true_)
#endif

    template <typename Matrix, typename Para, typename L_type, typename U_type>
    void factorize(const Matrix& A, const Para& p, L_type& L, U_type& U, boost::mpl::true_)

    {
	mtl::vampir_trace<5049> tracer;
	using std::abs; using mtl::traits::range_generator; using mtl::begin; using mtl::end;
	using namespace mtl::tag;
	MTL_THROW_IF(num_rows(A) != num_cols(A), mtl::matrix_not_square());

	typedef typename mtl::Collection<Matrix>::value_type      value_type;
	typedef typename mtl::Collection<Matrix>::size_type       size_type;
	typedef typename range_generator<row, Matrix>::type       cur_type;
	typedef typename range_generator<nz, cur_type>::type      icur_type;
	typename mtl::traits::col<Matrix>::type                   col(A);
	typename mtl::traits::const_value<Matrix>::type           value(A);

	size_type n= num_rows(A);
	L.change_dim(n, n);
	U.change_dim(n, n);
	{
	    mtl::matrix::inserter<L_type> L_ins(L, p.first);
	    mtl::matrix::inserter<U_type> U_ins(U, p.first + 1); // plus one for diagonal

	    mtl::vector::sparse_vector<value_type> vec(n); // corr. row in paper
	    cur_type ic= begin<row>(A); // , iend= end<row>(A);
	    for (size_type i= 0; i < n; ++i, ++ic) {

		for (icur_type kc= begin<nz>(ic), kend= end<nz>(ic); kc != kend; ++kc) // row= A[i][*]
		    vec.insert(col(*kc), value(*kc));
		// std::cerr << "vec_" << i << " = " << vec << std::endl;
		value_type tau_i= p.second * two_norm(vec); // threshold for i-th row
		// loop over non-zeros in vec; changes in vec considered
		for (size_type j= 0; j < vec.nnz() && vec.index(j) < i; j++) {
		    size_type k= vec.index(j);
		    value_type ukk= U_ins.value(k, k);
		    MTL_DEBUG_THROW_IF(ukk == value_type(0), mtl::missing_diagonal());
		    value_type vec_k= vec.value(j)/= ukk;
		    // std::cout << "vec after updating from U[" << k << "][" << k << "] is " << vec << '\n';
		    for (size_type j0= U_ins.ref_major()[k], j1= U_ins.ref_slot_ends()[k]; j0 < j1; j0++) { // U[k][k+1:n]
			size_type k1= U_ins.ref_minor()[j0];
			if (k1 > k)
			    vec[k1]-= vec_k * U_ins.ref_elements()[j0];
			// std::cout << "vec after updating from U[" << k << "][" << k1 << "] is " << vec << '\n';
		    }
		    // if (i > 1000 && i < 1010) std::cout << "vec before crop in row " << i << ", updating from row " << k << ": \n" << vec << "\n";
		    vec.crop(tau_i);
		    // if (i > 1000 && i < 1010) std::cout << "vec after crop: \n" << vec << "\n";
		}
		// std::cerr << "vec_" << i << " = " << vec << std::endl;
		vec.sort_on_data();
		// std::cerr << "vec_" << i << " sorted on data = " << vec << std::endl;

		// std::cout << "vec at " << i << " is " << vec << '\n';
		// mtl::vampir_trace<9904> tracer2;
		bool diag_found= false;
		for (size_type cntu= 0, cntl= 0, j= 0; j < vec.nnz() && (cntu < p.first || cntl < p.first); j++) {
		    size_type k= vec.index(j);
		    value_type v= vec.value(j);
		    // if (abs(v) < tau_i) break;
		    if (i == k) {
			U_ins[i][i] << v; diag_found= true;
		    } else if (i < k) {
			if (cntu++ < p.first)
			    U_ins[i][k] << v;
		    } else // i > k
			if (cntl++ < p.first)
			    L_ins[i][k] << v;
		}
		if (!diag_found) std::cerr << "Deleted diagonal!!!!\n";
		vec.make_empty();
	    }
	} // destroy inserters
	invert_diagonal(U);
    }
};

// Not usable yet !!!!!
template <typename Matrix, typename Value= typename mtl::Collection<Matrix>::value_type>
class ilut
  : public ilu<Matrix, ilut_factorizer, Value>
{
    typedef ilu<Matrix, ilut_factorizer, Value> base;
  public:
    ilut(const Matrix& A, std::size_t p, typename mtl::Collection<Matrix>::value_type tau)
      : base(A, std::make_pair(p, tau)) {}
};

}} // namespace itl::pc

#endif // ITL_PC_ILUT_INCLUDE