// ====================================================================
// 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


#include <iostream>
#include <cmath>
#include <algorithm>
#include <vector>
#include <boost/multiprecision/cpp_int.hpp>
using namespace boost::multiprecision;
using namespace std;
int main() {
    int n; cin >> n;
    vector<long long int> v(n, 0);
    cpp_int c = 0;
    for (int i = 0; i < n; i++) {
        long long int a, b; cin >> a >> b;
        c -= a, v[i] = 2 * a + b;
    }
    sort(v.begin(), v.end());
    reverse(v.begin(), v.end());
    int ans = 0;
    for (int i = 0; i < n; i++) {
        c += v[i];
        ans++;
        if (c > 0) break;
    }
    cout << ans << endl;
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 *  @file   testSubgraphSolver.cpp
 *  @brief  Unit tests for SubgraphSolver
 *  @author Yong-Dian Jian
 **/

#include <gtsam/linear/SubgraphSolver.h>

#include <tests/smallExample.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/iterative.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/SubgraphBuilder.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/inference/Ordering.h>
#include <gtsam/base/numericalDerivative.h>

#include <CppUnitLite/TestHarness.h>

#include <boost/assign/std/list.hpp>
using namespace boost::assign;

using namespace std;
using namespace gtsam;

static size_t N = 3;
static SubgraphSolverParameters kParameters;
static auto kOrdering = example::planarOrdering(N);

/* ************************************************************************* */
/** unnormalized error */
static double error(const GaussianFactorGraph& fg, const VectorValues& x) {
  double total_error = 0.;
  for(const GaussianFactor::shared_ptr& factor: fg)
    total_error += factor->error(x);
  return total_error;
}

/* ************************************************************************* */
TEST( SubgraphSolver, Parameters )
{
  LONGS_EQUAL(SubgraphSolverParameters::SILENT, kParameters.verbosity());
  LONGS_EQUAL(500, kParameters.maxIterations());
}

/* ************************************************************************* */
TEST( SubgraphSolver, splitFactorGraph )
{
  // Build a planar graph
  GaussianFactorGraph Ab;
  VectorValues xtrue;
  std::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b

  SubgraphBuilderParameters params;
  params.augmentationFactor = 0.0;
  SubgraphBuilder builder(params);
  auto subgraph = builder(Ab);
  EXPECT_LONGS_EQUAL(9, subgraph.size());

  GaussianFactorGraph Ab1, Ab2;
  std::tie(Ab1, Ab2) = splitFactorGraph(Ab, subgraph);
  EXPECT_LONGS_EQUAL(9, Ab1.size());
  EXPECT_LONGS_EQUAL(13, Ab2.size());
}

/* ************************************************************************* */
TEST( SubgraphSolver, constructor1 )
{
  // Build a planar graph
  GaussianFactorGraph Ab;
  VectorValues xtrue;
  std::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b

  // The first constructor just takes a factor graph (and kParameters)
  // and it will split the graph into A1 and A2, where A1 is a spanning tree
  SubgraphSolver solver(Ab, kParameters, kOrdering);
  VectorValues optimized = solver.optimize(); // does PCG optimization
  DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5);
}

/* ************************************************************************* */
TEST( SubgraphSolver, constructor2 )
{
  // Build a planar graph
  GaussianFactorGraph Ab;
  VectorValues xtrue;
  size_t N = 3;
  std::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b

  // Get the spanning tree
  GaussianFactorGraph Ab1, Ab2; // A1*x-b1 and A2*x-b2
  std::tie(Ab1, Ab2) = example::splitOffPlanarTree(N, Ab);

  // The second constructor takes two factor graphs, so the caller can specify
  // the preconditioner (Ab1) and the constraints that are left out (Ab2)
  SubgraphSolver solver(Ab1, Ab2, kParameters, kOrdering);
  VectorValues optimized = solver.optimize();
  DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5);
}

/* ************************************************************************* */
TEST( SubgraphSolver, constructor3 )
{
  // Build a planar graph
  GaussianFactorGraph Ab;
  VectorValues xtrue;
  size_t N = 3;
  std::tie(Ab, xtrue) = example::planarGraph(N); // A*x-b

  // Get the spanning tree and corresponding kOrdering
  GaussianFactorGraph Ab1, Ab2; // A1*x-b1 and A2*x-b2
  std::tie(Ab1, Ab2) = example::splitOffPlanarTree(N, Ab);

  // The caller solves |A1*x-b1|^2 == |R1*x-c1|^2, where R1 is square UT
  auto Rc1 = *Ab1.eliminateSequential();

  // The third constructor allows the caller to pass an already solved preconditioner Rc1_
  // as a Bayes net, in addition to the "loop closing constraints" Ab2, as before
  SubgraphSolver solver(Rc1, Ab2, kParameters);
  VectorValues optimized = solver.optimize();
  DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5);
}

/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */


/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2019, University of Stuttgart
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the University of Stuttgart nor the names
 *     of its contributors may be used to endorse or promote products
 *     derived from this software without specific prior written
 *     permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/* Author: Andreas Orthey */

#include "QuotientSpacePlanningCommon.h"
#include <ompl/base/spaces/RealVectorStateSpace.h>

#include <ompl/geometric/planners/quotientspace/QRRT.h>

#include <ompl/tools/benchmark/Benchmark.h>
#include <ompl/util/String.h>

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

const double edgeWidth = 0.1;
const unsigned int ndim = 10;
const double runtime_limit = 10;
const double memory_limit = 4096;
const int run_count = 10;

// Only states near some edges of a hypercube are valid. The valid edges form a
// narrow passage from (0,...,0) to (1,...,1). A state s is valid if there exists
// a k s.t. (a) 0<=s[k]<=1, (b) for all i<k s[i]<=edgeWidth, and (c) for all i>k
// s[i]>=1-edgewidth.
class HyperCubeValidityChecker : public ompl::base::StateValidityChecker
{
public:
    HyperCubeValidityChecker(const ompl::base::SpaceInformationPtr &si, int nDim)
      : ompl::base::StateValidityChecker(si), nDim_(nDim)
    {
    }

    bool isValid(const ompl::base::State *state) const override
    {
        const auto *s = static_cast<const ompl::base::RealVectorStateSpace::StateType *>(state);
        bool foundMaxDim = false;

        for (int i = nDim_ - 1; i >= 0; i--)
            if (!foundMaxDim)
            {
                if ((*s)[i] > edgeWidth)
                    foundMaxDim = true;
            }
            else if ((*s)[i] < (1. - edgeWidth))
                return false;
        return true;
    }

protected:
    int nDim_;
};

void addPlanner(ompl::tools::Benchmark &benchmark, const ompl::base::PlannerPtr &planner, double range)
{
    ompl::base::ParamSet &params = planner->params();
    if (params.hasParam(std::string("range")))
        params.setParam(std::string("range"), ompl::toString(range));
    benchmark.addPlanner(planner);
}

ob::PlannerPtr GetQRRT(ob::SpaceInformationPtr si, ob::ProblemDefinitionPtr pdef, unsigned int numLinks)
{
    // ompl::msg::setLogLevel(ompl::msg::LOG_DEV2);
    std::vector<ob::SpaceInformationPtr> si_vec;

    for (unsigned int k = 2; k < numLinks; k += 2)
    {
        OMPL_INFORM("Create QuotientSpace Chain with %d links.", k);

        auto spaceK(std::make_shared<ompl::base::RealVectorStateSpace>(k));
        ompl::base::RealVectorBounds bounds(k);
        bounds.setLow(0.);
        bounds.setHigh(1.);
        spaceK->setBounds(bounds);

        auto siK = std::make_shared<ob::SpaceInformation>(spaceK);
        siK->setStateValidityChecker(std::make_shared<HyperCubeValidityChecker>(siK, k));
        siK->setStateValidityCheckingResolution(0.001);

        spaceK->setup();
        si_vec.push_back(siK);
    }
    OMPL_INFORM("Add Original Chain with %d links.", numLinks);
    si_vec.push_back(si);

    auto planner = std::make_shared<og::QRRT>(si_vec);
    planner->setProblemDefinition(pdef);
    std::string qName = "QuotientSpaceRRT[" + std::to_string(si_vec.size()) + "lvl]";
    planner->setName(qName);
    return planner;
}

int main()
{
    double range = edgeWidth * 0.5;
    auto space(std::make_shared<ompl::base::RealVectorStateSpace>(ndim));
    ompl::base::RealVectorBounds bounds(ndim);
    ompl::geometric::SimpleSetup ss(space);
    ompl::base::ScopedState<> start(space), goal(space);

    bounds.setLow(0.);
    bounds.setHigh(1.);
    space->setBounds(bounds);
    ss.setStateValidityChecker(std::make_shared<HyperCubeValidityChecker>(ss.getSpaceInformation(), ndim));
    ss.getSpaceInformation()->setStateValidityCheckingResolution(0.001);
    for (unsigned int i = 0; i < ndim; ++i)
    {
        start[i] = 0.;
        goal[i] = 1.;
    }
    ss.setStartAndGoalStates(start, goal);

    ompl::tools::Benchmark::Request request(runtime_limit, memory_limit, run_count);
    ompl::tools::Benchmark b(ss, "HyperCube");
    b.addExperimentParameter("num_dims", "INTEGER", std::to_string(ndim));

    ob::SpaceInformationPtr si = ss.getSpaceInformation();

    ob::PlannerPtr quotientSpacePlanner = GetQRRT(ss.getSpaceInformation(), ss.getProblemDefinition(), ndim);
    addPlanner(b, quotientSpacePlanner, range);

    b.benchmark(request);
    b.saveResultsToFile(boost::str(boost::format("hypercube_%i.log") % ndim).c_str());

    printBenchmarkResults(b);

    return 0;
}


/*
   Copyright 2018 Simon Vogl <svogl@voxel.at>
                  Angel Merino-Sastre <amerino@voxel.at>

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

       http://www.apache.org/licenses/LICENSE-2.0

   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.
*/
#include <boost/log/core.hpp>
#include <boost/log/trivial.hpp>

#if OCV_VERSION_MAJOR >= 3
#  include <opencv2/imgproc.hpp>
#  include <opencv2/highgui.hpp>
#else
#  include <opencv2/imgproc/imgproc.hpp>
#  include <opencv2/highgui/highgui.hpp>
#endif

#include "toffy/smoothing/kalmanaverage.hpp"

#include <iostream>
#include <fstream>

#include <math.h>

using namespace std;
using namespace cv;
using namespace toffy::filters::smoothing;

std::size_t toffy::filters::smoothing::KalmanAverage::_filter_counter = 1;
const std::string toffy::filters::smoothing::KalmanAverage::id_name = "kalmanAverage";

KalmanAverage::KalmanAverage():
    Filter(KalmanAverage::id_name), _in_img("depth"), out_img("depth"),
    processNoiseCov(1e-5),
    measurementNoiseCov(1e-1),
    skipZeros(true)
{
}

KalmanAverage::~KalmanAverage() {}

/*
int KalmanAverage::loadConfig(const boost::property_tree::ptree& pt) {
    BOOST_LOG_TRIVIAL(debug) << __FUNCTION__ << "(const boost::property_tree::ptree& pt)";
    const boost::property_tree::ptree& tree = pt.get_child(this->type());

    loadGlobals(tree);

    updateConfig(tree);

    return true;
}
*/

void KalmanAverage::updateConfig(const boost::property_tree::ptree &pt) {
    BOOST_LOG_TRIVIAL(debug) << __FUNCTION__ <<  " " << id();

    using namespace boost::property_tree;

    Filter::updateConfig(pt);

    _in_img = pt.get<string>("inputs.img",_in_img);
    out_img = pt.get<string>("outputs.img",out_img);

     processNoiseCov = pt.get<float>("options.processNoiseCov", processNoiseCov);
     measurementNoiseCov = pt.get<float>("options.measurementNoiseCov", measurementNoiseCov);
     skipZeros = pt.get<bool>("options.skipZeros", skipZeros);
}

boost::property_tree::ptree KalmanAverage::getConfig() const {
    boost::property_tree::ptree pt;

    pt = Filter::getConfig();

    pt.put("inputs.img", _in_img);
    pt.put("outputs.img", out_img);

    pt.put("options.processNoiseCov", processNoiseCov);
    pt.put("options.measurementNoiseCov", measurementNoiseCov);
    pt.put("options.skipZeros", skipZeros);

    return pt;
}

bool KalmanAverage::filter(const toffy::Frame& in, toffy::Frame& out)
{
    BOOST_LOG_TRIVIAL(debug) << __FUNCTION__ <<  " " << id();
    boost::shared_ptr<cv::Mat> img;

    try {
	img = boost::any_cast<boost::shared_ptr<cv::Mat> >(in.getData(_in_img));
    } catch(const boost::bad_any_cast &) {
	BOOST_LOG_TRIVIAL(warning) <<
	    "Could not cast input " << _in_img <<
	    ", filter  " << id() <<" not applied.";
	return false;
    }
    float* imgptr;
    int vKFpos = 0;
    if (!vKF) {
	vKF.reset(new std::vector<cv::KalmanFilter >(img->size().area()));
	for(int row = 0; row < img->rows; ++row) {
	    imgptr = img->ptr<float>(row);

	    for(int col = 0; col < img->cols; ++col) {
		cv::KalmanFilter& kf = vKF->at(vKFpos);
		//vKFpos = ( ( (row+1)+((img->rows)*row) ) * (col+1) ) -1;
		kf.init(2,1,0);
		kf.transitionMatrix = (Mat_<float>(2, 2) << 1, 1, 0, 1);

		setIdentity(kf.measurementMatrix);
		setIdentity(kf.processNoiseCov, Scalar::all(1e-5));
		setIdentity(kf.measurementNoiseCov, Scalar::all(1e-1));
		setIdentity(kf.errorCovPost, Scalar::all(1));

		kf.statePost.at<float>(0) = imgptr[col];
		vKFpos++;
		//cout << "vKFpos: " << vKFpos << endl;
		//cout << "imgptr[col]: " << imgptr[col] << endl;
		//cout << "kf.statePost.at<float>(0): " << kf.statePost.at<float>(0) << endl;
	    }
	}
    } else {
	Mat measurement = Mat::zeros(1, 1, CV_32F);
	Mat estimated;
	for(int row = 0; row < img->rows; ++row) {
	    imgptr = img->ptr<float>(row);

	    for(int col = 0; col < img->cols; ++col) {
		cv::KalmanFilter& kf = vKF->at(vKFpos);

		if (std::numeric_limits<float>::quiet_NaN() == imgptr[col]
		    || ( skipZeros && imgptr[col] <= 0.0 ) ) {
		    imgptr[col] = imgptr[col]; // nothing to estimate..
		    vKFpos++;

		    //kf.statePost.at<float>(0) =imgptr[col] ;
		    continue;
		}
		// last state was nan..:
		if (  std::numeric_limits<float>::quiet_NaN() == kf.statePost.at<float>(0) ) {
		    // re-init state
		    setIdentity(kf.measurementMatrix);
		    setIdentity(kf.processNoiseCov, Scalar::all(1e-5));
		    setIdentity(kf.measurementNoiseCov, Scalar::all(1e-1));
		    setIdentity(kf.errorCovPost, Scalar::all(1));

		    kf.statePost.at<float>(0) = imgptr[col];
		}

		//vKFpos = ( ( (row+1)+((img->rows)*row) ) * (col+1) ) -1;
		kf.predict();

		//measurement = Mat::zeros(3, 1, CV_32F);
		measurement.at<float>(0) = imgptr[col];

		estimated = kf.correct(measurement);
		imgptr[col] = estimated.at<float>(0);
		vKFpos++;
	    }
	}
    }

    return true;
}


// 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


//==============================================================================
//         Copyright 2015 J.T. Lapreste
//
//          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_LINALG_FUNCTIONS_TIED_RSF2CSF_HPP_INCLUDED
#define NT2_LINALG_FUNCTIONS_TIED_RSF2CSF_HPP_INCLUDED

#include <nt2/linalg/functions/rsf2csf.hpp>
#include <boost/dispatch/attributes.hpp>
#include <nt2/core/container/table/table.hpp>
#include <nt2/include/functions/conj.hpp>
#include <nt2/include/functions/cons.hpp>
#include <nt2/include/functions/ctranspose.hpp>
#include <nt2/include/functions/dec.hpp>
#include <nt2/include/functions/hypot.hpp>
#include <nt2/include/functions/is_real.hpp>
#include <nt2/include/functions/mtimes.hpp>
#include <nt2/include/functions/multiplies.hpp>
#include <nt2/include/functions/nseig.hpp>
#include <nt2/include/functions/rec.hpp>
#include <nt2/sdk/meta/as_real.hpp>
#include <nt2/linalg/options.hpp>
#include <nt2/core/container/dsl/as_terminal.hpp>
#include <nt2/core/container/colon/colon.hpp>
#include <nt2/core/utility/of_size.hpp>
#include <nt2/core/utility/assign_swap.hpp>

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( rsf2csf_, tag::cpu_
                            , (A0)(A1)
                            , (scalar_<floating_<A0> >)
                              (scalar_<floating_<A1> >)
                            )
  {
    typedef typename meta::as_complex<A0>::type result_type;

    BOOST_FORCEINLINE result_type operator()(const A0& a0, const A1& ) const
    {
      BOOST_ASSERT_MSG(is_real(a0), "diagonal is not valid");
      return result_type(a0);
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( rsf2csf_, tag::cpu_
                            , (A0)(N0)(A1)(N1)
                            , ((node_<A0, nt2::tag::rsf2csf_
                                    , N0, nt2::container::domain
                                      >
                              ))
                              ((node_<A1, nt2::tag::tie_
                                    , N1, nt2::container::domain
                                     >
                              ))
                            )
  {
    typedef void  result_type;
    typedef typename boost::proto::result_of::child_c<A0&,0>::value_type    child0;
    typedef typename child0::value_type                                     type_t;
    typedef typename nt2::meta::as_real<type_t>::type                      rtype_t;
    typedef typename nt2::meta::as_complex<rtype_t>::type                  ctype_t;
    typedef nt2::memory::container<tag::table_, rtype_t, nt2::_2D> desired_semantic;

    BOOST_FORCEINLINE result_type operator()( A0& a0, A1& a1 ) const
    {
      eval(a0, a1, N0(), N1());
    }

    //==========================================================================
    /// INTERNAL ONLY - [v, w] = rsf2csf(cv, cw)
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<2> const&
              , boost::mpl::long_<2> const&
              ) const
    {
       auto& u = boost::proto::child_c<0>(a0);
       auto& t = boost::proto::child_c<1>(a0);
       container::table<ctype_t> cu = u;
       container::table<ctype_t> ct = t;

       std::size_t n = size(t, 2);
       for(std::size_t m = n; m >= 2; --m)
       {
         std::size_t dm = dec(m);
         if(t(m, dm))
         {
           auto k = _(dm, m);
           auto mu = nseig(t(k,k)) - t(m,m);
           rtype_t r = rec(hypot(mu(1), t(m,m-1)));
           ctype_t c = mu(1)*r;
           ctype_t s = ct(m,m-1)*r;
           container::table<ctype_t> g = cons<ctype_t>(of_size(2, 2), conj(c), -s, s, c);
           ct(k,_(m-1, n)) = mtimes(g, ct(k,_(m-1, n)));
           ct(_(1, m),k) = mtimes(ct(_(1, m),k), ctrans(g));
           cu(_,k) = mtimes(u(_,k), ctrans(g));
           ct(m,dm) = 0;
         }
       }
       assign_swap(boost::proto::child_c<0>(a1), cu);
       assign_swap(boost::proto::child_c<1>(a1), ct);
    }

  };
} }

#endif


#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <iostream>
#include <list>
#include <math.h>
#include <omp.h>
#include <sstream>
#include <string>
#include <vector>
#include <vtkCell.h>
#include <vtkCellData.h>
#include <vtkDoubleArray.h>
#include <vtkKdTree.h>
#include <vtkPointData.h>
#include <vtkPolyData.h>
#include <vtkPolyDataReader.h>
#include <vtkPolyDataWriter.h>
#include <vtkSmartPointer.h>
//#include "HelperFunctions.h"

using namespace std;
// using namespace OPS;

typedef Eigen::Vector3d Vector3d;

//! A struct to store a vtkIdType and an angle
struct neighbors {
  vtkIdType _id;
  double_t _angle;
  neighbors(vtkIdType i, double_t a) : _id(i), _angle(a) {}
  bool operator<(const neighbors &n) const { return _angle < n._angle; }
};

int main(int argc, char *argv[]) {
  if (argc != 5) {
    cout << "argc = " << argc << endl
         << "Usage: dualMesh baseName numStart numEnd numOPENMPThreads" << endl;
    return (0);
  }

  ////////////////////////////////////////////////////////////////////
  // Input section
  ////////////////////////////////////////////////////////////////////

  // read in vtk file
  string baseFileName = argv[1];
  size_t numStart = stoi(argv[2]);
  size_t numEnd = stoi(argv[3]);
  size_t numThreads = stoi(argv[4]);

  // Set the number of threads
  omp_set_num_threads(numThreads);

#pragma omp parallel for
  for (size_t bigI = numStart; bigI <= numEnd; bigI++) {
    stringstream sstm;
    string inputFileName, outFileName;
    sstm << baseFileName << "-" << bigI << ".vtk";
    inputFileName = sstm.str();
    sstm.str("");
    sstm.clear();

    auto reader = vtkSmartPointer<vtkPolyDataReader>::New();
    reader->SetFileName(inputFileName.c_str());
    reader->Update();
    vtkSmartPointer<vtkPolyData> inputMesh = reader->GetOutput();
    inputMesh->BuildLinks();
    size_t npts = inputMesh->GetNumberOfPoints();

    // get displacement vectors, if they exist
    string vectorName = "displacements";
    vtkSmartPointer<vtkDoubleArray> displacements =
        vtkDoubleArray::SafeDownCast(
            inputMesh->GetPointData()->GetVectors(vectorName.c_str()));

    // get vertex positions
    std::vector<Vector3d> points(npts, Vector3d::Zero());

    // Calculate centroid of each triangle while updating points vector
    auto newPts = vtkSmartPointer<vtkPoints>::New();
    vtkSmartPointer<vtkCellArray> cells = inputMesh->GetPolys();
    auto cellPointIds = vtkSmartPointer<vtkIdList>::New();
    cells->InitTraversal();
    while (cells->GetNextCell(cellPointIds)) {
      size_t numCellPoints = cellPointIds->GetNumberOfIds();
      Vector3d centroid(0.0, 0.0, 0.0);
      for (size_t i = 0; i < numCellPoints; i++) {
        vtkIdType currCellPoint = cellPointIds->GetId(i);
        inputMesh->GetPoint(currCellPoint, &points[currCellPoint][0]);
        if (displacements.GetPointer() != NULL) {
          Vector3d currDisp(0.0, 0.0, 0.0);
          displacements->GetTuple(currCellPoint, &currDisp[0]);
          points[currCellPoint] += currDisp;
        }
        centroid += points[currCellPoint];
      }
      centroid /= numCellPoints;
      newPts->InsertNextPoint(&centroid[0]);
    }

    // Prepare valence Cell Data array
    auto valence = vtkSmartPointer<vtkIntArray>::New();
    valence->SetName("Valence");
    valence->SetNumberOfComponents(1);

    // Prepare new cell array for polygons
    auto newPolys = vtkSmartPointer<vtkCellArray>::New();

    for (size_t a = 0; a < npts; a++) {

      auto currPolyPtIds = vtkSmartPointer<vtkIdList>::New();
      std::list<neighbors> currPoly;
      Vector3d vec0, vecj, currCross, axis, centroid(0.0, 0.0, 0.0);
      double vec0_norm, vecj_norm, sign, currSin, currCos, currAngle;

      inputMesh->GetPointCells(a, currPolyPtIds);
      size_t numCellPoints = currPolyPtIds->GetNumberOfIds();

      // Get coordinates of first cell's centroid
      vtkIdType currId = currPolyPtIds->GetId(0);
      newPts->GetPoint(currId, &centroid[0]);
      vec0 = (centroid - points[a]).normalized();
      neighbors pt0(currId, 0.0);
      currPoly.push_back(pt0);

      // For remaining centroids
      for (auto j = 1; j < numCellPoints; j++) {
        currId = currPolyPtIds->GetId(j);
        newPts->GetPoint(currId, &centroid[0]);
        vecj = (centroid - points[a]).normalized();
        currSin = (vec0.cross(vecj)).norm();
        axis = (vec0.cross(vecj)).normalized();
        sign = axis.dot(points[a]);
        currSin = (sign > 0.0) ? currSin : -1.0 * currSin;
        currCos = vec0.dot(vecj);
        currAngle = (180 / M_PI) * atan2(currSin, currCos);
        currAngle = (currAngle < 0) ? (360 + currAngle) : currAngle;
        neighbors ptj(currId, currAngle);
        currPoly.push_back(ptj);
      }

      // Sort the list of neigbors and make a polygon
      currPoly.sort();
      newPolys->InsertNextCell(numCellPoints);
      for (auto t = currPoly.begin(); t != currPoly.end(); ++t) {
        neighbors n = *t;
        newPolys->InsertCellPoint(n._id);
      }
      valence->InsertNextTuple1(numCellPoints);
    }

    // Assign points and polygons to a new polydata and write it out
    auto newPolyData = vtkSmartPointer<vtkPolyData>::New();
    auto writer = vtkSmartPointer<vtkPolyDataWriter>::New();

    newPolyData->SetPoints(newPts);
    newPolyData->SetPolys(newPolys);
    newPolyData->GetCellData()->AddArray(valence);
    writer->SetInputData(newPolyData);
    sstm << baseFileName << "-dual-" << bigI << ".vtk";
    outFileName = sstm.str();
    sstm.str("");
    sstm.clear();
    writer->SetFileName(outFileName.c_str());
    writer->Write();
  }
  return 0;
}


#pragma once

#include <Eigen/Core>

#include <vector>
#include <array>
#include <complex>

#include "Spirit_Defines.h"

// Dynamic Eigen typedefs
using VectorX    = Eigen::Matrix<scalar, -1,  1>;
using RowVectorX = Eigen::Matrix<scalar,  1, -1>;
using MatrixX    = Eigen::Matrix<scalar, -1, -1>;

// 3D Eigen typedefs
using Vector3    = Eigen::Matrix<scalar, 3, 1>;
using RowVector3 = Eigen::Matrix<scalar, 1, 3>;
using Matrix3    = Eigen::Matrix<scalar, 3, 3>;

using Vector3c   = Eigen::Matrix<std::complex<scalar>, 3, 1>;
using Matrix3c   = Eigen::Matrix<std::complex<scalar>, 3, 3>;

// Different definitions for regular C++ and CUDA
#ifdef SPIRIT_USE_CUDA
    // The general field, using the managed allocator
    #include "Managed_Allocator.hpp"
    template<typename T>
    using field = std::vector<T, managed_allocator<T>>;

    struct Site
    {
        // Basis index
        int i;
        // Translations of the basis cell
        int translations[3];
    };
    struct Pair
    {
        // Basis indices of first and second atom of pair
        int i, j;
        // Translations of the basis cell of second atom of pair
        int translations[3];
    };
    struct Triplet
    {
        int i, j, k;
        int d_j[3], d_k[3];
    };
    struct Quadruplet
    {
        int i, j, k, l;
        int d_j[3], d_k[3], d_l[3];
    };
#else
    // The general field
    template<typename T>
    using field = std::vector<T>;

    struct Site
    {
        // Basis index
        int i;
        // Translations of the basis cell
        std::array<int,3> translations;
    };
    struct Pair
    {
        int i, j;
        std::array<int,3> translations;
    };
    struct Triplet
    {
        int i, j, k;
        std::array<int,3> d_j, d_k;
    };
    struct Quadruplet
    {
        int i, j, k, l;
        std::array<int,3> d_j, d_k, d_l;
    };

    // Definition for OpenMP reduction operation using Vector3's
    #pragma omp declare reduction (+: Vector3: omp_out=omp_out+omp_in)\
        initializer(omp_priv=Vector3::Zero())
#endif

struct Neighbour : Pair
{
    // Shell index
    int idx_shell;
};

// Important fields
using intfield    = field<int>;
using scalarfield = field<scalar>;
using vectorfield = field<Vector3>;

// Additional fields
using pairfield       = field<Pair>;
using tripletfield    = field<Triplet>;
using quadrupletfield = field<Quadruplet>;
using neighbourfield  = field<Neighbour>;


#define BOOST_TEST_MODULE
#include <boost/test/unit_test.hpp>
#include <core/util/test_macros.hpp>
#include <core/storage/sgraph_data/sgraph.hpp>
#include <core/storage/sgraph_data/sgraph_engine.hpp>

#include "sgraph_test_util.hpp"
#include "sgraph_check_degree_count.hpp"
#include "sgraph_check_pagerank.hpp"

using namespace turi;

// Implement degree count function using sgraph_engine
std::vector<std::pair<flexible_type, flexible_type>> degree_count_fn (
  sgraph& g, sgraph::edge_direction dir) {

  sgraph_compute::sgraph_engine<flexible_type> ga;
  typedef sgraph_compute::sgraph_engine<flexible_type>::graph_data_type graph_data_type;
  typedef sgraph::edge_direction edge_direction;
  std::vector<std::shared_ptr<sarray<flexible_type>>>
      gather_results = ga.gather(g,
                                 [](const graph_data_type& center,
                                    const graph_data_type& edge,
                                    const graph_data_type& other,
                                    edge_direction edgedir,
                                    flexible_type& combiner) {
                                   combiner = combiner + 1;
                                 },
                                 flexible_type(0),
                                 dir);
  std::vector<std::shared_ptr<sarray<flexible_type>>> vertex_ids
      = g.fetch_vertex_data_field(sgraph::VID_COLUMN_NAME);

  TS_ASSERT_EQUALS(gather_results.size(), vertex_ids.size());
  std::vector<std::pair<flexible_type, flexible_type>> ret;

  for (size_t i = 0;i < gather_results.size(); ++i) {
    std::vector<flexible_type> degree_vec;
    std::vector<flexible_type> id_vec;
    gather_results[i]->get_reader()->read_rows(0, g.num_vertices(), degree_vec);
    vertex_ids[i]->get_reader()->read_rows(0, g.num_vertices(), id_vec);
    TS_ASSERT_EQUALS(degree_vec.size(), id_vec.size());
    for (size_t j = 0; j < degree_vec.size(); ++j) {
      ret.push_back({id_vec[j], degree_vec[j]});
    }
  }
  return ret;
}

// Implement degree count function using sgraph_engine
void pagerank_fn(sgraph& g,  size_t num_iterations) {
  sgraph_compute::sgraph_engine<flexible_type> ga;
  typedef sgraph_compute::sgraph_engine<flexible_type>::graph_data_type graph_data_type;
  typedef sgraph::edge_direction edge_direction;
  // count the outgoing degree
  std::vector<std::shared_ptr<sarray<flexible_type>>> vertex_combine = ga.gather(g,
                                                      [](const graph_data_type& center,
                                                         const graph_data_type& edge,
                                                         const graph_data_type& other,
                                                         edge_direction edgedir,
                                                         flexible_type& combiner) {
                                                      combiner = combiner + 1;
                                                      },
                                                      flexible_type(0),
                                                      edge_direction::OUT_EDGE);
  // merge the outgoing degree to graph
  std::vector<sframe>& vdata = g.vertex_group();
  for (size_t i = 0; i < g.get_num_partitions(); ++i) {
    ASSERT_LT(i, vdata.size());
    ASSERT_LT(i, vertex_combine.size());
    vdata[i] = vdata[i].add_column(vertex_combine[i], "__out_degree__");
  }

  size_t degree_idx = vdata[0].column_index("__out_degree__");
  size_t data_idx = vdata[0].column_index("vdata");

  // now we compute the pagerank
  for (size_t iter = 0; iter < num_iterations; ++iter) {
    vertex_combine = ga.gather(g,
        [=](const graph_data_type& center,
            const graph_data_type& edge,
            const graph_data_type& other,
            edge_direction edgedir,
            flexible_type& combiner) {
           combiner = combiner + 0.85 * (other[data_idx] / other[degree_idx]);
        },
        flexible_type(0.15),
        edge_direction::IN_EDGE);
    for (size_t i = 0; i < g.get_num_partitions(); ++i) {
      vdata[i] = vdata[i].replace_column(vertex_combine[i], "vdata");
    }
    // g.get_vertices().debug_print();
  }
}

struct sgraph_engine_test {

public:
 void test_degree_count() {
   check_degree_count(degree_count_fn);
 }

 void test_pagerank() {
   check_pagerank(pagerank_fn);
 }

};

BOOST_FIXTURE_TEST_SUITE(_sgraph_engine_test, sgraph_engine_test)
BOOST_AUTO_TEST_CASE(test_degree_count) {
  sgraph_engine_test::test_degree_count();
}
BOOST_AUTO_TEST_CASE(test_pagerank) {
  sgraph_engine_test::test_pagerank();
}
BOOST_AUTO_TEST_SUITE_END()