Lapack.cpp

/* 
This file is part of the BIAS library (Basic ImageAlgorithmS).

Copyright (C) 2003-2009    (see file CONTACT for details)
  Multimediale Systeme der Informationsverarbeitung
  Institut fuer Informatik
  Christian-Albrechts-Universitaet Kiel


BIAS is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation; either version 2.1 of the License, or
(at your option) any later version.

BIAS 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 Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public License
along with BIAS; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/


#include "Lapack.hh"
#include <Base/Math/tnt/fortran.h>
#include <Base/Math/tnt/vec.h>
#include <Base/Math/tnt/fmat.h>

#include <Base/Common/CompareFloatingPoint.hh>

using namespace std;

#ifdef BIAS_DEBUG
#include "SVD.hh"
#endif

#define F77_DGEEV   dgeev_
#define F77_DGELS   dgels_
#define F77_DGESV   dgesv_
#define F77_DGESVD  dgesvd_
#define F77_DGGGLM  dggglm_
#define F77_DSPEV   dspev_
#define F77_DSYEV   dsyev_
#define F77_DSYEVD  dsyevd_
#define F77_SGGEV   sggev_

#ifdef WIN32
#  define BIAS_LPK_INT long int
#  define BIAS_LPK_CONST_INT const long int
#else // WIN32
# ifdef __APPLE__
#  define BIAS_LPK_INT __CLPK_integer 
#  define BIAS_LPK_CONST_INT __CLPK_integer 
# else // __APPLE__
#  define BIAS_LPK_INT Fortran_integer
#  define BIAS_LPK_CONST_INT const Fortran_integer
# endif // __APPLE__
#endif // WIN32


#ifdef WIN32
// Windows
#  include "MathAlgo/minpack/win32_f2c.h"
#  include <blas.h>

// JW: VCLapack header.
// "small" conflicts with rpcndr.h:144 as define to char (JW)
#  pragma warning(push)
#  pragma warning (disable: 4518)
#  ifdef small 
#  pragma warning(disable: 4005)
#    define small smallUnique1852_
#    include "lapack.h"
#    define small char
#  else // small
#    include "lapack.h"
#  endif // small 
#  pragma warning(pop)
#else // WIN32
// Unix: Fortran Lapack, not CLapack

# ifdef __APPLE__
#  include <Accelerate/Accelerate.h> // for lapack
# else // __APPLE__

extern "C"
{
  // linear equations (general) using LU factorizaiton
  void F77_DGESV(cfi_ N, cfi_ nrhs, fda_ A, cfi_ lda,
                 fia_ ipiv, fda_ b, cfi_ ldb, fi_ info);

  // solve linear least squares using QR or LU factorization
  void F77_DGELS(cfch_ trans, cfi_ M, cfi_ N, cfi_ nrhs, fda_ A, cfi_ lda, 
                 fda_ B, cfi_ ldb, fda_ work, cfi_ lwork, fi_ info);

  // solve weighted linear least squares
  void F77_DGGGLM(cfi_ N, cfi_ M, cfi_ P, fda_ A, cfi_ lda, fda_ B, cfi_ ldb,
                  fda_ b, fda_ x, fda_ y, fda_ work, cfi_ lwork, fi_ info);

  // solve symmetric eigenvalues
  void F77_DSYEV(cfch_ jobz, cfch_ uplo, cfi_ N, fda_ A, cfi_ lda, 
                 fda_ W, fda_ work, cfi_ lwork, fi_ info);

  // solve symmetric eigenvalues and vectors
  void F77_DSYEVD(cfch_ jobz, cfch_ uplo, cfi_ N, fda_ A, cfi_ lda, 
                  fda_ W, fda_ work, cfi_ lwork, fia_ iwork, cfi_ liwork, fi_ info);

  // solve symmetric eigenvalues (packed matrix storage)
  void F77_DSPEV(cfch_ jobz, cfch_ uplo, cfi_ N, fda_ A, fda_ W, 
                 fda_ Z, cfi_ ldz, fda_ work, fi_ info);

  // solve unsymmetric eigenvalues
  void F77_DGEEV(cfch_ jobvl, cfch_ jobvr, cfi_ N, fda_ A, cfi_ lda,
                 fda_ wr, fda_ wi, fda_ vl, cfi_ ldvl, fda_ vr, 
                 cfi_ ldvr, fda_ work, cfi_ lwork, fi_ info);
  
  /**
     [see fortran.h for typedefs]
     SUBROUTINE DGESVD( JOBU, JOBVT, M, N, A, LDA, S, U, LDU, VT, LDVT, 
     WORK, LWORK, INFO )

     Scalar Arguments:
     CHARACTER          JOBU, JOBVT
     INTEGER            INFO, LDA, LDU, LDVT, LWORK, M, N
 
     Array Arguments:
     DOUBLE PRECISION   A( LDA, * ), S( * ), U( LDU, * ),  VT( LDVT, * ), WORK( * )
 
     Purpose
     =======
 
     DGESVD computes the singular value decomposition (SVD) of a real
     M-by-N matrix A, optionally computing the left and/or right singular
     vectors. The SVD is written
 
     A = U * SIGMA * transpose(V)

     where SIGMA is an M-by-N matrix which is zero except for its
     min(m,n) diagonal elements, U is an M-by-M orthogonal matrix, and
     V is an N-by-N orthogonal matrix.  The diagonal elements of SIGMA
     are the singular values of A; they are real and non-negative, and
     are returned in descending order.  The first min(m,n) columns of
     U and V are the left and right singular vectors of A.
 
     Note that the routine returns V**T, not V.
     (JW, 03/2002)
  */
  void F77_DGESVD(cfch_ JOBU, cfch_ JOBVT, fi_ M, fi_ N, 
                  fda_ A, fi_ LDA, fda_ S, fda_ U,fi_ LDU,
                  fda_ VT, fi_ LDVT, fda_ WORK, fi_ LWORK, fi_ INFO);

  void dpotf2_(cfch_ UPLO, cfi_ N, fda_ A, cfi_ LDA, fi_ INFO);

  void F77_SGGEV(cfch_ jobvl, cfch_ jobvr, fi_ n, fda_ a,
                 fi_ lda, fda_ b, fi_ ldb, fda_ alphar, fda_ alphai,
                 fda_ beta, fda_ vl, fi_ ldvl, fda_ vr, fi_ ldvr,
                 fda_ work, fi_ lwork, fda_ rwork, fi_ info);

} // end extern C
# endif // __APPLE__
#endif //WIN32


using namespace TNT;

BIAS::Matrix<double>  
Fortran_Matrix_to_Matrix(const Fortran_Matrix < double >&FMat) 
{
  BIAS::Matrix < double > res(FMat.num_rows(), FMat.num_cols());
  
  // get the pointer to the data array of the Matrix
  double *dataP = res.GetData();
  int nCols = res.num_cols(); // nr of rows of the Matrix
  
  // copy the elements to this, rember the different index base (0/1) 
  // and the difference between row-major / col-major order
  // transpose the values because Fortran_Matrices are column-major 
  // and Matrix is row-major
  for (int row = 0; row < res.num_rows(); row++) {
    for (int col = 0; col < res.num_cols(); col++) {
      dataP[nCols * row + col] = FMat(row + 1, col + 1); // assign elementwise
    }
  }
  return res;
}

void
Fortran_Matrix_to_Matrix(const Fortran_Matrix < double >&FMat, 
                         BIAS::Matrix<double>  &res) 
{
  res.newsize(FMat.num_rows(), FMat.num_cols());
  
  // get the pointer to the data array of the Matrix
  double *dataP = res.GetData();
  int nCols = res.num_cols(); // nr of rows of the Matrix
  
  // copy the elements to this, rember the different index base (0/1) 
  // and the difference between row-major / col-major order
  // transpose the values because Fortran_Matrices are column-major 
  // and Matrix is row-major
  for (int row = 0; row < res.num_rows(); row++) {
    for (int col = 0; col < res.num_cols(); col++) {
      dataP[nCols * row + col] = FMat(row + 1, col + 1); // assign elementwise
    }
  } 
}


int 
Lapack_Cholesky_SymmetricPositiveDefinit(const BIAS::Matrix<double> &A,
                                         BIAS::Matrix<double> &UL,
                                         const char ul)
{
  BIASASSERT(A.GetCols() == A.GetRows());
  BIASASSERT(ul=='U' || ul=='L');
  char UPLO=ul;
  Fortran_Matrix<double> FMA(A.num_rows(), A.num_cols(), 
                             ((BIAS::Matrix<double>)transpose(A)).GetData());

#ifdef WIN32   
  integer LDA = FMA.num_rows();
  integer N=LDA;
  integer INFO=0;
#else
    BIAS_LPK_CONST_INT LDA = FMA.num_rows();
    BIAS_LPK_CONST_INT N=LDA;
    BIAS_LPK_INT INFO=0;
#endif
  //std::cout << "N="<<N<<"\tA = "<<A<<"\nLDA = "<<LDA<<std::endl;
  dpotf2_(&UPLO, &N, &FMA(1,1), &LDA, &INFO);

 
  if (INFO==0){
    UL.newsize(LDA, LDA);
    UL.SetZero();
    if (ul=='U'){
      for (long int y=0; y<LDA; y++){
        for (long int x=y; x<LDA; x++){
          UL[y][x]=FMA(y+1,x+1);
        }
      }
    } else {
      for (long int x=0; x<LDA; x++){
        for (long int y=x; y<LDA; y++){
          UL[y][x]=FMA(y+1,x+1);
        }
      }
    }
  } else if (INFO>0) {
    // comment on what info means (LAPACK doc):
    // = 0: successful exit
    // < 0: if INFO = -k, the k-th argument had an illegal value
    // > 0: if INFO = k, the leading minor of order k is not
    //      positive definite, and the factorization could not be completed.
#ifdef BIAS_DEBUG
    BIASERR("You did not provide a positive definite symmetric matrix:" 
            <<A<<" Error Code:"<<INFO);
#endif
  }
  //std::cout << "FMA = "<<FMA<<"\nUL = "<<UL<<std::endl;

  return INFO;
}


BIAS::Vector<double> 
Lapack_LU_linear_solve(const BIAS::Matrix<double> &A, 
                       const BIAS::Vector<double> &b) 
{  
  Fortran_Matrix<double> FMA(A.num_rows(), A.num_cols(), 
                 ((BIAS::Matrix<double>)transpose(A)).GetData());
  Vector<double> FVY(b.size(),b.GetData());

    BIAS_LPK_CONST_INT one=1;
    BIAS_LPK_INT M=FMA.num_rows();
    BIAS_LPK_INT N=FMA.num_cols();
    BIAS_LPK_INT info = 0;

  Vector<double> x(b);
  Vector<Fortran_integer> index(M);
  
#ifdef WIN32
  //different calling arguments in WIN32
  long int NT(N);
  long int oneT(one);
  long int MT(M);
  long int* indT = new long int[M]; 
  long int infT(info);

  F77_DGESV(&NT, &oneT, &FMA(1,1), &MT, indT, &x(1), &MT, &infT); //WIN32
  info = infT;
  delete[] indT;
#else
  F77_DGESV(&N, &one, &FMA(1,1), &M, &index(1), &x(1), &M, &info); //UNIX
#endif

  BIAS::Vector<double> vecResult(A.num_cols());

  if (info != 0)
      return BIAS::Vector<double>(0);
  else {
    for (int i=0;i<vecResult.size();i++)
      vecResult[i]=x[i];    
    return vecResult;
  }
}

BIAS::Vector<double> 
Lapack_LLS_QR_linear_solve(const BIAS::Matrix<double> &A, 
                           const BIAS::Vector<double> &b, int &res) 
{  
    res=0;
#if defined(BIAS_DEBUG) && defined(COMPILE_DEBUG) && !defined(COMPILE_NDEBUG)
  BIAS::SVD svd(A);
  if (svd.Rank()!=(unsigned)((A.num_cols()>A.num_rows())?A.num_rows():A.num_cols())){
    BIASERR("cannot solve because rank of A is too low (see man dgels)"
            << "\nA is "<<A.num_rows()<<"x"<<A.num_cols()
            <<"  and has rank "<<svd.Rank()<<"   should have rank "
            <<((A.num_cols()>A.num_rows())?(A.num_rows()):(A.num_cols())));
    res=-1;
    BIASERR("A: "<<A<<"  Singular Values: "<<svd.GetS());
    //BIASASSERT(false);
  }
  if (A.num_rows()<A.num_cols()){
    BIASERR("system is not overdetermined since A is a "<<A.num_rows()
            <<"x"<<A.num_cols()<<" matrix");
  }
#endif
  Fortran_Matrix<double> FMA(A.num_rows(), A.num_cols(), 
                 ((BIAS::Matrix<double>)transpose(A)).GetData());
  Vector<double> FVY(b.size(),b.GetData());
  BIAS_LPK_CONST_INT one=1;
  long int M=FMA.num_rows();
  long int N=FMA.num_cols();
  
  Vector<double> x(b);
  BIAS_LPK_INT info = 0;
  
  char transp = 'N';
  BIAS_LPK_INT lwork = 5 * (M+N);      // temporary work space
  Vector<double> work(lwork);
  
#ifdef WIN32
  //different function prarams in WIN32
  long int NT(N);
  long int oneT(one);
  long int MT(M);
  long int infT(info);
  long int lworkT(lwork);

  F77_DGELS(&transp, &MT, &NT, &oneT, &FMA(1,1), &MT, &x(1), &MT,  &work(1),
            &lworkT, &infT);
  info = infT;
#else  //WIN32
  F77_DGELS(&transp, &M, &N, &one, &FMA(1,1), &M, &x(1), &M,  &work(1),
            &lwork, &info);
#endif //WIN32

  res|=info;
  BIAS::Vector<double> vecResult(N);
  if (res != 0) {
    BIASERR("error calculating linear least squares problem "<<res);
    return BIAS::Vector<double>(0);
  }
  else {
    for (int i=0;i<vecResult.size();i++)
      vecResult[i]=x[i];    
    return vecResult;
  }
}


BIAS::Vector<double> 
Lapack_WLLS_solve(const BIAS::Matrix<double> &A, 
                  const BIAS::Vector<double> &b,  
                  const BIAS::Matrix<double> &B, int &res) 
{  
#ifdef MIP_DEBUG
  if (A.num_rows()!=B.num_rows()){
    BIASERR("wrong matrix dimension ");
    res=-1;
  }
  if (A.num_rows()!=b.num_rows()){
    BIASERR("wrong vector dimension ");
    res=-2;
  }
  // to befinished (see man dggglm);
#endif
  Fortran_Matrix<double> FMA(A.num_rows(), A.num_cols(), 
                             ((BIAS::Matrix<double>)transpose(A)).GetData());
  Fortran_Matrix<double> FMB(B.num_rows(), B.num_cols(), 
                             ((BIAS::Matrix<double>)transpose(B)).GetData());
  Vector<double> FVY(b.size(),b.GetData());
  long int N=FMA.num_rows();
  long int M=FMA.num_cols();
  long int P=FMB.num_cols();
  Vector<double> x(M);
  Vector<double> y(P);
  Fortran_integer info = 0;
  
  Fortran_integer lwork = 5 * (M+N+P);      // temporary work space
  Vector<double> work(lwork);
  
#ifdef WIN32
  //different calling arguments in WIN32
  long int NT(N);
  long int MT(M);
  long int PT(P);
  long int infT(info);
  long int lworkT(lwork);

  F77_DGGGLM(&NT, &MT, &PT, &FMA(1,1), &NT, &FMB(1,1), &NT, &FVY(1), &x(1), &y(1), 
             &work(1), &lworkT, &infT);
  info = infT;
#else  //WIN32
  F77_DGGGLM(&N, &M, &P, &FMA(1,1), &N, &FMB(1,1), &N, &FVY(1), &x(1), &y(1), 
             &work(1), &lwork, &info);
#endif //WIN32
  
  res=info;
  BIAS::Vector<double> vecResult(x.size());
  if (info != 0) {
    BIASERR("error calculating weighted linear least squares problem "<<res);
    return BIAS::Vector<double>(0);
  } else {
    for (int i=0;i<vecResult.size();i++)
      vecResult[i]=x[i];    
    return vecResult;
  }
}

// *********************** Eigenvalue problems *******************

// solve symmetric eigenvalue problem (eigenvalues only)
BIAS::Vector<double> 
Upper_symmetric_eigenvalue_solve(const BIAS::Matrix<double> &A) 
{
  Fortran_Matrix<double> FMA(A.num_rows(), A.num_cols(), 
                 ((BIAS::Matrix<double>)transpose(A)).GetData());
  char jobz = 'N';
  char uplo = 'U';
  long int N = FMA.num_rows();
  
  assert(N == FMA.num_cols());
  
  Vector<double> eigvals(N);
  Fortran_integer worksize = 3*N;
  Fortran_integer info = 0;
  Vector<double> work(worksize);  
  
#ifdef WIN32
  //different calling arguments in WIN32
  long int NT(N);
  long int infT(info);
  long int worksizeT(worksize);

  F77_DSYEV(&jobz, &uplo, &NT, &FMA(1,1), &NT, eigvals.begin(), work.begin(),
        &worksizeT, &infT);
  info = infT;
#else  //WIN32
  F77_DSYEV(&jobz, &uplo, &N, &FMA(1,1), &N, eigvals.begin(), work.begin(),
            &worksize, &info);
#endif //WIN32
  
  BIAS::Vector<double> vecResult(eigvals.size());
  
  if (info != 0) return BIAS::Vector<double>(0);
  else {
    for (int i=0;i<vecResult.size();i++)
      vecResult[i]=eigvals[i];    
    return vecResult;
  }  
}

int Upper_symmetric_eigenvalue_solve(const BIAS::Matrix<double> &A,
                                     BIAS::Vector<double>& eigenvalues,
                                     std::vector<BIAS::Vector<double> >& eigenvectors)
{
  Fortran_Matrix<double> FMA(A.num_rows(), A.num_cols(), 
                             ((BIAS::Matrix<double>)transpose(A)).GetData());
  char jobz = 'V'; // compute eigenvalues and eigenvectors
  char uplo = 'U'; // upper triangle
  long int N = FMA.num_rows();
  
  assert(N == FMA.num_cols());
  
  eigenvalues.newsize(N);

  Fortran_integer info = 0;
#ifdef WIN32
  BIASABORT;
#else  //WIN32
  // compute work size
  double tmp_work;
  long int iwork_size = -1, work_size = -1;
  F77_DSYEVD(&jobz, &uplo, &N, &FMA(1,1), &N, eigenvalues.begin(), &tmp_work,
             &work_size, &iwork_size, &iwork_size, &info);
  work_size = (int)tmp_work;
  Vector<double> work(work_size);
  Vector<long int> iwork(iwork_size);
  F77_DSYEVD(&jobz, &uplo, &N, &FMA(1,1), &N, eigenvalues.begin(), work.begin(),
             &work_size, iwork.begin(), &iwork_size, &info);
#endif //WIN32
  if (info==0){
    eigenvectors.resize(N);
    for (int r=0; r<N; r++){
      eigenvectors[r].newsize(N);
      for (int c=0; c<N; c++){
        eigenvectors[r][c] = FMA(c + 1, r + 1);
      }
    }
  }
/*
#ifdef BIAS_DEBUG
  BIAS::Vector<double> mv, vv, diff;
  for (int i=0; i<N; i++){
    mv = A * eigenvectors[i];
    vv = eigenvalues[i] * eigenvectors[i];
    diff = mv - vv;
    //std::cout << "A * evec: "<<mv<<"\n eval * evec: "<<vv<<endl;
    if (fabs(diff.NormL2())>1e-20){
      std::cout << "diff: "<< diff << std::endl 
                << "diff.NormL2(): "<<diff.NormL2()<< std::endl ;
      //BIASASSERT(BIAS::Equal(diff.NormL2(), 0.0, 1e-15));
    }
  }
#endif
*/
  return info;
}

// Solve symmetric eigenvalue problem for matrix in packed storage
int Packed_symmetric_eigenvalue_solve(long int N,
                                      const BIAS::Vector<double> &A,
                                      BIAS::Vector<double>& eigVals,
                                      std::vector<BIAS::Vector<double> >& eigVecs,
                                      const bool upperTriangle)
{
  // Compute eigenvalues and eigenvectors of A
  char jobz = 'V';
  // Is upper or lower triangle of A given in packed data vector?
  char uplo = (upperTriangle ? 'U' : 'L');
  // Matrix size is N x N, vector length is N * (N+1) / 2
  BIASASSERT(N*(N+1) == 2*(int)A.Size());
  eigVals.newsize(N);
  eigVals.SetZero();

  // Compute work space
  BIAS::Vector<double> work(3*N);
  Fortran_Matrix<double> Z(N,N);
  BIAS::Vector<double> tmpA = A;

  // Call LAPACK routine
  Fortran_integer info = 0;
#ifdef WIN32
  Fortran_integer num = N;
  F77_DSPEV(&jobz, &uplo, &num, tmpA.begin(), eigVals.begin(),
            &(Z(1,1)), &num, work.begin(), &info);
#else
  F77_DSPEV(&jobz, &uplo, &N, tmpA.begin(), eigVals.begin(),
            &(Z(1,1)), &N, work.begin(), &info);
#endif

  // Return results
  if (info == 0) {
    eigVecs.resize(N);
    for (int i = 0; i < N; i++) {
      eigVecs[i].newsize(N);
      for (int j = 0; j < N; j++) {
        eigVecs[i][j] = Z(j+1,i+1);
      }
    }
  }
  return info;
}

// solve unsymmetric eigenvalue problems (for a rectangular matrix)
int Eigenvalue_solve(const BIAS::Matrix<double> &A,
                     BIAS::Vector<double> &wr, BIAS::Vector<double> &wi)
{
  Fortran_Matrix<double> FMA(A.num_rows(), A.num_cols(), 
                 ((BIAS::Matrix<double>)transpose(A)).GetData());
  char jobvl = 'N';
  char jobvr = 'N';
   
  Fortran_integer N = FMA.num_rows();    
  assert(N == FMA.num_cols());
   
  if (N<1) return 1;
  
  Fortran_Matrix<double> vl(1,N);  // should be NxN ?
  Fortran_Matrix<double> vr(1,N);  
  Fortran_integer one = 1;
  
  Fortran_integer worksize = 5*N;
  Fortran_integer info = 0;
  Vector<double> work(worksize, 0.0);   
  
  Vector<double> FWR(N), FWI(N);
    
#ifdef WIN32
  //different function arguments in WIN32
  long int NT(N);
  long int oneT(one);
  long int infT(info);
  long int worksizeT(worksize);

  F77_DGEEV(&jobvl, &jobvr, &NT, &FMA(1,1), &NT, &(FWR(1)), //WIN32
        &(FWI(1)), &(vl(1,1)), &oneT, &(vr(1,1)), &oneT,
        &(work(1)), &worksizeT, &infT);
   info = infT;
#else  //WIN32
  F77_DGEEV(&jobvl, &jobvr, &N, &FMA(1,1), &N, &(FWR(1)),   //UNIX
            &(FWI(1)), &(vl(1,1)), &one, &(vr(1,1)), &one,
            &(work(1)), &worksize, &info);
#endif //WIN32
  
  wr=FWR;
  wi=FWI;  
  return (info==0 ? 0: 1);
}

int
Eigenproblem_quadratic_matrix(const BIAS::Matrix<double> &A, 
                  BIAS::Vector<double> &ret_EigenValuesReal, 
                  BIAS::Vector<double> &ret_EigenValuesImag,
                  BIAS::Matrix<double> &eigenVectors) 
{
  Fortran_Matrix<double> FMA(A.num_rows(), A.num_cols(), 
                 ((BIAS::Matrix<double>)transpose(A)).GetData());
  char jobvl = 'V'; // calculate the left eigenvectors
  char jobvr = 'V'; // calculate the right eigenvectors
    
  Fortran_integer N = FMA.num_rows(); // dim of the Matrix should be NxN
  assert(N == FMA.num_cols() ); // A must be rectangular NxN matrix
  if (N<1) return 1; // 1x1 matrix is a scalar, decomposition is nonsense
  
  Fortran_integer ldvl = N; // size of the left eigenvectormatrix
  Fortran_integer ldvr = N; // size of the right eigenvectormatrix
  
  // should be NxN, but left Eigenvectorss are not calculated due to jobvl=N
  Fortran_Matrix<double> eigenVectsLeft( N,ldvl ); 
  // will contain a (NxN) Matrix with the right Eigenvectors in columns. 
  Fortran_Matrix<double> eigenVectsRight( N,ldvr ); 
  
  Fortran_integer worksize = 5*N;
  Fortran_integer info = 0;
  Vector<double> work(worksize, 0.0);
  
  Vector<double> ret_EigenValuesRealPart(N,0.0);
  Vector<double> ret_EigenValuesImagPart(N,0.0);
  
#ifdef WIN32
  //different function arguments in WIN32
  long int NT(N);
  long int infT(info);
  long int worksizeT(worksize);
  long int ldvlT(ldvl);
  long int ldvrT(ldvr);

  F77_DGEEV(&jobvl, &jobvr, &NT, &FMA(1,1), &NT, &(ret_EigenValuesRealPart(1)), //WIN32
            &(ret_EigenValuesImagPart(1)), 
            &(eigenVectsLeft(1,1)), &ldvlT, 
            &(eigenVectsRight(1,1)), &ldvrT,
            &(work(1)), &worksizeT, &infT);
   info = infT;
#else  //WIN32
  F77_DGEEV(&jobvl, &jobvr, &N, &FMA(1,1), &N, &(ret_EigenValuesRealPart(1)),   //UNIX
            &(ret_EigenValuesImagPart(1)), 
            &(eigenVectsLeft(1,1)), &ldvl, 
            &(eigenVectsRight(1,1)), &ldvr,
            &(work(1)), &worksize, &info);
#endif //WIN32

  eigenVectors.newsize(eigenVectsRight.num_rows(),eigenVectsRight.num_cols()); 
  for (int i = 0; i < eigenVectors.size(); i++) {
    eigenVectors.GetData()[i] = eigenVectsRight.begin()[i]; 
  };
  eigenVectors=transpose(eigenVectors);  
  ret_EigenValuesReal= ret_EigenValuesRealPart;
  ret_EigenValuesImag= ret_EigenValuesImagPart;  
  return (info==0 ? 0: 1); 
}

int
General_singular_value_decomposition(char jobu, char jobvt,
                                     const BIAS::Matrix<double> &A, 
                                     BIAS::Vector<double> &ret_S, 
                                     BIAS::Matrix<double> &ret_U, 
                                     BIAS::Matrix<double> &ret_VT) 
{

#ifdef BIAS_DEBUG
  // svd does not like nans/infs and hangs sometimes. 
  // It is better to abort before in that case
  for (int i=0; i<A.num_rows(); i++) {
    for (int j=0; j<A.num_cols(); j++) {
      INFNANCHECK(A[i][j]);
    }
  }
#endif

  Fortran_Matrix<double> FMA(A.num_rows(), A.num_cols(), 
                 ((BIAS::Matrix<double>)transpose(A)).GetData());
  //char jobu  = 'A'; // calculate all columns of U
  //char jobvt = 'A'; // calculate all n rows of V_h
  
  Fortran_integer M = FMA.num_rows(); // dim of the Matrix A should be MxN
  Fortran_integer N = FMA.num_cols(); // dim of the Matrix A should be MxN
  
  Fortran_Matrix<double> U ( M, M );// M x M Orthogonal/Unitary matrix
  Fortran_Matrix<double> V ( N, N );// N x N Orthogonal/Unitary matrix
  Fortran_Matrix<double> VT(N,N); // N x N Orthogonal/Unitary matrix transposed
  Vector<double> S ( (int)max( (int)M, (int)N) );// the M singular values 
  
  Fortran_integer lda  = M;
  Fortran_integer ldu  = M; // because jobu = A
  Fortran_integer ldvt = N; // because jobvt = A
  
  Fortran_integer worksize =  6 * (M > N ? M : N) ;
  
  Fortran_integer info = 0;
  Vector<double> work(worksize, 0.0);
  
#ifdef WIN32
  //different function arguments in WIN32
  long int NT(N);
  long int MT(M);
  long int infT(info);
  long int ldaT(lda);
  long int lduT(ldu);
  long int ldvtT(ldvt);
  long int worksizeT(worksize);

  // call extern Fortran2C routine from liblapack: 
  F77_DGESVD( &jobu, &jobvt, &MT, &NT, &FMA(1,1), &ldaT, &S(1), &U(1,1),
          &lduT, &VT(1,1),  &ldvtT, &work(1), &worksizeT, &infT);        //WIN32
   info = infT;
#else  //WIN32
  F77_DGESVD( &jobu, &jobvt, &M, &N, &FMA(1,1), &lda, &S(1),    &U(1,1),
              &ldu, &VT(1,1),   &ldvt, &work(1), &worksize, &info);      //UNIX
#endif //WIN32

  // S is zero exept for the first min(M,N) diagonal elements 
  // therefore only copy these elements
  ret_S.newsize((int)min((int) M,(int)N));
  for (int i=0;i<min((int) M,(int)N);i++)
    ret_S[i]  = S[i];

  ret_U=Fortran_Matrix_to_Matrix(U); 
  ret_VT=Fortran_Matrix_to_Matrix(VT);
  if (info==0) {
    return 0;
  } else {
    /* from documentaion of F77_DGESVD:
     *  INFO    (output) INTEGER
     *          = 0:  successful exit.
     *          < 0:  if INFO = -i, the i-th argument had an illegal value.
     *          > 0:  if DBDSQR did not converge, INFO specifies how many
     *                superdiagonals of an intermediate bidiagonal form B
     *                did not converge to zero. See the description of WORK
     *                above for details.
     *          On exit, if INFO = 0, WORK(1) returns the optimal LWORK;
     *          if INFO > 0, WORK(2:MIN(M,N)) contains the unconverged
     *          superdiagonal elements of an upper bidiagonal matrix B
     *          whose diagonal is in S (not necessarily sorted). B
     *          satisfies A = U * B * VT, so it has the same singular values
     *          as A, and singular vectors related by U and VT. */

    //  std::cout<<"F77_DGESVD returned "<<info<<std::endl;
    return 1;
  }
  return (info==0 ? 0: 1); // if info !=  0 the arrays were too small.
}



double MahalanobisDistance(const BIAS::Matrix<double> &Sigma,
                           const BIAS::Vector<double> &V) {
  double dist = SquaredMahalanobisDistance(Sigma,V);
  if (dist>0.0) return sqrt(dist);
  return dist;
}

double SquaredMahalanobisDistance(const BIAS::Matrix<double> &Sigma,
                                  const BIAS::Vector<double> &V) {
  // solve              dist = V^T A^-1 V
  // first convert to   dist = V^T (U U^T)^-1 V = V^T U^-T U^-1 V 
  //                         = (U^-1 V)^T * (U^-1 V)
  // compute U
  BIAS::Matrix<double> U=Sigma;

  U.MakeSymmetric();
  double scale = U.NormFrobenius()/U.num_rows();
  BIAS::Matrix<double> s2 = U *(1.0/scale);

  int result =
    Lapack_Cholesky_SymmetricPositiveDefinit(s2, U, 'L');
  if (result!=0) {
    BIASERR("cholesky failed with "<<result);
    return -1.0;
  }  

  // set m = (U^-1 V)    <=>   U m = V    with dist = m^T m
  // solve for m:

  // Optimization: dont call Lapack_LU_linear_solve here, this is already 
  // triangular. Call BLAS::DTRSV directly. This also avoids FORTRAN-BIAS 
  // conversion of matrices.

  BIAS::Vector<double> m(Lapack_LU_linear_solve(U, V));
  // compute dist = m^T m
  double dist = 0;
  for (unsigned int i=0; i<m.Size(); i++) {
    dist += m[i]*m[i];
  }
  return sqrt(dist/scale);
}

// The right generalized eigenvector v(j) corresponding to the
// generalized eigenvalue lambda(j) of (A,B) satisfies
//     A * v(j) = lambda(j) * B * v(j).
// A and B are both M x M matrices.
BIAS::Matrix<double> generalised_eigenvalue_matrix_solve(
  BIAS::Matrix<double> &A_, BIAS::Matrix<double> &B_)
{
#ifndef WIN32
  Fortran_Matrix<double> a(A_.num_rows(), A_.num_cols(),((BIAS::Matrix<double>)transpose(A_)).GetData());
  Fortran_Matrix<double> b(B_.num_rows(), B_.num_cols(),((BIAS::Matrix<double>)transpose(B_)).GetData());
  
  char jobvl  = 'N';
  char jobvr = 'V';
   
  Fortran_integer M = A_.num_rows();
  Fortran_integer N = B_.num_rows();
  Fortran_integer lda  = M;
  Fortran_integer n = M;
  Fortran_integer ldb = N;
  Fortran_integer ldvl = 1;
  Fortran_integer ldvr = N;
  Fortran_integer info = 0;

  Fortran_integer lwork = N*N+64;
  Vector<double> work(lwork, 0.0);
  Vector<double> rwork(N*8, 0.0);

  Vector<double> alpha(M, 0.0);
  Vector<double> alphai(M, 0.0);
  Vector<double> beta(M, 0.0);

  Fortran_Matrix<double> vl(ldvl, M);
  Fortran_Matrix<double> vr(ldvr, N);

  // call extern Fortran2C routine from liblapack
  F77_SGGEV(
    &jobvl,
    &jobvr,
    &n,
    &(a(1,1)),
    &lda,
    &(b(1,1)),
    &ldb,
    &(alpha(1)),
    &(alphai(1)),
    &(beta(1)),
    &(vl(1,1)),
    &ldvl,
    &(vr(1,1)),
    &ldvr,
    &work(1),
    &lwork,
    &rwork(1),
    &info);

  BIAS::Matrix<double> out;
  out.newsize(N,N);
  for(unsigned int i =0; i< N; i++){
    for(unsigned int j =0; j< M; j++){
      out[i][j]=vr(i+1,j+1);
    }
  } 
  return out;

#else // WIN32

  BIASERR("Disabled function generalised_eigenvalue_matrix_solve() "
          "because F77_SGGEV is not available yet in WIN32 build!");

  BIAS::Matrix<double> out;
  return out;

#endif // WIN32
}