// take a matrix and return its sqare root
//void corset( const TMatrixD& V, TMatrixD& C );

// Given the sqrt of the covariance matrix, generate correlated random numbers.  This assumes the mean of all variables is 0.  If it is not, you'll 
//void corgen( const TMatrixD& C, DVec& x );

TMatrixD get_corr_matr(const TMatrixD matr_cov) {

  TMatrixD matr = matr_cov;
  matr.Zero();
  Int_t n_rows = matr.GetNrows();
  for (Int_t ii = 0; ii < n_rows; ii++) {
    for (Int_t jj = 0; jj < n_rows; jj++) {
      matr(ii,jj) = matr_cov(ii,jj) 
								/ ( sqrt(matr_cov(ii,ii)) * sqrt(matr_cov(jj,jj)) );
    }
  }
  return matr;

}
void corset( const TMatrixD& V, TMatrixD& C ){ 
  // calculate sqrt(V) as lower diagonal matrix


		int NP =V.GetNcols();


  for( int i = 0; i < NP; ++i ) {
    for( int j = 0; j < NP; ++j ) {
      C[i][j] = 0;
    }
  }

  for( int j = 0; j < NP; ++j ) {
    // diagonal terms first
    double Ck = 0;
    for( int k = 0; k < j; ++k ) {
      Ck += C[j][k] * C[j][k];
    } // k 
    C[j][j] = sqrt( fabs( V[j][j] - Ck ) );

    // off-diagonal terms
    for( int i = j+1; i < NP; ++i ) {
      Ck = 0;
      for( int k = 0; k < j; ++k ) {
   Ck += C[i][k] * C[j][k];
      } //k
      C[i][j] = ( V[i][j] - Ck ) / C[j][j];
    }// i 
  } // j
}

void corgen( const TMatrixD& C, double * x){

  int NP = 3;

		double *z =new double[NP];

		
  // np random numbers from unit Gaussian
  for( int i = 0; i < NP; ++i ) 
				z[i] = gRandom->Gaus( 0.0, 1.0 );

				

  for( int i = 0; i < NP; ++i ) {
    x[i] = 0;
    for( int j = 0; j <= i; ++j ) {
      x[i] += C[i][j] * z[j];

    } // j
  } // i


		delete [] z; //free the array
}


void test_3 (){

		
		double fitpar[3] = {3.036010,
																						2.905944,
																						5.582753};

		double errpar[3] = {0.376452,
																						0.157458,
																						1.068356};


		double cov[9] = {0.141846,
																			-0.011840,
																			-0.144900,
																			-0.011840,
																			0.024803,
																			0.158485,
																			-0.144900,
																			0.158485,
																			1.143070};

	

		int NP = 3;
		int npar = NP;
		matr_cov = TMatrixD(npar,npar,cov);
		
		TMatrixD matr_corr    = get_corr_matr(matr_cov); 

	


		TMatrixD sqrtCov( NP, NP );
		corset( matr_cov, sqrtCov );



		int nrand = 200;
		
		double *mean = new double[npar];
		double *rms  = new double[npar];
		double *newpar = new double[npar*nrand];

		double *values = new double[NP];
	

		/* initilize mean and rms */
		for(int j=0;j<npar;j++){mean[j]=0; rms[j]=0;values[j]=0;}



		// --- this is the main loop
		for (int loop = 0; loop < nrand; ++loop){
				
			
				corgen (sqrtCov, values);				

				//--sum the mean to the parameters
				for (int i=0;i<NP;i++) {

						values[i]+=fitpar[i];
						mean[i]+=values[i]/nrand;
				
						newpar[npar*loop+i] = values[i];
				
				}
		} /* end of Monte Carlo loop */


			/* check correlation factors */
		TGraph *g1 = new TGraph(nrand);
		TGraph *g2 = new TGraph(nrand);
		TGraph *g3 = new TGraph(nrand);
		
		/* compute the rms */
		for(int i=0;i<nrand;i++){
				g1->SetPoint(i,newpar[npar*i+0], newpar[npar*i+1]);
				g2->SetPoint(i,newpar[npar*i+1], newpar[npar*i+2]);
				g3->SetPoint(i,newpar[npar*i+0], newpar[npar*i+2]);
				
				for(int j=0;j<npar;j++)
						rms[j]+=(newpar[npar*i+j] -mean[j]) * (newpar[npar*i+j]-mean[j])/nrand;
		}
		
		TH1F * h =new TH1F("a","a",30,-3,10);
		for (int j=0;j<nrand;j++)
				h->Fill(newpar[npar*j+0]);
		h->Draw();


		cout<<"HELLO"<<endl;


		/* print out debuggin lines */
		for(int j=0;j<npar;j++)
				cout<<"Par "<<j<<" Mean "<<mean[j]<<" RMS="<<sqrt(rms[j])<<endl;
				

		/* print Correlation factors */
		cout<<"Correlation 0_1 "<<g1->GetCorrelationFactor()<<
				" while it should be "<<matr_corr(0,1)<<endl;

		cout<<"Correlation 1_2 "<<g2->GetCorrelationFactor()<<
				" while it should be "<<matr_corr(1,2)<<endl;

		cout<<"Correlation 0_2 "<<g3->GetCorrelationFactor()<<
				" while it should be "<<matr_corr(0,2)<<endl;



}