#include <iostream>
#include "intflapack.h"
#include "tvector.h"
#include "tmatrix.h"
#include <typeinfo>

/*!
   \defgroup LinAlg LinAlg module
   This module contains classes and functions for complex linear
   algebra on arrays. This module is intended mainly to have 
   classes implementing C++ interfaces between Sophya objects
   and external linear algebra libraries, such as LAPACK.  
*/

/*! 
  \class SOPHYA::LapackServer
  \ingroup LinAlg
  This class implements an interface to LAPACK library driver routines.
  The LAPACK (Linear Algebra PACKage) is a collection high performance 
  routines to solve common problems in numerical linear algebra. 
  its is available from http://www.netlib.org.
  
  The present version of our LapackServer (Feb 2001) provides only 
  interfaces for the linear system solver and singular value 
  decomposition (SVD). Only arrays with BaseArray::FortranMemoryMapping 
  can be handled by LapackServer. LapackServer can be instanciated 
  for simple and double precision real or complex array types.

  The example below shows solving a linear system A*X = B

  \code
  #include "intflapack.h"
  // ...
  // Use FortranMemoryMapping as default
  BaseArray::SetDefaultMemoryMapping(BaseArray::FortranMemoryMapping);
  // Create an fill the arrays A and B
  int n = 20;
  Matrix A(n, n);
  A = RandomSequence();
  Vector X(n),B(n);
  X = RandomSequence();
  B = A*X;
  // Solve the linear system A*X = B
  LapackServer<r_8> lps;
  lps.LinSolve(A,B);
  // We get the result in B, which should be equal to X ...
  // Compute the difference B-X ;
  Vector diff = B-X;
  \endcode

*/

extern "C" {
// Drivers pour resolution de systemes lineaires
  void sgesv_(int_4* n, int_4* nrhs, r_4* a, int_4* lda, 
	      int_4* ipiv, r_4* b, int_4* ldb, int_4* info);
  void dgesv_(int_4* n, int_4* nrhs, r_8* a, int_4* lda, 
	      int_4* ipiv, r_8* b, int_4* ldb, int_4* info);
  void cgesv_(int_4* n, int_4* nrhs, complex<r_4>* a, int_4* lda, 
	      int_4* ipiv, complex<r_4>* b, int_4* ldb, int_4* info);
  void zgesv_(int_4* n, int_4* nrhs, complex<r_8>* a, int_4* lda, 
	      int_4* ipiv, complex<r_8>* b, int_4* ldb, int_4* info);

  // Driver pour resolution de systemes au sens de Xi2
  void sgels_(char * trans, int_4* m, int_4* n, int_4* nrhs, r_4* a, int_4* lda, 
	      r_4* b, int_4* ldb, r_4* work, int_4* lwork, int_4* info);
  void dgels_(char * trans, int_4* m, int_4* n, int_4* nrhs, r_8* a, int_4* lda, 
	      r_8* b, int_4* ldb, r_8* work, int_4* lwork, int_4* info);
  void cgels_(char * trans, int_4* m, int_4* n, int_4* nrhs, complex<r_4>* a, int_4* lda, 
	      complex<r_4>* b, int_4* ldb, complex<r_4>* work, int_4* lwork, int_4* info);
  void zgels_(char * trans, int_4* m, int_4* n, int_4* nrhs, complex<r_8>* a, int_4* lda, 
	      complex<r_8>* b, int_4* ldb, complex<r_8>* work, int_4* lwork, int_4* info);

// Driver pour decomposition SVD 
  void sgesvd_(char* jobu, char* jobvt, int_4* m, int_4* n, r_4* a, int_4* lda,
	       r_4* s, r_4* u, int_4* ldu, r_4* vt, int_4* ldvt, 
	       r_4* work, int_4* lwork, int_4* info);
  void dgesvd_(char* jobu, char* jobvt, int_4* m, int_4* n, r_8* a, int_4* lda,
	       r_8* s, r_8* u, int_4* ldu, r_8* vt, int_4* ldvt, 
	       r_8* work, int_4* lwork, int_4* info);
  void cgesvd_(char* jobu, char* jobvt, int_4* m, int_4* n, complex<r_4>* a, int_4* lda,
	       complex<r_4>* s, complex<r_4>* u, int_4* ldu, complex<r_4>* vt, int_4* ldvt, 
	       complex<r_4>* work, int_4* lwork, int_4* info);
  void zgesvd_(char* jobu, char* jobvt, int_4* m, int_4* n, complex<r_8>* a, int_4* lda,
	       complex<r_8>* s, complex<r_8>* u, int_4* ldu, complex<r_8>* vt, int_4* ldvt, 
	       complex<r_8>* work, int_4* lwork, int_4* info);
	       
}


//   -------------- Classe LapackServer<T> --------------

template <class T>
LapackServer<T>::LapackServer()
{
  SetWorkSpaceSizeFactor();
}

template <class T>
LapackServer<T>::~LapackServer()
{
}

//! Interface to Lapack linear system solver driver s/d/c/zgesvd().
/*! Solve the linear system a * x = b. Input arrays
  should have FortranMemory mapping (column packed).
  \param a : input matrix, overwritten on output
  \param b : input-output, input vector b, contains x on exit
  \return : return code from lapack driver _gesv()
 */
template <class T>
int LapackServer<T>::LinSolve(TArray<T>& a, TArray<T> & b)
{
  if ( ( a.NbDimensions() != 2 ) || ( b.NbDimensions() != 2 ) )
    throw(SzMismatchError("LapackServer::LinSolve(a,b) a Or b NbDimensions() != 2"));
  
  int_4 rowa = a.RowsKA();
  int_4 cola = a.ColsKA();
  int_4 rowb = b.RowsKA();
  int_4 colb = b.ColsKA();
  if ( a.Size(rowa) !=  a.Size(cola)) 
    throw(SzMismatchError("LapackServer::LinSolve(a,b) a Not a square Array"));
  if ( a.Size(rowa) !=  b.Size(rowb)) 
    throw(SzMismatchError("LapackServer::LinSolve(a,b) RowSize(a <> b) "));

  if (!a.IsPacked(rowa) || !b.IsPacked(rowb))
     throw(SzMismatchError("LapackServer::LinSolve(a,b) a Or b Not Column Packed"));

  int_4 n = a.Size(rowa);
  int_4 nrhs = b.Size(colb);
  int_4 lda = a.Step(cola);
  int_4 ldb = b.Step(colb);
  int_4 info;
  int_4* ipiv = new int_4[n];

  if (typeid(T) == typeid(r_4) )
    sgesv_(&n, &nrhs, (r_4 *)a.Data(), &lda, ipiv, (r_4 *)b.Data(), &ldb, &info);
  else if (typeid(T) == typeid(r_8) ) 
    dgesv_(&n, &nrhs, (r_8 *)a.Data(), &lda, ipiv, (r_8 *)b.Data(), &ldb, &info);
  else if (typeid(T) == typeid(complex<r_4>) ) 
    cgesv_(&n, &nrhs, (complex<r_4> *)a.Data(), &lda, ipiv, 
	   (complex<r_4> *)b.Data(), &ldb, &info);
  else if (typeid(T) == typeid(complex<r_8>) ) 
    zgesv_(&n, &nrhs, (complex<r_8> *)a.Data(), &lda, ipiv, 
	   (complex<r_8> *)b.Data(), &ldb, &info);
  else {
    delete[] ipiv;
    string tn = typeid(T).name();
    cerr << " LapackServer::LinSolve(a,b) - Unsupported DataType T = " << tn << endl;
    throw TypeMismatchExc("LapackServer::LinSolve(a,b) - Unsupported DataType (T)");
  }
  delete[] ipiv;
  return(info);
}

//! Interface to Lapack least squares solver driver s/d/c/zgels().
/*! Solves the linear least squares problem defined by an m-by-n matrix 
  \b a and an m element vector \b b .
  A solution \b x to the overdetermined system of linear equations
  b = a * x is computed, minimizing the norm of b-a*x. 
  Underdetermined systems (m<n) are not yet handled.
  Inout arrays should have FortranMemory mapping (column packed).
  \param a : input matrix, overwritten on output
  \param b : input-output, input vector b, contains x on exit.
  \return : return code from lapack driver _gels()
  \warning : b is not resized.
 */
/*
  $CHECK$ - A faire - cas m<n 
       If the linear system is underdetermined, the minimum norm 
       solution is computed. 
*/

template <class T>
int LapackServer<T>::LeastSquareSolve(TArray<T>& a, TArray<T> & b)
{
  if ( ( a.NbDimensions() != 2 ) || ( b.NbDimensions() != 2 ) )
    throw(SzMismatchError("LapackServer::LinSolve(a,b) a Or b NbDimensions() != 2"));
  
  int_4 rowa = a.RowsKA();
  int_4 cola = a.ColsKA();
  int_4 rowb = b.RowsKA();
  int_4 colb = b.ColsKA();


  if ( a.Size(rowa) !=  b.Size(rowb)) 
    throw(SzMismatchError("LapackServer::LeastSquareSolve(a,b) RowSize(a <> b) "));

  if (!a.IsPacked(rowa) || !b.IsPacked(rowb))
     throw(SzMismatchError("LapackServer::LeastSquareSolve(a,b) a Or b Not Column Packed"));

  if ( a.Size(rowa) <  a.Size(cola)) {  // $CHECK$ - m<n a changer
    cout << " LapackServer<T>::LeastSquareSolve() - m<n - Not yet implemented for "
	 << " underdetermined systems ! " << endl;
    throw(SzMismatchError("LapackServer::LeastSquareSolve(a,b) NRows<NCols - "));
  }
  int_4 m = a.Size(rowa);
  int_4 n = a.Size(cola);
  int_4 nrhs = b.Size(colb);

  int_4 lda = a.Step(cola);
  int_4 ldb = b.Step(colb);
  int_4 info;

  int_4 minmn = (m < n) ? m : n;
  int_4 maxmn = (m > n) ? m : n;
  int_4 maxmnrhs = (nrhs > maxmn) ? nrhs : maxmn;
  if (maxmnrhs < 1) maxmnrhs = 1;
  
  int_4 lwork = minmn+maxmnrhs*5;
  T * work = new T[lwork];

  char trans = 'N';
  
  if (typeid(T) == typeid(r_4) )
    sgels_(&trans, &m, &n, &nrhs, (r_4 *)a.Data(), &lda, 
	   (r_4 *)b.Data(), &ldb, (r_4 *)work, &lwork, &info);
  else if (typeid(T) == typeid(r_8) ) 
    dgels_(&trans, &m, &n, &nrhs, (r_8 *)a.Data(), &lda, 
	   (r_8 *)b.Data(), &ldb, (r_8 *)work, &lwork, &info);
  else if (typeid(T) == typeid(complex<r_4>) ) 
    cgels_(&trans, &m, &n, &nrhs, (complex<r_4> *)a.Data(), &lda, 
	   (complex<r_4> *)b.Data(), &ldb, (complex<r_4> *)work, &lwork, &info);
  else if (typeid(T) == typeid(complex<r_8>) ) 
    zgels_(&trans, &m, &n, &nrhs, (complex<r_8> *)a.Data(), &lda, 
	   (complex<r_8> *)b.Data(), &ldb, (complex<r_8> *)work, &lwork, &info);
  else {
    delete[] work;
    string tn = typeid(T).name();
    cerr << " LapackServer::LeastSquareSolve(a,b) - Unsupported DataType T = " << tn << endl;
    throw TypeMismatchExc("LapackServer::LeastSquareSolve(a,b) - Unsupported DataType (T)");
  }
  delete[] work;
  return(info);
}


//! Interface to Lapack SVD driver s/d/c/zgesv().
/*! Computes the vector of singular values of \b a. Input arrays
  should have FortranMemoryMapping (column packed).
  \param a : input m-by-n matrix 
  \param s : Vector of min(m,n) singular values (descending order)
  \return : return code from lapack driver _gesvd()
 */

template <class T>
int LapackServer<T>::SVD(TArray<T>& a, TArray<T> & s)
{
  return (SVDDriver(a, s, NULL, NULL) );
}

//! Interface to Lapack SVD driver s/d/c/zgesv().
/*! Computes the vector of singular values of \b a, as well as
  right and left singular vectors of \b a.
  \f[
  A = U \Sigma V^T , ( A = U \Sigma V^H \ complex)
  \f]
  \f[
  A v_i = \sigma_i u_i \ and A^T u_i = \sigma_i v_i \ (A^H \ complex)
  \f]
  U and V are orthogonal (unitary) matrices.
  \param a : input m-by-n matrix (in FotranMemoryMapping)
  \param s : Vector of min(m,n) singular values (descending order)
  \param u : Matrix of left singular vectors
  \param vt : Transpose of right singular vectors.
  \return : return code from lapack driver _gesvd()
 */
template <class T>
int LapackServer<T>::SVD(TArray<T>& a, TArray<T> & s, TArray<T> & u, TArray<T> & vt)
{
  return (SVDDriver(a, s, &u, &vt) );
}


//! Interface to Lapack SVD driver s/d/c/zgesv().
template <class T>
int LapackServer<T>::SVDDriver(TArray<T>& a, TArray<T> & s, TArray<T>* up, TArray<T>* vtp)
{
  if ( ( a.NbDimensions() != 2 )  )
    throw(SzMismatchError("LapackServer::SVD(a, ...) a.NbDimensions() != 2"));

  int_4 rowa = a.RowsKA();
  int_4 cola = a.ColsKA();

  if ( !a.IsPacked(rowa) )
     throw(SzMismatchError("LapackServer::SVD(a, ...) a Not Column Packed "));

  int_4 m = a.Size(rowa);
  int_4 n = a.Size(cola);
  int_4 maxmn = (m > n) ? m : n;
  int_4 minmn = (m < n) ? m : n;

  char jobu, jobvt;
  jobu = 'N';
  jobvt = 'N';

  sa_size_t sz[2];
  if ( up != NULL) {
    if ( dynamic_cast< TVector<T> * > (vtp) ) 
      throw( TypeMismatchExc("LapackServer::SVD() Wrong type (=TVector<T>) for u !") );
    up->SetMemoryMapping(BaseArray::FortranMemoryMapping);
    sz[0] = sz[1] = m;
    up->ReSize(2, sz );
    jobu = 'A';
  }
  else { 
    up = new TMatrix<T>(1,1);
    jobu = 'N';
  }
  if ( vtp != NULL) {
    if ( dynamic_cast< TVector<T> * > (vtp) ) 
      throw( TypeMismatchExc("LapackServer::SVD() Wrong type (=TVector<T>) for vt !") );
    vtp->SetMemoryMapping(BaseArray::FortranMemoryMapping);
    sz[0] = sz[1] = n;
    vtp->ReSize(2, sz );
    jobvt = 'A';
  }
  else { 
    vtp = new TMatrix<T>(1,1);
    jobvt = 'N';
  }

  TVector<T> *vs = dynamic_cast< TVector<T> * > (&s);
  if (vs) vs->ReSize(minmn);
  else {
    TMatrix<T> *ms = dynamic_cast< TMatrix<T> * > (&s);
    if (ms) ms->ReSize(minmn,1);
    else  { 
      sz[0] = minmn; sz[1] = 1;
      s.ReSize(1, sz);
    }
  }
  
  int_4 lda = a.Step(a.ColsKA());
  int_4 ldu = up->Step(up->ColsKA());
  int_4 ldvt = vtp->Step(vtp->ColsKA());

  int_4 lwork = maxmn*5*wspace_size_factor;
  T * work = new T[lwork];
  int_4 info;  

  if (typeid(T) == typeid(r_4) )
    sgesvd_(&jobu, &jobvt, &m, &n, (r_4 *)a.Data(), &lda,
	    (r_4 *)s.Data(), (r_4 *) up->Data(), &ldu, (r_4 *)vtp->Data(), &ldvt, 
	    (r_4 *)work, &lwork, &info);
  else if (typeid(T) == typeid(r_8) ) 
    dgesvd_(&jobu, &jobvt, &m, &n, (r_8 *)a.Data(), &lda,
	    (r_8 *)s.Data(), (r_8 *) up->Data(), &ldu, (r_8 *)vtp->Data(), &ldvt, 
	    (r_8 *)work, &lwork, &info);
  else if (typeid(T) == typeid(complex<r_4>) ) 
    cgesvd_(&jobu, &jobvt, &m, &n, (complex<r_4> *)a.Data(), &lda,
	    (complex<r_4> *)s.Data(), (complex<r_4> *) up->Data(), &ldu, 
	    (complex<r_4> *)vtp->Data(), &ldvt, 
	    (complex<r_4> *)work, &lwork, &info);
  else if (typeid(T) == typeid(complex<r_8>) ) 
    zgesvd_(&jobu, &jobvt, &m, &n, (complex<r_8> *)a.Data(), &lda,
	    (complex<r_8> *)s.Data(), (complex<r_8> *) up->Data(), &ldu, 
	    (complex<r_8> *)vtp->Data(), &ldvt, 
	    (complex<r_8> *)work, &lwork, &info);
  else {
    if (jobu == 'N') delete up;
    if (jobvt == 'N') delete vtp;
    string tn = typeid(T).name();
    cerr << " LapackServer::SVDDriver(...) - Unsupported DataType T = " << tn << endl;
    throw TypeMismatchExc("LapackServer::LinSolve(a,b) - Unsupported DataType (T)");
  }

  if (jobu == 'N') delete up;
  if (jobvt == 'N') delete vtp;
  return(info);
}

void rztest_lapack(TArray<r_4>& aa, TArray<r_4>& bb)
{
  if ( aa.NbDimensions() != 2 ) throw(SzMismatchError("rztest_lapack(TMatrix<r_4> A Not a Matrix"));
  if ( aa.SizeX() !=  aa.SizeY()) throw(SzMismatchError("rztest_lapack(TMatrix<r_4> A Not a square Matrix"));
  if ( bb.NbDimensions() != 2 ) throw(SzMismatchError("rztest_lapack(TMatrix<r_4> A Not a Matrix"));
  if ( bb.SizeX() !=  aa.SizeX() ) throw(SzMismatchError("rztest_lapack(TMatrix<r_4> A <> B "));
  if ( !bb.IsPacked() || !bb.IsPacked() ) 
    throw(SzMismatchError("rztest_lapack(TMatrix<r_4> Not packed A or B "));
  
  int_4 n = aa.SizeX();
  int_4 nrhs = bb.SizeY();
  int_4 lda = n;
  int_4 ldb = bb.SizeX();
  int_4 info;
  int_4* ipiv = new int_4[n];
  sgesv_(&n, &nrhs, aa.Data(), &lda, ipiv, bb.Data(), &ldb, &info);
  delete[] ipiv;
  cout << "rztest_lapack/Info= " << info << endl;
  cout << aa << "\n" << bb << endl;
  return;
}

///////////////////////////////////////////////////////////////
#ifdef __CXX_PRAGMA_TEMPLATES__
#pragma define_template LapackServer<r_4>
#pragma define_template LapackServer<r_8>
#pragma define_template LapackServer< complex<r_4> >
#pragma define_template LapackServer< complex<r_8> >
#endif

#if defined(ANSI_TEMPLATES) || defined(GNU_TEMPLATES)
template class LapackServer<r_4>;
template class LapackServer<r_8>;
template class LapackServer< complex<r_4> >;
template class LapackServer< complex<r_8> >;
#endif

#if defined(OS_LINUX)
//  Pour le link avec f2c sous Linux
extern "C" {
    void MAIN__();
}

void MAIN__()
{
  cerr << "MAIN__() function for linking with libf2c.a " << endl;
  cerr << "  This function should never be called !!! " << endl;
  throw PError("MAIN__() should not be called - see intflapack.cc");
}
#endif
