/*! 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;
}
int Unscented_Kalman_Filter::U_Mean(const vector<dcovector> &sP, 
			    dcovector & mean)
{
      int N=sP[0].l;
      int i;
      mean.resize(N);
      mean=sP[0]*w_0;
      N=sP.size();
      for (i=1;i<N;i++)
	    mean +=w * (sP[i]);
}
int multivariate_normal_draw(dcovector & X, gsl_rng * r, const dsymatrix & Q){
      X.resize(Q.n);
      dgematrix L;
      int i;
      for (i=0;i<X.l;i++)
	    X(i)=gsl_ran_gaussian(r,1.);
      if(  cholesky(Q,L)){
	    X.zero();
	    return 1;
      }
      X=L*X;
      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;
}
int multivariate_normal_draw(dcovector & X, gsl_rng * r, const dgematrix & Q){
      X.resize(Q.n);
      dgematrix L;
      int i;
      int j;
      int N=Q.m;
      dsymatrix S(N);
      for (i=0;i<X.l;i++)
	    X(i)=gsl_ran_gaussian(r,1.);
      
      for (j=0;j<N;j++)
	    for(i=j;i<N;i++)
		  S(i,j)=Q(i,j);

      if(  cholesky(S,L)){
	    X.zero();
	    return 1;
      }
      X=L*X;
      return 0;
}