/** Computes the stable elementary reflector H in a stable way such that:

  * \f$ H *this = [ beta 0 ... 0]^T \f$

  * where the transformation H is:

  * \f$ H = I - tau v v^*\f$

  * and the vector v is:

  * \f$ v^T = [1 essential^T] \f$

  *

  * \note This routine provides, not only a stable elementary reflector,

  * but  a compatible reflector with Lapack routines.

  * For instance here, beta is ensured to be greater than zero.

  *

  * On output:

  * \param essential the essential part of the vector \c v

  * \param tau the scaling factor of the Householder transformation

  * \param beta ( > 0) the result of H * \c *this

  *

  * \sa MatrixBase::makeHouseholder(), MatrixBase::makeHouseholderInPlace(),

  * MatrixBase::applyHouseholderOnTheLeft(), MatrixBase::applyHouseholderOnTheRight()

  */

template<typename Derived>

void MatrixBase<Derived>::makeStableHouseholderInPlace(Scalar& tau, RealScalar& beta)

{

  VectorBlock<Derived, internal::decrement_size<Base::SizeAtCompileTime>::ret> essentialPart(derived(), 1, size()-1);

  makeStableHouseholder(essentialPart, tau, beta);

}

/** Computes the stable elementary reflector H in a stable way such that:

  * \f$ H *this = [ beta 0 ... 0]^T \f$

  * where the transformation H is:

  * \f$ H = I - tau v v^*\f$

  * and the vector v is:

  * \f$ v^T = [1 essential^T] \f$

  *

  * \note This routine provides, not only a stable elementary reflector,

  * but  a compatible reflector with Lapack routines.

  * For instance here, beta is ensured to be greater than zero.

  *

  * On output:

  * \param essential the essential part of the vector \c v

  * \param tau the scaling factor of the Householder transformation

  * \param beta ( > 0) the result of H * \c *this

  *

  * \sa MatrixBase::makeHouseholder(), MatrixBase::makeHouseholderInPlace(),

  * MatrixBase::applyHouseholderOnTheLeft(), MatrixBase::applyHouseholderOnTheRight()

  */

template<typename Derived>

template<typename EssentialPart>

void MatrixBase<Derived>::makeStableHouseholder(

  EssentialPart& essential,

  Scalar& tau,

  RealScalar& beta) const

{

  using std::sqrt;

  using numext::conj;

  EIGEN_STATIC_ASSERT_VECTOR_ONLY(EssentialPart)

  VectorBlock<const Derived, EssentialPart::SizeAtCompileTime> tail(derived(), 1, size()-1);

  RealScalar tailNorm = size()==1 ? RealScalar(0) : tail.blueNorm();

  Scalar c0 = coeff(0);

  if(tailNorm == RealScalar(0) && numext::imag(c0)==RealScalar(0))

  {

    beta = numext::real(c0);

    if(beta >= 0)

      tau = RealScalar(0);

    else

    {

      tau = 2;

      essential.setZero();

      beta = -beta;

    }

  }

  else

  {

    // Compute beta  = sqrt(c0*c0 + tailNorm * tailNorm) in a stable way

    RealScalar c0_abs = (std::abs)(c0);

    RealScalar max_abs = (std::max)(c0_abs, tailNorm);

    RealScalar min_abs = (std::min)(c0_abs, tailNorm);

    if(min_abs == 0) beta = max_abs;

    else beta = max_abs * sqrt(1. + (min_abs / max_abs)*(min_abs / max_abs));

    if (numext::real(c0) < RealScalar(0)) beta = -beta;

    if(beta < 0)

    {

      beta = -beta;

      tau = numext::conj(beta-c0)/beta;

      essential = tail / (c0-beta);

     }

    else

    {

      RealScalar gamma = numext::real(c0) + beta;

      RealScalar delta = (numext::imag(c0)*(numext::imag(c0)/gamma) + tailNorm * (tailNorm/gamma));

      if(NumTraits<Scalar>::IsComplex)

      {

//        std::complex<RealScalar> delta_out(delta, numext::imag(c0));

        Scalar delta_out = c0;

        numext::real_ref(delta_out) = delta;

        tau = - delta_out/beta;

        essential = tail/(delta_out);

      }

      else

      {

        tau = delta / beta;

        essential = tail / (-delta);

      }

    }

  }

}

template<typename MatrixType, typename HCoeffsType, typename RealRowVectorType, typename IntRowVectorType, typename PermutationType, typename RowVectorType>

void colpivhouseholder_qr_inplace(MatrixType& qr, HCoeffsType& hCoeffs, RealRowVectorType& colSqNorms, 

                                  IntRowVectorType& colsTranspositions, PermutationType& colsPermutation,

                                  RowVectorType& temp, typename MatrixType::RealScalar& maxpivot, 

                                  typename MatrixType::Index& nonzero_pivots, typename MatrixType::Index& det_pq)

{

  typedef typename MatrixType::Index Index;

  typedef typename MatrixType::Scalar Scalar;

  typedef typename MatrixType::RealScalar RealScalar;

  typedef typename PermutationType::Index PermIndexType;

  using std::abs;

  Index rows = qr.rows();

  Index cols = qr.cols();

  Index size = qr.diagonalSize();

  // the column permutation is stored as int indices, so just to be sure:

  eigen_assert(cols<=NumTraits<int>::highest());

  hCoeffs.resize(size);

  temp.resize(cols); //FIXME Why is this not a local variable

  colsTranspositions.resize(qr.cols());

  Index number_of_transpositions = 0;

  colSqNorms.resize(cols);

  for(Index k = 0; k < cols; ++k)

    colSqNorms.coeffRef(k) = qr.col(k).squaredNorm();

  RealScalar threshold_helper = colSqNorms.maxCoeff() * numext::abs2(NumTraits<Scalar>::epsilon()) / RealScalar(rows);

  nonzero_pivots = size; // the generic case is that in which all pivots are nonzero (invertible case)

  maxpivot = RealScalar(0);

  for(Index k = 0; k < size; ++k)

  {

    // first, we look up in our table colSqNorms which column has the biggest squared norm

    Index biggest_col_index;

    RealScalar biggest_col_sq_norm = colSqNorms.tail(cols-k).maxCoeff(&biggest_col_index);

    biggest_col_index += k;

    // since our table colSqNorms accumulates imprecision at every step, we must now recompute

    // the actual squared norm of the selected column.

    // Note that not doing so does result in solve() sometimes returning inf/nan values

    // when running the unit test with 1000 repetitions.

    biggest_col_sq_norm = qr.col(biggest_col_index).tail(rows-k).squaredNorm();

    // we store that back into our table: it can't hurt to correct our table.

    colSqNorms.coeffRef(biggest_col_index) = biggest_col_sq_norm;

    // if the current biggest column is smaller than epsilon times the initial biggest column,

    // terminate to avoid generating nan/inf values.

    // Note that here, if we test instead for "biggest == 0", we get a failure every 1000 (or so)

    // repetitions of the unit test, with the result of solve() filled with large values of the order

    // of 1/(size*epsilon).

    if(biggest_col_sq_norm < threshold_helper * RealScalar(rows-k))

    {

      nonzero_pivots = k;

      hCoeffs.tail(size-k).setZero();

      qr.bottomRightCorner(rows-k,cols-k)

          .template triangularView<StrictlyLower>()

          .setZero();

      break;

    }

    // apply the transposition to the columns

    colsTranspositions.coeffRef(k) = biggest_col_index;

    if(k != biggest_col_index) {

      qr.col(k).swap(qr.col(biggest_col_index));

      std::swap(colSqNorms.coeffRef(k), colSqNorms.coeffRef(biggest_col_index));

      ++number_of_transpositions;

    }

    // generate the householder vector, store it below the diagonal

    RealScalar beta;

    qr.col(k).tail(rows-k).makeHouseholderInPlace(hCoeffs.coeffRef(k), beta);

//    qr.col(k).tail(rows-k).makeStableHouseholderInPlace(hCoeffs.coeffRef(k), beta);

    // apply the householder transformation to the diagonal coefficient

    qr.coeffRef(k,k) = beta;

    // remember the maximum absolute value of diagonal coefficients

    if(abs(beta) > maxpivot) maxpivot = abs(beta);

    // apply the householder transformation

    qr.bottomRightCorner(rows-k, cols-k-1)

        .applyHouseholderOnTheLeft(qr.col(k).tail(rows-k-1), hCoeffs.coeffRef(k), &temp.coeffRef(k+1));

    // update our table of squared norms of the columns

    colSqNorms.tail(cols-k-1) -= qr.row(k).tail(cols-k-1).cwiseAbs2();

  }

  colsPermutation.setIdentity(PermIndexType(cols));

  for(PermIndexType k = 0; k < nonzero_pivots; ++k)

    colsPermutation.applyTranspositionOnTheRight(k, PermIndexType(colsTranspositions.coeff(k)));

  det_pq = (number_of_transpositions%2) ? -1 : 1;

}

#include "qr.cpp"

#include "qr.cpp"

#include "qr.cpp"

// Copyright (C) 2013 Désiré Nuentsa-Wakam <desire.nuentsa_wakam@inria.fr>

// computes all the eigenvalues, and optionally, the eigenvectors of a real generalized symmetric-definite eigenproblem, of the form A*x=(lambda)*B*x, A*Bx=(lambda)*x, or B*A*x=(lambda)*x

EIGEN_LAPACK_FUNC(sygv, (int *itype, char *jobz, char *uplo, int *n, Scalar *a, int *lda, Scalar *b, int *ldb, Scalar *w, Scalar *work, int *lwork, int *info))

{

  bool query_size = (*lwork == -1);

  *info = 0;

  if (*itype < 1 || *itype > 3) *info = -1;

  else if (!(*jobz=='V' || *jobz == 'N')) *info = -2;

  else if (UPLO(*uplo)==INVALID) *info = -3;

  else if (*n < 0) *info = -4;

  else if (*lda == (std::max)(1, *n)) *info = -6;

  else if (*ldb == (std::max)(1, *n)) *info = -8;

  // TODO Estimate workspace and return if asked by query_size

  if (*info != 0)

  {

    int e = -*info;

    return xerbla_(SCALAR_SUFFIX_UP"SYGV", &e, 5);

  }

  if(*n == 0) return 0;

  // Call the GeneralizedSelfAdjointEigenSolver class

  PlainMatrixType matA(*n, *n), matB(*n, *n);

  int option;

  if (*jobz=='V' || *jobz=='v') option = ComputeEigenvectors;

  if (*itype == 1) option |= Ax_lBx;

  else if (*itype == 2) option |= ABx_lx;

  else option |= BAx_lx;

  if(UPLO(*uplo)==UP) matA = matrix(a,*n,*n,*lda).adjoint();

  else                matA = matrix(a,*n,*n,*lda);

  if(UPLO(*uplo)==UP) matB = matrix(b,*n,*n,*ldb).adjoint();

  else                matB = matrix(b,*n,*n,*ldb);

  GeneralizedSelfAdjointEigenSolver<PlainMatrixType> es(matA, matB, option);

  if(es.info() == NoConvergence)

  {

    vector(w,*n).setZero();

    if((option&ComputeEigenvectors) == ComputeEigenvectors)

      matrix(a, *n,*n, *lda).setIdentity();

    //FIXME Set the correct return code in *info

    return 0;

  }

  // Get the eigenvalues

  vector(w, *n) = es.eigenvalues();

  //Get the  eigenvectors

  matrix(a, *n, *n, *lda) = es.eigenvectors();

  return 0;

}

// This file is part of Eigen, a lightweight C++ template library

// for linear algebra.

//

// Copyright (C) 2013 Desire Nuentsa <desire.nuentsa_wakam@inria.fr>

//

// 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 "lapack_common.h"

#include <Eigen/QR>

// computes a QR factorization of a general M-by-N matrix A

EIGEN_LAPACK_FUNC(geqrf, (int *m, int *n, Scalar *a, int *lda, Scalar *tau, Scalar *work,

                          int *lwork, int *info))

{

  bool query_size = (*lwork == -1);

  *info = 0;

  if(*m < 0) *info = -1;

  else if(*n < 0) *info = -2;

  else if(*lda < (std::max)(1

      , *m)) *info = -4;

  else if((*lwork < (std::max)(1, *n)) && !query_size) *info = -7;

  if (*info != 0)

  {

    int e = -*info;

    return xerbla_(SCALAR_SUFFIX_UP"GEQRF", &e, 6);

  }

//  if (query_size)

//  {

//    *lwork = 0;

//    return 0;

//  }

  if (*m == 0 || *n == 0)

  {

    work[0] = 1;

    return 0;

  }

  MatrixType mat(a, *m, *n, OuterStride<>(*lda));

  CompactVectorType hcoeffs(tau, (std::min)(*m,*n));

  householder_qr_inplace_blocked(mat, hcoeffs, 32, work);

  hcoeffs = hcoeffs.conjugate();

  return 0;

}

 /* Compute the QR factorization with column pivoting */

 /* FIXME xgeqpf is deprecated in LAPACK, We should define xgeqp3 instead

  * However, the implementation of column pivoting QR in Eigen is more similar to xgeqpf than xgeqp3, in terms of performance (BLAS3 operations)

  */

 EIGEN_LAPACK_FUNC(geqpf, (int *m, int *n, Scalar *a, int *lda, int *jpvt, Scalar *tau,

                           Scalar *work, int *info))

 {

   *info = 0;

   if(*m < 0) *info = -1;

   else if(*n < 0) *info = -2;

   else if(*lda < (std::max)(1, *m)) *info = -4;

   if (*info != 0)

   {

     int e = -*info;

     return xerbla_(SCALAR_SUFFIX_UP"GEQPF", &e, 6);

   }

   if((std::min)(*m, *n) == 0) return 0;

   MatrixType qr(a, *m, *n, OuterStride<>(*lda));

   CompactVectorType hcoeffs(tau, (std::min)(*m, *n));

   Matrix<RealScalar, Dynamic, 1> colSqNorms;

   Matrix<int, Dynamic, 1> colsTranspositions;

   CompactVectorType temp(work, *n);

   RealScalar maxpivot;

   typename MatrixType::Index nonzero_pivots, det_pq;

   //FIXME Find a way to deal with 'free' columns specified in jpvt.

  PermutationMatrix<Dynamic, Dynamic, int> colsPerm(*n);

  // Call ColPivHouseholder kernel

   colpivhouseholder_qr_inplace(qr, hcoeffs, colSqNorms, colsTranspositions,

                                 colsPerm, temp, maxpivot, nonzero_pivots, det_pq);

   hcoeffs = hcoeffs.conjugate();

   for(int i = 0; i < *n; i++) jpvt[i] = colsPerm.indices()(i)+1;

   return 0;

 }

 template<typename T>

 int apply_reflectors(const char *method, char *side, char *trans, int *m, int *n, int *k,

                      T *a, int *lda, T *tau, T *c, int *ldc, T *work, int *lwork, int *info)

 {

   *info = 0;

   bool query_size = (*lwork==-1);

   int nq; // order of Q

   int nw; // minimum dimension of Work

   if(*side == 'L')

   {

     nq = *m;

     nw = *n;

   } else {

     nq = *n;

     nw = *m;

   }

   if( (*side != 'L') && (*side != 'R') ) *info = -1;

   else if( (*trans != 'N') && (*trans != 'T') && (*trans != 'C') ) *info = -2;

   else if (*m < 0) *info = -3;

   else if (*n < 0) *info = -4;

   else if (*k < 0 || *k > nq) *info = -5;

   else if (*lda < (std::max)(1, nq)) *info = -7;

   else if (*ldc < (std::max)(1, *m)) *info = -10;

   else if (*lwork < (std::max)(1, nw) && !query_size) *info = -12;

   if(*info != 0)

   {

     int e = -*info;

     return xerbla_(method, &e, 6);

   }

   if(query_size) return 0;

   if ( *m == 0 || *n == 0 || *k == 0)

   {

     work[0] = 1;

     return 0;

   }

   //Map input arrays to Eigen matrices

   //NOTE Here, k should be the the number of nonzero pivots as returned by xgeqpf

   int row = (*side == 'L') ? *m : *n;

   Map<Matrix<T,Dynamic,Dynamic,ColMajor>, 0, OuterStride<> > qr(a, row, *k, OuterStride<>(*lda));

   Map<Matrix<T,Dynamic,Dynamic,ColMajor>, 0, OuterStride<> > dst(c, *m, *n, OuterStride<>(*ldc));

   Map<Matrix<T,Dynamic,1> > hcoeffs(tau, *k);

   Matrix<T,Dynamic,Dynamic,ColMajor> matrixQ(*m, *n);

   matrixQ.setIdentity();

   matrixQ = householderSequence(qr, hcoeffs);

   // Apply the Householder reflectors

   if (*side == 'L')

   {

     if(*trans == 'N') dst = matrixQ * dst;

     //dst.applyOnTheLeft(householderSequence(qr, hcoeffs));

     else if(*trans =='T') dst = matrixQ.transpose() * dst;

     //dst.applyOnTheLeft(householderSequence(qr, hcoeffs).transpose());

     else  dst = matrixQ.adjoint() * dst;

       //dst.applyOnTheLeft(householderSequence(qr, hcoeffs.conjugate())/*.adjoint()*/);

   }

   else

   {

     if(*trans == 'N')  dst = dst * matrixQ;

       //dst.applyOnTheRight(householderSequence(qr, hcoeffs));

     else if(*trans =='T') dst = dst * matrixQ.transpose();

     //dst.applyOnTheRight(householderSequence(qr, hcoeffs).transpose());

     else dst = dst * matrixQ.adjoint();

     //dst.applyOnTheRight(householderSequence(qr, hcoeffs.conjugate())/*.adjoint()*/);

   }

   return 0;

 }

 /* Apply the product of k elementary reflectors to a real matrix

  */

 EIGEN_LAPACK_FUNC(ormqr, (char *side, char *trans, int *m, int *n, int *k, RealScalar *a,

                           int *lda, RealScalar *tau, RealScalar *c, int *ldc, RealScalar *work,

                           int *lwork, int *info))

 {

   int err = apply_reflectors(SCALAR_SUFFIX_UP"ORMQR", side, trans, m, n, k, a, lda, tau, c, ldc, work, lwork, info);

   return err;

 }

 /* Apply the product of k elementary reflectors to a complex matrix */

 EIGEN_LAPACK_FUNC(unmqr, (char *side, char *trans, int *m, int *n, int *k, Complex *a,

                           int *lda, Complex *tau, Complex *c, int *ldc, Complex *work,

                           int *lwork, int *info))

 {

   int err =  apply_reflectors(SCALAR_SUFFIX_UP"UNMQR", side, trans, m, n, k, a, lda, tau, c, ldc,

                    work, lwork, info);

   return err;

 }

template<typename T>

int generate_Q(const char *method, int *m, int *n, int *k, T *a, int *lda, T *tau,

                          T *work, int *lwork, int *info)

{

  bool query_size = (*lwork == -1);

  *info = 0;

  if (*m < 0) *info = -1;

  else if (*n < 0 || *n > *m) *info = -2;

  else if (*k < 0 || *k > *n) *info = -3;

  else if (*lda < (std::max)(1, *m)) *info = -5;

  else if (*lwork < (std::max)(1, *n) && !query_size) *info = -8;

  if (*info != 0)

  {

    int e = -*info;

    return xerbla_(method, &e, 6);

  }

  if (query_size)

  {

    work[0] = 0;

    return 0;

  }

  if (*n <= 0)

  {

    work[0] = 1;

    return 0;

  }

  Map<Matrix<T,Dynamic,Dynamic,ColMajor>, 0, OuterStride<> > qr(a, *m, *n, OuterStride<>(*lda));

  Matrix<T,Dynamic, Dynamic> tA(*m, *k);

  tA = matrix(a, *m, *k, *lda);

  Map<Matrix<T,Dynamic,1> > hcoeffs(tau, *k);

  qr.setIdentity();

  qr = householderSequence(tA, hcoeffs) * qr;

  return 0;

}

/* Generates a m-by-n real matrix Q from the k elementary reflectors */

EIGEN_LAPACK_FUNC(orgqr, (int *m, int *n, int *k, RealScalar *a, int *lda, RealScalar *tau,

                          RealScalar *work, int *lwork, int *info))

{

  int err =  generate_Q(SCALAR_SUFFIX_UP"ORGQR", m, n, k, a, lda, tau, work, lwork, info);

  return err;

}

/* Generates a m-by-n complex matrix Q from the k elementary reflectors */

EIGEN_LAPACK_FUNC(ungqr, (int *m, int *n, int *k, Complex *a, int *lda, Complex *tau,

                          Complex *work, int *lwork, int *info))

{

  int err =  generate_Q(SCALAR_SUFFIX_UP"UNGQR", m, n, k, a, lda, tau, work, lwork, info);

  return err;

}

#include "qr.cpp"