/*
 Genome-wide Efficient Mixed Model Association (GEMMA)
 Copyright (C) 2011-2017, Xiang Zhou

 This program is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.

 This program is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU General Public License for more details.

 You should have received a copy of the GNU General Public License
 along with this program. If not, see <http://www.gnu.org/licenses/>.
*/

#include <iostream>
#include <fstream>
#include <sstream>

#include <iomanip>
#include <cmath>
#include <iostream>
#include <stdio.h>
#include <stdlib.h>
#include <ctime>
#include <cstring>
#include <algorithm>

#include "gsl/gsl_vector.h"
#include "gsl/gsl_matrix.h"
#include "gsl/gsl_linalg.h"
#include "gsl/gsl_blas.h"
#include "gsl/gsl_eigen.h"
#include "gsl/gsl_randist.h"
#include "gsl/gsl_cdf.h"
#include "gsl/gsl_roots.h"

#include "lapack.h"
#include "param.h"
#include "bslmm.h"
#include "lmm.h"
#include "lm.h"
#include "mathfunc.h"

using namespace std;

void BSLMM::CopyFromParam (PARAM &cPar) {
	a_mode=cPar.a_mode;
	d_pace=cPar.d_pace;

	file_bfile=cPar.file_bfile;
	file_geno=cPar.file_geno;
	file_out=cPar.file_out;
	path_out=cPar.path_out;

	l_min=cPar.h_min;
	l_max=cPar.h_max;
	n_region=cPar.n_region;
	pve_null=cPar.pve_null;
	pheno_mean=cPar.pheno_mean;

	time_UtZ=0.0;
	time_Omega=0.0;
	n_accept=0;

	h_min=cPar.h_min;
	h_max=cPar.h_max;
	h_scale=cPar.h_scale;
	rho_min=cPar.rho_min;
	rho_max=cPar.rho_max;
	rho_scale=cPar.rho_scale;
	logp_min=cPar.logp_min;
	logp_max=cPar.logp_max;
	logp_scale=cPar.logp_scale;

	s_min=cPar.s_min;
	s_max=cPar.s_max;
	w_step=cPar.w_step;
	s_step=cPar.s_step;
	r_pace=cPar.r_pace;
	w_pace=cPar.w_pace;
	n_mh=cPar.n_mh;
	geo_mean=cPar.geo_mean;
	randseed=cPar.randseed;
	trace_G=cPar.trace_G;

	ni_total=cPar.ni_total;
	ns_total=cPar.ns_total;
	ni_test=cPar.ni_test;
	ns_test=cPar.ns_test;
	n_cvt=cPar.n_cvt;

	indicator_idv=cPar.indicator_idv;
	indicator_snp=cPar.indicator_snp;
	snpInfo=cPar.snpInfo;

	return;
}

void BSLMM::CopyToParam (PARAM &cPar) {
	cPar.time_UtZ=time_UtZ;
	cPar.time_Omega=time_Omega;
	cPar.time_Proposal=time_Proposal;
	cPar.cHyp_initial=cHyp_initial;
	cPar.n_accept=n_accept;
	cPar.pheno_mean=pheno_mean;
	cPar.randseed=randseed;

	return;
}

void BSLMM::WriteBV (const gsl_vector *bv) {
	string file_str;
	file_str=path_out+"/"+file_out;
	file_str+=".bv.txt";

	ofstream outfile (file_str.c_str(), ofstream::out);
	if (!outfile) {
	  cout<<"error writing file: "<<file_str.c_str()<<endl;
	  return;
	}

	size_t t=0;
	for (size_t i=0; i<ni_total; ++i) {
		if (indicator_idv[i]==0) {
			outfile<<"NA"<<endl;
		}
		else {
			outfile<<scientific<<setprecision(6)<<
			  gsl_vector_get(bv, t)<<endl;
			t++;
		}
	}

	outfile.clear();
	outfile.close();
	return;
}

void BSLMM::WriteParam (vector<pair<double, double> > &beta_g,
			const gsl_vector *alpha, const size_t w) {
	string file_str;
	file_str=path_out+"/"+file_out;
	file_str+=".param.txt";

	ofstream outfile (file_str.c_str(), ofstream::out);
	if (!outfile) {
	  cout<<"error writing file: "<<file_str.c_str()<<endl;
	  return;}

	outfile<<"chr"<<"\t"<<"rs"<<"\t"
			<<"ps"<<"\t"<<"n_miss"<<"\t"<<"alpha"<<"\t"
			<<"beta"<<"\t"<<"gamma"<<endl;

	size_t t=0;
	for (size_t i=0; i<ns_total; ++i) {
		if (indicator_snp[i]==0) {continue;}

		outfile<<snpInfo[i].chr<<"\t"<<snpInfo[i].rs_number<<"\t"
		<<snpInfo[i].base_position<<"\t"<<snpInfo[i].n_miss<<"\t";

		outfile<<scientific<<setprecision(6)<<
		  gsl_vector_get(alpha, t)<<"\t";
		if (beta_g[t].second!=0) {
			outfile<<beta_g[t].first/beta_g[t].second<<
			  "\t"<<beta_g[t].second/(double)w<<endl;
		}
		else {
			outfile<<0.0<<"\t"<<0.0<<endl;
		}
		t++;
	}

	outfile.clear();
	outfile.close();
	return;
}

void BSLMM::WriteParam (const gsl_vector *alpha) {
	string file_str;
	file_str=path_out+"/"+file_out;
	file_str+=".param.txt";

	ofstream outfile (file_str.c_str(), ofstream::out);
	if (!outfile) {
	  cout<<"error writing file: "<<file_str.c_str()<<endl;
	  return;
	}

	outfile<<"chr"<<"\t"<<"rs"<<"\t"
			<<"ps"<<"\t"<<"n_miss"<<"\t"<<"alpha"<<"\t"
			<<"beta"<<"\t"<<"gamma"<<endl;

	size_t t=0;
	for (size_t i=0; i<ns_total; ++i) {
		if (indicator_snp[i]==0) {continue;}

		outfile<<snpInfo[i].chr<<"\t"<<snpInfo[i].rs_number<<"\t"<<
	          snpInfo[i].base_position<<"\t"<<snpInfo[i].n_miss<<"\t";
		outfile<<scientific<<setprecision(6)<<
		  gsl_vector_get(alpha, t)<<"\t";
		outfile<<0.0<<"\t"<<0.0<<endl;
		t++;
	}

	outfile.clear();
	outfile.close();
	return;
}

void BSLMM::WriteResult (const int flag, const gsl_matrix *Result_hyp,
			 const gsl_matrix *Result_gamma, const size_t w_col) {
	string file_gamma, file_hyp;
	file_gamma=path_out+"/"+file_out;
	file_gamma+=".gamma.txt";
	file_hyp=path_out+"/"+file_out;
	file_hyp+=".hyp.txt";

	ofstream outfile_gamma, outfile_hyp;

	if (flag==0) {
		outfile_gamma.open (file_gamma.c_str(), ofstream::out);
		outfile_hyp.open (file_hyp.c_str(), ofstream::out);
		if (!outfile_gamma) {
		  cout<<"error writing file: "<<file_gamma<<endl;
		  return;
		}
		if (!outfile_hyp) {
		  cout<<"error writing file: "<<file_hyp<<endl;
		  return;
		}

		outfile_hyp<<"h \t pve \t rho \t pge \t pi \t n_gamma"<<endl;

		for (size_t i=0; i<s_max; ++i) {
			outfile_gamma<<"s"<<i<<"\t";
		}
		outfile_gamma<<endl;
	}
	else {
		outfile_gamma.open (file_gamma.c_str(), ofstream::app);
		outfile_hyp.open (file_hyp.c_str(), ofstream::app);
		if (!outfile_gamma) {
		  cout<<"error writing file: "<<file_gamma<<endl;
		  return;
		}
		if (!outfile_hyp) {
		  cout<<"error writing file: "<<file_hyp<<endl;
		  return;
		}

		size_t w;
		if (w_col==0) {w=w_pace;}
		else {w=w_col;}

		for (size_t i=0; i<w; ++i) {
			outfile_hyp<<scientific;
			for (size_t j=0; j<4; ++j) {
				outfile_hyp<<setprecision(6)<<
				  gsl_matrix_get (Result_hyp, i, j)<<"\t";
			}
			outfile_hyp<<setprecision(6)<<
			  exp(gsl_matrix_get (Result_hyp, i, 4))<<"\t";
			outfile_hyp<<(int)gsl_matrix_get(Result_hyp,i,5)<<"\t";
			outfile_hyp<<endl;
		}

		for (size_t i=0; i<w; ++i) {
			for (size_t j=0; j<s_max; ++j) {
				outfile_gamma<<
				  (int)gsl_matrix_get(Result_gamma,i,j)<<"\t";
			}
			outfile_gamma<<endl;
		}

	}

	outfile_hyp.close();
	outfile_hyp.clear();
	outfile_gamma.close();
	outfile_gamma.clear();
	return;
}

void BSLMM::CalcPgamma (double *p_gamma) {
	double p, s=0.0;
	for (size_t i=0; i<ns_test; ++i) {
		p=0.7*gsl_ran_geometric_pdf (i+1, 1.0/geo_mean)+0.3/
		  (double)ns_test;
		p_gamma[i]=p;
		s+=p;
	}
	for (size_t i=0; i<ns_test; ++i) {
		p=p_gamma[i];
		p_gamma[i]=p/s;
	}
	return;
}

void BSLMM::SetXgamma (gsl_matrix *Xgamma, const gsl_matrix *X,
		       vector<size_t> &rank) {
	size_t pos;
	for (size_t i=0; i<rank.size(); ++i) {
		pos=mapRank2pos[rank[i]];
		gsl_vector_view Xgamma_col=gsl_matrix_column (Xgamma, i);
		gsl_vector_const_view X_col=gsl_matrix_const_column (X, pos);
		gsl_vector_memcpy (&Xgamma_col.vector, &X_col.vector);
	}

	return;
}

double BSLMM::CalcPveLM (const gsl_matrix *UtXgamma, const gsl_vector *Uty,
			 const double sigma_a2) {
	double pve, var_y;

	gsl_matrix *Omega=gsl_matrix_alloc (UtXgamma->size2, UtXgamma->size2);
	gsl_vector *Xty=gsl_vector_alloc (UtXgamma->size2);
	gsl_vector *OiXty=gsl_vector_alloc (UtXgamma->size2);

	gsl_matrix_set_identity (Omega);
	gsl_matrix_scale (Omega, 1.0/sigma_a2);

	lapack_dgemm ((char *)"T", (char *)"N", 1.0, UtXgamma, UtXgamma,
		      1.0, Omega);
	gsl_blas_dgemv (CblasTrans, 1.0, UtXgamma, Uty, 0.0, Xty);

	CholeskySolve(Omega, Xty, OiXty);

	gsl_blas_ddot (Xty, OiXty, &pve);
	gsl_blas_ddot (Uty, Uty, &var_y);

	pve/=var_y;

	gsl_matrix_free (Omega);
	gsl_vector_free (Xty);
	gsl_vector_free (OiXty);

	return pve;
}

void BSLMM::InitialMCMC (const gsl_matrix *UtX, const gsl_vector *Uty,
			 vector<size_t> &rank, class HYPBSLMM &cHyp,
			 vector<pair<size_t, double> > &pos_loglr) {
	double q_genome=gsl_cdf_chisq_Qinv(0.05/(double)ns_test, 1);

	cHyp.n_gamma=0;
	for (size_t i=0; i<pos_loglr.size(); ++i) {
		if (2.0*pos_loglr[i].second>q_genome) {cHyp.n_gamma++;}
	}
	if (cHyp.n_gamma<10) {cHyp.n_gamma=10;}

	if (cHyp.n_gamma>s_max) {cHyp.n_gamma=s_max;}
	if (cHyp.n_gamma<s_min) {cHyp.n_gamma=s_min;}

	rank.clear();
	for (size_t i=0; i<cHyp.n_gamma; ++i) {
		rank.push_back(i);
	}

	cHyp.logp=log((double)cHyp.n_gamma/(double)ns_test);
	cHyp.h=pve_null;

	if (cHyp.logp==0) {cHyp.logp=-0.000001;}
	if (cHyp.h==0) {cHyp.h=0.1;}

	gsl_matrix *UtXgamma=gsl_matrix_alloc (ni_test, cHyp.n_gamma);
	SetXgamma (UtXgamma, UtX, rank);
	double sigma_a2;
	if (trace_G!=0) {
	  sigma_a2=cHyp.h*1.0/
	    (trace_G*(1-cHyp.h)*exp(cHyp.logp)*(double)ns_test);
	} else {
	  sigma_a2=cHyp.h*1.0/( (1-cHyp.h)*exp(cHyp.logp)*(double)ns_test);
	}
	if (sigma_a2==0) {sigma_a2=0.025;}
	cHyp.rho=CalcPveLM (UtXgamma, Uty, sigma_a2)/cHyp.h;
	gsl_matrix_free (UtXgamma);

	if (cHyp.rho>1.0) {cHyp.rho=1.0;}

	if (cHyp.h<h_min) {cHyp.h=h_min;}
	if (cHyp.h>h_max) {cHyp.h=h_max;}
	if (cHyp.rho<rho_min) {cHyp.rho=rho_min;}
	if (cHyp.rho>rho_max) {cHyp.rho=rho_max;}
	if (cHyp.logp<logp_min) {cHyp.logp=logp_min;}
	if (cHyp.logp>logp_max) {cHyp.logp=logp_max;}

	cout<<"initial value of h = "<<cHyp.h<<endl;
	cout<<"initial value of rho = "<<cHyp.rho<<endl;
	cout<<"initial value of pi = "<<exp(cHyp.logp)<<endl;
	cout<<"initial value of |gamma| = "<<cHyp.n_gamma<<endl;

	return;
}

double BSLMM::CalcPosterior (const gsl_vector *Uty, const gsl_vector *K_eval,
			     gsl_vector *Utu, gsl_vector *alpha_prime,
			     class HYPBSLMM &cHyp) {
	double sigma_b2=cHyp.h*(1.0-cHyp.rho)/(trace_G*(1-cHyp.h));

	gsl_vector *Utu_rand=gsl_vector_alloc (Uty->size);
	gsl_vector *weight_Hi=gsl_vector_alloc (Uty->size);

	double logpost=0.0;
	double d, ds, uy, Hi_yy=0, logdet_H=0.0;
	for (size_t i=0; i<ni_test; ++i) {
		d=gsl_vector_get (K_eval, i)*sigma_b2;
		ds=d/(d+1.0);
		d=1.0/(d+1.0);
		gsl_vector_set (weight_Hi, i, d);

		logdet_H-=log(d);
		uy=gsl_vector_get (Uty, i);
		Hi_yy+=d*uy*uy;

		gsl_vector_set (Utu_rand, i,
				gsl_ran_gaussian(gsl_r, 1)*sqrt(ds));
	}

	// Sample tau.
	double tau=1.0;
	if (a_mode==11) {
	  tau = gsl_ran_gamma (gsl_r, (double)ni_test/2.0,  2.0/Hi_yy);
	}

	// Sample alpha.
	gsl_vector_memcpy (alpha_prime, Uty);
	gsl_vector_mul (alpha_prime, weight_Hi);
	gsl_vector_scale (alpha_prime, sigma_b2);

	// Sample u.
	gsl_vector_memcpy (Utu, alpha_prime);
	gsl_vector_mul (Utu, K_eval);
	if (a_mode==11) {gsl_vector_scale (Utu_rand, sqrt(1.0/tau));}
	gsl_vector_add (Utu, Utu_rand);

	// For quantitative traits, calculate pve and ppe.
	if (a_mode==11) {
		gsl_blas_ddot (Utu, Utu, &d);
		cHyp.pve=d/(double)ni_test;
		cHyp.pve/=cHyp.pve+1.0/tau;
		cHyp.pge=0.0;
	}

	// Calculate likelihood.
	logpost=-0.5*logdet_H;
	if (a_mode==11) {logpost-=0.5*(double)ni_test*log(Hi_yy);}
	else {logpost-=0.5*Hi_yy;}

	logpost+=((double)cHyp.n_gamma-1.0)*cHyp.logp+
	  ((double)ns_test-(double)cHyp.n_gamma)*log(1-exp(cHyp.logp));

	gsl_vector_free (Utu_rand);
	gsl_vector_free (weight_Hi);

	return logpost;
}

double BSLMM::CalcPosterior (const gsl_matrix *UtXgamma,
			     const gsl_vector *Uty, const gsl_vector *K_eval,
			     gsl_vector *UtXb, gsl_vector *Utu,
			     gsl_vector *alpha_prime, gsl_vector *beta,
			     class HYPBSLMM &cHyp) {
	clock_t time_start;

	double sigma_a2=cHyp.h*cHyp.rho/
	  (trace_G*(1-cHyp.h)*exp(cHyp.logp)*(double)ns_test);
	double sigma_b2=cHyp.h*(1.0-cHyp.rho)/(trace_G*(1-cHyp.h));

	double logpost=0.0;
	double d, ds, uy, P_yy=0, logdet_O=0.0, logdet_H=0.0;

	gsl_matrix *UtXgamma_eval=gsl_matrix_alloc (UtXgamma->size1,
						    UtXgamma->size2);
	gsl_matrix *Omega=gsl_matrix_alloc (UtXgamma->size2, UtXgamma->size2);
	gsl_vector *XtHiy=gsl_vector_alloc (UtXgamma->size2);
	gsl_vector *beta_hat=gsl_vector_alloc (UtXgamma->size2);
	gsl_vector *Utu_rand=gsl_vector_alloc (UtXgamma->size1);
	gsl_vector *weight_Hi=gsl_vector_alloc (UtXgamma->size1);

	gsl_matrix_memcpy (UtXgamma_eval, UtXgamma);

	logdet_H=0.0; P_yy=0.0;
	for (size_t i=0; i<ni_test; ++i) {
		gsl_vector_view UtXgamma_row=
		  gsl_matrix_row (UtXgamma_eval, i);
		d=gsl_vector_get (K_eval, i)*sigma_b2;
		ds=d/(d+1.0);
		d=1.0/(d+1.0);
		gsl_vector_set (weight_Hi, i, d);

		logdet_H-=log(d);
		uy=gsl_vector_get (Uty, i);
		P_yy+=d*uy*uy;
		gsl_vector_scale (&UtXgamma_row.vector, d);

		gsl_vector_set(Utu_rand,i,gsl_ran_gaussian(gsl_r,1)*sqrt(ds));
	}

	// Calculate Omega.
	gsl_matrix_set_identity (Omega);

	time_start=clock();
	lapack_dgemm ((char *)"T", (char *)"N", sigma_a2, UtXgamma_eval,
		      UtXgamma, 1.0, Omega);
	time_Omega+=(clock()-time_start)/(double(CLOCKS_PER_SEC)*60.0);


	// Calculate beta_hat.
	gsl_blas_dgemv (CblasTrans, 1.0, UtXgamma_eval, Uty, 0.0, XtHiy);

	logdet_O=CholeskySolve(Omega, XtHiy, beta_hat);

	gsl_vector_scale (beta_hat, sigma_a2);

	gsl_blas_ddot (XtHiy, beta_hat, &d);
	P_yy-=d;

	// Sample tau.
	double tau=1.0;
	if (a_mode==11) {
	  tau =gsl_ran_gamma (gsl_r, (double)ni_test/2.0,  2.0/P_yy);
	}

	// Sample beta.
	for (size_t i=0; i<beta->size; i++)
	{
		d=gsl_ran_gaussian(gsl_r, 1);
		gsl_vector_set(beta, i, d);
	}
	gsl_blas_dtrsv(CblasUpper, CblasNoTrans, CblasNonUnit, Omega, beta);

	// This computes inv(L^T(Omega)) %*% beta.
	gsl_vector_scale(beta, sqrt(sigma_a2/tau));
	gsl_vector_add(beta, beta_hat);
	gsl_blas_dgemv (CblasNoTrans, 1.0, UtXgamma, beta, 0.0, UtXb);

	// Sample alpha.
	gsl_vector_memcpy (alpha_prime, Uty);
	gsl_vector_sub (alpha_prime, UtXb);
	gsl_vector_mul (alpha_prime, weight_Hi);
	gsl_vector_scale (alpha_prime, sigma_b2);

	// Sample u.
	gsl_vector_memcpy (Utu, alpha_prime);
	gsl_vector_mul (Utu, K_eval);

	if (a_mode==11) {gsl_vector_scale (Utu_rand, sqrt(1.0/tau));}
	gsl_vector_add (Utu, Utu_rand);

	// For quantitative traits, calculate pve and pge.
	if (a_mode==11) {
		gsl_blas_ddot (UtXb, UtXb, &d);
		cHyp.pge=d/(double)ni_test;

		gsl_blas_ddot (Utu, Utu, &d);
		cHyp.pve=cHyp.pge+d/(double)ni_test;

		if (cHyp.pve==0) {cHyp.pge=0.0;}
		else {cHyp.pge/=cHyp.pve;}
		cHyp.pve/=cHyp.pve+1.0/tau;
	}

	gsl_matrix_free (UtXgamma_eval);
	gsl_matrix_free (Omega);
	gsl_vector_free (XtHiy);
	gsl_vector_free (beta_hat);
	gsl_vector_free (Utu_rand);
	gsl_vector_free (weight_Hi);

	logpost=-0.5*logdet_H-0.5*logdet_O;
	if (a_mode==11) {logpost-=0.5*(double)ni_test*log(P_yy);}
	else {logpost-=0.5*P_yy;}
	logpost+=((double)cHyp.n_gamma-1.0)*cHyp.logp+
	  ((double)ns_test-(double)cHyp.n_gamma)*log(1.0-exp(cHyp.logp));

	return logpost;
}

// Calculate pve and pge, and calculate z_hat for case-control data.
void BSLMM::CalcCC_PVEnZ (const gsl_matrix *U, const gsl_vector *Utu,
			  gsl_vector *z_hat, class HYPBSLMM &cHyp) {
	double d;

	gsl_blas_ddot (Utu, Utu, &d);
	cHyp.pve=d/(double)ni_test;

	gsl_blas_dgemv (CblasNoTrans, 1.0, U, Utu, 0.0, z_hat);

	cHyp.pve/=cHyp.pve+1.0;
	cHyp.pge=0.0;

	return;
}

// Calculate pve and pge, and calculate z_hat for case-control data.
void BSLMM::CalcCC_PVEnZ (const gsl_matrix *U, const gsl_vector *UtXb,
			  const gsl_vector *Utu, gsl_vector *z_hat,
			  class HYPBSLMM &cHyp) {
	double d;
	gsl_vector *UtXbU=gsl_vector_alloc (Utu->size);

	gsl_blas_ddot (UtXb, UtXb, &d);
	cHyp.pge=d/(double)ni_test;

	gsl_blas_ddot (Utu, Utu, &d);
	cHyp.pve=cHyp.pge+d/(double)ni_test;

	gsl_vector_memcpy (UtXbU, Utu);
	gsl_vector_add (UtXbU, UtXb);
	gsl_blas_dgemv (CblasNoTrans, 1.0, U, UtXbU, 0.0, z_hat);

	if (cHyp.pve==0) {cHyp.pge=0.0;}
	else {cHyp.pge/=cHyp.pve;}

	cHyp.pve/=cHyp.pve+1.0;

	gsl_vector_free(UtXbU);
	return;
}

void BSLMM::SampleZ (const gsl_vector *y, const gsl_vector *z_hat,
		     gsl_vector *z) {
	double d1, d2, z_rand=0.0;
	for (size_t i=0; i<z->size; ++i) {
		d1=gsl_vector_get (y, i);
		d2=gsl_vector_get (z_hat, i);

		// y is centered for case control studies.
		if (d1<=0.0) {

		        // Control, right truncated.
			do {
				z_rand=d2+gsl_ran_gaussian(gsl_r, 1.0);
			} while (z_rand>0.0);
		}
		else {
			do {
				z_rand=d2+gsl_ran_gaussian(gsl_r, 1.0);
			} while (z_rand<0.0);
		}

		gsl_vector_set (z, i, z_rand);
	}

	return;
}

double BSLMM::ProposeHnRho (const class HYPBSLMM &cHyp_old,
			    class HYPBSLMM &cHyp_new, const size_t &repeat) {

	double h=cHyp_old.h, rho=cHyp_old.rho;

	double d_h=(h_max-h_min)*h_scale, d_rho=(rho_max-rho_min)*rho_scale;

	for (size_t i=0; i<repeat; ++i) {
		h=h+(gsl_rng_uniform(gsl_r)-0.5)*d_h;
		if (h<h_min) {h=2*h_min-h;}
		if (h>h_max) {h=2*h_max-h;}

		rho=rho+(gsl_rng_uniform(gsl_r)-0.5)*d_rho;
		if (rho<rho_min) {rho=2*rho_min-rho;}
		if (rho>rho_max) {rho=2*rho_max-rho;}
	}
	cHyp_new.h=h;
	cHyp_new.rho=rho;
	return 0.0;
}

double BSLMM::ProposePi (const class HYPBSLMM &cHyp_old,
			 class HYPBSLMM &cHyp_new, const size_t &repeat) {
	double logp_old=cHyp_old.logp, logp_new=cHyp_old.logp;
	double log_ratio=0.0;

	double d_logp=min(0.1, (logp_max-logp_min)*logp_scale);

	for (size_t i=0; i<repeat; ++i) {
		logp_new=logp_old+(gsl_rng_uniform(gsl_r)-0.5)*d_logp;
		if (logp_new<logp_min) {logp_new=2*logp_min-logp_new;}
		if (logp_new>logp_max) {logp_new=2*logp_max-logp_new;}
		log_ratio+=logp_new-logp_old;
		logp_old=logp_new;
	}
	cHyp_new.logp=logp_new;

	return log_ratio;
}

bool comp_vec (size_t a, size_t b) {
	return (a < b);
}

double BSLMM::ProposeGamma (const vector<size_t> &rank_old,
			    vector<size_t> &rank_new,
			    const double *p_gamma,
			    const class HYPBSLMM &cHyp_old,
			    class HYPBSLMM &cHyp_new,
			    const size_t &repeat) {
	map<size_t, int> mapRank2in;
	size_t r;
	double unif, logp=0.0;
	int flag_gamma;
	size_t r_add, r_remove, col_id;

	rank_new.clear();
	if (cHyp_old.n_gamma!=rank_old.size()) {cout<<"size wrong"<<endl;}

	if (cHyp_old.n_gamma!=0) {
		for (size_t i=0; i<rank_old.size(); ++i) {
			r=rank_old[i];
			rank_new.push_back(r);
			mapRank2in[r]=1;
		}
	}
	cHyp_new.n_gamma=cHyp_old.n_gamma;

	for (size_t i=0; i<repeat; ++i) {
		unif=gsl_rng_uniform(gsl_r);

		if (unif < 0.40 && cHyp_new.n_gamma<s_max) {flag_gamma=1;}
		else if (unif>=0.40 && unif < 0.80 &&
			 cHyp_new.n_gamma>s_min) {
		  flag_gamma=2;
		}
		else if (unif>=0.80 && cHyp_new.n_gamma>0 &&
			 cHyp_new.n_gamma<ns_test) {
		  flag_gamma=3;
		}
		else {flag_gamma=4;}

		if(flag_gamma==1)  {

		        // Add a SNP.
			do {
				r_add=gsl_ran_discrete (gsl_r, gsl_t);
			} while (mapRank2in.count(r_add)!=0);

			double prob_total=1.0;
			for (size_t i=0; i<cHyp_new.n_gamma; ++i) {
				r=rank_new[i];
				prob_total-=p_gamma[r];
			}

			mapRank2in[r_add]=1;
			rank_new.push_back(r_add);
			cHyp_new.n_gamma++;
			logp+=-log(p_gamma[r_add]/prob_total)-
			  log((double)cHyp_new.n_gamma);
		}
		else if (flag_gamma==2) {

		        // Delete a SNP.
			col_id=gsl_rng_uniform_int(gsl_r, cHyp_new.n_gamma);
			r_remove=rank_new[col_id];

			double prob_total=1.0;
			for (size_t i=0; i<cHyp_new.n_gamma; ++i) {
				r=rank_new[i];
				prob_total-=p_gamma[r];
			}
			prob_total+=p_gamma[r_remove];

			mapRank2in.erase(r_remove);
			rank_new.erase(rank_new.begin()+col_id);
			logp+=log(p_gamma[r_remove]/prob_total)+
			  log((double)cHyp_new.n_gamma);
			cHyp_new.n_gamma--;
		}
		else if (flag_gamma==3) {

		        // Switch a SNP.
			col_id=gsl_rng_uniform_int(gsl_r, cHyp_new.n_gamma);
			r_remove=rank_new[col_id];

		        // Be careful with the proposal.
			do {
				r_add=gsl_ran_discrete (gsl_r, gsl_t);
			} while (mapRank2in.count(r_add)!=0);

			double prob_total=1.0;
			for (size_t i=0; i<cHyp_new.n_gamma; ++i) {
				r=rank_new[i];
				prob_total-=p_gamma[r];
			}

			logp+=log(p_gamma[r_remove]/
			  (prob_total+p_gamma[r_remove]-p_gamma[r_add]));
			logp-=log(p_gamma[r_add]/prob_total);

			mapRank2in.erase(r_remove);
			mapRank2in[r_add]=1;
			rank_new.erase(rank_new.begin()+col_id);
			rank_new.push_back(r_add);
		}
		else {logp+=0;} // Do not change.
	}

	stable_sort (rank_new.begin(), rank_new.end(), comp_vec);

	mapRank2in.clear();
	return logp;
}

bool comp_lr (pair<size_t, double> a, pair<size_t, double> b) {
	return (a.second > b.second);
}

// If a_mode==13 then Uty==y.
void BSLMM::MCMC (const gsl_matrix *U, const gsl_matrix *UtX,
		  const gsl_vector *Uty, const gsl_vector *K_eval,
		  const gsl_vector *y) {
	clock_t time_start;

	class HYPBSLMM cHyp_old, cHyp_new;

	gsl_matrix *Result_hyp=gsl_matrix_alloc (w_pace, 6);
	gsl_matrix *Result_gamma=gsl_matrix_alloc (w_pace, s_max);

	gsl_vector *alpha_prime=gsl_vector_alloc (ni_test);
	gsl_vector *alpha_new=gsl_vector_alloc (ni_test);
	gsl_vector *alpha_old=gsl_vector_alloc (ni_test);
	gsl_vector *Utu=gsl_vector_alloc (ni_test);
	gsl_vector *Utu_new=gsl_vector_alloc (ni_test);
	gsl_vector *Utu_old=gsl_vector_alloc (ni_test);

	gsl_vector *UtXb_new=gsl_vector_alloc (ni_test);
	gsl_vector *UtXb_old=gsl_vector_alloc (ni_test);

	gsl_vector *z_hat=gsl_vector_alloc (ni_test);
	gsl_vector *z=gsl_vector_alloc (ni_test);
	gsl_vector *Utz=gsl_vector_alloc (ni_test);

	gsl_vector_memcpy (Utz, Uty);

	double logPost_new, logPost_old;
	double logMHratio;
	double mean_z=0.0;

	gsl_matrix_set_zero (Result_gamma);
	gsl_vector_set_zero (Utu);
	gsl_vector_set_zero (alpha_prime);
	if (a_mode==13) {
		pheno_mean=0.0;
	}

	vector<pair<double, double> > beta_g;
	for (size_t i=0; i<ns_test; i++) {
		beta_g.push_back(make_pair(0.0, 0.0));
	}

	vector<size_t> rank_new, rank_old;
	vector<double> beta_new, beta_old;

	vector<pair<size_t, double> > pos_loglr;

	time_start=clock();
	MatrixCalcLR (U, UtX, Utz, K_eval, l_min, l_max, n_region, pos_loglr);
	time_Proposal=(clock()-time_start)/(double(CLOCKS_PER_SEC)*60.0);

	stable_sort (pos_loglr.begin(), pos_loglr.end(), comp_lr);
	for (size_t i=0; i<ns_test; ++i) {
		mapRank2pos[i]=pos_loglr[i].first;
	}

	// Calculate proposal distribution for gamma (unnormalized),
	// and set up gsl_r and gsl_t.
	gsl_rng_env_setup();
	const gsl_rng_type * gslType;
	gslType = gsl_rng_default;
	if (randseed<0)
	{
		time_t rawtime;
		time (&rawtime);
		tm * ptm = gmtime (&rawtime);

		randseed = (unsigned) (ptm->tm_hour%24*3600+
				       ptm->tm_min*60+ptm->tm_sec);
	}
	gsl_r = gsl_rng_alloc(gslType);
	gsl_rng_set(gsl_r, randseed);

	double *p_gamma = new double[ns_test];
	CalcPgamma (p_gamma);

	gsl_t=gsl_ran_discrete_preproc (ns_test, p_gamma);

	// Initial parameters.
	InitialMCMC (UtX, Utz, rank_old, cHyp_old, pos_loglr);

	cHyp_initial=cHyp_old;

	if (cHyp_old.n_gamma==0 || cHyp_old.rho==0) {
		logPost_old=CalcPosterior(Utz, K_eval, Utu_old, alpha_old,
					  cHyp_old);

		beta_old.clear();
		for (size_t i=0; i<cHyp_old.n_gamma; ++i) {
		  beta_old.push_back(0);
		}
	}
	else {
		gsl_matrix *UtXgamma=gsl_matrix_alloc (ni_test,
						       cHyp_old.n_gamma);
		gsl_vector *beta=gsl_vector_alloc (cHyp_old.n_gamma);
		SetXgamma (UtXgamma, UtX, rank_old);
		logPost_old=CalcPosterior(UtXgamma, Utz, K_eval, UtXb_old,
					  Utu_old, alpha_old, beta, cHyp_old);

		beta_old.clear();
		for (size_t i=0; i<beta->size; ++i) {
			beta_old.push_back(gsl_vector_get(beta, i));
		}
		gsl_matrix_free (UtXgamma);
		gsl_vector_free (beta);
	}

	// Calculate centered z_hat, and pve.
	if (a_mode==13) {
		time_start=clock();
		if (cHyp_old.n_gamma==0 || cHyp_old.rho==0) {
			CalcCC_PVEnZ (U, Utu_old, z_hat, cHyp_old);
		}
		else {
			CalcCC_PVEnZ (U, UtXb_old, Utu_old, z_hat, cHyp_old);
		}
		time_UtZ+=(clock()-time_start)/(double(CLOCKS_PER_SEC)*60.0);
	}

	// Start MCMC.
	int accept;
	size_t total_step=w_step+s_step;
	size_t w=0, w_col, pos;
	size_t repeat=0;

	for (size_t t=0; t<total_step; ++t) {
		if (t%d_pace==0 || t==total_step-1) {
		  ProgressBar ("Running MCMC ", t, total_step-1,
			       (double)n_accept/(double)(t*n_mh+1));
		}

		if (a_mode==13) {
			SampleZ (y, z_hat, z);
			mean_z=CenterVector (z);

			time_start=clock();
			gsl_blas_dgemv (CblasTrans, 1.0, U, z, 0.0, Utz);
			time_UtZ+=(clock()-time_start)/
			  (double(CLOCKS_PER_SEC)*60.0);

			// First proposal.
			if (cHyp_old.n_gamma==0 || cHyp_old.rho==0) {
				logPost_old=
				  CalcPosterior(Utz, K_eval, Utu_old,
						alpha_old, cHyp_old);
				beta_old.clear();
				for (size_t i=0; i<cHyp_old.n_gamma; ++i) {
				  beta_old.push_back(0);
				}
			}
			else {
				gsl_matrix *UtXgamma=
				  gsl_matrix_alloc (ni_test, cHyp_old.n_gamma);
				gsl_vector *beta=
				  gsl_vector_alloc (cHyp_old.n_gamma);
				SetXgamma (UtXgamma, UtX, rank_old);
				logPost_old=
				  CalcPosterior(UtXgamma, Utz, K_eval,
						UtXb_old, Utu_old, alpha_old,
						beta, cHyp_old);

				beta_old.clear();
				for (size_t i=0; i<beta->size; ++i) {
				  beta_old.push_back(gsl_vector_get(beta, i));
				}
				gsl_matrix_free (UtXgamma);
				gsl_vector_free (beta);
			}
		}

		// M-H steps.
		for (size_t i=0; i<n_mh; ++i) {
			if (gsl_rng_uniform(gsl_r)<0.33) {
			  repeat = 1+gsl_rng_uniform_int(gsl_r, 20);
			}
			else {
			  repeat=1;
			}

			logMHratio=0.0;
			logMHratio+=ProposeHnRho(cHyp_old, cHyp_new, repeat);
			logMHratio+=ProposeGamma (rank_old, rank_new, p_gamma,
						  cHyp_old, cHyp_new, repeat);
			logMHratio+=ProposePi(cHyp_old, cHyp_new, repeat);

			if (cHyp_new.n_gamma==0 || cHyp_new.rho==0) {
				logPost_new=CalcPosterior(Utz, K_eval, Utu_new,
							  alpha_new, cHyp_new);
				beta_new.clear();
				for (size_t i=0; i<cHyp_new.n_gamma; ++i) {
				  beta_new.push_back(0);
				}
			}
			else {
				gsl_matrix *UtXgamma=
				  gsl_matrix_alloc (ni_test, cHyp_new.n_gamma);
				gsl_vector *beta=
				  gsl_vector_alloc (cHyp_new.n_gamma);
				SetXgamma (UtXgamma, UtX, rank_new);
				logPost_new=
				  CalcPosterior(UtXgamma, Utz, K_eval,
						UtXb_new, Utu_new, alpha_new,
						beta, cHyp_new);
				beta_new.clear();
				for (size_t i=0; i<beta->size; ++i) {
				  beta_new.push_back(gsl_vector_get(beta, i));
				}
				gsl_matrix_free (UtXgamma);
				gsl_vector_free (beta);
			}

			logMHratio+=logPost_new-logPost_old;

			if (logMHratio>0 ||
			    log(gsl_rng_uniform(gsl_r))<logMHratio) {
			  accept=1; n_accept++;
			}
			else {accept=0;}

			if (accept==1) {
				logPost_old=logPost_new;
				rank_old.clear(); beta_old.clear();
				if (rank_new.size()!=0) {
				  for (size_t i=0; i<rank_new.size(); ++i) {
				    rank_old.push_back(rank_new[i]);
				    beta_old.push_back(beta_new[i]);
				  }
				}
				cHyp_old=cHyp_new;
				gsl_vector_memcpy (alpha_old, alpha_new);
				gsl_vector_memcpy (UtXb_old, UtXb_new);
				gsl_vector_memcpy (Utu_old, Utu_new);
			}
			else {cHyp_new=cHyp_old;}
		}

		// Calculate z_hat, and pve.
		if (a_mode==13) {
			time_start=clock();
			if (cHyp_old.n_gamma==0 || cHyp_old.rho==0) {
				CalcCC_PVEnZ (U, Utu_old, z_hat, cHyp_old);
			}
			else {
				CalcCC_PVEnZ (U, UtXb_old, Utu_old,
					      z_hat, cHyp_old);
			}

			// Sample mu and update z_hat.
			gsl_vector_sub (z, z_hat);
			mean_z+=CenterVector(z);
			mean_z+=
			  gsl_ran_gaussian(gsl_r, sqrt(1.0/(double) ni_test));
			gsl_vector_add_constant (z_hat, mean_z);

			time_UtZ+=(clock()-time_start)/
			  (double(CLOCKS_PER_SEC)*60.0);
		}

		// Save data.
		if (t<w_step) {continue;}
		else {
			if (t%r_pace==0) {
				w_col=w%w_pace;
				if (w_col==0) {
					if (w==0) {
					  WriteResult (0, Result_hyp,
						       Result_gamma, w_col);
					}
					else {
					  WriteResult (1, Result_hyp,
						       Result_gamma, w_col);
					  gsl_matrix_set_zero (Result_hyp);
					  gsl_matrix_set_zero (Result_gamma);
					}
				}

				gsl_matrix_set(Result_hyp,w_col,0,cHyp_old.h);
				gsl_matrix_set(Result_hyp,w_col,1,cHyp_old.pve);
				gsl_matrix_set(Result_hyp,w_col,2,cHyp_old.rho);
				gsl_matrix_set(Result_hyp,w_col,3,cHyp_old.pge);
				gsl_matrix_set(Result_hyp,w_col,4,cHyp_old.logp);
				gsl_matrix_set(Result_hyp,w_col,5,cHyp_old.n_gamma);

				for (size_t i=0; i<cHyp_old.n_gamma; ++i) {
					pos=mapRank2pos[rank_old[i]]+1;

					gsl_matrix_set(Result_gamma,w_col,i,
						       pos);

					beta_g[pos-1].first+=beta_old[i];
					beta_g[pos-1].second+=1.0;
				}

				gsl_vector_add (alpha_prime, alpha_old);
				gsl_vector_add (Utu, Utu_old);

				if (a_mode==13) {
					pheno_mean+=mean_z;
				}

				w++;

			}

		}
	}
	cout<<endl;

	w_col=w%w_pace;
	WriteResult (1, Result_hyp, Result_gamma, w_col);

	gsl_matrix_free(Result_hyp);
	gsl_matrix_free(Result_gamma);

	gsl_vector_free(z_hat);
	gsl_vector_free(z);
	gsl_vector_free(Utz);
	gsl_vector_free(UtXb_new);
	gsl_vector_free(UtXb_old);
	gsl_vector_free(alpha_new);
	gsl_vector_free(alpha_old);
	gsl_vector_free(Utu_new);
	gsl_vector_free(Utu_old);

	gsl_vector_scale (alpha_prime, 1.0/(double)w);
	gsl_vector_scale (Utu, 1.0/(double)w);
	if (a_mode==13) {
		pheno_mean/=(double)w;
	}

	gsl_vector *alpha=gsl_vector_alloc (ns_test);
	gsl_blas_dgemv (CblasTrans, 1.0/(double)ns_test, UtX,
			alpha_prime, 0.0, alpha);
	WriteParam (beta_g, alpha, w);
	gsl_vector_free(alpha);

	gsl_blas_dgemv (CblasNoTrans, 1.0, U, Utu, 0.0, alpha_prime);
	WriteBV(alpha_prime);

	gsl_vector_free(alpha_prime);
	gsl_vector_free(Utu);

	delete [] p_gamma;
	beta_g.clear();

	return;
}

void BSLMM::RidgeR(const gsl_matrix *U, const gsl_matrix *UtX,
		   const gsl_vector *Uty, const gsl_vector *eval,
		   const double lambda) {
	gsl_vector *beta=gsl_vector_alloc (UtX->size2);
	gsl_vector *H_eval=gsl_vector_alloc (Uty->size);
	gsl_vector *bv=gsl_vector_alloc (Uty->size);

	gsl_vector_memcpy (H_eval, eval);
	gsl_vector_scale (H_eval, lambda);
	gsl_vector_add_constant (H_eval, 1.0);

	gsl_vector_memcpy (bv, Uty);
	gsl_vector_div (bv, H_eval);

	gsl_blas_dgemv (CblasTrans, lambda/(double)UtX->size2,
			UtX, bv, 0.0, beta);
	gsl_vector_add_constant (H_eval, -1.0);
	gsl_vector_mul (H_eval, bv);
	gsl_blas_dgemv (CblasNoTrans, 1.0, U, H_eval, 0.0, bv);

	WriteParam (beta);
	WriteBV(bv);

	gsl_vector_free (H_eval);
	gsl_vector_free (beta);
	gsl_vector_free (bv);

	return;
}

// Below fits MCMC for rho=1.
void BSLMM::CalcXtX (const gsl_matrix *X, const gsl_vector *y,
		     const size_t s_size, gsl_matrix *XtX, gsl_vector *Xty) {
  time_t time_start=clock();
  gsl_matrix_const_view X_sub=gsl_matrix_const_submatrix(X, 0, 0, X->size1,
							 s_size);
  gsl_matrix_view XtX_sub=gsl_matrix_submatrix(XtX, 0, 0, s_size, s_size);
  gsl_vector_view Xty_sub=gsl_vector_subvector(Xty, 0, s_size);

  lapack_dgemm ((char *)"T", (char *)"N", 1.0, &X_sub.matrix,
		&X_sub.matrix, 0.0, &XtX_sub.matrix);
  gsl_blas_dgemv(CblasTrans, 1.0, &X_sub.matrix, y, 0.0, &Xty_sub.vector);

  time_Omega+=(clock()-time_start)/(double(CLOCKS_PER_SEC)*60.0);

  return;
}

void BSLMM::SetXgamma (const gsl_matrix *X, const gsl_matrix *X_old,
		       const gsl_matrix *XtX_old, const gsl_vector *Xty_old,
		       const gsl_vector *y, const vector<size_t> &rank_old,
		       const vector<size_t> &rank_new, gsl_matrix *X_new,
		       gsl_matrix *XtX_new, gsl_vector *Xty_new) {
  double d;

  // rank_old and rank_new are sorted already inside PorposeGamma
  // calculate vectors rank_remove and rank_add.
  // make sure that v_size is larger than repeat.
  size_t v_size=20;
  vector<size_t> rank_remove(v_size), rank_add(v_size),
    rank_union(s_max+v_size);
  vector<size_t>::iterator it;

  it=set_difference(rank_old.begin(), rank_old.end(), rank_new.begin(),
		    rank_new.end(), rank_remove.begin());
  rank_remove.resize(it-rank_remove.begin());

  it=set_difference (rank_new.begin(), rank_new.end(), rank_old.begin(),
		     rank_old.end(), rank_add.begin());
  rank_add.resize(it-rank_add.begin());

  it=set_union (rank_new.begin(), rank_new.end(), rank_old.begin(),
		rank_old.end(), rank_union.begin());
  rank_union.resize(it-rank_union.begin());

  // Map rank_remove and rank_add.
  map<size_t, int> mapRank2in_remove, mapRank2in_add;
  for (size_t i=0; i<rank_remove.size(); i++) {
    mapRank2in_remove[rank_remove[i]]=1;
  }
  for (size_t i=0; i<rank_add.size(); i++) {
    mapRank2in_add[rank_add[i]]=1;
  }

  // Obtain the subset of matrix/vector.
  gsl_matrix_const_view Xold_sub=
    gsl_matrix_const_submatrix(X_old, 0, 0, X_old->size1, rank_old.size());
  gsl_matrix_const_view XtXold_sub=
    gsl_matrix_const_submatrix(XtX_old, 0, 0, rank_old.size(),
			       rank_old.size());
  gsl_vector_const_view Xtyold_sub=
    gsl_vector_const_subvector(Xty_old, 0, rank_old.size());

  gsl_matrix_view Xnew_sub=
    gsl_matrix_submatrix(X_new, 0, 0, X_new->size1, rank_new.size());
  gsl_matrix_view XtXnew_sub=
    gsl_matrix_submatrix(XtX_new, 0, 0, rank_new.size(), rank_new.size());
  gsl_vector_view Xtynew_sub=
    gsl_vector_subvector(Xty_new, 0, rank_new.size());

  // Get X_new and calculate XtX_new.
  if (rank_remove.size()==0 && rank_add.size()==0) {
    gsl_matrix_memcpy(&Xnew_sub.matrix, &Xold_sub.matrix);
    gsl_matrix_memcpy(&XtXnew_sub.matrix, &XtXold_sub.matrix);
    gsl_vector_memcpy(&Xtynew_sub.vector, &Xtyold_sub.vector);
  } else {
    size_t i_old, j_old, i_new, j_new, i_add, j_add, i_flag, j_flag;
    if (rank_add.size()==0) {
      i_old=0; i_new=0;
      for (size_t i=0; i<rank_union.size(); i++) {
	if (mapRank2in_remove.count(rank_old[i_old])!=0) {i_old++; continue;}

	gsl_vector_view Xnew_col=gsl_matrix_column(X_new, i_new);
	gsl_vector_const_view Xcopy_col=gsl_matrix_const_column(X_old, i_old);
	gsl_vector_memcpy (&Xnew_col.vector, &Xcopy_col.vector);

	d=gsl_vector_get (Xty_old, i_old);
	gsl_vector_set (Xty_new, i_new, d);

	j_old=i_old; j_new=i_new;
	for (size_t j=i; j<rank_union.size(); j++) {
          if (mapRank2in_remove.count(rank_old[j_old])!=0) {j_old++; continue;}

	  d=gsl_matrix_get(XtX_old, i_old, j_old);

	  gsl_matrix_set (XtX_new, i_new, j_new, d);
	  if (i_new!=j_new) {gsl_matrix_set (XtX_new, j_new, i_new, d);}

	  j_old++; j_new++;
        }
	i_old++; i_new++;
      }
    } else {
      gsl_matrix *X_add=gsl_matrix_alloc(X_old->size1, rank_add.size() );
      gsl_matrix *XtX_aa=gsl_matrix_alloc(X_add->size2, X_add->size2);
      gsl_matrix *XtX_ao=gsl_matrix_alloc(X_add->size2, X_old->size2);
      gsl_vector *Xty_add=gsl_vector_alloc(X_add->size2);

      // Get X_add.
      SetXgamma (X_add, X, rank_add);

      // Get t(X_add)X_add and t(X_add)X_temp.
      clock_t time_start=clock();

      // Somehow the lapack_dgemm does not work here.
      gsl_blas_dgemm (CblasTrans, CblasNoTrans, 1.0, X_add, X_add,
		      0.0, XtX_aa);
      gsl_blas_dgemm (CblasTrans, CblasNoTrans, 1.0, X_add, X_old,
		      0.0, XtX_ao);
      gsl_blas_dgemv(CblasTrans, 1.0, X_add, y, 0.0, Xty_add);

      time_Omega+=(clock()-time_start)/(double(CLOCKS_PER_SEC)*60.0);

      // Save to X_new, XtX_new and Xty_new.
      i_old=0; i_new=0; i_add=0;
      for (size_t i=0; i<rank_union.size(); i++) {
	if (mapRank2in_remove.count(rank_old[i_old])!=0) {
	  i_old++;
	  continue;
	}
	if (mapRank2in_add.count(rank_new[i_new])!=0) {
	  i_flag=1;
	} else {
	  i_flag=0;
	}

	gsl_vector_view Xnew_col=gsl_matrix_column(X_new, i_new);
	if (i_flag==1) {
	  gsl_vector_view Xcopy_col=gsl_matrix_column(X_add, i_add);
	  gsl_vector_memcpy (&Xnew_col.vector, &Xcopy_col.vector);
	} else {
	  gsl_vector_const_view Xcopy_col=
	    gsl_matrix_const_column(X_old, i_old);
	  gsl_vector_memcpy (&Xnew_col.vector, &Xcopy_col.vector);
	}

	if (i_flag==1) {
          d=gsl_vector_get (Xty_add, i_add);
        } else {
          d=gsl_vector_get (Xty_old, i_old);
        }
	gsl_vector_set (Xty_new, i_new, d);

	j_old=i_old; j_new=i_new; j_add=i_add;
	for (size_t j=i; j<rank_union.size(); j++) {
	  if (mapRank2in_remove.count(rank_old[j_old])!=0) {
	    j_old++;
	    continue;
	  }
	  if (mapRank2in_add.count(rank_new[j_new])!=0) {
	    j_flag=1;
	  } else {
	    j_flag=0;
	  }

	  if (i_flag==1 && j_flag==1) {
            d=gsl_matrix_get(XtX_aa, i_add, j_add);
	  } else if (i_flag==1) {
	    d=gsl_matrix_get(XtX_ao, i_add, j_old);
	  } else if (j_flag==1) {
	    d=gsl_matrix_get(XtX_ao, j_add, i_old);
	  } else {
	    d=gsl_matrix_get(XtX_old, i_old, j_old);
	  }

	  gsl_matrix_set (XtX_new, i_new, j_new, d);
	  if (i_new!=j_new) {gsl_matrix_set (XtX_new, j_new, i_new, d);}

	  j_new++; if (j_flag==1) {j_add++;} else {j_old++;}
        }
	i_new++; if (i_flag==1) {i_add++;} else {i_old++;}
      }

      gsl_matrix_free(X_add);
      gsl_matrix_free(XtX_aa);
      gsl_matrix_free(XtX_ao);
      gsl_vector_free(Xty_add);
    }

  }

  rank_remove.clear();
  rank_add.clear();
  rank_union.clear();
  mapRank2in_remove.clear();
  mapRank2in_add.clear();

  return;
}

double BSLMM::CalcPosterior (const double yty, class HYPBSLMM &cHyp) {
	double logpost=0.0;

	// For quantitative traits, calculate pve and pge.
	// Pve and pge for case/control data are calculted in CalcCC_PVEnZ.
	if (a_mode==11) {
		cHyp.pve=0.0;
		cHyp.pge=1.0;
	}

	// Calculate likelihood.
	if (a_mode==11) {logpost-=0.5*(double)ni_test*log(yty);}
	else {logpost-=0.5*yty;}

	logpost+=((double)cHyp.n_gamma-1.0)*cHyp.logp+
	  ((double)ns_test-(double)cHyp.n_gamma)*log(1-exp(cHyp.logp));

	return logpost;
}

double BSLMM::CalcPosterior (const gsl_matrix *Xgamma, const gsl_matrix *XtX,
			     const gsl_vector *Xty, const double yty,
			     const size_t s_size, gsl_vector *Xb,
			     gsl_vector *beta, class HYPBSLMM &cHyp) {
	double sigma_a2=cHyp.h/( (1-cHyp.h)*exp(cHyp.logp)*(double)ns_test);
	double logpost=0.0;
	double d, P_yy=yty, logdet_O=0.0;

	gsl_matrix_const_view Xgamma_sub=
	  gsl_matrix_const_submatrix (Xgamma, 0, 0, Xgamma->size1, s_size);
	gsl_matrix_const_view XtX_sub=
	  gsl_matrix_const_submatrix (XtX, 0, 0, s_size, s_size);
	gsl_vector_const_view Xty_sub=
	  gsl_vector_const_subvector (Xty, 0, s_size);

	gsl_matrix *Omega=gsl_matrix_alloc (s_size, s_size);
	gsl_matrix *M_temp=gsl_matrix_alloc (s_size, s_size);
	gsl_vector *beta_hat=gsl_vector_alloc (s_size);
	gsl_vector *Xty_temp=gsl_vector_alloc (s_size);

	gsl_vector_memcpy (Xty_temp, &Xty_sub.vector);

	// Calculate Omega.
	gsl_matrix_memcpy (Omega, &XtX_sub.matrix);
	gsl_matrix_scale (Omega, sigma_a2);
	gsl_matrix_set_identity (M_temp);
	gsl_matrix_add (Omega, M_temp);

	// Calculate beta_hat.
	logdet_O=CholeskySolve(Omega, Xty_temp, beta_hat);
	gsl_vector_scale (beta_hat, sigma_a2);

	gsl_blas_ddot (Xty_temp, beta_hat, &d);
	P_yy-=d;

	// Sample tau.
	double tau=1.0;
	if (a_mode==11) {
	  tau = gsl_ran_gamma (gsl_r, (double)ni_test/2.0,  2.0/P_yy);
	}

	// Sample beta.
	for (size_t i=0; i<s_size; i++)
	{
		d=gsl_ran_gaussian(gsl_r, 1);
		gsl_vector_set(beta, i, d);
	}
	gsl_vector_view beta_sub=gsl_vector_subvector(beta, 0, s_size);
	gsl_blas_dtrsv(CblasUpper, CblasNoTrans, CblasNonUnit, Omega,
		       &beta_sub.vector);

	// This computes inv(L^T(Omega)) %*% beta.
	gsl_vector_scale(&beta_sub.vector, sqrt(sigma_a2/tau));
	gsl_vector_add(&beta_sub.vector, beta_hat);
	gsl_blas_dgemv (CblasNoTrans, 1.0, &Xgamma_sub.matrix,
			&beta_sub.vector, 0.0, Xb);

	// For quantitative traits, calculate pve and pge.
	if (a_mode==11) {
		gsl_blas_ddot (Xb, Xb, &d);
		cHyp.pve=d/(double)ni_test;
		cHyp.pve/=cHyp.pve+1.0/tau;
		cHyp.pge=1.0;
	}

	logpost=-0.5*logdet_O;
	if (a_mode==11) {logpost-=0.5*(double)ni_test*log(P_yy);}
	else {logpost-=0.5*P_yy;}

	logpost+=((double)cHyp.n_gamma-1.0)*cHyp.logp+
	  ((double)ns_test-(double)cHyp.n_gamma)*log(1.0-exp(cHyp.logp));

	gsl_matrix_free (Omega);
	gsl_matrix_free (M_temp);
	gsl_vector_free (beta_hat);
	gsl_vector_free (Xty_temp);

	return logpost;
}

// Calculate pve and pge, and calculate z_hat for case-control data.
void BSLMM::CalcCC_PVEnZ (gsl_vector *z_hat, class HYPBSLMM &cHyp)
{
  gsl_vector_set_zero(z_hat);
  cHyp.pve=0.0;
  cHyp.pge=1.0;
  return;
}

// Calculate pve and pge, and calculate z_hat for case-control data.
void BSLMM::CalcCC_PVEnZ (const gsl_vector *Xb, gsl_vector *z_hat,
			  class HYPBSLMM &cHyp) {
	double d;

	gsl_blas_ddot (Xb, Xb, &d);
	cHyp.pve=d/(double)ni_test;
	cHyp.pve/=cHyp.pve+1.0;
	cHyp.pge=1.0;

	gsl_vector_memcpy (z_hat, Xb);

	return;
}

// If a_mode==13, then run probit model.
void BSLMM::MCMC (const gsl_matrix *X, const gsl_vector *y) {
	clock_t time_start;
	double time_set=0, time_post=0;

	class HYPBSLMM cHyp_old, cHyp_new;

	gsl_matrix *Result_hyp=gsl_matrix_alloc (w_pace, 6);
	gsl_matrix *Result_gamma=gsl_matrix_alloc (w_pace, s_max);

	gsl_vector *Xb_new=gsl_vector_alloc (ni_test);
	gsl_vector *Xb_old=gsl_vector_alloc (ni_test);
	gsl_vector *z_hat=gsl_vector_alloc (ni_test);
	gsl_vector *z=gsl_vector_alloc (ni_test);

	gsl_matrix *Xgamma_old=gsl_matrix_alloc (ni_test, s_max);
	gsl_matrix *XtX_old=gsl_matrix_alloc (s_max, s_max);
	gsl_vector *Xtz_old=gsl_vector_alloc (s_max);
	gsl_vector *beta_old=gsl_vector_alloc (s_max);

	gsl_matrix *Xgamma_new=gsl_matrix_alloc (ni_test, s_max);
	gsl_matrix *XtX_new=gsl_matrix_alloc (s_max, s_max);
	gsl_vector *Xtz_new=gsl_vector_alloc (s_max);
	gsl_vector *beta_new=gsl_vector_alloc (s_max);

	double ztz=0.0;
	gsl_vector_memcpy (z, y);

	// For quantitative traits, y is centered already in
	// gemma.cpp, but just in case.
	double mean_z=CenterVector (z);
	gsl_blas_ddot(z, z, &ztz);

	double logPost_new, logPost_old;
	double logMHratio;

	gsl_matrix_set_zero (Result_gamma);
	if (a_mode==13) {
		pheno_mean=0.0;
	}

	vector<pair<double, double> > beta_g;
	for (size_t i=0; i<ns_test; i++) {
		beta_g.push_back(make_pair(0.0, 0.0));
	}

	vector<size_t> rank_new, rank_old;
	vector<pair<size_t, double> > pos_loglr;

	time_start=clock();
	MatrixCalcLmLR (X, z, pos_loglr);
	time_Proposal=(clock()-time_start)/(double(CLOCKS_PER_SEC)*60.0);

	stable_sort (pos_loglr.begin(), pos_loglr.end(), comp_lr);
	for (size_t i=0; i<ns_test; ++i) {
		mapRank2pos[i]=pos_loglr[i].first;
	}

	// Calculate proposal distribution for gamma (unnormalized),
	// and set up gsl_r and gsl_t.
	gsl_rng_env_setup();
	const gsl_rng_type * gslType;
	gslType = gsl_rng_default;
	if (randseed<0)
	{
		time_t rawtime;
		time (&rawtime);
		tm * ptm = gmtime (&rawtime);

		randseed = (unsigned) (ptm->tm_hour%24*3600+
				       ptm->tm_min*60+ptm->tm_sec);
	}
	gsl_r = gsl_rng_alloc(gslType);
	gsl_rng_set(gsl_r, randseed);

	double *p_gamma = new double[ns_test];
	CalcPgamma (p_gamma);

	gsl_t=gsl_ran_discrete_preproc (ns_test, p_gamma);

	// Initial parameters.
	InitialMCMC (X, z, rank_old, cHyp_old, pos_loglr);

	cHyp_initial=cHyp_old;

	if (cHyp_old.n_gamma==0) {
	    logPost_old=CalcPosterior (ztz, cHyp_old);
	}
	else {
	  SetXgamma (Xgamma_old, X, rank_old);
	  CalcXtX (Xgamma_old, z, rank_old.size(), XtX_old, Xtz_old);
	  logPost_old=CalcPosterior (Xgamma_old, XtX_old, Xtz_old, ztz,
				     rank_old.size(), Xb_old, beta_old,
				     cHyp_old);
	}

	// Calculate centered z_hat, and pve.
	if (a_mode==13) {
		if (cHyp_old.n_gamma==0) {
			CalcCC_PVEnZ (z_hat, cHyp_old);
		}
		else {
			CalcCC_PVEnZ (Xb_old, z_hat, cHyp_old);
		}
	}

	// Start MCMC.
	int accept;
	size_t total_step=w_step+s_step;
	size_t w=0, w_col, pos;
	size_t repeat=0;

	for (size_t t=0; t<total_step; ++t) {
		if (t%d_pace==0 || t==total_step-1) {
		  ProgressBar ("Running MCMC ", t, total_step-1,
			       (double)n_accept/(double)(t*n_mh+1));
		}

		if (a_mode==13) {
			SampleZ (y, z_hat, z);
			mean_z=CenterVector (z);
			gsl_blas_ddot(z,z,&ztz);

			// First proposal.
			if (cHyp_old.n_gamma==0) {
			  logPost_old=CalcPosterior (ztz, cHyp_old);
			} else {
			  gsl_matrix_view Xold_sub=
			    gsl_matrix_submatrix(Xgamma_old, 0, 0, ni_test,
						 rank_old.size());
			  gsl_vector_view Xtz_sub=
			    gsl_vector_subvector(Xtz_old, 0, rank_old.size());
			  gsl_blas_dgemv (CblasTrans, 1.0, &Xold_sub.matrix,
					  z, 0.0, &Xtz_sub.vector);
			  logPost_old=
			    CalcPosterior (Xgamma_old, XtX_old, Xtz_old, ztz,
					   rank_old.size(), Xb_old, beta_old,
					   cHyp_old);
			}
		}

		// M-H steps.
		for (size_t i=0; i<n_mh; ++i) {
			if (gsl_rng_uniform(gsl_r)<0.33) {
			  repeat = 1+gsl_rng_uniform_int(gsl_r, 20);
			}
			else {repeat=1;}

			logMHratio=0.0;
			logMHratio+=
			  ProposeHnRho(cHyp_old, cHyp_new, repeat);
			logMHratio+=
			  ProposeGamma (rank_old, rank_new, p_gamma,
					cHyp_old, cHyp_new, repeat);
			logMHratio+=ProposePi(cHyp_old, cHyp_new, repeat);

			if (cHyp_new.n_gamma==0) {
				logPost_new=CalcPosterior (ztz, cHyp_new);
			} else {

			  // This makes sure that rank_old.size() ==
			  // rank_remove.size() does not happen.
			  if (cHyp_new.n_gamma<=20 || cHyp_old.n_gamma<=20) {
			    time_start=clock();
			    SetXgamma (Xgamma_new, X, rank_new);
			    CalcXtX (Xgamma_new, z, rank_new.size(),
				     XtX_new, Xtz_new);
			    time_set+=(clock()-time_start)/
			      (double(CLOCKS_PER_SEC)*60.0);
			  } else {
			    time_start=clock();
			    SetXgamma (X, Xgamma_old, XtX_old, Xtz_old, z,
				       rank_old, rank_new, Xgamma_new,
				       XtX_new, Xtz_new);
			    time_set+=(clock()-time_start)/
			      (double(CLOCKS_PER_SEC)*60.0);
			  }
			  time_start=clock();
			  logPost_new=
			    CalcPosterior (Xgamma_new, XtX_new, Xtz_new, ztz,
					   rank_new.size(), Xb_new, beta_new,
					   cHyp_new);
			  time_post+=(clock()-time_start)/
			    (double(CLOCKS_PER_SEC)*60.0);
			}
			logMHratio+=logPost_new-logPost_old;

			if (logMHratio>0 ||
			    log(gsl_rng_uniform(gsl_r))<logMHratio) {
			  accept=1;
			  n_accept++;
			}
			else {accept=0;}

			if (accept==1) {
				logPost_old=logPost_new;
				cHyp_old=cHyp_new;
				gsl_vector_memcpy (Xb_old, Xb_new);

				rank_old.clear();
				if (rank_new.size()!=0) {
					for (size_t i=0;
					     i<rank_new.size();
					     ++i) {
					  rank_old.push_back(rank_new[i]);
					}

					gsl_matrix_view Xold_sub=gsl_matrix_submatrix(Xgamma_old, 0, 0, ni_test, rank_new.size());
					gsl_matrix_view XtXold_sub=gsl_matrix_submatrix(XtX_old, 0, 0, rank_new.size(), rank_new.size());
					gsl_vector_view Xtzold_sub=gsl_vector_subvector(Xtz_old, 0, rank_new.size());
					gsl_vector_view betaold_sub=gsl_vector_subvector(beta_old, 0, rank_new.size());

					gsl_matrix_view Xnew_sub=gsl_matrix_submatrix(Xgamma_new, 0, 0, ni_test, rank_new.size());
					gsl_matrix_view XtXnew_sub=gsl_matrix_submatrix(XtX_new, 0, 0, rank_new.size(), rank_new.size());
					gsl_vector_view Xtznew_sub=gsl_vector_subvector(Xtz_new, 0, rank_new.size());
					gsl_vector_view betanew_sub=gsl_vector_subvector(beta_new, 0, rank_new.size());

					gsl_matrix_memcpy(&Xold_sub.matrix,
							  &Xnew_sub.matrix);
					gsl_matrix_memcpy(&XtXold_sub.matrix,
							  &XtXnew_sub.matrix);
					gsl_vector_memcpy(&Xtzold_sub.vector,
							  &Xtznew_sub.vector);
					gsl_vector_memcpy(&betaold_sub.vector,
							  &betanew_sub.vector);
				}
			} else {
			  cHyp_new=cHyp_old;
			}

		}

		// Calculate z_hat, and pve.
		if (a_mode==13) {
			if (cHyp_old.n_gamma==0) {
				CalcCC_PVEnZ (z_hat, cHyp_old);
			}
			else {
				CalcCC_PVEnZ (Xb_old, z_hat, cHyp_old);
			}

			// Sample mu and update z_hat.
			gsl_vector_sub (z, z_hat);
			mean_z+=CenterVector(z);
			mean_z+=gsl_ran_gaussian(gsl_r,
						 sqrt(1.0/(double) ni_test));

			gsl_vector_add_constant (z_hat, mean_z);
		}

		// Save data.
		if (t<w_step) {continue;}
		else {
			if (t%r_pace==0) {
				w_col=w%w_pace;
				if (w_col==0) {
					if (w==0) {
					  WriteResult(0,Result_hyp,
						      Result_gamma,w_col);
					}
					else {
					  WriteResult(1,Result_hyp,
						      Result_gamma,w_col);
					  gsl_matrix_set_zero (Result_hyp);
					  gsl_matrix_set_zero (Result_gamma);
					}
				}

				gsl_matrix_set(Result_hyp,w_col,0,
					       cHyp_old.h);
				gsl_matrix_set(Result_hyp,w_col,1,
					       cHyp_old.pve);
				gsl_matrix_set(Result_hyp,w_col,2,
					       cHyp_old.rho);
				gsl_matrix_set(Result_hyp,w_col,3,
					       cHyp_old.pge);
				gsl_matrix_set(Result_hyp,w_col,4,
					       cHyp_old.logp);
				gsl_matrix_set(Result_hyp,w_col,5,
					       cHyp_old.n_gamma);

				for (size_t i=0; i<cHyp_old.n_gamma; ++i) {
					pos=mapRank2pos[rank_old[i]]+1;
					gsl_matrix_set(Result_gamma,w_col,
						       i,pos);

					beta_g[pos-1].first+=
					  gsl_vector_get(beta_old, i);
					beta_g[pos-1].second+=1.0;
				}

				if (a_mode==13) {
					pheno_mean+=mean_z;
				}

				w++;
			}
		}
	}
	cout<<endl;

	cout<<"time on selecting Xgamma: "<<time_set<<endl;
	cout<<"time on calculating posterior: "<<time_post<<endl;

	w_col=w%w_pace;
	WriteResult (1, Result_hyp, Result_gamma, w_col);

	gsl_vector *alpha=gsl_vector_alloc (ns_test);
	gsl_vector_set_zero (alpha);
	WriteParam (beta_g, alpha, w);
	gsl_vector_free(alpha);

	gsl_matrix_free(Result_hyp);
	gsl_matrix_free(Result_gamma);

	gsl_vector_free(z_hat);
	gsl_vector_free(z);
	gsl_vector_free(Xb_new);
	gsl_vector_free(Xb_old);

	gsl_matrix_free(Xgamma_old);
	gsl_matrix_free(XtX_old);
	gsl_vector_free(Xtz_old);
	gsl_vector_free(beta_old);

	gsl_matrix_free(Xgamma_new);
	gsl_matrix_free(XtX_new);
	gsl_vector_free(Xtz_new);
	gsl_vector_free(beta_new);

	delete [] p_gamma;
	beta_g.clear();

	return;
}