std::vector<double> TheEngine::drifts(const std::vector<double>& times, const std::vector<double>& fwd_rtes, size_t p) {
	//take from hunter,jaeckel, joshi, 2001, drift approximations in a fwd_rte_bsd LIBOR mkt model

	std::vector<double> drifts(fwd_rtes.size());

	std::vector<double> tau(times.size() - 1);
	for (size_t i(0); i < fwd_rtes.size() - 1; ++i)	tau[i] = times[i + 1] - times[i];
	
	for (size_t k(0); k < drifts.size(); ++k){

		double tmp(0.0);
	
		if (k < p){
			for (size_t j(k+1); j < p; ++j)
				tmp -= (cov(k,j) * tau[j] * fwd_rtes[j]) / (1.0 + tau[j] * fwd_rtes[j]);
			
		}else if (k > p){
			for (size_t j(p); j <= k; ++j)
				tmp += (cov(k,j) * tau[j] * fwd_rtes[j]) / (1.0 + tau[j] * fwd_rtes[j]);
		}

		drifts[k] = tmp;
	}

	return drifts;
}

 /**
  * Create a Conditional Gaussian distribution with conditional mean function
  * obtained by running RegressionFunction on predictors, responses.
  *
  * @param predictors Matrix of predictors (X).
  * @param responses Vector of responses (y).
  */
 RegressionDistribution(const arma::mat& predictors,
                        const arma::vec& responses) :
     rf(regression::LinearRegression(predictors, responses))
 {
   err = GaussianDistribution(1);
   arma::mat cov(1, 1);
   cov(0, 0) = rf.ComputeError(predictors, responses);
   err.Covariance(std::move(cov));
 }

void
HHKinFit2::HHFitObjectMET::setCovMatrix(double xx, double yy, double xy){
  TMatrixD cov(4,4);
  cov(0,0)=xx;
  cov(1,1)=yy;
  cov(0,1)=xy;
  cov(1,0)=xy;
  m_covmatrix=cov;
}

//if the outline is a circle, this will give a good estimate of the parameters
	//to feed into the fit.
	//if it is not a circle, estimated eccentricity will typically be close to 1.
	void TOutline::estimateParams() {
		if(nPoints < 1) {
			x0 = NAN;
			y0 = NAN;
			r = NAN;
			eccentricity = NAN;
			return;
		} else {
				x0 = 0;
				y0 = 0;
			for(int i=0; i<nPoints; i++) {
				x0 += xValues[i];
				y0 += yValues[i];
			}
			x0/=nPoints;
			y0/=nPoints;
		}
		ublas::matrix<double> cov = calculateCovariance2D(xValues, yValues, nPoints);
		//calculate eigenvalues of covariane matrix
		double a = cov(0,0);
		double b = cov(0,1);
		double c = cov(1,0);
		double d = cov(1,1);

		if(_isnan(a) || _isnan(b)  || _isnan(c)  || _isnan(d)) {
			r = NAN;
			eccentricity = NAN;
			return;
		}
		double root = 4 * b*c + (a - d) * (a-d);
		if(root < 0.0) root = 0;
		else root = sqrt(root);
		double e1 = (a+d + root)/2; //eigenvalue 1 = major radius squared /2
		double e2 = (a+d - root)/2; //eigenvalue 2 = minor radius squared /2


		r = (e1 + e2);
		if(r < 0.0) r = 0.0;
		else r = sqrt(r);

		if (e1<e2) {
			double term = 1-e1/e2;
			term = (term > 0) ? term : 0;
			eccentricity = sqrt(term);
		} else if(e2 < e1) {
			double term = 1-e2/e1;
			term = (term > 0) ? term : 0;
			eccentricity = sqrt(term);
		} else {
			eccentricity = 0.0;
		}
	}

void Gaussian_3d::zero_init(){

    for(int i=0; i<3; i++){
        mean[i] = 0.0;

        for(int j=0; j<3; j++){
            if(i==j)
                cov(i,j) = 1.0;
            else
                cov(i,j) = 0;
        }
    }
}

static MAT CalcShapeCov(
    const vec_Shape& shapes,    // in
    const Shape&     meanshape) // in
{
    int npoints = meanshape.rows;
    int i, j;

    MAT cov(2 * npoints, 2 * npoints, 0.); // covariance of every x and y coord
    MAT nused(npoints, npoints, 0.);

    // Because of possible unused points, we have to iterate by
    // hand (we can't use standard matrix multiplies).

    for (int ishape = 0; ishape < NSIZE(shapes); ishape++)
    {
        Shape shape(shapes[ishape]);
        for (i = 0; i < npoints; i++)
            if (PointUsed(shape, i))
                for (j = 0; j < npoints; j++)
                    if (PointUsed(shape, j))
                    {
                        cov(2*i,   2*j)   +=  // x * x
                            (shape(i, IX) - meanshape(i, IX)) *
                            (shape(j, IX) - meanshape(j, IX));

                        cov(2*i+1, 2*j)   +=  // y * x
                            (shape(i, IY) - meanshape(i, IY)) *
                            (shape(j, IX) - meanshape(j, IX));

                        cov(2*i,   2*j+1) +=  // x * y
                            (shape(i, IX) - meanshape(i, IX)) *
                            (shape(j, IY) - meanshape(j, IY));

                        cov(2*i+1, 2*j+1) +=  // y * y
                            (shape(i, IY) - meanshape(i, IY)) *
                            (shape(j, IY) - meanshape(j, IY));

                        nused(i, j)++;
                    }
       }

    for (i = 0; i < npoints; i++)
        for (j = 0; j < npoints; j++)
        {
            const double n = nused(i, j);
            if (n < 3) // 3 is somewhat arb
                Err("Cannot calculate covariance of %g shape%s (need more shapes)",
                    n, plural(int(n)));
            cov(2*i,   2*j)   /=  n;
            cov(2*i+1, 2*j)   /=  n;
            cov(2*i,   2*j+1) /=  n;
            cov(2*i+1, 2*j+1) /=  n;
        }

    return cov;
}

float
Wavelet::getWaveletValue(float   z,
                         float * Wavelet,
                         int     center,
                         int     nz,
                         float   dz)
{
  // returns the value of the vavelet in the location z. Wavelet have the length nz
  // and the center value is Wavelet[center]
  // uses kriging with ricker 20Hz wavelet as correlation function.

  NRLib::IntegerVector ind(6);  // iL1,iL2,iL3,iR1,iR2,iR3;
  NRLib::Vector        val(6);  // vL1,vL2,vL3,vR1,vR2,vR3;

  ind(2) = static_cast<int>( floor( (z/dz) ) );
  for(int k=0 ; k<6 ; k++)
    ind(k) = ind(2) + k-2;

  for(int k=0;k<6;k++)
    val(k) = getArrayValueOrZero(ind(k) + center, Wavelet,  nz);

  NRLib::SymmetricMatrix Cov = NRLib::SymmetricZeroMatrix(6);
  NRLib::Vector cov(6);
  NRLib::Vector x(6);

  double   nu  = 20;
  double   deltaT;
  double   factor;

  for (int k=0 ; k<6 ; k++)
  {
    deltaT = (dz*ind(k) - z)*0.001;
    factor = nu*nu*NRLib::Pi*NRLib::Pi*deltaT*deltaT;
    cov(k) = (1-2*factor)*exp(-factor);
    for (int l=0 ; l<=k ; l++)
    {
      deltaT   = (dz*ind(k) - dz*ind(l))*0.001;
      factor   = nu*nu*NRLib::Pi*NRLib::Pi*deltaT*deltaT;
      Cov(l,k) = (1-2*factor)*exp(-factor);
    }
  }
  //OK not very intellegent implementation since chol is done for each time step.

  NRLib::CholeskySolve(Cov, cov, x);

  float value = static_cast<float>(val * x);

  return value;
}

    tmv::SymMatrix<double, tmv::FortranStyle|tmv::Upper> calculateCovarianceSymMatrix(
        const SBProfile& sbp, const Bounds<int>& bounds, double dx)
    {
        // Calculate the required dimensions
        int idim = 1 + bounds.getXMax() - bounds.getXMin();
        int jdim = 1 + bounds.getYMax() - bounds.getYMin();
        int covdim = idim * jdim;

        int k, ell; // k and l are indices that refer to image pixel separation vectors in the 
                    // correlation func.
        double x_k, y_ell; // physical vector separations in the correlation func, dx * k etc.

        tmv::SymMatrix<double, tmv::FortranStyle|tmv::Upper> cov = tmv::SymMatrix<
            double, tmv::FortranStyle|tmv::Upper>(covdim);

        for (int i=1; i<=covdim; i++){ // note that the Image indices use the FITS convention and 
                                       // start from 1!!
            for (int j=i; j<=covdim; j++){

                k = ((j - 1) / jdim) - ((i - 1) / idim);  // using integer division rules here
                ell = ((j - 1) % jdim) - ((i - 1) % idim);
                x_k = double(k) * dx;
                y_ell = double(ell) * dx;
                Position<double> p = Position<double>(x_k, y_ell);
                cov(i, j) = sbp.xValue(p); // fill in the upper triangle with the correct value

            }

        }
        return cov;
    }

double BorderMattingHandler::dataTermForPixel(const Vec3f &pixel, const double alpha, const Gauss &bgd, const Gauss &fgd){
    Vec3f miu;
    Mat cov(3,3,CV_64FC1);
    miu = (1-alpha)*fgd.getmean() + alpha*bgd.getmean();
    cov = (1-alpha)*(1-alpha)*fgd.getcovmat() + alpha*alpha*bgd.getcovmat();
    return Gauss::probability(miu, cov, pixel);
}

// Write statistics to binary file. Pass std::ios::app as writemode to append to end of existing file.
bool TStats::write_binary_old(std::string fname, std::ios::openmode writemode) const {
	std::fstream f;
	f.open(fname.c_str(), writemode | std::ios::out | std::ios::binary);
	if(!f) { f.close(); return false; }	// Return false if the file could not be opened
	
	// Write number of dimensions
	f.write(reinterpret_cast<const char*>(&N), sizeof(N));
	
	// Write mean values
	double tmp;
	for(unsigned int i=0; i<N; i++) {
		tmp = mean(i);
		f.write(reinterpret_cast<char*>(&tmp), sizeof(tmp));
	}
	
	// Write upper triangle (including diagonal) of covariance matrix
	for(unsigned int i=0; i<N; i++) {
		for(unsigned int j=i; j<N; j++) {
			tmp = cov(i, j);
			f.write(reinterpret_cast<char*>(&tmp), sizeof(tmp));
		}
	}
	
	// Write raw data
	f.write(reinterpret_cast<char*>(E_k), N * sizeof(double));
	f.write(reinterpret_cast<char*>(E_ij), N*N * sizeof(double));
	f.write(reinterpret_cast<const char*>(&N_items_tot), sizeof(N_items_tot));
	
	// Return false if there was a write error, else true
	if(!f) { f.close(); return false; }
	f.close();
	return true;
}

int main(int argc, char ** argv)
{
  if (argc != 3) {
    std::cerr << "Usage: chol <num_threads> <matrix_size>" << std::endl;
    return 1;
  }

  omp_set_num_threads(atoi(argv[1]));

  double L = 1.0;
  unsigned int N = atoi(argv[2]);
  double dx = (double) L / (N - 1);

  double * A = (double *)malloc(N * N * sizeof(double));
  for (unsigned int i = 0; i < N; i++) {
    for (unsigned int j = 0; j < N; j++) {
      A[i*N+j] = cov(i*dx, j*dx);
    }
  }

  double start = omp_get_wtime();

  // We're letting MKL choose the workspace array at runtime
  // Assume unsigned int N converts to lapack_int
  int info = LAPACKE_dpotrf(LAPACK_ROW_MAJOR, 'U', N, A, N);

  double time = omp_get_wtime() - start;

  std::cout << "LAPACKE_dpotrf executed in " << time << " secs." << std::endl;

  std::cout << "LAPACKE_dpotrf return value is: " << info << std::endl;

  free(A);
  return info;
}

	static void Statistics (dgSphere &sphere, dgVector &eigenValues, dgVector &scaleVector, const hacd::HaF32 vertex[], hacd::HaI32 vertexCount, hacd::HaI32 stride)
	{
		dgBigVector var (hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f));
		dgBigVector cov (hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f));
		dgBigVector massCenter (hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f));
	
		const hacd::HaF32* ptr = vertex;
		for (hacd::HaI32 i = 0; i < vertexCount; i ++) {
			hacd::HaF32 x = ptr[0] * scaleVector.m_x;
			hacd::HaF32 y = ptr[1] * scaleVector.m_y;
			hacd::HaF32 z = ptr[2] * scaleVector.m_z;
			ptr += stride;
			massCenter += dgBigVector (x, y, z, hacd::HaF32 (0.0f));
			var += dgBigVector (x * x, y * y, z * z, hacd::HaF32 (0.0f));
			cov += dgBigVector (x * y, x * z, y * z, hacd::HaF32 (0.0f));
		}
	
		hacd::HaF64 k = hacd::HaF64 (1.0) / vertexCount;
		var = var.Scale (k);
		cov = cov.Scale (k);
		massCenter = massCenter.Scale (k);
	
		hacd::HaF64 Ixx = var.m_x - massCenter.m_x * massCenter.m_x;
		hacd::HaF64 Iyy = var.m_y - massCenter.m_y * massCenter.m_y;
		hacd::HaF64 Izz = var.m_z - massCenter.m_z * massCenter.m_z;
	
		hacd::HaF64 Ixy = cov.m_x - massCenter.m_x * massCenter.m_y;
		hacd::HaF64 Ixz = cov.m_y - massCenter.m_x * massCenter.m_z;
		hacd::HaF64 Iyz = cov.m_z - massCenter.m_y * massCenter.m_z;
	
		sphere.m_front = dgVector (hacd::HaF32(Ixx), hacd::HaF32(Ixy), hacd::HaF32(Ixz), hacd::HaF32 (0.0f));
		sphere.m_up    = dgVector (hacd::HaF32(Ixy), hacd::HaF32(Iyy), hacd::HaF32(Iyz), hacd::HaF32 (0.0f));
		sphere.m_right = dgVector (hacd::HaF32(Ixz), hacd::HaF32(Iyz), hacd::HaF32(Izz), hacd::HaF32 (0.0f));
 		sphere.EigenVectors (eigenValues);
	}

Measurement* getMeasurement(bhep::hit& hit)
{
  EVector hit_pos(2,0);
  EVector meas_pos(3,0);
  
  meas_pos[0] = hit.x()[0];
  meas_pos[1] = hit.x()[1];
  meas_pos[2] = hit.x()[2];
  
  hit_pos[0] = meas_pos[0];
  hit_pos[1] = meas_pos[1];

  //covariance and meastype hardwired for now.
  EMatrix cov(2,2,0);
  cov[0][0] = 1.; cov[1][1] = 1.;
  string meastype = "xy";

  Measurement* me = new Measurement();
  me->set_name(meastype);
  me->set_hv(HyperVector(hit_pos,cov));
  me->set_name("volume", "Detector");
  me->set_position( meas_pos );

  //Add the hit energy deposit as a key to the Measurement.
  const dict::Key Edep = "E_dep";
  const dict::Key EdepVal = hit.data("E_dep");
  me->set_name(Edep, EdepVal);

  return me;
}

Matrix getSampleVar(const Matrix &train_data, const std::vector<double>& mean){
  assert(train_data.size() >= 1);
  assert(train_data[0].size() >= 1);
  assert(mean.size() >= 1);
  assert(train_data[0].size() == mean.size());

  int dim = train_data[0].size();
  int train_size = train_data.size();
  Matrix cov(dim, std::vector<double>(dim));

  std::vector<double> temp(dim);

  for(int point = 0; point < train_size; point++){
    temp = train_data[point] - mean;
    for(int col = 0; col < dim; col++){
      for(int row = 0; row < dim; row++){
        if(!std::isnan(temp[row])){
          cov[row][col] +=  temp[row] * temp[col] / train_size;
        }
      }
    }
  }

  return cov;
}

// Calculates the covariance matrix Sigma, alongside Sigma^{-1} and det(Sigma)
void TStats::get_cov_matrix(gsl_matrix* Sigma, gsl_matrix* invSigma, double* detSigma) const {
	// Check that the matrices are the correct size
	assert(Sigma->size1 == N);
	assert(Sigma->size2 == N);
	assert(invSigma->size1 == N);
	assert(invSigma->size2 == N);
	
	// Calculate the covariance matrix Sigma
	double tmp;
	for(unsigned int i=0; i<N; i++) {
		for(unsigned int j=i; j<N; j++) {
			tmp = cov(i,j);
			gsl_matrix_set(Sigma, i, j, tmp);
			if(i != j) { gsl_matrix_set(Sigma, j, i, tmp); }
		}
	}
	
	// Get the inverse of Sigma
	int s;
	gsl_permutation* p = gsl_permutation_alloc(N);
	gsl_matrix* LU = gsl_matrix_alloc(N, N);
	gsl_matrix_memcpy(LU, Sigma);
	gsl_linalg_LU_decomp(LU, p, &s);
	gsl_linalg_LU_invert(LU, p, invSigma);
	
	// Get the determinant of sigma
	*detSigma = gsl_linalg_LU_det(LU, s);
	
	// Cleanup
	gsl_matrix_free(LU);
	gsl_permutation_free(p);
}

/*
 * Fills in a new HLL with a default prior.
 */
static void hll_default_prior(hll_t *hll)
{
  int dx = hll->dx;
  int n = hll->n;

  safe_calloc(hll->x0, dx, double);
  mean(hll->x0, hll->X, n, dx);  // x0 = mean(X)

  hll->S0 = new_matrix2(dx, dx);
  cov(hll->S0, hll->X, hll->x0, n, dx);

  /*
  int i;
  double d2_tot = 0.0;
  for (i = 0; i < n; i++) {
    double d = norm(hll->X[i], dx);
    d2_tot += d*d;
  }
  double sigma = d2_tot/(3.0*n);
  for (i = 0; i < dx; i++)
    hll->S0[i][i] = sigma;
  */

  hll->w0 = 2;

  /*
    x0 = zeros(1,3);
    S0 = mean(sum(X.^2,2))/3 * eye(3);
    w0 = 2;
  */
}

void energy_test(std::initializer_list<double> x_, const double e_true)
{
    pele::Array<double> x(x_);
    pele::Array<double> mean(x.size(), 0);
    pele::Array<double> cov(x.size(), 1);
    pele::GaussianPot gauss(mean, cov);
    EXPECT_DOUBLE_EQ(e_true, gauss.get_energy(x));
}

void simple_cokriging_markI(
    const sugarbox_grid_t & grid,
    const cont_property_array_t & input_prop,
    const cont_property_array_t & secondary_data,
    mean_t primary_mean,
    mean_t secondary_mean,
    double secondary_variance,
    double correlation_coef,
    const neighbourhood_param_t & neighbourhood_params,
    const covariance_param_t & primary_cov_params,
    cont_property_array_t & output_prop)
{
    if (input_prop.size() != output_prop.size())
        throw hpgl_exception("simple_cokriging", boost::format("Input data size: %s. Output data size: %s. Must be equal.") % input_prop.size() % output_prop.size());

    print_algo_name("Simple Colocated Cokriging Markov Model I");
    print_params(neighbourhood_params);
    print_params(primary_cov_params);
    print_param("Primary mean", primary_mean);
    print_param("Secondary mean", secondary_mean);
    print_param("Secondary variance", secondary_variance);
    print_param("Correllation coef", correlation_coef);

    cov_model_t cov(primary_cov_params);

    cross_cov_model_mark_i_t<cov_model_t> cross_cov(correlation_coef, secondary_variance, &cov);

    int data_size = input_prop.size();

    neighbour_lookup_t<sugarbox_grid_t, cov_model_t> n_lookup(&grid, &cov, neighbourhood_params);

    progress_reporter_t report(data_size);

    report.start(data_size);

    // for each node
    for (node_index_t i = 0; i < data_size; ++i)
    {
        // 		calc value
        cont_value_t result = -500;
        if (input_prop.is_informed(i))
        {
            result = input_prop[i];
        }
        else
        {
            cont_value_t secondary_value = secondary_data.is_informed(i) ? secondary_data[i] : secondary_mean;
            if (!calc_value(i, input_prop, secondary_value, primary_mean, secondary_mean,
                            secondary_variance, cov, cross_cov, n_lookup, result))
            {
                result = primary_mean + secondary_value - secondary_mean;
            }
        }
        // 		set value at node
        output_prop.set_at(i, result);
        report.next_lap();
    }
}

void  TheEngine::GetOnePath(std::vector<std::vector<double>>& fwd_rtes, MJArray& z, method m){
	//Generate log of fwd rate, yt = log(ft)
	//correlated normal distr. of perturbations
	//plain mc. no acceleration technique.

	//substepping
	//unsigned int p((unsigned long) (the_product->GetEvolutionTimes()[0] / sub_stepping)); //assume time interval equally spaced
	
	unsigned int p(the_product->GetEvolutionTimes().size() / sub_stepping);
	
	

	for (size_t i(0); i < the_product->GetEvolutionTimes().size(); ++i){
		//log-Euler scheme
		// taken from hunter, jaeckel, joshi, 2001, drift approx. in a fwd-rte bsd libor mkt model

		for (size_t j(0); j < fwd_rtes.size(); ++j){

			double tmp(fwd_rtes[j][i]);

			for (size_t k(0); k < p; ++k){ //sub stepping
				the_generator->GetGaussians(z);
				tmp *= exp(mu[j] - .5 * cov(j, j) + StochasticTerm(A, j, z));
			}
			fwd_rtes[j][i + 1] = tmp;
		}
		
		if (m == PC){
			//predictor-corrector method
			//two steps 1)prediction, 2) correction
			//fwd rte prediction  step, use Euler results

			the_generator->GetGaussians(z);

			//assign rates time i to vector
			std::vector<double> _fwd_rtes(fwd_rtes.size());
			for (size_t j(0); j < fwd_rtes.size(); ++j)	_fwd_rtes[j] = fwd_rtes[j][i+1];

			std::vector<double> mu_t = drifts(times, _fwd_rtes, numeraire);

			for (size_t j(0); j < fwd_rtes.size(); ++j)
				fwd_rtes[j][i + 1] = fwd_rtes[j][i] * exp( .5*(mu[j] + mu_t[j] - cov(j,j)) + StochasticTerm(A, j, z));
		}
	}
}

inline tinymat<qm_real,2,2>
atsmodel<N>::fwdratecovar(qm_real t, qm_real T1, qm_real tau1, qm_real T2, qm_real tau2) const {
	tinymat<qm_real,2,2> cov;
	REXPONENT  re1 = this->fwdexp(t,T1,T1+tau1,1.0);
	REXPONENT  re2 = this->fwdexp(t,T2,T2+tau2,1.0);
	qm_real F1  = (std::exp(re1->value(m_model->charFactors())) - 1.0)/tau1;
	qm_real F2  = (std::exp(re2->value(m_model->charFactors())) - 1.0)/tau2;
	qm_real cv  = m_model->affine_covariance(re1->b,re2->b,m_model->charFactors());
	qm_real v1  = m_model->affine_covariance(re1->b,re1->b,m_model->charFactors());
	qm_real v2  = m_model->affine_covariance(re2->b,re2->b,m_model->charFactors());
	qm_real f1  = (tau1*F1 + 1.)/tau1;
	qm_real f2  = (tau2*F2 + 1.)/tau2;
	cov(0,0) = f1*f1*v1;
	cov(1,1) = f2*f2*v2;
	cov(0,1) = f1*f2*cv;
	cov(1,0) = cov(0,1);
	return cov;
}