#include "sopnamsp.h"
#include "machdefs.h"

#include <math.h>
#include <iostream>

#include "srandgen.h"
#include "tarrinit.h"
#include "array.h"
#include "timing.h"
#include "intflapack.h"


int lpk_linsolve_mtx(int n);                             // ErrorCode = 8
int lpk_svd_mtx(int l, int c, int wsf, bool covu=true);  // ErrorCode = 16
int lpk_leastsquare_mtx(int l, int c);                   // ErrorCode = 32   
int lpk_inverse_mtx(int l);                              // ErrorCode = 64

static double TOLERANCE = 1.e-6;
static int nprt = 100;
static int prtlev = 1;

int main(int narg, char* arg[])
{

  SophyaInit();
  InitTim();   // Initializing the CPU timer



  if (narg < 2) {
    cout << " lpk - LinAlg/LapackServer test \n" 
	 << " Usage: lpk select [sizeL,C=5,5] [prtlev=1] \n" 
	 << "             [nprtmax=100] [WorkSpaceSizeFactor=2] \n" 
	 << "   select= linsolve svd svds lss inverse / all= \n"
	 << "   linsolve:  lpk_linsolve_mtx() LapackServer::LinSolve with TMatrix<r_8> \n" 
	 << "   svd:  lpk_svd_mtx() LapackServer::SVD(a,s,u,vt) with TMatrix<r_8> \n" 
	 << "   svds:  lpk_svd_mtx() LapackServer::SVD(a,s); with TMatrix<r_8> \n"
	 << "   lss:  lpk_leastsquare_mtx() LapackServer::LeastSquareSolve() r_8 \n"
	 << "   inverse:  LapackServer::ComputeInverse() r_8 \n" << endl;
    exit(0);
  }
  int l,c;
  l = c = 5;
  int wsf = 2;
  string opt = arg[1];
  if (narg > 2) sscanf(arg[2], "%d,%d", &l, &c); 
  if (narg > 3) prtlev = atoi(arg[3]);
  if (narg > 4) nprt = atoi(arg[4]);
  if (narg > 5) wsf = atoi(arg[5]);

  cout << " lpk - LinAlg/LapackServer_Test sizeL,C=" << l << "," << c 
       << " opt= " << opt << endl;
  int rc = 0;
  BaseArray::SetMaxPrint(nprt, prtlev);
  try {
    if (opt == "all") {
      rc += lpk_linsolve_mtx(l);
      rc += lpk_svd_mtx(l,c,wsf,true);
      rc += lpk_leastsquare_mtx(l,c);
      rc += lpk_inverse_mtx(l);
    }
    else if (opt == "linsolve") rc = lpk_linsolve_mtx(l);
    else if (opt == "svd") rc = lpk_svd_mtx(l,c,wsf,true);
    else if (opt == "svds") rc = lpk_svd_mtx(l,c,wsf,false);
    else if (opt == "lss") rc = lpk_leastsquare_mtx(l,c);
    else if (opt == "inverse") rc = lpk_inverse_mtx(l);
    else { cout << " Unknown option " << opt << " ! " << endl; rc = 66; }
  }
  catch (PThrowable exc) {
    cerr << " catched Exception (lpk.cc) " << exc.Msg() << endl;
    rc = 77;
  }  
  catch (...) {
    cerr << " catched unknown (...) exception (lpk.cc) " << endl; 
    rc = 78; 
  } 
  
  PrtTim(" End of lpk LinAlg/Lapack test ");
  cout << " ---------------  END of Programme -------- (Rc= " 
       << rc << ") --- " << endl;
  return(rc);
}

// -----------------------------------------------------------------------
/* Nouvelle-Fonction */
int lpk_linsolve_mtx(int n)
{
  int i,j;
  BaseArray::SetDefaultMemoryMapping(BaseArray::FortranMemoryMapping);
  cout << " lpk_linsolve_mtx() - Test of LapackServer::LinSolve()  " << endl;
  Matrix a(n,n);
  for(i=0; i<n; i++)
    for(j=0; j<n; j++)  a(j,i) = GauRnd(0., 1.);
  
  Vector x(n), b;
  //  Matrix  x(n,1), b;
  if (prtlev > 0) 
    cout << " ------------ Vector X = \n " << x << "\n" << endl; 
  for(i=0; i<n; i++) x(i) = GauRnd(2., 1.5);
  b = a*x;

  if (prtlev > 0) {
    cout << " ---- lpk_tmtx() LapackServer::LinSolve Test Using TMatrix<r_8> ----- " << endl; 
    cout << " ------------ Matrix A = \n " << a << "\n" << endl; 
    cout << " ------------ Matrix X = \n " << x << "\n" << endl; 
    cout << " ------------ Matrix B = \n " << b << "\n" << endl; 
  }
  cout << "\n   Calling LapackLinSolve(a,b) .... " << endl;
  PrtTim(" Calling LapackLinSolve(a,b) ");
  LapackLinSolve(a,b);
  PrtTim(" End LapackLinSolve(a,b) ");

  if (prtlev > 0) 
    cout << " ------------ Result B(=X ?) = \n " << b << "\n" << endl; 
  Vector diff = b-x;
  PrtTim(" End of Compute(diff)");
  if (prtlev > 0) 
    cout << " ------------ Vector diff B-X = \n " << diff << "\n" << endl; 
  double min,max;
  diff.MinMax(min, max);
  cout << " Min/Max difference Vector (?=0) , Min= " << min 
       << " Max= " << max << endl;
  if ((fabs(min) > TOLERANCE) || (fabs(max) > TOLERANCE)) {
    cout << " !!! Difference exceeding tolerance (=" << TOLERANCE << ") !!!" 
	 << endl;
    return(8);
  } 
  return(0);

}

// -----------------------------------------------------------------------
/* Nouvelle-Fonction */
int lpk_svd_mtx(int m, int n, int wsf, bool covu)
{
  BaseArray::SetDefaultMemoryMapping(BaseArray::FortranMemoryMapping);
  cout << " lpk_svd_mtx() - Test of LapackServer::SVD " << endl;
  
  Matrix a(m , n), aa;
  a = RandomSequence(RandomSequence::Gaussian, 0., 4.);
  aa = a;
  Vector s;
  Matrix u, vt;
  cout << " ---- lpk_svd_tmtx() LapackServer::SVD Test Using TMatrix<r_8> ---- " << endl; 
  if (prtlev > 0) 
    cout << " ------------ Matrix A = \n " << a << "\n" << endl; 

  cout << "\n   Calling LapackSVD(a,s,u,vt) .... " << endl;
  PrtTim(" Calling LapackSVD() ");
  LapackServer<r_8> lpks;
  lpks.SetWorkSpaceSizeFactor(wsf);
  if (covu) lpks.SVD(aa, s, u, vt);
  else lpks.SVD(aa, s);
  PrtTim(" End LapackSVD() ");

  if (prtlev > 0) 
    cout << " ------------ Result S  = \n " << s << "\n" << endl; 
  if (!covu) return(0);
  if (prtlev > 0) {
    cout << " ------------ Result U  = \n " << u << "\n" << endl; 
    cout << " ------------ Result VT = \n " << vt << "\n" << endl; 
  }
  Matrix sm(m,n);
  int minmn = (m<n) ? m : n ;
  for(int k=0; k< minmn ; k++) sm(k,k) = s(k);
  Matrix diff = u*(sm*vt) - a;
  PrtTim(" End of Compute(diff)");
  if (prtlev > 0) 
    cout << " ------------ Matrix diff U*S*Vt - A = \n " << diff << "\n" << endl; 
  double min,max;
  diff.MinMax(min, max);
  cout << " Min/Max difference Matrix (?=0) , Min= " << min 
       << " Max= " << max << endl;
  if ((fabs(min) > TOLERANCE) || (fabs(max) > TOLERANCE)) {
    cout << " !!! Difference exceeding tolerance (=" << TOLERANCE << ") !!!" 
	 << endl;
    return(16);
  } 
  return(0);

}

// -----------------------------------------------------------------------
/* Nouvelle-Fonction */
int lpk_leastsquare_mtx(int m, int n)
{
  BaseArray::SetDefaultMemoryMapping(BaseArray::FortranMemoryMapping);
  cout << " lpk_leastsquare_mtx() - Test of LapackLeastSquareSolve " << endl;
  
  Matrix a(m , n), aa;
  a = RandomSequence(RandomSequence::Gaussian, 0., 4.);
  aa = a;
  Vector x(n),noise(m),b,bx;
  x = RandomSequence(RandomSequence::Gaussian, 2., 1.);
  double signoise = 0.1;
  noise = RandomSequence(RandomSequence::Gaussian, 0., 0.1);
  b = a*x;
  bx = b+noise;
 
  cout << " ---- lpk_leastsquare_tmtx() LapackLeastSquareSolve<r_8> ---- " << endl; 
  if (prtlev > 0) {
    cout << " ------------ Matrix A = \n " << a << "\n" << endl; 
    cout << " ------------ Vector B = \n " << bx << "\n" << endl; 
    cout << " ------------ Vector X = \n " << x << "\n" << endl; 
  }
  cout << "\n   Calling  LapackLeastSquareSolve(a,bx) .... " << endl;
  PrtTim(" Calling LapackLeastSquareSolve() ");
  LapackLeastSquareSolve(aa, bx);
  PrtTim(" End LapackLeastSquareSolve() ");
  bx.Share(bx(Range(0,0,n)));
  if (prtlev > 0) {
    cout << " ------------ Result X  = \n " << bx << "\n" << endl; 
    cout << " ------------ X-X_Real  = \n " << bx-x << "\n" << endl; 
  }
  Vector diff = b-a*bx;
  PrtTim(" End of Compute(diff)");
  if (prtlev > 0) 
    cout << " ------------ Matrix diff b-a*x = \n " << diff << "\n" << endl; 
  double mean,sigma;
  MeanSigma(diff, mean, sigma);
  cout << " MeanSigma(diff) , Mean= " << mean << " Sigma=" << sigma
       << " SigNoise= " << signoise << endl;
  if ((fabs(mean) > signoise) || (fabs(sigma-signoise) > signoise)) {
    cout << " !!! Difference exceeding tolerance (=" << TOLERANCE << ") !!!" 
	 << endl;
    return 32;
  } 
  return 0;

}
// -----------------------------------------------------------------------
/* Nouvelle-Fonction */
int lpk_inverse_mtx(int m)
{
  BaseArray::SetDefaultMemoryMapping(BaseArray::FortranMemoryMapping);
  cout << " lpk_inverse_mtx() - Test of LapackComputeInverse<r_8> " << endl;
  
  Matrix a(m , m), aa, ainv, idmx(m,m), mxprod, diff;
  a = RandomSequence(RandomSequence::Gaussian, 0., 4.);
  aa = a;  // We make a copy of a, as it is modified by LapackServer
  PrtTim(" End of a_inint");

  cout << "\n   Calling  LapackInverse() .... " << endl;
  ainv = LapackInverse(aa);
  PrtTim(" End of LapackInverse(a)");
  mxprod = a*ainv;
  PrtTim(" End of mxprod = a*ainv");
  idmx = IdentityMatrix();
  PrtTim(" End of idmx = IdentityMatrix()");
  diff = mxprod-idmx;
  PrtTim(" End of Compute(diff)");

  if (prtlev > 0) {
    cout << " ------------ Matrix A = \n " << a << "\n" << endl; 
    cout << " ------------ Matrix Inverse(A) = \n " << ainv << "\n" << endl; 
    cout << " ------------ Matrix mxprod = A*Inverse(A) = \n " << mxprod << "\n" << endl; 
  }
  double min,max;
  diff.MinMax(min, max);
  PrtTim(" End of diffCheck");
  cout << " Min/Max difference Matrix (?=0) , Min= " << min 
       << " Max= " << max << endl;
  if ((fabs(min) > TOLERANCE) || (fabs(max) > TOLERANCE)) {
    cout << " !!! Difference exceeding tolerance (=" << TOLERANCE << ") !!!" 
	 << endl;
    return 64;
  }
  return 0;

}