/*! compute the singular value decomposition (SVD)\n
  The arguments are dcocector S, dgematrix U and VT.
  All of them need not to be initialized.
  S, U and VT are overwitten and become singular values, 
  left singular vectors,
  and right singular vectors respectively.
  This matrix also overwritten.
*/
inline long dgematrix::dgesvd(dcovector& S, dgematrix& U, dgematrix& VT)
{
#ifdef  CPPL_VERBOSE
  std::cerr << "# [MARK] dgematrix::dgesvd(dcovector&, dgematrix&, dgematrix&)"
            << std::endl;
#endif//CPPL_VERBOSE
  
  char JOBU('A'), JOBVT('A');
  long LDA(M), LDU(M), LDVT(N),
    LWORK(max(3*min(M,N)+max(M,N),5*min(M,N))), INFO(1);
  double *WORK(new double[LWORK]);
  S.resize(min(M,N)); U.resize(LDU,M); VT.resize(LDVT,N);
  
  dgesvd_(JOBU, JOBVT, M, N, Array, LDA, S.Array, U.Array,
          LDU, VT.Array, LDVT, WORK, LWORK, INFO);
  delete [] WORK;
  
  if(INFO!=0){
    std::cerr << "[WARNING] dgematrix::dgesvd"
              << "(dceovector&, dgematrix&, dcovector&) "
              << "Serious trouble happend. INFO = " << INFO << "."
              << std::endl;
  }
  return INFO;
}
/*! solve overdetermined or underdetermined A*X=Y using dgels
  with the sum of residual squares output\n
  The residual is set as the columnwise sum of residual squares 
  for overdetermined problems
  while it is always zero for underdetermined problems.
*/
inline long dgematrix::dgels(dgematrix& mat, drovector& residual)
{
#ifdef  CPPL_VERBOSE
  std::cerr << "# [MARK] dgematrix::dgels(dgematrix&, drovector&)"
            << std::endl;
#endif//CPPL_VERBOSE
  
#ifdef  CPPL_DEBUG
  if(M!=mat.M){
    std::cerr << "[ERROR] dgematrix::dgels(dgematrix&, drovector&) "
              << std::endl
              << "These two matrices cannot be solved." << std::endl
              << "Your input was (" << M << "x" << N << ") and ("
              << mat.M << "x" << mat.N << ")." << std::endl;
    exit(1);
  }
#endif//CPPL_DEBUG
  
  residual.resize(mat.N); residual.zero();
  
  if(M<N){ //underdetermined
    dgematrix tmp(N,mat.N);
    for(long i=0; i<mat.M; i++){ for(long j=0; j<mat.N; j++){
      tmp(i,j) =mat(i,j);
    }}
    mat.clear();
    swap(mat,tmp);
  }
  
  char TRANS('N');
  long NRHS(mat.N), LDA(M), LDB(mat.M),
    LWORK(min(M,N)+max(min(M,N),NRHS)), INFO(1);
  double *WORK(new double[LWORK]);
  dgels_(TRANS, M, N, NRHS, Array, LDA, mat.Array, LDB, WORK, LWORK, INFO);
  delete [] WORK;
  
  if(M>N){ //overdetermined
    for(long i=0; i<residual.L; i++){ for(long j=0; j<M-N; j++){
      residual(i) += std::pow(mat(N+j,i), 2.0);
    }}
    
    dgematrix tmp(N,mat.N);
    for(long i=0; i<tmp.M; i++){ for(long j=0; j<tmp.N; j++){
      tmp(i,j) =mat(i,j);
    }}
    mat.clear();
    swap(mat,tmp);
  }
  
  if(INFO!=0){
    std::cerr << "[WARNING] dgematrix::dgels(dgematrix&, drovector&) "
              << "Serious trouble happend. INFO = " << INFO << "."
              << std::endl;
  }
  return INFO;
}
int Unscented_Kalman_Filter::U_Cov(const vector<dcovector> &sP1, 
			   const dcovector &m1,
			   const vector<dcovector> &sP2, 
			   const dcovector &m2,
			   dgematrix & cov)
{
      int N=sP1[0].l;
      int M=sP2[0].l;
      int i;
      cov.resize(N,M);
      cov.zero();
      N=sP1.size();
      drovector Xt = CPPL::t(sP2[0] - m2);
      dcovector X = (sP1[0] - m1);
      cov =  X* Xt;
      cov*=w_0c;
      for(i=1;i<N;i++)
	    cov += w * ((sP1[i] - m1) * CPPL::t(sP2[i] - m2));
}
int cholesky(const dsymatrix & A, dgematrix &L){

      const int N=A.n;
      L.resize(N,N);
      L.zero();
  
      int i,j,k;
      double sum,x;

      x=(A(0,0));
      if(x<=0.)
            {
                  return 1;
            }
      L(0,0)=sqrt(x);
      for (i=1;i<N;i++)
	    L(i,0)=A(0,i)/L(0,0);
      i=1;
      for (i=1;i<N;i++){
	    sum=0.;
	    for (k=0;k<i;k++)
		  sum+=L(i,k)*L(i,k);
	    x=(A(i,i)-sum);
	    if(x<=0.)
                  { 
                        return 1;
                  }
	    L(i,i)=sqrt(x);
	    for (j=i+1;j<N;j++){
		  sum=0.;
		  for (k=0;k<i;k++)
			sum+=L(i,k)*L(j,k);
		  L(j,i)=(A(i,j) - sum)/L(i,i);
	    }
      }

      return 0; 
}
int eigen_decomposition(const dgematrix &A, dgematrix & P, dgematrix &D)
{
      int N= A.n;
      vector<double> wr, wi;
      vector<dcovector > vr, vi;
      dgematrix _A(A);
      int i,j;
      if(A.n != A.m)
            {
                  cerr<<"BFilt :: Matrix must be square"<<endl;
                  return 1;
            }
      D.resize(N,N);
      P.resize(N,N);

      if(_A.dgeev(wr,wi,vr,vi))
            return 1;
      P.zero();
      for(j=0; j<P.m; j++)
            { 
                  for(i=0; i<P.n; i++)
                        {
                              P(i,j) = vr[j](i);
                        }
            }
      D.zero();
      for(i=0;i<N;i++)
            {
                  if(wi[i] != 0)
                        {
                              cerr<<"BFilt :: Matrix must be real"<<endl;
                              return 1;
                        }

                  D(i,i) = wr[i];
            }
      return 0;
}
/*! calculate the least-squares-least-norm solution for overdetermined or 
  underdetermined A*x=y using dgelss\n
*/
inline long dgematrix::dgelss(dgematrix& B, dcovector& S, long& RANK,
                              const double RCOND =-1. )
{
#ifdef  CPPL_VERBOSE
  std::cerr << "# [MARK] dgematrix::dgelss(dgematrix&, dcovector&, long& const double)"
            << std::endl;
#endif//CPPL_VERBOSE
  
#ifdef  CPPL_DEBUG
  if(M!=B.M){
    std::cerr << "[ERROR] dgematrix::dgelss"
              << "(dgematrix&, dcovector&, long&, const double) " << std::endl
              << "These matrix and vector cannot be solved." << std::endl
              << "Your input was (" << M << "x" << N << ") and ("
              << B.M << "x" << B.N  << ")." << std::endl;
    exit(1);
  }
#endif//CPPL_DEBUG    
  
  if(M<N){ //underdetermined
    dgematrix tmp(N,B.N);
    for(long i=0; i<B.M; i++){
      for(long j=0; j<B.N; j++){
        tmp(i,j)=B(i,j);
      }
    }
    B.clear();
    swap(B,tmp);
  }
  
  S.resize(min(M,N));
  
  long NRHS(B.N), LDA(M), LDB(B.M),
    LWORK(3*min(M,N)+max(max(2*min(M,N),max(M,N)), NRHS)), INFO(1);
  double *WORK(new double[LWORK]);
  dgelss_(M, N, NRHS, Array, LDA, B.Array, LDB, S.Array,
          RCOND, RANK, WORK, LWORK, INFO);
  delete [] WORK;

  if(INFO!=0){
    std::cerr << "[WARNING] dgematrix::dgelss"
              << "(dgematrix&, docvector&, long, const double) "
              << "Serious trouble happend. INFO = " << INFO << "."
              << std::endl;
  }
  return INFO;
}