#include <stdio.h>
#include <stdlib.h>
#include <sys/time.h>
#include <gsl/gsl_statistics.h>
#include <gsl/gsl_statistics_int.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_blas.h>

#define SAMPLE_SIZE 187
#define MARKER_LENGTH 85
#define N_ITERATIONS 25000
#define N_BURNS 5000
#define N_STEPS 10
#define N_PARS 2
#define N_GROUPS 50
#define COE 1.0
#define JITTER 0.01
#define ALPHA 0.02
#define SEP 10
#define EPS 2
#define DELTA 1e-3
#define NN 10

int main(void){
    FILE *fp_in,*fp_out;
    int i,j,k,i_iter,i_real,s,**x;
    int do_eta_sample=0,do_sigma_update=1,update_sigma0=1;
    int n_used_groups,group_index[SAMPLE_SIZE],n_each_group[N_GROUPS];
    int group_change[N_GROUPS];
    int nonparametric=0;
    double t_age[SAMPLE_SIZE],t_group_value[N_GROUPS];
    double rdum,fs[N_GROUPS],fss[N_GROUPS],bb,ss;
    double epsilon,aa,mu1,sigma11,mu_bar,sigma_bar;
    double y_qtl[SAMPLE_SIZE],alpha[N_PARS];
    double b[MARKER_LENGTH],bs[MARKER_LENGTH],sigma0,sigma[MARKER_LENGTH];
    double bss[MARKER_LENGTH],eta[SAMPLE_SIZE],tau;
    double beta1,beta_bar,gamma11,gamma_bar;
    double bj_bar,sj_bar,xb,**x_temp,*y_temp,x_square;
    double t_age_min,t_age_max,delta_age,p_max;
    double at,bt,tsum,ybar,ytbar;
    int chromosome[MARKER_LENGTH],index,nn[4],**x0;
    int left[MARKER_LENGTH],middle[MARKER_LENGTH],right[MARKER_LENGTH];
    int fixed[MARKER_LENGTH],**missing,include[MARKER_LENGTH];
    double cc=100,is[MARKER_LENGTH];
    double pos[MARKER_LENGTH],setoff;
    double low_bound,up_bound,delta=2.0,lambda_temp;
    double pl,pr,pb[4],rho;
    double pp[MARKER_LENGTH][4],xdb,range,range_star;
    double lambda[MARKER_LENGTH];
    double sep1,sep2,sep,prob1,prob2;
    double rlr,prob,rl[MARKER_LENGTH],rr[MARKER_LENGTH];
    double lambda_s[MARKER_LENGTH];

    const gsl_rng_type *T;
    gsl_rng *r;
    gsl_ran_discrete_t *g;

    gsl_vector *mu_res,*mu_v,*mu_star,*mu_temp,*y_res,*y_v,*eta_res,*eta_temp;
    gsl_vector *s12,*s12c,*eta_v,*eta_c,*b_v,*alpha_v,*alpha_res,*alpha_temp;
    gsl_vector *g12,*g12c,*beta_v,*beta_star;
    gsl_vector_view eta_view,mu_view,beta_view,alpha_view;
    gsl_matrix *cov,*cov_org,*cov_res,*cov_inv,*cov_star,*cov_temp,*id_m,*m_m,*id_g;
    gsl_matrix *gamma_left,*gamma_middle,*gamma_star,*gamma_m,*gamma_inv;
    gsl_matrix *sigma_v,*x_v;
    gsl_matrix_view cov1_view,cov2_view;
    gsl_matrix_view gamma1_view,gamma2_view;
    gsl_permutation *p,*p1,*p2,*p3;
    
    gsl_rng_env_setup();
    T=gsl_rng_default;
    r=gsl_rng_alloc(T);
    
    alpha_v=gsl_vector_alloc(N_PARS);
    alpha_res=gsl_vector_alloc(N_PARS);
    alpha_temp=gsl_vector_alloc(N_PARS);
    beta_v=gsl_vector_alloc(N_PARS);
    beta_star=gsl_vector_alloc(N_PARS);
    g12=gsl_vector_alloc(N_PARS-1);
    g12c=gsl_vector_alloc(N_PARS-1);
    
    b_v=gsl_vector_alloc(MARKER_LENGTH);

    x = (int **)malloc(SAMPLE_SIZE*sizeof(int **));
    x0 = (int **)malloc(SAMPLE_SIZE*sizeof(int **));
    missing = (int **)malloc(SAMPLE_SIZE*sizeof(int **));
    for(i = 0; i < SAMPLE_SIZE;i++){
	x[i] = (int *)malloc(MARKER_LENGTH*sizeof(int *));
	x0[i] = (int *)malloc(MARKER_LENGTH*sizeof(int *));
	missing[i] = (int *)malloc(MARKER_LENGTH*sizeof(int *));
    }

    /*Read input data for marker positions and chromosome*/
    fp_in = fopen("lbwt_pos.txt","r");
    for(j = 0;j < MARKER_LENGTH;j++)
	fscanf(fp_in,"%d",&chromosome[j]);
    for(j = 0;j < MARKER_LENGTH;j++)
	fscanf(fp_in,"%lf",&pos[j]);
    fclose(fp_in);
    setoff = 0.0;
    for(j = 0;j < MARKER_LENGTH;j++){
	fixed[j] = 0;	 
	middle[j] = 0;
	right[j] = 0;
    }
    left[0] = 1;
    right[MARKER_LENGTH - 1] = 1;
    for(j = 0;j < MARKER_LENGTH;j++){
	if(j > 0){
	    if(chromosome[j] != chromosome[j - 1]){
		setoff = pos[j - 1] + 10;
		left[j] = 1;
		right[j - 1] = 1;
		middle[j -1] = 1;
	    }
	    if(chromosome[j] != chromosome[j - 1]){
		if(j == MARKER_LENGTH - 1){
		    fixed[j] = 1;
		    middle[j] = 0;
		}else if(chromosome[j] != chromosome[j + 1]){
		    fixed[j] = 1;
		    middle[j] = 0;
		}
	    }
	    pos[j] += setoff;
	}else{
	    left[j] = 1;
	}
    }
    if(!fixed[MARKER_LENGTH - 1])
	middle[MARKER_LENGTH - 1] = 1;
    /*Read input data to fill x[][] and y_qtl[]*/
    fp_in = fopen("mes.txt","r");
    tsum = 0.0;
    for(i = 0;i < SAMPLE_SIZE;i++){
	for(j = 0;j < MARKER_LENGTH;j++){
	    fscanf(fp_in,"%d",&x0[i][j]);
	    if(x0[i][j]==9){
		missing[i][j] = 1;
		x0[i][j] = 1; /*Initialization*/
	    }
	}
	/*Read in the covariate and phenotypic values*/
	fscanf(fp_in,"%lf",&t_age[i]);
	fscanf(fp_in,"%lf",&y_qtl[i]);
	tsum += t_age[i]*t_age[i];
    }
    fclose(fp_in);

    /*Construct the eta group*/
    t_age_min = gsl_stats_min(t_age,1,SAMPLE_SIZE);
    t_age_max = gsl_stats_max(t_age,1,SAMPLE_SIZE);
    delta_age = (t_age_max - t_age_min)/(N_GROUPS - 1);
    for(i=0;i<N_GROUPS;i++){
	n_each_group[i] = 0;
	t_group_value[i] = t_age_min + (double)i*delta_age;
    }
    for(i=0;i<SAMPLE_SIZE;i++){
	for(j=0;j<N_GROUPS;j++){
	    if(fabs(t_age[i]-t_group_value[j])<=0.5*delta_age){
		group_index[i] = j;
		n_each_group[j]++;
		break;
	    }
	}
    }
    n_used_groups = 0;
    for(i=0;i<N_GROUPS;i++){
	if(n_each_group[i]!=0){
	    n_each_group[n_used_groups] = n_each_group[i];
	    t_group_value[n_used_groups] = t_group_value[i];
	    group_change[i] = n_used_groups;
	    n_used_groups++;
	}
    }
    for(i=0;i<SAMPLE_SIZE;i++){
	group_index[i] = group_change[group_index[i]];
    }
    /*Initialization*/
    sigma0 = 1.0;
    for(j = 0;j < MARKER_LENGTH;j++){
	b[j] = 0.0;
	include[j] = 0;
	gsl_vector_set(b_v,j,b[j]);
	sigma[j] = sigma0*cc/SAMPLE_SIZE;
	if(!(fixed[j]||middle[j]))
	    sep = pos[j + 1] - pos[j];
	else
	    sep = 0;
	lambda[j] = 0.5*sep;
	prob = 0.5*(1 - exp(-ALPHA*lambda[j]))
	    *(1 - exp(-ALPHA*(sep - lambda[j])))
	    /(1 + exp(-ALPHA*sep));
	for(i = 0;i<SAMPLE_SIZE;i++){
	    if(fixed[j])
		x[i][j] = x0[i][j];
	    else{
		rdum = gsl_rng_uniform(r);
		if(x0[i][j]*x0[i][j + 1]==1){
		    if(rdum < prob)x[i][j] = - x0[i][j];
		    else x[i][j] = x0[i][j];
		}else{
		    if(rdum < 0.5)x[i][j] = - x0[i][j];
		    else x[i][j] = x0[i][j];
		}
	    }
	}
	is[j]=0.0;
	bs[j] = 0.0;
	bss[j] = 0.0;
	lambda_s[j]=0.0;
    }
    tau = 1.0;
    /*Initialize alpha[]*/
    for(i = 0; i<N_PARS; i++){
	alpha[i] = 1.0;
	gsl_vector_set(alpha_res,i,alpha[i]);
    }
    /*Initialize eta[] using prior mean*/
    m_m=gsl_matrix_alloc(n_used_groups,N_PARS);
    for(i=0;i<n_used_groups;i++){
	ss=1.0;
	for(j=0;j<N_PARS;j++){
	    gsl_matrix_set(m_m,i,j,ss);
	    ss*=t_group_value[i];
	}
    }
    if(!nonparametric){
	/*Initialize linear parameters*/
	at = 0.0;
	bt = 0.0;
    }
    mu_v=gsl_vector_alloc(n_used_groups);
    mu_res=gsl_vector_alloc(n_used_groups);
    mu_star=gsl_vector_alloc(n_used_groups);
    mu_temp=gsl_vector_alloc(n_used_groups);

    id_m=gsl_matrix_alloc(n_used_groups,n_used_groups);

    eta_v=gsl_vector_alloc(n_used_groups);
    eta_res=gsl_vector_alloc(n_used_groups);
    eta_temp=gsl_vector_alloc(n_used_groups);
    eta_c=gsl_vector_alloc(n_used_groups);
    gsl_vector_set_zero(eta_res);
    gsl_blas_dgemv(CblasNoTrans,1,m_m,alpha_res,1,eta_res);

    id_g=gsl_matrix_alloc(N_PARS,N_PARS);
    gamma_m=gsl_matrix_alloc(N_PARS,N_PARS);
    gamma_left=gsl_matrix_alloc(N_PARS,n_used_groups);
    gamma_middle=gsl_matrix_alloc(N_PARS,n_used_groups);
    gamma_star=gsl_matrix_alloc(N_PARS,N_PARS);
    gamma_inv=gsl_matrix_alloc(N_PARS,N_PARS);

    x_v = gsl_matrix_alloc(n_used_groups,MARKER_LENGTH);
    
    for(i=0;i<SAMPLE_SIZE;i++){
	eta[i]=gsl_vector_get(eta_res,group_index[i]);
    }

    y_res=gsl_vector_alloc(n_used_groups); /*reserved y values from data*/
    y_v=gsl_vector_alloc(n_used_groups); 
    s12=gsl_vector_alloc(n_used_groups-1);
    s12c=gsl_vector_alloc(n_used_groups-1);

    x_temp = (double **)malloc(n_used_groups*sizeof(double *));
    for(i=0;i<n_used_groups;i++)
	x_temp[i] = (double *)malloc(MARKER_LENGTH*sizeof(double));
    y_temp = (double *)malloc(n_used_groups*sizeof(double));
    for(i=0;i<n_used_groups;i++){
	y_temp[i] = 0.0;
	for(j=0;j<MARKER_LENGTH;j++){
	    x_temp[i][j] = 0.0;
	}
    }
    for(i=0;i<SAMPLE_SIZE;i++){
	y_temp[group_index[i]]+=y_qtl[i];
	for(j=0;j<MARKER_LENGTH;j++){
	    x_temp[group_index[i]][j]+=(double)x[i][j];
	}
    }
    for(i=0;i<n_used_groups;i++){
	gsl_vector_set(y_res,i,y_temp[i]/(double)n_each_group[i]);
	for(j=0;j<MARKER_LENGTH;j++){
	    gsl_matrix_set(x_v,i,j,x_temp[i][j]/(double)n_each_group[i]);
	}
    }

    for(i=0;i<n_used_groups;i++){
	fs[i]=0.0;
	fss[i]=0.0;
    }
    i_iter=0;
    i_real=0;
    p=gsl_permutation_alloc(n_used_groups);
    p1=gsl_permutation_alloc(n_used_groups - 1);
    /*Calculate the covariance matrix of gaussian prior*/
    cov=gsl_matrix_alloc(n_used_groups,n_used_groups);
    cov_org = gsl_matrix_alloc(n_used_groups,n_used_groups);
    cov_res = gsl_matrix_alloc(n_used_groups,n_used_groups);
    cov_inv = gsl_matrix_alloc(n_used_groups,n_used_groups);
    cov_star = gsl_matrix_alloc(n_used_groups,n_used_groups);
    cov_temp = gsl_matrix_alloc(n_used_groups,n_used_groups);
    sigma_v = gsl_matrix_alloc(n_used_groups,n_used_groups);
    for(i=0;i<n_used_groups-1;i++){
	gsl_matrix_set(cov_org,i,i,1.0+JITTER);
	gsl_matrix_set(sigma_v,i,i,(double)n_each_group[i]/sigma0);
	for(j=i+1;j<n_used_groups;j++){
	    ss=-COE*(t_group_value[i]-t_group_value[j])
		*(t_group_value[i]-t_group_value[j]);
	    ss=exp(ss);
	    gsl_matrix_set(cov_org,i,j,ss);
	    gsl_matrix_set(cov_org,j,i,ss);
	    gsl_matrix_set(sigma_v,i,j,0.0);
	    gsl_matrix_set(sigma_v,j,i,0.0);
	}
    }
    gsl_matrix_set(cov_org,n_used_groups-1,n_used_groups-1,1.0+JITTER);
    gsl_matrix_set(sigma_v,n_used_groups-1,n_used_groups-1,
		   (double)n_each_group[n_used_groups-1]/sigma0);
    /*Start iteration loop*/
    fp_out=fopen("mes_linear_out.txt","w");
    while(i_iter<N_ITERATIONS){
	if(nonparametric){
	    /*Update eta[]*/
	    /*Calculate the mean of gaussian prior*/
	    gsl_vector_set_zero(mu_res);
	    gsl_blas_dgemv(CblasNoTrans,1,m_m,alpha_res,1,mu_res);
	    /*Compute Sigma^star=sigma*(D*sigma+1)^{-1}*/
	    gsl_matrix_memcpy(cov_res,cov_org); /*cov=cov_res*/
	    gsl_matrix_scale(cov_res,1.0/tau); /*sigma=exp(-c*(t-t1)^2)/tau*/
	    gsl_matrix_memcpy(cov_temp,cov_res); /*cov=cov_res*/
	    gsl_matrix_set_zero(cov);
	    gsl_blas_dsymm(CblasRight,CblasUpper,1,
			   sigma_v,cov_temp,1,cov); /*cov*D*/
	    gsl_matrix_set_identity(id_m); 
	    gsl_matrix_add(cov,id_m); /*1+cov*D*/
	    gsl_linalg_LU_decomp(cov,p,&s);
	    gsl_linalg_LU_invert(cov,p,cov_inv); /*cov_inv=(1+cov*D)^(-1)*/
	    gsl_matrix_set_zero(cov_star);
	    /*cov^star=cov*(1+cov*D)^{-1}*/
	    gsl_blas_dsymm(CblasRight,CblasUpper,1,cov_res,cov_inv,1,cov_star);
	    /*Compute mu^star*/
	    gsl_vector_memcpy(y_v,y_res);    /*y=y_res*/
	    gsl_vector_memcpy(mu_v,mu_res); /*mu=mu_res*/
	    gsl_blas_dgemv(CblasNoTrans,-1,x_v,b_v,1,y_v); /*y_v=y-x*b*/
	    gsl_vector_sub(y_v,mu_v);    /*y=y-mu*/
	    gsl_vector_set_zero(mu_temp);
	    /*mu^star=cov^star*y*/
	    gsl_blas_dsymv(CblasUpper,1,sigma_v,y_v,1,mu_temp);
	    gsl_vector_set_zero(mu_star);
	    gsl_blas_dsymv(CblasUpper,1,cov_star,mu_temp,1,mu_star);
	    gsl_vector_add(mu_star,mu_res); /*mu^star=mu^star+mu*/
	    /*Sampling each eta*/
	    if(do_eta_sample){
		for(i=0;i<n_used_groups;i++){
		    gsl_vector_memcpy(eta_v,eta_res);  /*eta=eta_res*/
		    gsl_vector_memcpy(mu_v,mu_star);  /*mu=mu^star*/ 
		    gsl_matrix_memcpy(cov,cov_star); /*cov=cov^star*/
		    mu1=gsl_vector_get(mu_v,i);  /*mu1=mu[i]*/    
		    sigma11=gsl_matrix_get(cov,i,i);  /*sigma11=cov[1,1]*/
		    if(i!=n_used_groups-1){
			gsl_vector_swap_elements(mu_v,i,n_used_groups-1);
			gsl_vector_swap_elements(eta_v,i,n_used_groups-1);
		    }
		    mu_view=gsl_vector_subvector(mu_v,0,n_used_groups-1);
		    eta_view=gsl_vector_subvector(eta_v,0,n_used_groups-1);
		    gsl_matrix_swap_rows(cov,i,n_used_groups-1);
		    gsl_matrix_swap_columns(cov,i,n_used_groups-1);
		    cov1_view=gsl_matrix_submatrix(cov,0,0,n_used_groups,
						   n_used_groups-1);
		    /*s12=cov[i,]*/
		    gsl_matrix_get_row(s12,&cov1_view.matrix,n_used_groups-1);  
		    cov2_view=gsl_matrix_submatrix(&cov1_view.matrix,0,0,
						   n_used_groups-1,
						   n_used_groups-1);
		    gsl_linalg_LU_decomp(&cov2_view.matrix,p1,&s);
		    gsl_linalg_LU_solve(&cov2_view.matrix,p1,s12,s12c);
		    gsl_blas_ddot(s12c,s12,&sigma_bar);
		    sigma_bar=sigma11-sigma_bar;
		    if(sigma_bar<=0){
			printf("Sigma less than 0: %d\t%lf\t%lf\n",
			       i,sigma_bar,sigma11);
			exit(1);
		    }
		    gsl_vector_sub(&eta_view.vector,&mu_view.vector);
		    gsl_blas_ddot(s12c,&eta_view.vector,&mu_bar);
		    mu_bar+=mu1;
		    gsl_vector_set(eta_res,i,
				   mu_bar+gsl_ran_gaussian(r,sqrt(sigma_bar)));
		}
	    }else{
		gsl_vector_memcpy(mu_v,mu_star);  /*mu=mu^star*/ 
		gsl_matrix_memcpy(cov,cov_star); /*cov=cov^star*/
		for(i=0;i<n_used_groups;i++){
		    gsl_vector_set(eta_v,i,gsl_ran_gaussian(r,1.0));
		}
		gsl_linalg_cholesky_decomp(cov);
		gsl_vector_set_zero(eta_temp);
		gsl_blas_dsymv(CblasLower,1.0,cov,eta_v,1.0,eta_temp);
		gsl_vector_add(eta_temp,mu_v);
		for(i=0;i<n_used_groups;i++){
		    gsl_vector_set(eta_res,i,gsl_vector_get(eta_temp,i));
		}
	    }
	    for(i=0;i<SAMPLE_SIZE;i++){
		eta[i]=gsl_vector_get(eta_res,group_index[i]);
	    }
	    /*Update alpha*/
	    /*Calculate the mean and covariance matrix for alpha*/
	    p2=gsl_permutation_alloc(N_PARS);
	    p3=gsl_permutation_alloc(N_PARS-1);
	    gsl_matrix_memcpy(cov,cov_res); /*cov=cov_res*/
	    gsl_linalg_LU_decomp(cov,p,&s);
	    gsl_linalg_LU_invert(cov,p,cov_inv); /*cov_inv=cov^(-1)*/
	    gsl_matrix_set_zero(gamma_left);
	    /*gamma^star=(m^T*(cov)^{-1}*m)^(-1)*/
	    gsl_blas_dgemm(CblasTrans,CblasNoTrans,1,m_m,
			   cov_inv,1,gamma_left);
	    /*gamma_left=m^T*cov^(-1)*/
	    gsl_matrix_set_zero(gamma_inv);
	    gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,1,gamma_left,
			   m_m,1,gamma_inv);
	    /*Include a small term to make prior for alpha proper*/ 
	    gsl_matrix_set_identity(id_g);
	    gsl_matrix_scale(id_g,1e-6/tau);
	    gsl_matrix_add(gamma_inv,id_g);
	    /*gamma_star=gamma_inv^(-1)*/
	    gsl_linalg_LU_decomp(gamma_inv,p2,&s);
	    gsl_linalg_LU_invert(gamma_inv,p2,gamma_star); 
	    /*gamma_middle=gamma_star*m^T*sigma^-1*/
	    gsl_matrix_set_zero(gamma_middle);
	    gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,1,gamma_star,
			   gamma_left,1,gamma_middle);
	    gsl_vector_set_zero(beta_star);
	    /*beta^star=gamma^star*m^T*cov^(-1)*eta*/
	    gsl_blas_dgemv(CblasNoTrans,1,gamma_middle,eta_res,1,beta_star);
	    /*Sampling each alpha*/
	    if(do_eta_sample){
		for(i=0;i<N_PARS;i++){
		    gsl_vector_memcpy(alpha_v,alpha_res);/*alpha_v=alpha_res*/
		    gsl_vector_memcpy(beta_v,beta_star); /*beta_v=beta^star*/ 
		    gsl_matrix_memcpy(gamma_m,gamma_star);/*gamma_m=gamma^star*/
		    beta1=gsl_vector_get(beta_v,i);/*beta1=beta[i]*/    
		    gamma11=gsl_matrix_get(gamma_m,i,i);/*gamma11=gamma_m[i,i]*/
		    if(i!=N_PARS-1){
			gsl_vector_swap_elements(beta_v,i,N_PARS-1);
			gsl_vector_swap_elements(alpha_v,i,N_PARS-1);
		    }
		    beta_view=gsl_vector_subvector(beta_v,0,N_PARS-1);
		    alpha_view=gsl_vector_subvector(alpha_v,0,N_PARS-1);
		    gsl_matrix_swap_rows(gamma_m,i,N_PARS-1);
		    gsl_matrix_swap_columns(gamma_m,i,N_PARS-1);
		    gamma1_view=gsl_matrix_submatrix(gamma_m,0,0,N_PARS,
						     N_PARS-1);
		    /*g12=gamma_m[i,]*/
		    gsl_matrix_get_row(g12,&gamma1_view.matrix,N_PARS-1);  
		    gamma2_view=gsl_matrix_submatrix(&gamma1_view.matrix,0,0,
						     N_PARS-1,N_PARS-1);
		    gsl_linalg_LU_decomp(&gamma2_view.matrix,p3,&s);
		    gsl_linalg_LU_solve(&gamma2_view.matrix,p3,g12,g12c);
		    gsl_blas_ddot(g12c,g12,&gamma_bar);
		    gamma_bar=gamma11-gamma_bar;
		    if(gamma_bar<=0){
			printf("Sigma less than 0: %lf\t%lf\n",
			       gamma_bar,gamma11);
			exit(1);
		    }
		    gsl_vector_sub(&alpha_view.vector,&beta_view.vector);
		    gsl_blas_ddot(g12c,&alpha_view.vector,&beta_bar);
		    beta_bar+=beta1;
		    alpha[i]=beta_bar+gsl_ran_gaussian(r,sqrt(gamma_bar));
		    gsl_vector_set(alpha_res,i,alpha[i]);
		}
	    }else{
		gsl_vector_memcpy(beta_v,beta_star);  /*mu=mu^star*/ 
		gsl_matrix_memcpy(gamma_m,gamma_star); /*cov=cov^star*/
		for(i=0;i<N_PARS;i++){
		    gsl_vector_set(alpha_v,i,gsl_ran_gaussian(r,1.0));
		}
		gsl_linalg_cholesky_decomp(gamma_m);
		gsl_vector_set_zero(alpha_temp);
		gsl_blas_dsymv(CblasLower,1,gamma_m,alpha_v,1,alpha_temp);
		gsl_vector_add(alpha_temp,beta_v);
		for(i=0;i<N_PARS;i++){
		    alpha[i]=gsl_vector_get(alpha_temp,i);
		    gsl_vector_set(alpha_res,i,alpha[i]);
		}
	    }
	    /*Update tau*/
	    gsl_vector_memcpy(eta_v,eta_res); /*eta=eta_res*/
	    gsl_vector_set_zero(mu_res);
	    gsl_blas_dgemv(CblasNoTrans,1,m_m,alpha_res,1,mu_res);
	    gsl_vector_memcpy(mu_v,mu_res); /*mu=mu_res*/ 
	    gsl_matrix_memcpy(cov,cov_org); /*cov=cov_res*/
	    gsl_vector_sub(eta_v,mu_v); /*eta-mu*/
	    gsl_linalg_LU_decomp(cov,p,&s);
	    gsl_linalg_LU_solve(cov,p,eta_v,eta_c); /*cov^-1*(eta-mu)*/
	    gsl_blas_ddot(eta_v,eta_c,&bb);
	    aa=0.5*n_used_groups;
	    bb*=0.5;
	    tau=gsl_ran_gamma(r,aa+0.1,1.0/(bb+0.1));
	}else{
	    for(i=0;i<SAMPLE_SIZE;i++)
		eta[i] = at + bt*t_age[i];
	}
	/*Update b[j]*/
	for(j = 0;j < MARKER_LENGTH;j++){
	    if(!middle[j]){
		bj_bar=0.0;
		x_square=0.0;
		for(i=0;i<SAMPLE_SIZE;i++){
		    x_square+=x[i][j]*x[i][j];
		    xb=0.0;
		    for(k=0;k<MARKER_LENGTH;k++)
			if(k!=j)xb+=x[i][k]*b[k];
		    bj_bar+=x[i][j]*(y_qtl[i]-eta[i]-xb);
		}
		bj_bar/=x_square+sigma0/sigma[j];
		sj_bar=sqrt(sigma0/(x_square+sigma0/sigma[j]));
		b[j]=bj_bar+gsl_ran_gaussian(r,sj_bar);
	    }else
		b[j] = 0;
	    gsl_vector_set(b_v,j,b[j]);
	}
	if(!nonparametric){
	    /*Update at and bt*/
	    ybar = 0.0;
	    ytbar = 0.0;
	    for(i=0;i<SAMPLE_SIZE;i++){
		ybar += y_qtl[i] - bt*t_age[i];
		ytbar += t_age[i]*(y_qtl[i]  - at); 
		for(j=0;j<MARKER_LENGTH;j++){
		    ybar -= x[i][j]*b[j];
		    ytbar -= t_age[i]*x[i][j]*b[j]; 
		}
	    }
	    ybar/=(double)SAMPLE_SIZE;
	    ytbar/=tsum;
	    at = ybar + gsl_ran_gaussian(r,sqrt(sigma0/(double)SAMPLE_SIZE));
	    bt = ytbar + gsl_ran_gaussian(r,sqrt(sigma0/tsum));
	}
	if(update_sigma0){
	    sigma0=0.1;
	    for(i=0;i<SAMPLE_SIZE;i++){
		xb=0.0;
		for(j=0;j<MARKER_LENGTH;j++){
		    xb+=x[i][j]*b[j];
		}
		sigma0+=(y_qtl[i]-eta[i]-xb)*(y_qtl[i]-eta[i]-xb);
	    }
	    rdum=gsl_ran_chisq(r,(double)SAMPLE_SIZE+0.1);
	    sigma0/=rdum;
	}
	/*Update sigma[j]*/
	for(j = 0;j < MARKER_LENGTH;j++){
	    if(!middle[j]){
		xb = exp(0.5*b[j]*b[j]*SAMPLE_SIZE*(1.0 - 1.0/cc)/sigma0);
		rdum = gsl_rng_uniform(r);
		if(rdum < sqrt(cc)/(sqrt(cc) + xb*0.1/0.9)){
		    include[j] = 0;
		    sigma[j] = sigma0/SAMPLE_SIZE;
		}else{
		    include[j] = 1;
		    sigma[j] = cc*sigma0/SAMPLE_SIZE;
		}
	    }
	}
	/*Update the QTL genotype x[i][j]*/
	for(j = 0;j < MARKER_LENGTH - 1;j++){
	    sep = pos[j + 1] - pos[j];
	    rlr = 0.5*(1.0 - exp( - ALPHA*sep));
	    rl[j] = 0.5*(1.0 - exp( - ALPHA*lambda[j]));
	    rr[j] = 0.5*(1.0 - exp( - ALPHA*(sep - lambda[j])));
	    pp[j][0] = rl[j]*rr[j]/(1.0 - rlr);
	    pp[j][1] = rl[j]*(1.0 - rr[j])/rlr;
	    pp[j][2] = (1.0 - rl[j])*rr[j]/rlr;
	    pp[j][3] = (1.0 - rl[j])*(1.0 - rr[j])/(1.0 - rlr);
	}
	for(i = 0;i < SAMPLE_SIZE;i++){
	    for(j = 0;j < MARKER_LENGTH - 1;j++){
		if(!fixed[j]){
		    if(!middle[j]){
			index = (1 + x0[i][j]) + ((1 + x0[i][j + 1])>>1);
			xdb = y_qtl[i] - eta[i];
			for(k = 0;k < MARKER_LENGTH - 1;k++){
			    if(k != j)
				xdb -= x[i][k]*b[k];
			}
			prob = exp( - 2.0*b[j]*xdb/sigma0);
			prob = pp[j][index]/
			    (pp[j][index] + (1.0 - pp[j][index])*prob);
			rdum = gsl_rng_uniform(r);
			if(rdum < prob)
			    x[i][j] = 1;
			else 
			    x[i][j] = - 1;
		    }
		}else
		    x[i][j] = x0[i][j];
	    }
	}
	/*Update the x_temp which is used in the matrices calculation*/
	for(i = 0;i < n_used_groups;i++){
	    for(j = 0;j < MARKER_LENGTH;j++){
		x_temp[i][j] = 0.0;
	    }
	}
	for(i = 0;i < SAMPLE_SIZE;i++){
	    for(j = 0;j < MARKER_LENGTH;j++){
		x_temp[group_index[i]][j] += (double)x[i][j];
	    }
	}
	for(i = 0;i < n_used_groups;i++){
	    for(j = 0;j < MARKER_LENGTH;j++){
		gsl_matrix_set(x_v,i,j,x_temp[i][j]/(double)n_each_group[i]);
	    }
	}
	/*Update the missing genotypes*/
	for(j = 0;j < MARKER_LENGTH;j++){
	    if(j > 0)
		sep1 = pos[j] - (pos[j - 1] + lambda[j - 1]);
	    else 
		sep1 = 0.0;
	    if(j < MARKER_LENGTH - 1)
		sep2 = lambda[j];
	    else sep2 = 0.0;
	    sep = sep1 + sep2;
	    prob1 = 0.5*(1.0 - exp( - ALPHA*sep1));
	    prob2 = 0.5*(1.0 - exp( - ALPHA*sep2));
	    prob = 0.5*(1.0 - exp( - ALPHA*sep));
	    for(i = 0;i < SAMPLE_SIZE;i++){
		if(missing[i][j]){
		    if(!fixed[j]){
			rdum = gsl_rng_uniform(r);
			if(left[j]){
			    if(rdum < prob2)
				x0[i][j] = - x[i][j];
			    else 
				x0[i][j] = x[i][j];
			}else if(right[j]){
			    if(rdum < prob1)
				x0[i][j] = - x[i][j - 1];
			    else 
				x0[i][j] = x[i][j - 1];
			}else{
			    if(x[i][j - 1]*x[i][j]==1){
				if(rdum<(prob1*prob2/(1.0 - prob)))
				    x0[i][j] = - x[i][j - 1];
				else 
				    x0[i][j] = x[i][j - 1];
			    }else{
				if(rdum<(prob1*(1.0 - prob2)/prob))
				    x0[i][j] = - x[i][j - 1];
				else 
				    x0[i][j] = x[i][j - 1];
			    }
			}
		    }else{
			xdb = y_qtl[i] - eta[i];
			for(k = 0;k < MARKER_LENGTH;k++){
			    if(k != j)
				xdb -= x[i][k]*b[k];
			}
			rho = 1/(1 + exp( - 2*xdb*b[j]/sigma0));
			if(rdum < rho)
			    x0[i][j] = 1;
			else
			    x0[i][j] = - 1;
		    }
		}
	    }
	}
	/*Update the QTL location lambda[j]*/
	for(j = 0;j < MARKER_LENGTH;j++){
	    if((!middle[j])&&(!fixed[j])){
		for(k = 0;k < 4;k++){
		    nn[k] = 0;
		}
		/*Limit of old position*/
		sep = pos[j + 1] - pos[j];
		low_bound = lambda[j] - delta;
		up_bound = lambda[j] + delta;
		if(low_bound < 0)
		    low_bound = 0.0;
		if(up_bound > sep)
		    up_bound = sep;
		rdum = gsl_rng_uniform(r);
		lambda_temp = low_bound + rdum*(up_bound - low_bound);
		range = up_bound - low_bound;
		/*Limit of new position*/
		low_bound = lambda_temp - delta;
		up_bound = lambda_temp + delta;
		if(low_bound < 0)
		    low_bound = 0.0;
		if(up_bound > sep)
		    up_bound = sep;
		range_star = up_bound - low_bound;
		/*Recombination possibility*/
		pl = 0.5*(1.0 - exp( - ALPHA*lambda_temp));
		pr = 0.5*(1.0 - exp( - ALPHA*(sep - lambda_temp)));
		pb[0] = (1.0 - pl)*(1.0 - pr)/(1.0 - rl[j])/(1.0 - rr[j]);
		pb[1] = (1.0 - pl)*pr/(1.0 - rl[j])/rr[j];
		pb[2] = pl*pr/rl[j]/rr[j];
		pb[3] = pl*(1.0 - pr)/rl[j]/(1.0 - rr[j]);
		for(i = 0;i < SAMPLE_SIZE;i++){
		    index = ((1 + x0[i][j])<<1) + (1 + x[i][j])
			+((1 + x0[i][j + 1])>>1);
		    if(index > 3)
			index = 7 - index;
		    nn[index]++;
		}
		prob = 0.0;
		for(k = 0;k < 4;k++)
		    prob += (double)nn[k]*log(pb[k]);
		prob = exp(prob)*range_star/range;
		rdum = gsl_rng_uniform(r);
		if(rdum < prob)
		    lambda[j] = lambda_temp;
	    }
	}
	printf("%d\t%lf\t%lf\t%lf\t%lf\n",i_iter,b[2],b[22],b[42],b[62]);
	i_iter++;
	if(i_iter>N_BURNS){
	    if((i_iter-N_BURNS)%N_STEPS==0){
		for(j=0;j<MARKER_LENGTH;j++){
		    lambda_s[j]+=lambda[j];
		    is[j]+=include[j];
		    bs[j]+=b[j];
		    bss[j]+=b[j]*b[j];
		}
		for(i=0;i<n_used_groups;i++){
		    fs[i]+=gsl_vector_get(eta_res,i);
		    fss[i]+=gsl_vector_get(eta_res,i)
			*gsl_vector_get(eta_res,i);
		}
		i_real++;
	    }
	}
    }
    for(j=0;j<MARKER_LENGTH;j++){
	bs[j]/=(double)i_real;
	is[j]/=(double)i_real;
	bss[j] = sqrt((bss[j] - bs[j]*bs[j]*i_real)/(i_real - 1));
	lambda_s[j]/=(double)i_real;
	fprintf(fp_out,"%d\t%lf\t%lf\t%lf\t%lf\n",chromosome[j],
		pos[j]+lambda_s[j],is[j],bs[j],bss[j]);
    }
    fclose(fp_out);
    if(nonparametric){
	fp_out=fopen("fun_mes_ssvs_out.txt","w");
	for(i=0;i<n_used_groups;i++){
	    fs[i]/=(double)i_real;
	    fss[i]=sqrt((fss[i]-fs[i]*fs[i]*i_real)/(i_real-1));
	    fprintf(fp_out,"%lf\t%lf\t%lf\n",t_group_value[i],fs[i],fss[i]);
	}
	fclose(fp_out);
    }
    gsl_rng_free(r);
    return 0;
}