/*! calculate the least-squares-least-norm solution for overdetermined or 
  underdetermined A*x=y using dgelss\n
*/
inline long dgematrix::dgelss(dcovector& B, dcovector& S, long& RANK,
                              const double RCOND =-1. )
{
#ifdef  CPPL_VERBOSE
  std::cerr << "# [MARK] dgematrix::dgelss(dcovector&, dcovector&, long&, const double)"
            << std::endl;
#endif//CPPL_VERBOSE
  
#ifdef  CPPL_DEBUG
  if(M!=B.L){
    std::cerr << "[ERROR] dgematrix::dgelss"
              << "(dcovector&, dcovector&, long, double) " << std::endl
              << "These matrix and vector cannot be solved." << std::endl
              << "Your input was (" << M << "x" << N << ") and ("
              << B.L << ")." << std::endl;
    exit(1);
  }
#endif//CPPL_DEBUG    
  
  if(M<N){ //underdetermined
    dcovector tmp(N);
    for(long i=0; i<B.L; i++){ tmp(i)=B(i); }
    B.clear();
    swap(B,tmp);
  }
  
  S.resize(min(M,N));
  
  long NRHS(1), LDA(M), LDB(B.L),
    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"
              << "(dcovector&, docvector&, long, const double) "
              << "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 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;
}