Massive patch using LLVM coding style. It was generated with:

  clang-format -style=LLVM -i *.cpp *.h

Please set your editor to replace tabs with spaces and use indentation of 2 spaces.

1 files changed, 5101 insertions, 5058 deletions

diff --git a/src/mvlmm.cpp b/src/mvlmm.cpp
index 78cd926..f1ab3fc 100644
--- a/src/mvlmm.cpp
+++ b/src/mvlmm.cpp
@@ -16,895 +16,914 @@
  along with this program. If not, see <http://www.gnu.org/licenses/>.
 */
 
-#include <iostream>
 #include <fstream>
+#include <iostream>
 #include <sstream>
 
-#include <iomanip>
+#include <assert.h>
+#include <bitset>
 #include <cmath>
+#include <cstring>
+#include <iomanip>
 #include <iostream>
 #include <stdio.h>
 #include <stdlib.h>
-#include <bitset>
-#include <cstring>
-#include <assert.h>
 
-#include "gsl/gsl_vector.h"
-#include "gsl/gsl_matrix.h"
-#include "gsl/gsl_linalg.h"
 #include "gsl/gsl_blas.h"
 #include "gsl/gsl_cdf.h"
-#include "gsl/gsl_roots.h"
-#include "gsl/gsl_min.h"
 #include "gsl/gsl_integration.h"
+#include "gsl/gsl_linalg.h"
+#include "gsl/gsl_matrix.h"
+#include "gsl/gsl_min.h"
+#include "gsl/gsl_roots.h"
+#include "gsl/gsl_vector.h"
 
-#include "io.h"
-#include "lapack.h"
 #include "eigenlib.h"
 #include "gzstream.h"
+#include "io.h"
+#include "lapack.h"
 #include "lmm.h"
 #include "mvlmm.h"
 
 using namespace std;
 
 // In this file, X, Y are already transformed (i.e. UtX and UtY).
-void MVLMM::CopyFromParam (PARAM &cPar) {
-	a_mode=cPar.a_mode;
-	d_pace=cPar.d_pace;
-
-	file_bfile=cPar.file_bfile;
-	file_geno=cPar.file_geno;
-	file_oxford=cPar.file_oxford;
-	file_out=cPar.file_out;
-	path_out=cPar.path_out;
-
-	l_min=cPar.l_min;
-	l_max=cPar.l_max;
-	n_region=cPar.n_region;
-	p_nr=cPar.p_nr;
-	em_iter=cPar.em_iter;
-	nr_iter=cPar.nr_iter;
-	em_prec=cPar.em_prec;
-	nr_prec=cPar.nr_prec;
-	crt=cPar.crt;
-
-	Vg_remle_null=cPar.Vg_remle_null;
-	Ve_remle_null=cPar.Ve_remle_null;
-	Vg_mle_null=cPar.Vg_mle_null;
-	Ve_mle_null=cPar.Ve_mle_null;
-
-	time_UtX=0.0;
-	time_opt=0.0;
-
-	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;
-
-	n_ph=cPar.n_ph;
-
-	indicator_idv=cPar.indicator_idv;
-	indicator_snp=cPar.indicator_snp;
-	snpInfo=cPar.snpInfo;
-
-	return;
+void MVLMM::CopyFromParam(PARAM &cPar) {
+  a_mode = cPar.a_mode;
+  d_pace = cPar.d_pace;
+
+  file_bfile = cPar.file_bfile;
+  file_geno = cPar.file_geno;
+  file_oxford = cPar.file_oxford;
+  file_out = cPar.file_out;
+  path_out = cPar.path_out;
+
+  l_min = cPar.l_min;
+  l_max = cPar.l_max;
+  n_region = cPar.n_region;
+  p_nr = cPar.p_nr;
+  em_iter = cPar.em_iter;
+  nr_iter = cPar.nr_iter;
+  em_prec = cPar.em_prec;
+  nr_prec = cPar.nr_prec;
+  crt = cPar.crt;
+
+  Vg_remle_null = cPar.Vg_remle_null;
+  Ve_remle_null = cPar.Ve_remle_null;
+  Vg_mle_null = cPar.Vg_mle_null;
+  Ve_mle_null = cPar.Ve_mle_null;
+
+  time_UtX = 0.0;
+  time_opt = 0.0;
+
+  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;
+
+  n_ph = cPar.n_ph;
+
+  indicator_idv = cPar.indicator_idv;
+  indicator_snp = cPar.indicator_snp;
+  snpInfo = cPar.snpInfo;
+
+  return;
 }
 
-void MVLMM::CopyToParam (PARAM &cPar) {
-	cPar.time_UtX=time_UtX;
-	cPar.time_opt=time_opt;
+void MVLMM::CopyToParam(PARAM &cPar) {
+  cPar.time_UtX = time_UtX;
+  cPar.time_opt = time_opt;
 
-	cPar.Vg_remle_null=Vg_remle_null;
-	cPar.Ve_remle_null=Ve_remle_null;
-	cPar.Vg_mle_null=Vg_mle_null;
-	cPar.Ve_mle_null=Ve_mle_null;
+  cPar.Vg_remle_null = Vg_remle_null;
+  cPar.Ve_remle_null = Ve_remle_null;
+  cPar.Vg_mle_null = Vg_mle_null;
+  cPar.Ve_mle_null = Ve_mle_null;
 
-	cPar.VVg_remle_null=VVg_remle_null;
-	cPar.VVe_remle_null=VVe_remle_null;
-	cPar.VVg_mle_null=VVg_mle_null;
-	cPar.VVe_mle_null=VVe_mle_null;
+  cPar.VVg_remle_null = VVg_remle_null;
+  cPar.VVe_remle_null = VVe_remle_null;
+  cPar.VVg_mle_null = VVg_mle_null;
+  cPar.VVe_mle_null = VVe_mle_null;
 
-	cPar.beta_remle_null=beta_remle_null;
-	cPar.se_beta_remle_null=se_beta_remle_null;
-	cPar.beta_mle_null=beta_mle_null;
-	cPar.se_beta_mle_null=se_beta_mle_null;
+  cPar.beta_remle_null = beta_remle_null;
+  cPar.se_beta_remle_null = se_beta_remle_null;
+  cPar.beta_mle_null = beta_mle_null;
+  cPar.se_beta_mle_null = se_beta_mle_null;
 
-	cPar.logl_remle_H0=logl_remle_H0;
-	cPar.logl_mle_H0=logl_mle_H0;
-	return;
+  cPar.logl_remle_H0 = logl_remle_H0;
+  cPar.logl_mle_H0 = logl_mle_H0;
+  return;
 }
 
-void MVLMM::WriteFiles () {
-	string file_str;
-	file_str=path_out+"/"+file_out;
-	file_str+=".assoc.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"
-	       <<"allele1"<<"\t"<<"allele0"<<"\t"<<"af"<<"\t";
-
-	for (size_t i=0; i<n_ph; i++) {
-		outfile<<"beta_"<<i+1<<"\t";
-	}
-	for (size_t i=0; i<n_ph; i++) {
-		for (size_t j=i; j<n_ph; j++) {
-			outfile<<"Vbeta_"<<i+1<<"_"<<j+1<<"\t";
-		}
-	}
-
-	if (a_mode==1) {
-		outfile<<"p_wald"<<endl;
-	} else if (a_mode==2) {
-		outfile<<"p_lrt"<<endl;
-	} else if (a_mode==3) {
-		outfile<<"p_score"<<endl;
-	} else if (a_mode==4) {
-		outfile<<"p_wald"<<"\t"<<"p_lrt"<<"\t"<<"p_score"<<endl;
-	} else {}
-
-
-	size_t t=0, c=0;
-	for (size_t i=0; i<snpInfo.size(); ++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"<<snpInfo[i].a_minor<<"\t"<<snpInfo[i].a_major<<"\t"<<
-		  fixed<<setprecision(3)<<snpInfo[i].maf<<"\t";
-
-		outfile<<scientific<<setprecision(6);
-
-		for (size_t i=0; i<n_ph; i++) {
-			outfile<<sumStat[t].v_beta[i]<<"\t";
-		}
-
-		c=0;
-		for (size_t i=0; i<n_ph; i++) {
-			for (size_t j=i; j<n_ph; j++) {
-				outfile<<sumStat[t].v_Vbeta[c]<<"\t";
-				c++;
-			}
-		}
-
-		if (a_mode==1) {
-			outfile<<sumStat[t].p_wald <<endl;
-		} else if (a_mode==2) {
-			outfile<<sumStat[t].p_lrt<<endl;
-		} else if (a_mode==3) {
-			outfile<<sumStat[t].p_score<<endl;
-		} else if (a_mode==4) {
-			outfile<<sumStat[t].p_wald <<"\t"<<sumStat[t].p_lrt<<
-			  "\t"<<sumStat[t].p_score<<endl;
-		} else {}
-
-		t++;
-	}
-
-	outfile.close();
-	outfile.clear();
-	return;
+void MVLMM::WriteFiles() {
+  string file_str;
+  file_str = path_out + "/" + file_out;
+  file_str += ".assoc.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"
+          << "allele1"
+          << "\t"
+          << "allele0"
+          << "\t"
+          << "af"
+          << "\t";
+
+  for (size_t i = 0; i < n_ph; i++) {
+    outfile << "beta_" << i + 1 << "\t";
+  }
+  for (size_t i = 0; i < n_ph; i++) {
+    for (size_t j = i; j < n_ph; j++) {
+      outfile << "Vbeta_" << i + 1 << "_" << j + 1 << "\t";
+    }
+  }
+
+  if (a_mode == 1) {
+    outfile << "p_wald" << endl;
+  } else if (a_mode == 2) {
+    outfile << "p_lrt" << endl;
+  } else if (a_mode == 3) {
+    outfile << "p_score" << endl;
+  } else if (a_mode == 4) {
+    outfile << "p_wald"
+            << "\t"
+            << "p_lrt"
+            << "\t"
+            << "p_score" << endl;
+  } else {
+  }
+
+  size_t t = 0, c = 0;
+  for (size_t i = 0; i < snpInfo.size(); ++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"
+            << snpInfo[i].a_minor << "\t" << snpInfo[i].a_major << "\t" << fixed
+            << setprecision(3) << snpInfo[i].maf << "\t";
+
+    outfile << scientific << setprecision(6);
+
+    for (size_t i = 0; i < n_ph; i++) {
+      outfile << sumStat[t].v_beta[i] << "\t";
+    }
+
+    c = 0;
+    for (size_t i = 0; i < n_ph; i++) {
+      for (size_t j = i; j < n_ph; j++) {
+        outfile << sumStat[t].v_Vbeta[c] << "\t";
+        c++;
+      }
+    }
+
+    if (a_mode == 1) {
+      outfile << sumStat[t].p_wald << endl;
+    } else if (a_mode == 2) {
+      outfile << sumStat[t].p_lrt << endl;
+    } else if (a_mode == 3) {
+      outfile << sumStat[t].p_score << endl;
+    } else if (a_mode == 4) {
+      outfile << sumStat[t].p_wald << "\t" << sumStat[t].p_lrt << "\t"
+              << sumStat[t].p_score << endl;
+    } else {
+    }
+
+    t++;
+  }
+
+  outfile.close();
+  outfile.clear();
+  return;
 }
 
 // Below are functions for EM algorithm.
-double EigenProc (const gsl_matrix *V_g, const gsl_matrix *V_e, 
-		  gsl_vector *D_l, gsl_matrix *UltVeh, 
-		  gsl_matrix *UltVehi) {
-	size_t d_size=V_g->size1;
-	double d, logdet_Ve=0.0;
-
-	// Eigen decomposition of V_e.
-	gsl_matrix *Lambda=gsl_matrix_alloc (d_size, d_size);
-	gsl_matrix *V_e_temp=gsl_matrix_alloc (d_size, d_size);
-	gsl_matrix *V_e_h=gsl_matrix_alloc (d_size, d_size);
-	gsl_matrix *V_e_hi=gsl_matrix_alloc (d_size, d_size);
-	gsl_matrix *VgVehi=gsl_matrix_alloc (d_size, d_size);
-	gsl_matrix *U_l=gsl_matrix_alloc (d_size, d_size);
-
-	gsl_matrix_memcpy(V_e_temp, V_e);
-	EigenDecomp(V_e_temp, U_l, D_l, 0);
-
-	// Calculate V_e_h and V_e_hi.
-	gsl_matrix_set_zero(V_e_h);
-	gsl_matrix_set_zero(V_e_hi);
-	for (size_t i=0; i<d_size; i++) {
-		d=gsl_vector_get (D_l, i);
-		if (d<=0) {continue;}
-		logdet_Ve+=log(d);
-
-		gsl_vector_view U_col=gsl_matrix_column(U_l, i);
-		d=sqrt(d);
-		gsl_blas_dsyr (CblasUpper, d, &U_col.vector, V_e_h);
-		d=1.0/d;
-		gsl_blas_dsyr (CblasUpper, d, &U_col.vector, V_e_hi);
-	}
-
-	// Copy the upper part to lower part.
-	for (size_t i=0; i<d_size; i++) {
-		for (size_t j=0; j<i; j++) {
-		  gsl_matrix_set (V_e_h, i, j, gsl_matrix_get(V_e_h, j, i));
-		  gsl_matrix_set (V_e_hi, i, j, gsl_matrix_get(V_e_hi, j, i));
-		}
-	}
-
-	// Calculate Lambda=V_ehi V_g V_ehi.
-	gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,1.0,V_g,V_e_hi,0.0,VgVehi);
-	gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,1.0,V_e_hi,VgVehi,0.0,Lambda);
-
-	// Eigen decomposition of Lambda.
-	EigenDecomp(Lambda, U_l, D_l, 0);
-
-	for (size_t i=0; i<d_size; i++) {
-	  d=gsl_vector_get (D_l, i);
-	  if (d<0) {gsl_vector_set (D_l, i, 0);}
-	}
-
-	// Calculate UltVeh and UltVehi.
-	gsl_blas_dgemm(CblasTrans,CblasNoTrans,1.0,U_l,V_e_h,0.0,UltVeh);
-	gsl_blas_dgemm(CblasTrans,CblasNoTrans,1.0,U_l,V_e_hi,0.0,UltVehi);
-
-	//free memory
-	gsl_matrix_free (Lambda);
-	gsl_matrix_free (V_e_temp);
-	gsl_matrix_free (V_e_h);
-	gsl_matrix_free (V_e_hi);
-	gsl_matrix_free (VgVehi);
-	gsl_matrix_free (U_l);
-
-	return logdet_Ve;
+double EigenProc(const gsl_matrix *V_g, const gsl_matrix *V_e, gsl_vector *D_l,
+                 gsl_matrix *UltVeh, gsl_matrix *UltVehi) {
+  size_t d_size = V_g->size1;
+  double d, logdet_Ve = 0.0;
+
+  // Eigen decomposition of V_e.
+  gsl_matrix *Lambda = gsl_matrix_alloc(d_size, d_size);
+  gsl_matrix *V_e_temp = gsl_matrix_alloc(d_size, d_size);
+  gsl_matrix *V_e_h = gsl_matrix_alloc(d_size, d_size);
+  gsl_matrix *V_e_hi = gsl_matrix_alloc(d_size, d_size);
+  gsl_matrix *VgVehi = gsl_matrix_alloc(d_size, d_size);
+  gsl_matrix *U_l = gsl_matrix_alloc(d_size, d_size);
+
+  gsl_matrix_memcpy(V_e_temp, V_e);
+  EigenDecomp(V_e_temp, U_l, D_l, 0);
+
+  // Calculate V_e_h and V_e_hi.
+  gsl_matrix_set_zero(V_e_h);
+  gsl_matrix_set_zero(V_e_hi);
+  for (size_t i = 0; i < d_size; i++) {
+    d = gsl_vector_get(D_l, i);
+    if (d <= 0) {
+      continue;
+    }
+    logdet_Ve += log(d);
+
+    gsl_vector_view U_col = gsl_matrix_column(U_l, i);
+    d = sqrt(d);
+    gsl_blas_dsyr(CblasUpper, d, &U_col.vector, V_e_h);
+    d = 1.0 / d;
+    gsl_blas_dsyr(CblasUpper, d, &U_col.vector, V_e_hi);
+  }
+
+  // Copy the upper part to lower part.
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = 0; j < i; j++) {
+      gsl_matrix_set(V_e_h, i, j, gsl_matrix_get(V_e_h, j, i));
+      gsl_matrix_set(V_e_hi, i, j, gsl_matrix_get(V_e_hi, j, i));
+    }
+  }
+
+  // Calculate Lambda=V_ehi V_g V_ehi.
+  gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, V_g, V_e_hi, 0.0, VgVehi);
+  gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, V_e_hi, VgVehi, 0.0, Lambda);
+
+  // Eigen decomposition of Lambda.
+  EigenDecomp(Lambda, U_l, D_l, 0);
+
+  for (size_t i = 0; i < d_size; i++) {
+    d = gsl_vector_get(D_l, i);
+    if (d < 0) {
+      gsl_vector_set(D_l, i, 0);
+    }
+  }
+
+  // Calculate UltVeh and UltVehi.
+  gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, U_l, V_e_h, 0.0, UltVeh);
+  gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, U_l, V_e_hi, 0.0, UltVehi);
+
+  // free memory
+  gsl_matrix_free(Lambda);
+  gsl_matrix_free(V_e_temp);
+  gsl_matrix_free(V_e_h);
+  gsl_matrix_free(V_e_hi);
+  gsl_matrix_free(VgVehi);
+  gsl_matrix_free(U_l);
+
+  return logdet_Ve;
 }
 
-//Qi=(\sum_{k=1}^n x_kx_k^T\otimes(delta_k*Dl+I)^{-1} )^{-1}.
-double CalcQi (const gsl_vector *eval, const gsl_vector *D_l, 
-	       const gsl_matrix *X, gsl_matrix *Qi) {
-	size_t n_size=eval->size, d_size=D_l->size, dc_size=Qi->size1;
-	size_t c_size=dc_size/d_size;
-
-	double delta, dl, d1, d2, d, logdet_Q;
-
-	gsl_matrix *Q=gsl_matrix_alloc (dc_size, dc_size);
-	gsl_matrix_set_zero (Q);
-
-	for (size_t i=0; i<c_size; i++) {
-		for (size_t j=0; j<c_size; j++) {
-			for (size_t l=0; l<d_size; l++) {
-				dl=gsl_vector_get(D_l, l);
-
-				if (j<i) {
-				  d=gsl_matrix_get (Q, j*d_size+l, i*d_size+l);
-				} else {
-					d=0.0;
-					for (size_t k=0; k<n_size; k++) {
-					  d1=gsl_matrix_get(X, i, k);
-					  d2=gsl_matrix_get(X, j, k);
-					  delta=gsl_vector_get(eval, k);
-					  d+=d1*d2/(dl*delta+1.0);
-					}
-				}
-
-				gsl_matrix_set (Q, i*d_size+l, j*d_size+l, d);
-			}
-		}
-	}
-
-	// Calculate LU decomposition of Q, and invert Q and calculate |Q|.
-	int sig;
-	gsl_permutation * pmt=gsl_permutation_alloc (dc_size);
-	LUDecomp (Q, pmt, &sig);
-	LUInvert (Q, pmt, Qi);
-
-	logdet_Q=LULndet (Q);
-
-	gsl_matrix_free (Q);
-	gsl_permutation_free (pmt);
-
-	return logdet_Q;
+// Qi=(\sum_{k=1}^n x_kx_k^T\otimes(delta_k*Dl+I)^{-1} )^{-1}.
+double CalcQi(const gsl_vector *eval, const gsl_vector *D_l,
+              const gsl_matrix *X, gsl_matrix *Qi) {
+  size_t n_size = eval->size, d_size = D_l->size, dc_size = Qi->size1;
+  size_t c_size = dc_size / d_size;
+
+  double delta, dl, d1, d2, d, logdet_Q;
+
+  gsl_matrix *Q = gsl_matrix_alloc(dc_size, dc_size);
+  gsl_matrix_set_zero(Q);
+
+  for (size_t i = 0; i < c_size; i++) {
+    for (size_t j = 0; j < c_size; j++) {
+      for (size_t l = 0; l < d_size; l++) {
+        dl = gsl_vector_get(D_l, l);
+
+        if (j < i) {
+          d = gsl_matrix_get(Q, j * d_size + l, i * d_size + l);
+        } else {
+          d = 0.0;
+          for (size_t k = 0; k < n_size; k++) {
+            d1 = gsl_matrix_get(X, i, k);
+            d2 = gsl_matrix_get(X, j, k);
+            delta = gsl_vector_get(eval, k);
+            d += d1 * d2 / (dl * delta + 1.0);
+          }
+        }
+
+        gsl_matrix_set(Q, i * d_size + l, j * d_size + l, d);
+      }
+    }
+  }
+
+  // Calculate LU decomposition of Q, and invert Q and calculate |Q|.
+  int sig;
+  gsl_permutation *pmt = gsl_permutation_alloc(dc_size);
+  LUDecomp(Q, pmt, &sig);
+  LUInvert(Q, pmt, Qi);
+
+  logdet_Q = LULndet(Q);
+
+  gsl_matrix_free(Q);
+  gsl_permutation_free(pmt);
+
+  return logdet_Q;
 }
 
 // xHiy=\sum_{k=1}^n x_k\otimes ((delta_k*Dl+I)^{-1}Ul^TVe^{-1/2}y.
-void CalcXHiY(const gsl_vector *eval, const gsl_vector *D_l, 
-	      const gsl_matrix *X, const gsl_matrix *UltVehiY, 
-	      gsl_vector *xHiy) {
-	size_t n_size=eval->size, c_size=X->size1, d_size=D_l->size;
-
-	gsl_vector_set_zero (xHiy);
-
-	double x, delta, dl, y, d;
-	for (size_t i=0; i<d_size; i++) {
-		dl=gsl_vector_get(D_l, i);
-		for (size_t j=0; j<c_size; j++) {
-			d=0.0;
-			for (size_t k=0; k<n_size; k++) {
-				x=gsl_matrix_get(X, j, k);
-				y=gsl_matrix_get(UltVehiY, i, k);
-				delta=gsl_vector_get(eval, k);
-				d+=x*y/(delta*dl+1.0);
-			}
-			gsl_vector_set(xHiy, j*d_size+i, d);
-		}
-	}
-
-	return;
-}
+void CalcXHiY(const gsl_vector *eval, const gsl_vector *D_l,
+              const gsl_matrix *X, const gsl_matrix *UltVehiY,
+              gsl_vector *xHiy) {
+  size_t n_size = eval->size, c_size = X->size1, d_size = D_l->size;
+
+  gsl_vector_set_zero(xHiy);
+
+  double x, delta, dl, y, d;
+  for (size_t i = 0; i < d_size; i++) {
+    dl = gsl_vector_get(D_l, i);
+    for (size_t j = 0; j < c_size; j++) {
+      d = 0.0;
+      for (size_t k = 0; k < n_size; k++) {
+        x = gsl_matrix_get(X, j, k);
+        y = gsl_matrix_get(UltVehiY, i, k);
+        delta = gsl_vector_get(eval, k);
+        d += x * y / (delta * dl + 1.0);
+      }
+      gsl_vector_set(xHiy, j * d_size + i, d);
+    }
+  }
 
+  return;
+}
 
 // OmegaU=D_l/(delta Dl+I)^{-1}
 // OmegaE=delta D_l/(delta Dl+I)^{-1}
-void CalcOmega (const gsl_vector *eval, const gsl_vector *D_l, 
-gsl_matrix *OmegaU, gsl_matrix *OmegaE) {
-	size_t n_size=eval->size, d_size=D_l->size;
-	double delta, dl, d_u, d_e;
+void CalcOmega(const gsl_vector *eval, const gsl_vector *D_l,
+               gsl_matrix *OmegaU, gsl_matrix *OmegaE) {
+  size_t n_size = eval->size, d_size = D_l->size;
+  double delta, dl, d_u, d_e;
 
-	for (size_t k=0; k<n_size; k++) {
-		delta=gsl_vector_get(eval, k);
-		for (size_t i=0; i<d_size; i++) {
-			dl=gsl_vector_get(D_l, i);
+  for (size_t k = 0; k < n_size; k++) {
+    delta = gsl_vector_get(eval, k);
+    for (size_t i = 0; i < d_size; i++) {
+      dl = gsl_vector_get(D_l, i);
 
-			d_u=dl/(delta*dl+1.0);
-			d_e=delta*d_u;
+      d_u = dl / (delta * dl + 1.0);
+      d_e = delta * d_u;
 
-			gsl_matrix_set(OmegaU, i, k, d_u);
-			gsl_matrix_set(OmegaE, i, k, d_e);
-		}
-	}
+      gsl_matrix_set(OmegaU, i, k, d_u);
+      gsl_matrix_set(OmegaE, i, k, d_e);
+    }
+  }
 
-	return;
+  return;
 }
 
-void UpdateU (const gsl_matrix *OmegaE, const gsl_matrix *UltVehiY, 
-	      const gsl_matrix *UltVehiBX, gsl_matrix *UltVehiU) {
-	gsl_matrix_memcpy (UltVehiU, UltVehiY);
-	gsl_matrix_sub (UltVehiU, UltVehiBX);
+void UpdateU(const gsl_matrix *OmegaE, const gsl_matrix *UltVehiY,
+             const gsl_matrix *UltVehiBX, gsl_matrix *UltVehiU) {
+  gsl_matrix_memcpy(UltVehiU, UltVehiY);
+  gsl_matrix_sub(UltVehiU, UltVehiBX);
 
-	gsl_matrix_mul_elements (UltVehiU, OmegaE);
-	return;
+  gsl_matrix_mul_elements(UltVehiU, OmegaE);
+  return;
 }
 
-void UpdateE (const gsl_matrix *UltVehiY, const gsl_matrix *UltVehiBX, 
-	      const gsl_matrix *UltVehiU, gsl_matrix *UltVehiE) {
-	gsl_matrix_memcpy (UltVehiE, UltVehiY);
-	gsl_matrix_sub (UltVehiE, UltVehiBX);
-	gsl_matrix_sub (UltVehiE, UltVehiU);
+void UpdateE(const gsl_matrix *UltVehiY, const gsl_matrix *UltVehiBX,
+             const gsl_matrix *UltVehiU, gsl_matrix *UltVehiE) {
+  gsl_matrix_memcpy(UltVehiE, UltVehiY);
+  gsl_matrix_sub(UltVehiE, UltVehiBX);
+  gsl_matrix_sub(UltVehiE, UltVehiU);
 
-	return;
+  return;
 }
 
-void UpdateL_B (const gsl_matrix *X, const gsl_matrix *XXti, 
-		const gsl_matrix *UltVehiY, const gsl_matrix *UltVehiU, 
-		gsl_matrix *UltVehiBX, gsl_matrix *UltVehiB) {
-	size_t c_size=X->size1, d_size=UltVehiY->size1;
+void UpdateL_B(const gsl_matrix *X, const gsl_matrix *XXti,
+               const gsl_matrix *UltVehiY, const gsl_matrix *UltVehiU,
+               gsl_matrix *UltVehiBX, gsl_matrix *UltVehiB) {
+  size_t c_size = X->size1, d_size = UltVehiY->size1;
 
-	gsl_matrix *YUX=gsl_matrix_alloc (d_size, c_size);
+  gsl_matrix *YUX = gsl_matrix_alloc(d_size, c_size);
 
-	gsl_matrix_memcpy (UltVehiBX, UltVehiY);
-	gsl_matrix_sub (UltVehiBX, UltVehiU);
+  gsl_matrix_memcpy(UltVehiBX, UltVehiY);
+  gsl_matrix_sub(UltVehiBX, UltVehiU);
 
-	gsl_blas_dgemm(CblasNoTrans,CblasTrans,1.0,UltVehiBX,X,0.0,YUX);
-	gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,1.0,YUX,XXti,0.0,UltVehiB);
+  gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, UltVehiBX, X, 0.0, YUX);
+  gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, YUX, XXti, 0.0, UltVehiB);
 
-	gsl_matrix_free(YUX);
+  gsl_matrix_free(YUX);
 
-	return;
+  return;
 }
 
-void UpdateRL_B (const gsl_vector *xHiy, const gsl_matrix *Qi, 
-		 gsl_matrix *UltVehiB) {
-	size_t d_size=UltVehiB->size1, c_size=UltVehiB->size2, 
-	  dc_size=Qi->size1;
+void UpdateRL_B(const gsl_vector *xHiy, const gsl_matrix *Qi,
+                gsl_matrix *UltVehiB) {
+  size_t d_size = UltVehiB->size1, c_size = UltVehiB->size2,
+         dc_size = Qi->size1;
 
-	gsl_vector *b=gsl_vector_alloc (dc_size);
+  gsl_vector *b = gsl_vector_alloc(dc_size);
 
-	// Calculate b=Qiv.
-	gsl_blas_dgemv(CblasNoTrans, 1.0, Qi, xHiy, 0.0, b);
+  // Calculate b=Qiv.
+  gsl_blas_dgemv(CblasNoTrans, 1.0, Qi, xHiy, 0.0, b);
 
-	// Copy b to UltVehiB.
-	for (size_t i=0; i<c_size; i++) {
-		gsl_vector_view UltVehiB_col=gsl_matrix_column (UltVehiB, i);
-		gsl_vector_const_view b_subcol=
-		  gsl_vector_const_subvector (b, i*d_size, d_size);
-		gsl_vector_memcpy (&UltVehiB_col.vector, &b_subcol.vector);
-	}
+  // Copy b to UltVehiB.
+  for (size_t i = 0; i < c_size; i++) {
+    gsl_vector_view UltVehiB_col = gsl_matrix_column(UltVehiB, i);
+    gsl_vector_const_view b_subcol =
+        gsl_vector_const_subvector(b, i * d_size, d_size);
+    gsl_vector_memcpy(&UltVehiB_col.vector, &b_subcol.vector);
+  }
 
-	gsl_vector_free(b);
+  gsl_vector_free(b);
 
-	return;
+  return;
 }
 
-void UpdateV (const gsl_vector *eval, const gsl_matrix *U, 
-	      const gsl_matrix *E, const gsl_matrix *Sigma_uu, 
-	      const gsl_matrix *Sigma_ee, gsl_matrix *V_g, gsl_matrix *V_e) {
-	size_t n_size=eval->size, d_size=U->size1;
+void UpdateV(const gsl_vector *eval, const gsl_matrix *U, const gsl_matrix *E,
+             const gsl_matrix *Sigma_uu, const gsl_matrix *Sigma_ee,
+             gsl_matrix *V_g, gsl_matrix *V_e) {
+  size_t n_size = eval->size, d_size = U->size1;
 
-	gsl_matrix_set_zero (V_g);
-	gsl_matrix_set_zero (V_e);
+  gsl_matrix_set_zero(V_g);
+  gsl_matrix_set_zero(V_e);
 
-	double delta;
+  double delta;
 
-	// Calculate the first part: UD^{-1}U^T and EE^T.
-	for (size_t k=0; k<n_size; k++) {
-		delta=gsl_vector_get (eval, k);
-		if (delta==0) {continue;}
+  // Calculate the first part: UD^{-1}U^T and EE^T.
+  for (size_t k = 0; k < n_size; k++) {
+    delta = gsl_vector_get(eval, k);
+    if (delta == 0) {
+      continue;
+    }
 
-		gsl_vector_const_view U_col=gsl_matrix_const_column (U, k);
-		gsl_blas_dsyr (CblasUpper, 1.0/delta, &U_col.vector, V_g);
-	}
+    gsl_vector_const_view U_col = gsl_matrix_const_column(U, k);
+    gsl_blas_dsyr(CblasUpper, 1.0 / delta, &U_col.vector, V_g);
+  }
 
-	gsl_blas_dsyrk(CblasUpper, CblasNoTrans, 1.0, E, 0.0, V_e);
+  gsl_blas_dsyrk(CblasUpper, CblasNoTrans, 1.0, E, 0.0, V_e);
 
-	// Copy the upper part to lower part.
-	for (size_t i=0; i<d_size; i++) {
-		for (size_t j=0; j<i; j++) {
-			gsl_matrix_set (V_g, i, j, gsl_matrix_get(V_g, j, i));
-			gsl_matrix_set (V_e, i, j, gsl_matrix_get(V_e, j, i));
-		}
-	}
+  // Copy the upper part to lower part.
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = 0; j < i; j++) {
+      gsl_matrix_set(V_g, i, j, gsl_matrix_get(V_g, j, i));
+      gsl_matrix_set(V_e, i, j, gsl_matrix_get(V_e, j, i));
+    }
+  }
 
-	// Add Sigma.
-	gsl_matrix_add (V_g, Sigma_uu);
-	gsl_matrix_add (V_e, Sigma_ee);
+  // Add Sigma.
+  gsl_matrix_add(V_g, Sigma_uu);
+  gsl_matrix_add(V_e, Sigma_ee);
 
-	// Scale by 1/n.
-	gsl_matrix_scale (V_g, 1.0/(double)n_size);
-	gsl_matrix_scale (V_e, 1.0/(double)n_size);
+  // Scale by 1/n.
+  gsl_matrix_scale(V_g, 1.0 / (double)n_size);
+  gsl_matrix_scale(V_e, 1.0 / (double)n_size);
 
-	return;
+  return;
 }
 
-void CalcSigma (const char func_name, const gsl_vector *eval, 
-		const gsl_vector *D_l, const gsl_matrix *X, 
-		const gsl_matrix *OmegaU, const gsl_matrix *OmegaE, 
-		const gsl_matrix *UltVeh, const gsl_matrix *Qi, 
-		gsl_matrix *Sigma_uu, gsl_matrix *Sigma_ee) {
-	if (func_name!='R' && func_name!='L' && func_name!='r' && 
-	    func_name!='l') {
-	  cout<<"func_name only takes 'R' or 'L': 'R' for log-restricted "<<
-	    "likelihood, 'L' for log-likelihood."<<endl; 
-	  return;
-	}
-
-	size_t n_size=eval->size, c_size=X->size1;
-	size_t d_size=D_l->size, dc_size=Qi->size1;
-
-	gsl_matrix_set_zero(Sigma_uu);
-	gsl_matrix_set_zero(Sigma_ee);
-
-	double delta, dl, x, d;
-
-	// Calculate the first diagonal term.
-	gsl_vector_view Suu_diag=gsl_matrix_diagonal (Sigma_uu);
-	gsl_vector_view See_diag=gsl_matrix_diagonal (Sigma_ee);
-
-	for (size_t k=0; k<n_size; k++) {
-	  gsl_vector_const_view OmegaU_col=gsl_matrix_const_column (OmegaU, k);
-	  gsl_vector_const_view OmegaE_col=gsl_matrix_const_column (OmegaE, k);
-
-	  gsl_vector_add (&Suu_diag.vector, &OmegaU_col.vector);
-	  gsl_vector_add (&See_diag.vector, &OmegaE_col.vector);
-	}
-
-	// Calculate the second term for REML.
-	if (func_name=='R' || func_name=='r') {
-		gsl_matrix *M_u=gsl_matrix_alloc(dc_size, d_size);
-		gsl_matrix *M_e=gsl_matrix_alloc(dc_size, d_size);
-		gsl_matrix *QiM=gsl_matrix_alloc(dc_size, d_size);
-
-		gsl_matrix_set_zero(M_u);
-		gsl_matrix_set_zero(M_e);
-
-		for (size_t k=0; k<n_size; k++) {
-		  delta=gsl_vector_get(eval, k);
-		  
-		  for (size_t i=0; i<d_size; i++) {
-		    dl=gsl_vector_get(D_l, i);
-		    for (size_t j=0; j<c_size; j++) {
-		      x=gsl_matrix_get(X, j, k);
-		      d=x/(delta*dl+1.0);
-		      gsl_matrix_set(M_e, j*d_size+i, i, d);
-		      gsl_matrix_set(M_u, j*d_size+i, i, d*dl);
-		    }
-		  }
-		  gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,1.0,Qi,M_u,0.0,QiM);
-		  gsl_blas_dgemm(CblasTrans,CblasNoTrans,delta,M_u,QiM,1.0,
-				 Sigma_uu);
-		  
-		  gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,1.0,Qi,M_e,0.0,QiM);
-		  gsl_blas_dgemm(CblasTrans,CblasNoTrans,1.0,M_e,QiM,1.0,
-				 Sigma_ee);
-		}
-		
-		gsl_matrix_free(M_u);
-		gsl_matrix_free(M_e);
-		gsl_matrix_free(QiM);
-	}
-
-	// Multiply both sides by VehUl.
-	gsl_matrix *M=gsl_matrix_alloc (d_size, d_size);
-
-	gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,1.0,Sigma_uu,UltVeh,0.0,M);
-	gsl_blas_dgemm(CblasTrans,CblasNoTrans,1.0,UltVeh,M,0.0,Sigma_uu);
-	gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,1.0,Sigma_ee,UltVeh,0.0,M);
-	gsl_blas_dgemm(CblasTrans, CblasNoTrans,1.0,UltVeh,M,0.0,Sigma_ee);
-
-	gsl_matrix_free(M);
-	return;
+void CalcSigma(const char func_name, const gsl_vector *eval,
+               const gsl_vector *D_l, const gsl_matrix *X,
+               const gsl_matrix *OmegaU, const gsl_matrix *OmegaE,
+               const gsl_matrix *UltVeh, const gsl_matrix *Qi,
+               gsl_matrix *Sigma_uu, gsl_matrix *Sigma_ee) {
+  if (func_name != 'R' && func_name != 'L' && func_name != 'r' &&
+      func_name != 'l') {
+    cout << "func_name only takes 'R' or 'L': 'R' for log-restricted "
+         << "likelihood, 'L' for log-likelihood." << endl;
+    return;
+  }
+
+  size_t n_size = eval->size, c_size = X->size1;
+  size_t d_size = D_l->size, dc_size = Qi->size1;
+
+  gsl_matrix_set_zero(Sigma_uu);
+  gsl_matrix_set_zero(Sigma_ee);
+
+  double delta, dl, x, d;
+
+  // Calculate the first diagonal term.
+  gsl_vector_view Suu_diag = gsl_matrix_diagonal(Sigma_uu);
+  gsl_vector_view See_diag = gsl_matrix_diagonal(Sigma_ee);
+
+  for (size_t k = 0; k < n_size; k++) {
+    gsl_vector_const_view OmegaU_col = gsl_matrix_const_column(OmegaU, k);
+    gsl_vector_const_view OmegaE_col = gsl_matrix_const_column(OmegaE, k);
+
+    gsl_vector_add(&Suu_diag.vector, &OmegaU_col.vector);
+    gsl_vector_add(&See_diag.vector, &OmegaE_col.vector);
+  }
+
+  // Calculate the second term for REML.
+  if (func_name == 'R' || func_name == 'r') {
+    gsl_matrix *M_u = gsl_matrix_alloc(dc_size, d_size);
+    gsl_matrix *M_e = gsl_matrix_alloc(dc_size, d_size);
+    gsl_matrix *QiM = gsl_matrix_alloc(dc_size, d_size);
+
+    gsl_matrix_set_zero(M_u);
+    gsl_matrix_set_zero(M_e);
+
+    for (size_t k = 0; k < n_size; k++) {
+      delta = gsl_vector_get(eval, k);
+
+      for (size_t i = 0; i < d_size; i++) {
+        dl = gsl_vector_get(D_l, i);
+        for (size_t j = 0; j < c_size; j++) {
+          x = gsl_matrix_get(X, j, k);
+          d = x / (delta * dl + 1.0);
+          gsl_matrix_set(M_e, j * d_size + i, i, d);
+          gsl_matrix_set(M_u, j * d_size + i, i, d * dl);
+        }
+      }
+      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Qi, M_u, 0.0, QiM);
+      gsl_blas_dgemm(CblasTrans, CblasNoTrans, delta, M_u, QiM, 1.0, Sigma_uu);
+
+      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Qi, M_e, 0.0, QiM);
+      gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, M_e, QiM, 1.0, Sigma_ee);
+    }
+
+    gsl_matrix_free(M_u);
+    gsl_matrix_free(M_e);
+    gsl_matrix_free(QiM);
+  }
+
+  // Multiply both sides by VehUl.
+  gsl_matrix *M = gsl_matrix_alloc(d_size, d_size);
+
+  gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Sigma_uu, UltVeh, 0.0, M);
+  gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, UltVeh, M, 0.0, Sigma_uu);
+  gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Sigma_ee, UltVeh, 0.0, M);
+  gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, UltVeh, M, 0.0, Sigma_ee);
+
+  gsl_matrix_free(M);
+  return;
 }
 
 // 'R' for restricted likelihood and 'L' for likelihood.
 // 'R' update B and 'L' don't.
 // only calculate -0.5*\sum_{k=1}^n|H_k|-0.5yPxy.
-double MphCalcLogL (const gsl_vector *eval, const gsl_vector *xHiy, 
-		    const gsl_vector *D_l, const gsl_matrix *UltVehiY, 
-		    const gsl_matrix *Qi) {
-	size_t n_size=eval->size, d_size=D_l->size, dc_size=Qi->size1;
-	double logl=0.0, delta, dl, y, d;
-
-	// Calculate yHiy+log|H_k|.
-	for (size_t k=0; k<n_size; k++) {
-		delta=gsl_vector_get(eval, k);
-		for (size_t i=0; i<d_size; i++) {
-			y=gsl_matrix_get(UltVehiY, i, k);
-			dl=gsl_vector_get(D_l, i);
-			d=delta*dl+1.0;
-
-			logl+=y*y/d+log(d);
-		}
-	}
+double MphCalcLogL(const gsl_vector *eval, const gsl_vector *xHiy,
+                   const gsl_vector *D_l, const gsl_matrix *UltVehiY,
+                   const gsl_matrix *Qi) {
+  size_t n_size = eval->size, d_size = D_l->size, dc_size = Qi->size1;
+  double logl = 0.0, delta, dl, y, d;
+
+  // Calculate yHiy+log|H_k|.
+  for (size_t k = 0; k < n_size; k++) {
+    delta = gsl_vector_get(eval, k);
+    for (size_t i = 0; i < d_size; i++) {
+      y = gsl_matrix_get(UltVehiY, i, k);
+      dl = gsl_vector_get(D_l, i);
+      d = delta * dl + 1.0;
+
+      logl += y * y / d + log(d);
+    }
+  }
 
-	// Calculate the rest of yPxy.
-	gsl_vector *Qiv=gsl_vector_alloc(dc_size);
+  // Calculate the rest of yPxy.
+  gsl_vector *Qiv = gsl_vector_alloc(dc_size);
 
-	gsl_blas_dgemv(CblasNoTrans, 1.0, Qi, xHiy, 0.0, Qiv);
-	gsl_blas_ddot(xHiy, Qiv, &d);
+  gsl_blas_dgemv(CblasNoTrans, 1.0, Qi, xHiy, 0.0, Qiv);
+  gsl_blas_ddot(xHiy, Qiv, &d);
 
-	logl-=d;
+  logl -= d;
 
-	gsl_vector_free(Qiv);
+  gsl_vector_free(Qiv);
 
-	return -0.5*logl;
+  return -0.5 * logl;
 }
 
 // Y is a dxn matrix, X is a cxn matrix, B is a dxc matrix, V_g is a
 // dxd matrix, V_e is a dxd matrix, eval is a size n vector
 //'R' for restricted likelihood and 'L' for likelihood.
-double MphEM (const char func_name, const size_t max_iter, 
-	      const double max_prec, const gsl_vector *eval, 
-	      const gsl_matrix *X, const gsl_matrix *Y, gsl_matrix *U_hat, 
-	      gsl_matrix *E_hat, gsl_matrix *OmegaU, gsl_matrix *OmegaE, 
-	      gsl_matrix *UltVehiY, gsl_matrix *UltVehiBX, 
-	      gsl_matrix *UltVehiU, gsl_matrix *UltVehiE, 
-	      gsl_matrix *V_g, gsl_matrix *V_e, gsl_matrix *B) {
-	if (func_name!='R' && func_name!='L' && 
-	    func_name!='r' && func_name!='l') {
-	  cout<<"func_name only takes 'R' or 'L': 'R' for log-restricted "<<
-	    "likelihood, 'L' for log-likelihood."<<endl; 
-	  return 0.0;
-	}
-
-	size_t n_size=eval->size, c_size=X->size1, d_size=Y->size1;
-	size_t dc_size=d_size*c_size;
-
-	gsl_matrix *XXt=gsl_matrix_alloc (c_size, c_size);
-	gsl_matrix *XXti=gsl_matrix_alloc (c_size, c_size);
-	gsl_vector *D_l=gsl_vector_alloc (d_size);
-	gsl_matrix *UltVeh=gsl_matrix_alloc (d_size, d_size);
-	gsl_matrix *UltVehi=gsl_matrix_alloc (d_size, d_size);
-	gsl_matrix *UltVehiB=gsl_matrix_alloc (d_size, c_size);
-	gsl_matrix *Qi=gsl_matrix_alloc (dc_size, dc_size);
-	gsl_matrix *Sigma_uu=gsl_matrix_alloc (d_size, d_size);
-	gsl_matrix *Sigma_ee=gsl_matrix_alloc (d_size, d_size);
-	gsl_vector *xHiy=gsl_vector_alloc (dc_size);
-	gsl_permutation * pmt=gsl_permutation_alloc (c_size);
-
-	double logl_const=0.0, logl_old=0.0, logl_new=0.0;
-	double logdet_Q, logdet_Ve;
-	int sig;
-
-	// Calculate |XXt| and (XXt)^{-1}.
-	gsl_blas_dsyrk (CblasUpper, CblasNoTrans, 1.0, X, 0.0, XXt);
-	for (size_t i=0; i<c_size; ++i) {
-		for (size_t j=0; j<i; ++j) {
-		  gsl_matrix_set (XXt, i, j, gsl_matrix_get (XXt, j, i));
-		}
-	}
-
-	LUDecomp (XXt, pmt, &sig);
-	LUInvert (XXt, pmt, XXti);
-
-	// Calculate the constant for logl.
-	if (func_name=='R' || func_name=='r') {
-	  logl_const=-0.5*(double)(n_size-c_size)*
-	    (double)d_size*log(2.0*M_PI)+0.5*(double)d_size*LULndet (XXt);
-	} else {
-	  logl_const=-0.5*(double)n_size*(double)d_size*log(2.0*M_PI);
-	}
-
-	// Start EM.
-	for (size_t t=0; t<max_iter; t++) {
-	  logdet_Ve=EigenProc (V_g, V_e, D_l, UltVeh, UltVehi);
-	  
-	  logdet_Q=CalcQi (eval, D_l, X, Qi);
-	  
-	  gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, UltVehi, 
-			 Y, 0.0, UltVehiY);
-	  CalcXHiY(eval, D_l, X, UltVehiY, xHiy);
-
-	  // Calculate log likelihood/restricted likelihood value, and
-	  // terminate if change is small.
-	  logl_new=logl_const+MphCalcLogL (eval, xHiy, D_l, UltVehiY, Qi) - 
-	    0.5*(double)n_size*logdet_Ve;
-	  if (func_name=='R' || func_name=='r') {
-	    logl_new+=-0.5*(logdet_Q-(double)c_size*logdet_Ve);
-	  }
-	  if (t!=0 && abs(logl_new-logl_old)<max_prec) {break;}
-	  logl_old=logl_new;
-	  
-	  CalcOmega (eval, D_l, OmegaU, OmegaE);
-
-	  // Update UltVehiB, UltVehiU.
-	  if (func_name=='R' || func_name=='r') {
-	    UpdateRL_B(xHiy, Qi, UltVehiB);
-	    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, UltVehiB, X, 
-			   0.0, UltVehiBX);
-	  } else if (t==0) {
-	    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, UltVehi, B, 
-			   0.0, UltVehiB);
-	    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, UltVehiB, X, 
-			   0.0, UltVehiBX);
-	  }
-	  
-	  UpdateU(OmegaE, UltVehiY, UltVehiBX, UltVehiU);
-	  
-	  if (func_name=='L' || func_name=='l') {
-
-	    // UltVehiBX is destroyed here.
-	    UpdateL_B(X, XXti, UltVehiY, UltVehiU, UltVehiBX, UltVehiB);
-	    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, UltVehiB, X, 
-			   0.0, UltVehiBX);
-	  }
-	  
-	  UpdateE(UltVehiY, UltVehiBX, UltVehiU, UltVehiE);
-
-	  // Calculate U_hat, E_hat and B.
-	  gsl_blas_dgemm(CblasTrans,CblasNoTrans,1.0,UltVeh,UltVehiU,
-			 0.0,U_hat);
-	  gsl_blas_dgemm(CblasTrans,CblasNoTrans,1.0,UltVeh,UltVehiE,
-			 0.0,E_hat);
-	  gsl_blas_dgemm(CblasTrans,CblasNoTrans,1.0,UltVeh,UltVehiB,0.0,B);
-	  
-	  // Calculate Sigma_uu and Sigma_ee.
-	  CalcSigma (func_name, eval, D_l, X, OmegaU, OmegaE, UltVeh, 
-		     Qi, Sigma_uu, Sigma_ee);
-	  
-	  // Update V_g and V_e.
-	  UpdateV (eval, U_hat, E_hat, Sigma_uu, Sigma_ee, V_g, V_e);
-	}
-
-	gsl_matrix_free(XXt);
-	gsl_matrix_free(XXti);
-	gsl_vector_free(D_l);
-	gsl_matrix_free(UltVeh);
-	gsl_matrix_free(UltVehi);
-	gsl_matrix_free(UltVehiB);
-	gsl_matrix_free(Qi);
-	gsl_matrix_free(Sigma_uu);
-	gsl_matrix_free(Sigma_ee);
-	gsl_vector_free(xHiy);
-	gsl_permutation_free(pmt);
-
-	return logl_new;
+double MphEM(const char func_name, const size_t max_iter, const double max_prec,
+             const gsl_vector *eval, const gsl_matrix *X, const gsl_matrix *Y,
+             gsl_matrix *U_hat, gsl_matrix *E_hat, gsl_matrix *OmegaU,
+             gsl_matrix *OmegaE, gsl_matrix *UltVehiY, gsl_matrix *UltVehiBX,
+             gsl_matrix *UltVehiU, gsl_matrix *UltVehiE, gsl_matrix *V_g,
+             gsl_matrix *V_e, gsl_matrix *B) {
+  if (func_name != 'R' && func_name != 'L' && func_name != 'r' &&
+      func_name != 'l') {
+    cout << "func_name only takes 'R' or 'L': 'R' for log-restricted "
+         << "likelihood, 'L' for log-likelihood." << endl;
+    return 0.0;
+  }
+
+  size_t n_size = eval->size, c_size = X->size1, d_size = Y->size1;
+  size_t dc_size = d_size * c_size;
+
+  gsl_matrix *XXt = gsl_matrix_alloc(c_size, c_size);
+  gsl_matrix *XXti = gsl_matrix_alloc(c_size, c_size);
+  gsl_vector *D_l = gsl_vector_alloc(d_size);
+  gsl_matrix *UltVeh = gsl_matrix_alloc(d_size, d_size);
+  gsl_matrix *UltVehi = gsl_matrix_alloc(d_size, d_size);
+  gsl_matrix *UltVehiB = gsl_matrix_alloc(d_size, c_size);
+  gsl_matrix *Qi = gsl_matrix_alloc(dc_size, dc_size);
+  gsl_matrix *Sigma_uu = gsl_matrix_alloc(d_size, d_size);
+  gsl_matrix *Sigma_ee = gsl_matrix_alloc(d_size, d_size);
+  gsl_vector *xHiy = gsl_vector_alloc(dc_size);
+  gsl_permutation *pmt = gsl_permutation_alloc(c_size);
+
+  double logl_const = 0.0, logl_old = 0.0, logl_new = 0.0;
+  double logdet_Q, logdet_Ve;
+  int sig;
+
+  // Calculate |XXt| and (XXt)^{-1}.
+  gsl_blas_dsyrk(CblasUpper, CblasNoTrans, 1.0, X, 0.0, XXt);
+  for (size_t i = 0; i < c_size; ++i) {
+    for (size_t j = 0; j < i; ++j) {
+      gsl_matrix_set(XXt, i, j, gsl_matrix_get(XXt, j, i));
+    }
+  }
+
+  LUDecomp(XXt, pmt, &sig);
+  LUInvert(XXt, pmt, XXti);
+
+  // Calculate the constant for logl.
+  if (func_name == 'R' || func_name == 'r') {
+    logl_const =
+        -0.5 * (double)(n_size - c_size) * (double)d_size * log(2.0 * M_PI) +
+        0.5 * (double)d_size * LULndet(XXt);
+  } else {
+    logl_const = -0.5 * (double)n_size * (double)d_size * log(2.0 * M_PI);
+  }
+
+  // Start EM.
+  for (size_t t = 0; t < max_iter; t++) {
+    logdet_Ve = EigenProc(V_g, V_e, D_l, UltVeh, UltVehi);
+
+    logdet_Q = CalcQi(eval, D_l, X, Qi);
+
+    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, UltVehi, Y, 0.0, UltVehiY);
+    CalcXHiY(eval, D_l, X, UltVehiY, xHiy);
+
+    // Calculate log likelihood/restricted likelihood value, and
+    // terminate if change is small.
+    logl_new = logl_const + MphCalcLogL(eval, xHiy, D_l, UltVehiY, Qi) -
+               0.5 * (double)n_size * logdet_Ve;
+    if (func_name == 'R' || func_name == 'r') {
+      logl_new += -0.5 * (logdet_Q - (double)c_size * logdet_Ve);
+    }
+    if (t != 0 && abs(logl_new - logl_old) < max_prec) {
+      break;
+    }
+    logl_old = logl_new;
+
+    CalcOmega(eval, D_l, OmegaU, OmegaE);
+
+    // Update UltVehiB, UltVehiU.
+    if (func_name == 'R' || func_name == 'r') {
+      UpdateRL_B(xHiy, Qi, UltVehiB);
+      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, UltVehiB, X, 0.0,
+                     UltVehiBX);
+    } else if (t == 0) {
+      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, UltVehi, B, 0.0,
+                     UltVehiB);
+      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, UltVehiB, X, 0.0,
+                     UltVehiBX);
+    }
+
+    UpdateU(OmegaE, UltVehiY, UltVehiBX, UltVehiU);
+
+    if (func_name == 'L' || func_name == 'l') {
+
+      // UltVehiBX is destroyed here.
+      UpdateL_B(X, XXti, UltVehiY, UltVehiU, UltVehiBX, UltVehiB);
+      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, UltVehiB, X, 0.0,
+                     UltVehiBX);
+    }
+
+    UpdateE(UltVehiY, UltVehiBX, UltVehiU, UltVehiE);
+
+    // Calculate U_hat, E_hat and B.
+    gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, UltVeh, UltVehiU, 0.0, U_hat);
+    gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, UltVeh, UltVehiE, 0.0, E_hat);
+    gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, UltVeh, UltVehiB, 0.0, B);
+
+    // Calculate Sigma_uu and Sigma_ee.
+    CalcSigma(func_name, eval, D_l, X, OmegaU, OmegaE, UltVeh, Qi, Sigma_uu,
+              Sigma_ee);
+
+    // Update V_g and V_e.
+    UpdateV(eval, U_hat, E_hat, Sigma_uu, Sigma_ee, V_g, V_e);
+  }
+
+  gsl_matrix_free(XXt);
+  gsl_matrix_free(XXti);
+  gsl_vector_free(D_l);
+  gsl_matrix_free(UltVeh);
+  gsl_matrix_free(UltVehi);
+  gsl_matrix_free(UltVehiB);
+  gsl_matrix_free(Qi);
+  gsl_matrix_free(Sigma_uu);
+  gsl_matrix_free(Sigma_ee);
+  gsl_vector_free(xHiy);
+  gsl_permutation_free(pmt);
+
+  return logl_new;
 }
 
 // Calculate p-value, beta (d by 1 vector) and V(beta).
-double MphCalcP (const gsl_vector *eval, const gsl_vector *x_vec, 
-		 const gsl_matrix *W, const gsl_matrix *Y, 
-		 const gsl_matrix *V_g, const gsl_matrix *V_e, 
-		 gsl_matrix *UltVehiY, gsl_vector *beta, gsl_matrix *Vbeta) {
-	size_t n_size=eval->size, c_size=W->size1, d_size=V_g->size1;
-	size_t dc_size=d_size*c_size;
-	double delta, dl, d, d1, d2, dy, dx, dw, logdet_Ve, logdet_Q, p_value;
-
-	gsl_vector *D_l=gsl_vector_alloc (d_size);
-	gsl_matrix *UltVeh=gsl_matrix_alloc (d_size, d_size);
-	gsl_matrix *UltVehi=gsl_matrix_alloc (d_size, d_size);
-	gsl_matrix *Qi=gsl_matrix_alloc (dc_size, dc_size);
-	gsl_matrix *WHix=gsl_matrix_alloc (dc_size, d_size);
-	gsl_matrix *QiWHix=gsl_matrix_alloc(dc_size, d_size);
-
-	gsl_matrix *xPx=gsl_matrix_alloc (d_size, d_size);
-	gsl_vector *xPy=gsl_vector_alloc (d_size);
-	gsl_vector *WHiy=gsl_vector_alloc (dc_size);
-
-	gsl_matrix_set_zero (xPx);
-	gsl_matrix_set_zero (WHix);
-	gsl_vector_set_zero (xPy);
-	gsl_vector_set_zero (WHiy);
-
-	// Eigen decomposition and calculate log|Ve|.
-	logdet_Ve=EigenProc (V_g, V_e, D_l, UltVeh, UltVehi);
-
-	// Calculate Qi and log|Q|.
-	logdet_Q=CalcQi (eval, D_l, W, Qi);
-
-	// Calculate UltVehiY.
-	gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, UltVehi, Y, 
-		       0.0, UltVehiY);
-
-	// Calculate WHix, WHiy, xHiy, xHix.
-	for (size_t i=0; i<d_size; i++) {
-		dl=gsl_vector_get(D_l, i);
-
-		d1=0.0; d2=0.0;
-		for (size_t k=0; k<n_size; k++) {
-			delta=gsl_vector_get(eval, k);
-			dx=gsl_vector_get(x_vec, k);
-			dy=gsl_matrix_get(UltVehiY, i, k);
-
-			d1+=dx*dy/(delta*dl+1.0);
-			d2+=dx*dx/(delta*dl+1.0);
-		}
-		gsl_vector_set (xPy, i, d1);
-		gsl_matrix_set (xPx, i, i, d2);
-
-		for (size_t j=0; j<c_size; j++) {
-			d1=0.0; d2=0.0;
-			for (size_t k=0; k<n_size; k++) {
-				delta=gsl_vector_get(eval, k);
-				dx=gsl_vector_get(x_vec, k);
-				dw=gsl_matrix_get(W, j, k);
-				dy=gsl_matrix_get(UltVehiY, i, k);
-
-				d1+=dx*dw/(delta*dl+1.0);
-				d2+=dy*dw/(delta*dl+1.0);
-			}
-			gsl_matrix_set(WHix, j*d_size+i, i, d1);
-			gsl_vector_set(WHiy, j*d_size+i, d2);
-		}
-	}
-
-	gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Qi, WHix, 0.0, QiWHix);
-	gsl_blas_dgemm(CblasTrans, CblasNoTrans, -1.0, WHix, QiWHix, 1.0, xPx);
-	gsl_blas_dgemv(CblasTrans, -1.0, QiWHix, WHiy, 1.0, xPy);
-
-	// Calculate V(beta) and beta.
-	int sig;
-	gsl_permutation * pmt=gsl_permutation_alloc (d_size);
-	LUDecomp (xPx, pmt, &sig);
-	LUSolve (xPx, pmt, xPy, D_l);
-	LUInvert (xPx, pmt, Vbeta);
-
-	// Need to multiply UltVehi on both sides or one side.
-	gsl_blas_dgemv(CblasTrans, 1.0, UltVeh, D_l, 0.0, beta);
-	gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Vbeta, UltVeh, 
-		       0.0, xPx);
-	gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, UltVeh, xPx, 0.0, Vbeta);
-
-	// Calculate test statistic and p value.
-	gsl_blas_ddot(D_l, xPy, &d);
-
-	p_value=gsl_cdf_chisq_Q (d, (double)d_size);
-
-	gsl_vector_free(D_l);
-	gsl_matrix_free(UltVeh);
-	gsl_matrix_free(UltVehi);
-	gsl_matrix_free(Qi);
-	gsl_matrix_free(WHix);
-	gsl_matrix_free(QiWHix);
-
-	gsl_matrix_free(xPx);
-	gsl_vector_free(xPy);
-	gsl_vector_free(WHiy);
-
-	gsl_permutation_free(pmt);
-
-	return p_value;
+double MphCalcP(const gsl_vector *eval, const gsl_vector *x_vec,
+                const gsl_matrix *W, const gsl_matrix *Y, const gsl_matrix *V_g,
+                const gsl_matrix *V_e, gsl_matrix *UltVehiY, gsl_vector *beta,
+                gsl_matrix *Vbeta) {
+  size_t n_size = eval->size, c_size = W->size1, d_size = V_g->size1;
+  size_t dc_size = d_size * c_size;
+  double delta, dl, d, d1, d2, dy, dx, dw, logdet_Ve, logdet_Q, p_value;
+
+  gsl_vector *D_l = gsl_vector_alloc(d_size);
+  gsl_matrix *UltVeh = gsl_matrix_alloc(d_size, d_size);
+  gsl_matrix *UltVehi = gsl_matrix_alloc(d_size, d_size);
+  gsl_matrix *Qi = gsl_matrix_alloc(dc_size, dc_size);
+  gsl_matrix *WHix = gsl_matrix_alloc(dc_size, d_size);
+  gsl_matrix *QiWHix = gsl_matrix_alloc(dc_size, d_size);
+
+  gsl_matrix *xPx = gsl_matrix_alloc(d_size, d_size);
+  gsl_vector *xPy = gsl_vector_alloc(d_size);
+  gsl_vector *WHiy = gsl_vector_alloc(dc_size);
+
+  gsl_matrix_set_zero(xPx);
+  gsl_matrix_set_zero(WHix);
+  gsl_vector_set_zero(xPy);
+  gsl_vector_set_zero(WHiy);
+
+  // Eigen decomposition and calculate log|Ve|.
+  logdet_Ve = EigenProc(V_g, V_e, D_l, UltVeh, UltVehi);
+
+  // Calculate Qi and log|Q|.
+  logdet_Q = CalcQi(eval, D_l, W, Qi);
+
+  // Calculate UltVehiY.
+  gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, UltVehi, Y, 0.0, UltVehiY);
+
+  // Calculate WHix, WHiy, xHiy, xHix.
+  for (size_t i = 0; i < d_size; i++) {
+    dl = gsl_vector_get(D_l, i);
+
+    d1 = 0.0;
+    d2 = 0.0;
+    for (size_t k = 0; k < n_size; k++) {
+      delta = gsl_vector_get(eval, k);
+      dx = gsl_vector_get(x_vec, k);
+      dy = gsl_matrix_get(UltVehiY, i, k);
+
+      d1 += dx * dy / (delta * dl + 1.0);
+      d2 += dx * dx / (delta * dl + 1.0);
+    }
+    gsl_vector_set(xPy, i, d1);
+    gsl_matrix_set(xPx, i, i, d2);
+
+    for (size_t j = 0; j < c_size; j++) {
+      d1 = 0.0;
+      d2 = 0.0;
+      for (size_t k = 0; k < n_size; k++) {
+        delta = gsl_vector_get(eval, k);
+        dx = gsl_vector_get(x_vec, k);
+        dw = gsl_matrix_get(W, j, k);
+        dy = gsl_matrix_get(UltVehiY, i, k);
+
+        d1 += dx * dw / (delta * dl + 1.0);
+        d2 += dy * dw / (delta * dl + 1.0);
+      }
+      gsl_matrix_set(WHix, j * d_size + i, i, d1);
+      gsl_vector_set(WHiy, j * d_size + i, d2);
+    }
+  }
+
+  gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Qi, WHix, 0.0, QiWHix);
+  gsl_blas_dgemm(CblasTrans, CblasNoTrans, -1.0, WHix, QiWHix, 1.0, xPx);
+  gsl_blas_dgemv(CblasTrans, -1.0, QiWHix, WHiy, 1.0, xPy);
+
+  // Calculate V(beta) and beta.
+  int sig;
+  gsl_permutation *pmt = gsl_permutation_alloc(d_size);
+  LUDecomp(xPx, pmt, &sig);
+  LUSolve(xPx, pmt, xPy, D_l);
+  LUInvert(xPx, pmt, Vbeta);
+
+  // Need to multiply UltVehi on both sides or one side.
+  gsl_blas_dgemv(CblasTrans, 1.0, UltVeh, D_l, 0.0, beta);
+  gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Vbeta, UltVeh, 0.0, xPx);
+  gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, UltVeh, xPx, 0.0, Vbeta);
+
+  // Calculate test statistic and p value.
+  gsl_blas_ddot(D_l, xPy, &d);
+
+  p_value = gsl_cdf_chisq_Q(d, (double)d_size);
+
+  gsl_vector_free(D_l);
+  gsl_matrix_free(UltVeh);
+  gsl_matrix_free(UltVehi);
+  gsl_matrix_free(Qi);
+  gsl_matrix_free(WHix);
+  gsl_matrix_free(QiWHix);
+
+  gsl_matrix_free(xPx);
+  gsl_vector_free(xPy);
+  gsl_vector_free(WHiy);
+
+  gsl_permutation_free(pmt);
+
+  return p_value;
 }
 
 // Calculate B and its standard error (which is a matrix of the same
 // dimension as B).
-void MphCalcBeta (const gsl_vector *eval, const gsl_matrix *W, 
-		  const gsl_matrix *Y, const gsl_matrix *V_g, 
-		  const gsl_matrix *V_e, gsl_matrix *UltVehiY, 
-		  gsl_matrix *B, gsl_matrix *se_B) {
-	size_t n_size=eval->size, c_size=W->size1, d_size=V_g->size1;
-	size_t dc_size=d_size*c_size;
-	double delta, dl, d, dy, dw, logdet_Ve, logdet_Q;
-
-	gsl_vector *D_l=gsl_vector_alloc (d_size);
-	gsl_matrix *UltVeh=gsl_matrix_alloc (d_size, d_size);
-	gsl_matrix *UltVehi=gsl_matrix_alloc (d_size, d_size);
-	gsl_matrix *Qi=gsl_matrix_alloc (dc_size, dc_size);
-	gsl_matrix *Qi_temp=gsl_matrix_alloc (dc_size, dc_size);
-	gsl_vector *WHiy=gsl_vector_alloc (dc_size);
-	gsl_vector *QiWHiy=gsl_vector_alloc (dc_size);
-	gsl_vector *beta=gsl_vector_alloc (dc_size);
-	gsl_matrix *Vbeta=gsl_matrix_alloc (dc_size, dc_size);
-
-	gsl_vector_set_zero (WHiy);
-
-	// Eigen decomposition and calculate log|Ve|.
-	logdet_Ve=EigenProc (V_g, V_e, D_l, UltVeh, UltVehi);
-
-	// Calculate Qi and log|Q|.
-	logdet_Q=CalcQi (eval, D_l, W, Qi);
-
-	// Calculate UltVehiY.
-	gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, UltVehi, Y, 
-		       0.0, UltVehiY);
-
-	// Calculate WHiy.
-	for (size_t i=0; i<d_size; i++) {
-		dl=gsl_vector_get(D_l, i);
-
-		for (size_t j=0; j<c_size; j++) {
-			d=0.0;
-			for (size_t k=0; k<n_size; k++) {
-				delta=gsl_vector_get(eval, k);
-				dw=gsl_matrix_get(W, j, k);
-				dy=gsl_matrix_get(UltVehiY, i, k);
-
-				d+=dy*dw/(delta*dl+1.0);
-			}
-			gsl_vector_set(WHiy, j*d_size+i, d);
-		}
-	}
-
-	gsl_blas_dgemv(CblasNoTrans, 1.0, Qi, WHiy, 0.0, QiWHiy);
-
-	// Need to multiply I_c\otimes UltVehi on both sides or one side.
-	for (size_t i=0; i<c_size; i++) {
-	  gsl_vector_view QiWHiy_sub=
-	    gsl_vector_subvector(QiWHiy, i*d_size, d_size);
-	  gsl_vector_view beta_sub=gsl_vector_subvector(beta,i*d_size,d_size);
-	  gsl_blas_dgemv(CblasTrans, 1.0, UltVeh, &QiWHiy_sub.vector, 0.0, 
-			 &beta_sub.vector);
-	  
-	  for (size_t j=0; j<c_size; j++) {
-	    gsl_matrix_view Qi_sub=
-	      gsl_matrix_submatrix (Qi, i*d_size, j*d_size, d_size, d_size);
-	    gsl_matrix_view Qitemp_sub=
-	      gsl_matrix_submatrix (Qi_temp,i*d_size,j*d_size,d_size,d_size);
-	    gsl_matrix_view Vbeta_sub=
-	      gsl_matrix_submatrix (Vbeta, i*d_size, j*d_size, d_size, d_size);
-	    
-	    if (j<i) {
-	      gsl_matrix_view Vbeta_sym=
-		gsl_matrix_submatrix(Vbeta,j*d_size,i*d_size,d_size,d_size);
-	      gsl_matrix_transpose_memcpy(&Vbeta_sub.matrix,&Vbeta_sym.matrix);
-	    } else {
-	      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &Qi_sub.matrix, 
-			     UltVeh, 0.0, &Qitemp_sub.matrix);
-	      gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, UltVeh, 
-			     &Qitemp_sub.matrix, 0.0, &Vbeta_sub.matrix);
-	    }
-	  }
-	}
-
-	// Copy beta to B, and Vbeta to se_B.
-	for (size_t j=0; j<B->size2; j++) {
-	  for (size_t i=0; i<B->size1; i++) {
-	    gsl_matrix_set(B, i, j, gsl_vector_get(beta, j*d_size+i));
-	    gsl_matrix_set(se_B, i, j, 
-			   sqrt(gsl_matrix_get(Vbeta,j*d_size+i,j*d_size+i)));
-	  }
-	}
-	
-	// Free matrices.
-	gsl_vector_free(D_l);
-	gsl_matrix_free(UltVeh);
-	gsl_matrix_free(UltVehi);
-	gsl_matrix_free(Qi);
-	gsl_matrix_free(Qi_temp);
-	gsl_vector_free(WHiy);
-	gsl_vector_free(QiWHiy);
-	gsl_vector_free(beta);
-	gsl_matrix_free(Vbeta);
-
-	return;
+void MphCalcBeta(const gsl_vector *eval, const gsl_matrix *W,
+                 const gsl_matrix *Y, const gsl_matrix *V_g,
+                 const gsl_matrix *V_e, gsl_matrix *UltVehiY, gsl_matrix *B,
+                 gsl_matrix *se_B) {
+  size_t n_size = eval->size, c_size = W->size1, d_size = V_g->size1;
+  size_t dc_size = d_size * c_size;
+  double delta, dl, d, dy, dw, logdet_Ve, logdet_Q;
+
+  gsl_vector *D_l = gsl_vector_alloc(d_size);
+  gsl_matrix *UltVeh = gsl_matrix_alloc(d_size, d_size);
+  gsl_matrix *UltVehi = gsl_matrix_alloc(d_size, d_size);
+  gsl_matrix *Qi = gsl_matrix_alloc(dc_size, dc_size);
+  gsl_matrix *Qi_temp = gsl_matrix_alloc(dc_size, dc_size);
+  gsl_vector *WHiy = gsl_vector_alloc(dc_size);
+  gsl_vector *QiWHiy = gsl_vector_alloc(dc_size);
+  gsl_vector *beta = gsl_vector_alloc(dc_size);
+  gsl_matrix *Vbeta = gsl_matrix_alloc(dc_size, dc_size);
+
+  gsl_vector_set_zero(WHiy);
+
+  // Eigen decomposition and calculate log|Ve|.
+  logdet_Ve = EigenProc(V_g, V_e, D_l, UltVeh, UltVehi);
+
+  // Calculate Qi and log|Q|.
+  logdet_Q = CalcQi(eval, D_l, W, Qi);
+
+  // Calculate UltVehiY.
+  gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, UltVehi, Y, 0.0, UltVehiY);
+
+  // Calculate WHiy.
+  for (size_t i = 0; i < d_size; i++) {
+    dl = gsl_vector_get(D_l, i);
+
+    for (size_t j = 0; j < c_size; j++) {
+      d = 0.0;
+      for (size_t k = 0; k < n_size; k++) {
+        delta = gsl_vector_get(eval, k);
+        dw = gsl_matrix_get(W, j, k);
+        dy = gsl_matrix_get(UltVehiY, i, k);
+
+        d += dy * dw / (delta * dl + 1.0);
+      }
+      gsl_vector_set(WHiy, j * d_size + i, d);
+    }
+  }
+
+  gsl_blas_dgemv(CblasNoTrans, 1.0, Qi, WHiy, 0.0, QiWHiy);
+
+  // Need to multiply I_c\otimes UltVehi on both sides or one side.
+  for (size_t i = 0; i < c_size; i++) {
+    gsl_vector_view QiWHiy_sub =
+        gsl_vector_subvector(QiWHiy, i * d_size, d_size);
+    gsl_vector_view beta_sub = gsl_vector_subvector(beta, i * d_size, d_size);
+    gsl_blas_dgemv(CblasTrans, 1.0, UltVeh, &QiWHiy_sub.vector, 0.0,
+                   &beta_sub.vector);
+
+    for (size_t j = 0; j < c_size; j++) {
+      gsl_matrix_view Qi_sub =
+          gsl_matrix_submatrix(Qi, i * d_size, j * d_size, d_size, d_size);
+      gsl_matrix_view Qitemp_sub =
+          gsl_matrix_submatrix(Qi_temp, i * d_size, j * d_size, d_size, d_size);
+      gsl_matrix_view Vbeta_sub =
+          gsl_matrix_submatrix(Vbeta, i * d_size, j * d_size, d_size, d_size);
+
+      if (j < i) {
+        gsl_matrix_view Vbeta_sym =
+            gsl_matrix_submatrix(Vbeta, j * d_size, i * d_size, d_size, d_size);
+        gsl_matrix_transpose_memcpy(&Vbeta_sub.matrix, &Vbeta_sym.matrix);
+      } else {
+        gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &Qi_sub.matrix, UltVeh,
+                       0.0, &Qitemp_sub.matrix);
+        gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, UltVeh,
+                       &Qitemp_sub.matrix, 0.0, &Vbeta_sub.matrix);
+      }
+    }
+  }
+
+  // Copy beta to B, and Vbeta to se_B.
+  for (size_t j = 0; j < B->size2; j++) {
+    for (size_t i = 0; i < B->size1; i++) {
+      gsl_matrix_set(B, i, j, gsl_vector_get(beta, j * d_size + i));
+      gsl_matrix_set(se_B, i, j, sqrt(gsl_matrix_get(Vbeta, j * d_size + i,
+                                                     j * d_size + i)));
+    }
+  }
+
+  // Free matrices.
+  gsl_vector_free(D_l);
+  gsl_matrix_free(UltVeh);
+  gsl_matrix_free(UltVehi);
+  gsl_matrix_free(Qi);
+  gsl_matrix_free(Qi_temp);
+  gsl_vector_free(WHiy);
+  gsl_vector_free(QiWHiy);
+  gsl_vector_free(beta);
+  gsl_matrix_free(Vbeta);
+
+  return;
 }
 
 // Below are functions for Newton-Raphson's algorithm.
@@ -912,996 +931,962 @@ void MphCalcBeta (const gsl_vector *eval, const gsl_matrix *W,
 // Calculate all Hi and return logdet_H=\sum_{k=1}^{n}log|H_k|
 // and calculate Qi and return logdet_Q
 // and calculate yPy.
-void CalcHiQi (const gsl_vector *eval, const gsl_matrix *X,
-	       const gsl_matrix *V_g, const gsl_matrix *V_e,
-	       gsl_matrix *Hi_all, gsl_matrix *Qi, double &logdet_H,
-	       double &logdet_Q) {
-	gsl_matrix_set_zero (Hi_all);
-	gsl_matrix_set_zero (Qi);
-	logdet_H=0.0; logdet_Q=0.0;
-
-	size_t n_size=eval->size, c_size=X->size1, d_size=V_g->size1;
-	double logdet_Ve=0.0, delta, dl, d;
-
-	gsl_matrix *mat_dd=gsl_matrix_alloc (d_size, d_size);
-	gsl_matrix *UltVeh=gsl_matrix_alloc (d_size, d_size);
-	gsl_matrix *UltVehi=gsl_matrix_alloc (d_size, d_size);
-	gsl_vector *D_l=gsl_vector_alloc (d_size);
-
-	// Calculate D_l, UltVeh and UltVehi.
-	logdet_Ve=EigenProc (V_g, V_e, D_l, UltVeh, UltVehi);
-
-	// Calculate each Hi and log|H_k|.
-	logdet_H=(double)n_size*logdet_Ve;
-	for (size_t k=0; k<n_size; k++) {
-		delta=gsl_vector_get (eval, k);
-
-		gsl_matrix_memcpy (mat_dd, UltVehi);
-		for (size_t i=0; i<d_size; i++) {
-			dl=gsl_vector_get(D_l, i);
-			d=delta*dl+1.0;
-
-			gsl_vector_view mat_row=gsl_matrix_row (mat_dd, i);
-			gsl_vector_scale (&mat_row.vector, 1.0/d);
-
-			logdet_H+=log(d);
-		}
-
-		gsl_matrix_view Hi_k=
-		  gsl_matrix_submatrix(Hi_all, 0, k*d_size, d_size, d_size);
-		gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, UltVehi,
-			       mat_dd, 0.0, &Hi_k.matrix);
-	}
-
-	// Calculate Qi, and multiply I\o times UtVeh on both side and
-	// calculate logdet_Q, don't forget to substract
-	// c_size*logdet_Ve.
-	logdet_Q=CalcQi (eval, D_l, X, Qi)-(double)c_size*logdet_Ve;
-
-	for (size_t i=0; i<c_size; i++) {
-		for (size_t j=0; j<c_size; j++) {
-		  gsl_matrix_view Qi_sub=
-		    gsl_matrix_submatrix(Qi,i*d_size,j*d_size,d_size,d_size);
-		  if (j<i) {
-		    gsl_matrix_view Qi_sym=
-		      gsl_matrix_submatrix(Qi,j*d_size,i*d_size,d_size,d_size);
-		    gsl_matrix_transpose_memcpy(&Qi_sub.matrix,&Qi_sym.matrix);
-		  } else {
-		    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0,
-				   &Qi_sub.matrix, UltVeh, 0.0, mat_dd);
-		    gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, UltVeh,
-				   mat_dd, 0.0, &Qi_sub.matrix);
-		  }
-		}
-	}
-
-	// Free memory.
-	gsl_matrix_free(mat_dd);
-	gsl_matrix_free(UltVeh);
-	gsl_matrix_free(UltVehi);
-	gsl_vector_free(D_l);
-
-	return;
+void CalcHiQi(const gsl_vector *eval, const gsl_matrix *X,
+              const gsl_matrix *V_g, const gsl_matrix *V_e, gsl_matrix *Hi_all,
+              gsl_matrix *Qi, double &logdet_H, double &logdet_Q) {
+  gsl_matrix_set_zero(Hi_all);
+  gsl_matrix_set_zero(Qi);
+  logdet_H = 0.0;
+  logdet_Q = 0.0;
+
+  size_t n_size = eval->size, c_size = X->size1, d_size = V_g->size1;
+  double logdet_Ve = 0.0, delta, dl, d;
+
+  gsl_matrix *mat_dd = gsl_matrix_alloc(d_size, d_size);
+  gsl_matrix *UltVeh = gsl_matrix_alloc(d_size, d_size);
+  gsl_matrix *UltVehi = gsl_matrix_alloc(d_size, d_size);
+  gsl_vector *D_l = gsl_vector_alloc(d_size);
+
+  // Calculate D_l, UltVeh and UltVehi.
+  logdet_Ve = EigenProc(V_g, V_e, D_l, UltVeh, UltVehi);
+
+  // Calculate each Hi and log|H_k|.
+  logdet_H = (double)n_size * logdet_Ve;
+  for (size_t k = 0; k < n_size; k++) {
+    delta = gsl_vector_get(eval, k);
+
+    gsl_matrix_memcpy(mat_dd, UltVehi);
+    for (size_t i = 0; i < d_size; i++) {
+      dl = gsl_vector_get(D_l, i);
+      d = delta * dl + 1.0;
+
+      gsl_vector_view mat_row = gsl_matrix_row(mat_dd, i);
+      gsl_vector_scale(&mat_row.vector, 1.0 / d);
+
+      logdet_H += log(d);
+    }
+
+    gsl_matrix_view Hi_k =
+        gsl_matrix_submatrix(Hi_all, 0, k * d_size, d_size, d_size);
+    gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, UltVehi, mat_dd, 0.0,
+                   &Hi_k.matrix);
+  }
+
+  // Calculate Qi, and multiply I\o times UtVeh on both side and
+  // calculate logdet_Q, don't forget to substract
+  // c_size*logdet_Ve.
+  logdet_Q = CalcQi(eval, D_l, X, Qi) - (double)c_size * logdet_Ve;
+
+  for (size_t i = 0; i < c_size; i++) {
+    for (size_t j = 0; j < c_size; j++) {
+      gsl_matrix_view Qi_sub =
+          gsl_matrix_submatrix(Qi, i * d_size, j * d_size, d_size, d_size);
+      if (j < i) {
+        gsl_matrix_view Qi_sym =
+            gsl_matrix_submatrix(Qi, j * d_size, i * d_size, d_size, d_size);
+        gsl_matrix_transpose_memcpy(&Qi_sub.matrix, &Qi_sym.matrix);
+      } else {
+        gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &Qi_sub.matrix, UltVeh,
+                       0.0, mat_dd);
+        gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, UltVeh, mat_dd, 0.0,
+                       &Qi_sub.matrix);
+      }
+    }
+  }
+
+  // Free memory.
+  gsl_matrix_free(mat_dd);
+  gsl_matrix_free(UltVeh);
+  gsl_matrix_free(UltVehi);
+  gsl_vector_free(D_l);
+
+  return;
 }
 
 // Calculate all Hiy.
-void Calc_Hiy_all (const gsl_matrix *Y, const gsl_matrix *Hi_all,
-		   gsl_matrix *Hiy_all) {
-	gsl_matrix_set_zero (Hiy_all);
+void Calc_Hiy_all(const gsl_matrix *Y, const gsl_matrix *Hi_all,
+                  gsl_matrix *Hiy_all) {
+  gsl_matrix_set_zero(Hiy_all);
 
-	size_t n_size=Y->size2, d_size=Y->size1;
+  size_t n_size = Y->size2, d_size = Y->size1;
 
-	for (size_t k=0; k<n_size; k++) {
-		gsl_matrix_const_view Hi_k=
-		  gsl_matrix_const_submatrix(Hi_all,0,k*d_size,d_size,d_size);
-		gsl_vector_const_view y_k=gsl_matrix_const_column(Y,k);
-		gsl_vector_view Hiy_k=gsl_matrix_column(Hiy_all, k);
+  for (size_t k = 0; k < n_size; k++) {
+    gsl_matrix_const_view Hi_k =
+        gsl_matrix_const_submatrix(Hi_all, 0, k * d_size, d_size, d_size);
+    gsl_vector_const_view y_k = gsl_matrix_const_column(Y, k);
+    gsl_vector_view Hiy_k = gsl_matrix_column(Hiy_all, k);
 
-		gsl_blas_dgemv(CblasNoTrans,1.0,&Hi_k.matrix,&y_k.vector,
-			       0.0,&Hiy_k.vector);
-	}
+    gsl_blas_dgemv(CblasNoTrans, 1.0, &Hi_k.matrix, &y_k.vector, 0.0,
+                   &Hiy_k.vector);
+  }
 
-	return;
+  return;
 }
 
 // Calculate all xHi.
-void Calc_xHi_all (const gsl_matrix *X, const gsl_matrix *Hi_all,
-		   gsl_matrix *xHi_all) {
-	gsl_matrix_set_zero (xHi_all);
+void Calc_xHi_all(const gsl_matrix *X, const gsl_matrix *Hi_all,
+                  gsl_matrix *xHi_all) {
+  gsl_matrix_set_zero(xHi_all);
 
-	size_t n_size=X->size2, c_size=X->size1, d_size=Hi_all->size1;
+  size_t n_size = X->size2, c_size = X->size1, d_size = Hi_all->size1;
 
-	double d;
+  double d;
 
-	for (size_t k=0; k<n_size; k++) {
-		gsl_matrix_const_view Hi_k=
-		  gsl_matrix_const_submatrix(Hi_all,0,k*d_size,d_size,d_size);
+  for (size_t k = 0; k < n_size; k++) {
+    gsl_matrix_const_view Hi_k =
+        gsl_matrix_const_submatrix(Hi_all, 0, k * d_size, d_size, d_size);
 
-		for (size_t i=0; i<c_size; i++) {
-			d=gsl_matrix_get (X, i, k);
-			gsl_matrix_view xHi_sub=
-			  gsl_matrix_submatrix(xHi_all,i*d_size,k*d_size,
-					       d_size,d_size);
-			gsl_matrix_memcpy(&xHi_sub.matrix, &Hi_k.matrix);
-			gsl_matrix_scale(&xHi_sub.matrix, d);
-		}
-	}
+    for (size_t i = 0; i < c_size; i++) {
+      d = gsl_matrix_get(X, i, k);
+      gsl_matrix_view xHi_sub =
+          gsl_matrix_submatrix(xHi_all, i * d_size, k * d_size, d_size, d_size);
+      gsl_matrix_memcpy(&xHi_sub.matrix, &Hi_k.matrix);
+      gsl_matrix_scale(&xHi_sub.matrix, d);
+    }
+  }
 
-	return;
+  return;
 }
 
 // Calculate scalar yHiy.
-double Calc_yHiy (const gsl_matrix *Y, const gsl_matrix *Hiy_all) {
-	double yHiy=0.0, d;
-	size_t n_size=Y->size2;
+double Calc_yHiy(const gsl_matrix *Y, const gsl_matrix *Hiy_all) {
+  double yHiy = 0.0, d;
+  size_t n_size = Y->size2;
 
-	for (size_t k=0; k<n_size; k++) {
-	  gsl_vector_const_view y_k=gsl_matrix_const_column(Y, k);
-	  gsl_vector_const_view Hiy_k=gsl_matrix_const_column(Hiy_all, k);
+  for (size_t k = 0; k < n_size; k++) {
+    gsl_vector_const_view y_k = gsl_matrix_const_column(Y, k);
+    gsl_vector_const_view Hiy_k = gsl_matrix_const_column(Hiy_all, k);
 
-	  gsl_blas_ddot (&Hiy_k.vector, &y_k.vector, &d);
-	  yHiy+=d;
-	}
+    gsl_blas_ddot(&Hiy_k.vector, &y_k.vector, &d);
+    yHiy += d;
+  }
 
-	return yHiy;
+  return yHiy;
 }
 
 // Calculate the vector xHiy.
-void Calc_xHiy (const gsl_matrix *Y, const gsl_matrix *xHi, gsl_vector *xHiy) {
-	gsl_vector_set_zero (xHiy);
+void Calc_xHiy(const gsl_matrix *Y, const gsl_matrix *xHi, gsl_vector *xHiy) {
+  gsl_vector_set_zero(xHiy);
 
-	size_t n_size=Y->size2, d_size=Y->size1, dc_size=xHi->size1;
+  size_t n_size = Y->size2, d_size = Y->size1, dc_size = xHi->size1;
 
-	for (size_t k=0; k<n_size; k++) {
-		gsl_matrix_const_view xHi_k=
-		  gsl_matrix_const_submatrix(xHi,0,k*d_size,dc_size,d_size);
-		gsl_vector_const_view y_k=gsl_matrix_const_column(Y, k);
+  for (size_t k = 0; k < n_size; k++) {
+    gsl_matrix_const_view xHi_k =
+        gsl_matrix_const_submatrix(xHi, 0, k * d_size, dc_size, d_size);
+    gsl_vector_const_view y_k = gsl_matrix_const_column(Y, k);
 
-		gsl_blas_dgemv(CblasNoTrans,1.0,&xHi_k.matrix,&y_k.vector,
-			       1.0,xHiy);
-	}
+    gsl_blas_dgemv(CblasNoTrans, 1.0, &xHi_k.matrix, &y_k.vector, 1.0, xHiy);
+  }
 
-	return;
+  return;
 }
 
 // 0<=i,j<d_size
-size_t GetIndex (const size_t i, const size_t j, const size_t d_size) {
-	if (i>=d_size || j>=d_size) {
-	  cout<<"error in GetIndex."<<endl;
-	  return 0;
-	}
-
-	size_t s, l;
-	if (j<i) {
-	  s=j;
-	  l=i;
-	} else {
-	  s=i;
-	  l=j;
-	}
-
-	return (2*d_size-s+1)*s/2+l-s;
+size_t GetIndex(const size_t i, const size_t j, const size_t d_size) {
+  if (i >= d_size || j >= d_size) {
+    cout << "error in GetIndex." << endl;
+    return 0;
+  }
+
+  size_t s, l;
+  if (j < i) {
+    s = j;
+    l = i;
+  } else {
+    s = i;
+    l = j;
+  }
+
+  return (2 * d_size - s + 1) * s / 2 + l - s;
 }
 
-void Calc_yHiDHiy (const gsl_vector *eval, const gsl_matrix *Hiy,
-		   const size_t i, const size_t j, double &yHiDHiy_g,
-		   double &yHiDHiy_e) {
-	yHiDHiy_g=0.0;
-	yHiDHiy_e=0.0;
+void Calc_yHiDHiy(const gsl_vector *eval, const gsl_matrix *Hiy, const size_t i,
+                  const size_t j, double &yHiDHiy_g, double &yHiDHiy_e) {
+  yHiDHiy_g = 0.0;
+  yHiDHiy_e = 0.0;
 
-	size_t n_size=eval->size;
+  size_t n_size = eval->size;
 
-	double delta, d1, d2;
+  double delta, d1, d2;
 
-	for (size_t k=0; k<n_size; k++) {
-		delta=gsl_vector_get (eval, k);
-		d1=gsl_matrix_get (Hiy, i, k);
-		d2=gsl_matrix_get (Hiy, j, k);
+  for (size_t k = 0; k < n_size; k++) {
+    delta = gsl_vector_get(eval, k);
+    d1 = gsl_matrix_get(Hiy, i, k);
+    d2 = gsl_matrix_get(Hiy, j, k);
 
-		if (i==j) {
-			yHiDHiy_g+=delta*d1*d2;
-			yHiDHiy_e+=d1*d2;
-		} else {
-			yHiDHiy_g+=delta*d1*d2*2.0;
-			yHiDHiy_e+=d1*d2*2.0;
-		}
-	}
+    if (i == j) {
+      yHiDHiy_g += delta * d1 * d2;
+      yHiDHiy_e += d1 * d2;
+    } else {
+      yHiDHiy_g += delta * d1 * d2 * 2.0;
+      yHiDHiy_e += d1 * d2 * 2.0;
+    }
+  }
 
-	return;
+  return;
 }
 
-void Calc_xHiDHiy (const gsl_vector *eval, const gsl_matrix *xHi,
-		   const gsl_matrix *Hiy, const size_t i, const size_t j,
-		   gsl_vector *xHiDHiy_g, gsl_vector *xHiDHiy_e) {
-	gsl_vector_set_zero(xHiDHiy_g);
-	gsl_vector_set_zero(xHiDHiy_e);
+void Calc_xHiDHiy(const gsl_vector *eval, const gsl_matrix *xHi,
+                  const gsl_matrix *Hiy, const size_t i, const size_t j,
+                  gsl_vector *xHiDHiy_g, gsl_vector *xHiDHiy_e) {
+  gsl_vector_set_zero(xHiDHiy_g);
+  gsl_vector_set_zero(xHiDHiy_e);
+
+  size_t n_size = eval->size, d_size = Hiy->size1;
 
-	size_t n_size=eval->size, d_size=Hiy->size1;
+  double delta, d;
 
-	double delta, d;
+  for (size_t k = 0; k < n_size; k++) {
+    delta = gsl_vector_get(eval, k);
 
-	for (size_t k=0; k<n_size; k++) {
-		delta=gsl_vector_get (eval, k);
+    gsl_vector_const_view xHi_col_i =
+        gsl_matrix_const_column(xHi, k * d_size + i);
+    d = gsl_matrix_get(Hiy, j, k);
 
-		gsl_vector_const_view xHi_col_i=
-		  gsl_matrix_const_column (xHi, k*d_size+i);
-		d=gsl_matrix_get (Hiy, j, k);
+    gsl_blas_daxpy(d * delta, &xHi_col_i.vector, xHiDHiy_g);
+    gsl_blas_daxpy(d, &xHi_col_i.vector, xHiDHiy_e);
 
-		gsl_blas_daxpy (d*delta, &xHi_col_i.vector, xHiDHiy_g);
-		gsl_blas_daxpy (d, &xHi_col_i.vector, xHiDHiy_e);
+    if (i != j) {
+      gsl_vector_const_view xHi_col_j =
+          gsl_matrix_const_column(xHi, k * d_size + j);
+      d = gsl_matrix_get(Hiy, i, k);
 
-		if (i!=j) {
-		  gsl_vector_const_view xHi_col_j=
-		    gsl_matrix_const_column (xHi, k*d_size+j);
-		  d=gsl_matrix_get (Hiy, i, k);
-		  
-		  gsl_blas_daxpy (d*delta, &xHi_col_j.vector, xHiDHiy_g);
-		  gsl_blas_daxpy (d, &xHi_col_j.vector, xHiDHiy_e);
-		}
-	}
+      gsl_blas_daxpy(d * delta, &xHi_col_j.vector, xHiDHiy_g);
+      gsl_blas_daxpy(d, &xHi_col_j.vector, xHiDHiy_e);
+    }
+  }
 
-	return;
+  return;
 }
 
-void Calc_xHiDHix (const gsl_vector *eval, const gsl_matrix *xHi,
-		   const size_t i, const size_t j, gsl_matrix *xHiDHix_g,
-		   gsl_matrix *xHiDHix_e) {
-	gsl_matrix_set_zero(xHiDHix_g);
-	gsl_matrix_set_zero(xHiDHix_e);
+void Calc_xHiDHix(const gsl_vector *eval, const gsl_matrix *xHi, const size_t i,
+                  const size_t j, gsl_matrix *xHiDHix_g,
+                  gsl_matrix *xHiDHix_e) {
+  gsl_matrix_set_zero(xHiDHix_g);
+  gsl_matrix_set_zero(xHiDHix_e);
 
-	size_t n_size=eval->size, dc_size=xHi->size1;
-	size_t d_size=xHi->size2/n_size;
+  size_t n_size = eval->size, dc_size = xHi->size1;
+  size_t d_size = xHi->size2 / n_size;
 
-	double delta;
+  double delta;
 
-	gsl_matrix *mat_dcdc=gsl_matrix_alloc (dc_size, dc_size);
-	gsl_matrix *mat_dcdc_t=gsl_matrix_alloc (dc_size, dc_size);
+  gsl_matrix *mat_dcdc = gsl_matrix_alloc(dc_size, dc_size);
+  gsl_matrix *mat_dcdc_t = gsl_matrix_alloc(dc_size, dc_size);
 
-	for (size_t k=0; k<n_size; k++) {
-		delta=gsl_vector_get (eval, k);
+  for (size_t k = 0; k < n_size; k++) {
+    delta = gsl_vector_get(eval, k);
 
-		gsl_vector_const_view xHi_col_i=
-		  gsl_matrix_const_column (xHi, k*d_size+i);
-		gsl_vector_const_view xHi_col_j=
-		  gsl_matrix_const_column (xHi, k*d_size+j);
+    gsl_vector_const_view xHi_col_i =
+        gsl_matrix_const_column(xHi, k * d_size + i);
+    gsl_vector_const_view xHi_col_j =
+        gsl_matrix_const_column(xHi, k * d_size + j);
 
-		gsl_matrix_set_zero (mat_dcdc);
-		gsl_blas_dger(1.0,&xHi_col_i.vector,&xHi_col_j.vector,
-			      mat_dcdc);
+    gsl_matrix_set_zero(mat_dcdc);
+    gsl_blas_dger(1.0, &xHi_col_i.vector, &xHi_col_j.vector, mat_dcdc);
 
-		gsl_matrix_transpose_memcpy (mat_dcdc_t, mat_dcdc);
+    gsl_matrix_transpose_memcpy(mat_dcdc_t, mat_dcdc);
 
-		gsl_matrix_add (xHiDHix_e, mat_dcdc);
+    gsl_matrix_add(xHiDHix_e, mat_dcdc);
 
-		gsl_matrix_scale (mat_dcdc, delta);
-		gsl_matrix_add (xHiDHix_g, mat_dcdc);
+    gsl_matrix_scale(mat_dcdc, delta);
+    gsl_matrix_add(xHiDHix_g, mat_dcdc);
 
-		if (i!=j) {
-			gsl_matrix_add (xHiDHix_e, mat_dcdc_t);
+    if (i != j) {
+      gsl_matrix_add(xHiDHix_e, mat_dcdc_t);
 
-			gsl_matrix_scale (mat_dcdc_t, delta);
-			gsl_matrix_add (xHiDHix_g, mat_dcdc_t);
-		}
-	}
+      gsl_matrix_scale(mat_dcdc_t, delta);
+      gsl_matrix_add(xHiDHix_g, mat_dcdc_t);
+    }
+  }
 
-	gsl_matrix_free(mat_dcdc);
-	gsl_matrix_free(mat_dcdc_t);
+  gsl_matrix_free(mat_dcdc);
+  gsl_matrix_free(mat_dcdc_t);
 
-	return;
+  return;
 }
 
-void Calc_yHiDHiDHiy (const gsl_vector *eval, const gsl_matrix *Hi,
-		      const gsl_matrix *Hiy, const size_t i1,
-		      const size_t j1, const size_t i2, const size_t j2,
-		      double &yHiDHiDHiy_gg, double &yHiDHiDHiy_ee,
-		      double &yHiDHiDHiy_ge) {
-	yHiDHiDHiy_gg=0.0;
-	yHiDHiDHiy_ee=0.0;
-	yHiDHiDHiy_ge=0.0;
-
-	size_t n_size=eval->size, d_size=Hiy->size1;
-
-	double delta, d_Hiy_i1, d_Hiy_j1, d_Hiy_i2, d_Hiy_j2;
-	double d_Hi_i1i2, d_Hi_i1j2, d_Hi_j1i2, d_Hi_j1j2;
-
-	for (size_t k=0; k<n_size; k++) {
-		delta=gsl_vector_get (eval, k);
-
-		d_Hiy_i1=gsl_matrix_get (Hiy, i1, k);
-		d_Hiy_j1=gsl_matrix_get (Hiy, j1, k);
-		d_Hiy_i2=gsl_matrix_get (Hiy, i2, k);
-		d_Hiy_j2=gsl_matrix_get (Hiy, j2, k);
-
-		d_Hi_i1i2=gsl_matrix_get (Hi, i1, k*d_size+i2);
-		d_Hi_i1j2=gsl_matrix_get (Hi, i1, k*d_size+j2);
-		d_Hi_j1i2=gsl_matrix_get (Hi, j1, k*d_size+i2);
-		d_Hi_j1j2=gsl_matrix_get (Hi, j1, k*d_size+j2);
-
-		if (i1==j1) {
-		  yHiDHiDHiy_gg+=delta*delta*(d_Hiy_i1*d_Hi_j1i2*d_Hiy_j2);
-		  yHiDHiDHiy_ee+=(d_Hiy_i1*d_Hi_j1i2*d_Hiy_j2);
-		  yHiDHiDHiy_ge+=delta*(d_Hiy_i1*d_Hi_j1i2*d_Hiy_j2);
-		  
-		  if (i2!=j2) {
-		    yHiDHiDHiy_gg+=delta*delta*(d_Hiy_i1*d_Hi_j1j2*d_Hiy_i2);
-		    yHiDHiDHiy_ee+=(d_Hiy_i1*d_Hi_j1j2*d_Hiy_i2);
-		    yHiDHiDHiy_ge+=delta*(d_Hiy_i1*d_Hi_j1j2*d_Hiy_i2);
-		  }
-		} else {
-		  yHiDHiDHiy_gg+=
-		    delta*delta*(d_Hiy_i1*d_Hi_j1i2*d_Hiy_j2 +
-				 d_Hiy_j1*d_Hi_i1i2*d_Hiy_j2);
-		  yHiDHiDHiy_ee+=
-		    (d_Hiy_i1*d_Hi_j1i2*d_Hiy_j2+d_Hiy_j1*d_Hi_i1i2*d_Hiy_j2);
-		  yHiDHiDHiy_ge+=
-		    delta*(d_Hiy_i1*d_Hi_j1i2*d_Hiy_j2 +
-			   d_Hiy_j1*d_Hi_i1i2*d_Hiy_j2);
-		  
-		  if (i2!=j2) {
-		    yHiDHiDHiy_gg+=
-		      delta*delta*(d_Hiy_i1*d_Hi_j1j2*d_Hiy_i2 +
-				   d_Hiy_j1*d_Hi_i1j2*d_Hiy_i2);
-		    yHiDHiDHiy_ee+=(d_Hiy_i1*d_Hi_j1j2*d_Hiy_i2 +
-				    d_Hiy_j1*d_Hi_i1j2*d_Hiy_i2);
-		    yHiDHiDHiy_ge+=delta*(d_Hiy_i1*d_Hi_j1j2*d_Hiy_i2 +
-					  d_Hiy_j1*d_Hi_i1j2*d_Hiy_i2);
-		  }
-		}
-	}
-	
-	return;
+void Calc_yHiDHiDHiy(const gsl_vector *eval, const gsl_matrix *Hi,
+                     const gsl_matrix *Hiy, const size_t i1, const size_t j1,
+                     const size_t i2, const size_t j2, double &yHiDHiDHiy_gg,
+                     double &yHiDHiDHiy_ee, double &yHiDHiDHiy_ge) {
+  yHiDHiDHiy_gg = 0.0;
+  yHiDHiDHiy_ee = 0.0;
+  yHiDHiDHiy_ge = 0.0;
+
+  size_t n_size = eval->size, d_size = Hiy->size1;
+
+  double delta, d_Hiy_i1, d_Hiy_j1, d_Hiy_i2, d_Hiy_j2;
+  double d_Hi_i1i2, d_Hi_i1j2, d_Hi_j1i2, d_Hi_j1j2;
+
+  for (size_t k = 0; k < n_size; k++) {
+    delta = gsl_vector_get(eval, k);
+
+    d_Hiy_i1 = gsl_matrix_get(Hiy, i1, k);
+    d_Hiy_j1 = gsl_matrix_get(Hiy, j1, k);
+    d_Hiy_i2 = gsl_matrix_get(Hiy, i2, k);
+    d_Hiy_j2 = gsl_matrix_get(Hiy, j2, k);
+
+    d_Hi_i1i2 = gsl_matrix_get(Hi, i1, k * d_size + i2);
+    d_Hi_i1j2 = gsl_matrix_get(Hi, i1, k * d_size + j2);
+    d_Hi_j1i2 = gsl_matrix_get(Hi, j1, k * d_size + i2);
+    d_Hi_j1j2 = gsl_matrix_get(Hi, j1, k * d_size + j2);
+
+    if (i1 == j1) {
+      yHiDHiDHiy_gg += delta * delta * (d_Hiy_i1 * d_Hi_j1i2 * d_Hiy_j2);
+      yHiDHiDHiy_ee += (d_Hiy_i1 * d_Hi_j1i2 * d_Hiy_j2);
+      yHiDHiDHiy_ge += delta * (d_Hiy_i1 * d_Hi_j1i2 * d_Hiy_j2);
+
+      if (i2 != j2) {
+        yHiDHiDHiy_gg += delta * delta * (d_Hiy_i1 * d_Hi_j1j2 * d_Hiy_i2);
+        yHiDHiDHiy_ee += (d_Hiy_i1 * d_Hi_j1j2 * d_Hiy_i2);
+        yHiDHiDHiy_ge += delta * (d_Hiy_i1 * d_Hi_j1j2 * d_Hiy_i2);
+      }
+    } else {
+      yHiDHiDHiy_gg += delta * delta * (d_Hiy_i1 * d_Hi_j1i2 * d_Hiy_j2 +
+                                        d_Hiy_j1 * d_Hi_i1i2 * d_Hiy_j2);
+      yHiDHiDHiy_ee +=
+          (d_Hiy_i1 * d_Hi_j1i2 * d_Hiy_j2 + d_Hiy_j1 * d_Hi_i1i2 * d_Hiy_j2);
+      yHiDHiDHiy_ge += delta * (d_Hiy_i1 * d_Hi_j1i2 * d_Hiy_j2 +
+                                d_Hiy_j1 * d_Hi_i1i2 * d_Hiy_j2);
+
+      if (i2 != j2) {
+        yHiDHiDHiy_gg += delta * delta * (d_Hiy_i1 * d_Hi_j1j2 * d_Hiy_i2 +
+                                          d_Hiy_j1 * d_Hi_i1j2 * d_Hiy_i2);
+        yHiDHiDHiy_ee +=
+            (d_Hiy_i1 * d_Hi_j1j2 * d_Hiy_i2 + d_Hiy_j1 * d_Hi_i1j2 * d_Hiy_i2);
+        yHiDHiDHiy_ge += delta * (d_Hiy_i1 * d_Hi_j1j2 * d_Hiy_i2 +
+                                  d_Hiy_j1 * d_Hi_i1j2 * d_Hiy_i2);
+      }
+    }
+  }
+
+  return;
 }
 
-void Calc_xHiDHiDHiy (const gsl_vector *eval, const gsl_matrix *Hi,
-		      const gsl_matrix *xHi, const gsl_matrix *Hiy,
-		      const size_t i1, const size_t j1, const size_t i2,
-		      const size_t j2, gsl_vector *xHiDHiDHiy_gg,
-		      gsl_vector *xHiDHiDHiy_ee, gsl_vector *xHiDHiDHiy_ge) {
-	gsl_vector_set_zero(xHiDHiDHiy_gg);
-	gsl_vector_set_zero(xHiDHiDHiy_ee);
-	gsl_vector_set_zero(xHiDHiDHiy_ge);
-
-	size_t n_size=eval->size, d_size=Hiy->size1;
-
-	double delta, d_Hiy_i, d_Hiy_j, d_Hi_i1i2, d_Hi_i1j2;
-	double d_Hi_j1i2, d_Hi_j1j2;
-
-	for (size_t k=0; k<n_size; k++) {
-	  delta=gsl_vector_get (eval, k);
-	  
-	  gsl_vector_const_view xHi_col_i=
-	    gsl_matrix_const_column (xHi, k*d_size+i1);
-	  gsl_vector_const_view xHi_col_j=
-	    gsl_matrix_const_column (xHi, k*d_size+j1);
-	  
-	  d_Hiy_i=gsl_matrix_get (Hiy, i2, k);
-	  d_Hiy_j=gsl_matrix_get (Hiy, j2, k);
-	  
-	  d_Hi_i1i2=gsl_matrix_get (Hi, i1, k*d_size+i2);
-	  d_Hi_i1j2=gsl_matrix_get (Hi, i1, k*d_size+j2);
-	  d_Hi_j1i2=gsl_matrix_get (Hi, j1, k*d_size+i2);
-	  d_Hi_j1j2=gsl_matrix_get (Hi, j1, k*d_size+j2);
-	  
-	  if (i1==j1) {
-	    gsl_blas_daxpy (delta*delta*d_Hi_j1i2*d_Hiy_j, &xHi_col_i.vector,
-			    xHiDHiDHiy_gg);
-	    gsl_blas_daxpy (d_Hi_j1i2*d_Hiy_j, &xHi_col_i.vector,
-			    xHiDHiDHiy_ee);
-	    gsl_blas_daxpy (delta*d_Hi_j1i2*d_Hiy_j, &xHi_col_i.vector,
-			    xHiDHiDHiy_ge);
-		  
-	    if (i2!=j2) {
-	      gsl_blas_daxpy (delta*delta*d_Hi_j1j2*d_Hiy_i,
-			      &xHi_col_i.vector, xHiDHiDHiy_gg);
-	      gsl_blas_daxpy (d_Hi_j1j2*d_Hiy_i, &xHi_col_i.vector,
-			      xHiDHiDHiy_ee);
-	      gsl_blas_daxpy (delta*d_Hi_j1j2*d_Hiy_i, &xHi_col_i.vector,
-			      xHiDHiDHiy_ge);
-	    }
-	  } else {
-	    gsl_blas_daxpy (delta*delta*d_Hi_j1i2*d_Hiy_j, &xHi_col_i.vector,
-			    xHiDHiDHiy_gg);
-	    gsl_blas_daxpy (d_Hi_j1i2*d_Hiy_j, &xHi_col_i.vector,
-			    xHiDHiDHiy_ee);
-	    gsl_blas_daxpy (delta*d_Hi_j1i2*d_Hiy_j, &xHi_col_i.vector,
-			    xHiDHiDHiy_ge);
-	    
-	    gsl_blas_daxpy (delta*delta*d_Hi_i1i2*d_Hiy_j, &xHi_col_j.vector,
-			    xHiDHiDHiy_gg);
-	    gsl_blas_daxpy (d_Hi_i1i2*d_Hiy_j, &xHi_col_j.vector,
-			    xHiDHiDHiy_ee);
-	    gsl_blas_daxpy (delta*d_Hi_i1i2*d_Hiy_j, &xHi_col_j.vector,
-			    xHiDHiDHiy_ge);
-	    
-	    if (i2!=j2) {
-	      gsl_blas_daxpy (delta*delta*d_Hi_j1j2*d_Hiy_i,
-			      &xHi_col_i.vector, xHiDHiDHiy_gg);
-	      gsl_blas_daxpy (d_Hi_j1j2*d_Hiy_i, &xHi_col_i.vector,
-			      xHiDHiDHiy_ee);
-	      gsl_blas_daxpy (delta*d_Hi_j1j2*d_Hiy_i, &xHi_col_i.vector,
-			      xHiDHiDHiy_ge);
-	      
-	      gsl_blas_daxpy (delta*delta*d_Hi_i1j2*d_Hiy_i,
-			      &xHi_col_j.vector, xHiDHiDHiy_gg);
-	      gsl_blas_daxpy (d_Hi_i1j2*d_Hiy_i, &xHi_col_j.vector,
-			      xHiDHiDHiy_ee);
-	      gsl_blas_daxpy (delta*d_Hi_i1j2*d_Hiy_i, &xHi_col_j.vector,
-			      xHiDHiDHiy_ge);
-	    }
-	  }
-	}
-	
-	return;
+void Calc_xHiDHiDHiy(const gsl_vector *eval, const gsl_matrix *Hi,
+                     const gsl_matrix *xHi, const gsl_matrix *Hiy,
+                     const size_t i1, const size_t j1, const size_t i2,
+                     const size_t j2, gsl_vector *xHiDHiDHiy_gg,
+                     gsl_vector *xHiDHiDHiy_ee, gsl_vector *xHiDHiDHiy_ge) {
+  gsl_vector_set_zero(xHiDHiDHiy_gg);
+  gsl_vector_set_zero(xHiDHiDHiy_ee);
+  gsl_vector_set_zero(xHiDHiDHiy_ge);
+
+  size_t n_size = eval->size, d_size = Hiy->size1;
+
+  double delta, d_Hiy_i, d_Hiy_j, d_Hi_i1i2, d_Hi_i1j2;
+  double d_Hi_j1i2, d_Hi_j1j2;
+
+  for (size_t k = 0; k < n_size; k++) {
+    delta = gsl_vector_get(eval, k);
+
+    gsl_vector_const_view xHi_col_i =
+        gsl_matrix_const_column(xHi, k * d_size + i1);
+    gsl_vector_const_view xHi_col_j =
+        gsl_matrix_const_column(xHi, k * d_size + j1);
+
+    d_Hiy_i = gsl_matrix_get(Hiy, i2, k);
+    d_Hiy_j = gsl_matrix_get(Hiy, j2, k);
+
+    d_Hi_i1i2 = gsl_matrix_get(Hi, i1, k * d_size + i2);
+    d_Hi_i1j2 = gsl_matrix_get(Hi, i1, k * d_size + j2);
+    d_Hi_j1i2 = gsl_matrix_get(Hi, j1, k * d_size + i2);
+    d_Hi_j1j2 = gsl_matrix_get(Hi, j1, k * d_size + j2);
+
+    if (i1 == j1) {
+      gsl_blas_daxpy(delta * delta * d_Hi_j1i2 * d_Hiy_j, &xHi_col_i.vector,
+                     xHiDHiDHiy_gg);
+      gsl_blas_daxpy(d_Hi_j1i2 * d_Hiy_j, &xHi_col_i.vector, xHiDHiDHiy_ee);
+      gsl_blas_daxpy(delta * d_Hi_j1i2 * d_Hiy_j, &xHi_col_i.vector,
+                     xHiDHiDHiy_ge);
+
+      if (i2 != j2) {
+        gsl_blas_daxpy(delta * delta * d_Hi_j1j2 * d_Hiy_i, &xHi_col_i.vector,
+                       xHiDHiDHiy_gg);
+        gsl_blas_daxpy(d_Hi_j1j2 * d_Hiy_i, &xHi_col_i.vector, xHiDHiDHiy_ee);
+        gsl_blas_daxpy(delta * d_Hi_j1j2 * d_Hiy_i, &xHi_col_i.vector,
+                       xHiDHiDHiy_ge);
+      }
+    } else {
+      gsl_blas_daxpy(delta * delta * d_Hi_j1i2 * d_Hiy_j, &xHi_col_i.vector,
+                     xHiDHiDHiy_gg);
+      gsl_blas_daxpy(d_Hi_j1i2 * d_Hiy_j, &xHi_col_i.vector, xHiDHiDHiy_ee);
+      gsl_blas_daxpy(delta * d_Hi_j1i2 * d_Hiy_j, &xHi_col_i.vector,
+                     xHiDHiDHiy_ge);
+
+      gsl_blas_daxpy(delta * delta * d_Hi_i1i2 * d_Hiy_j, &xHi_col_j.vector,
+                     xHiDHiDHiy_gg);
+      gsl_blas_daxpy(d_Hi_i1i2 * d_Hiy_j, &xHi_col_j.vector, xHiDHiDHiy_ee);
+      gsl_blas_daxpy(delta * d_Hi_i1i2 * d_Hiy_j, &xHi_col_j.vector,
+                     xHiDHiDHiy_ge);
+
+      if (i2 != j2) {
+        gsl_blas_daxpy(delta * delta * d_Hi_j1j2 * d_Hiy_i, &xHi_col_i.vector,
+                       xHiDHiDHiy_gg);
+        gsl_blas_daxpy(d_Hi_j1j2 * d_Hiy_i, &xHi_col_i.vector, xHiDHiDHiy_ee);
+        gsl_blas_daxpy(delta * d_Hi_j1j2 * d_Hiy_i, &xHi_col_i.vector,
+                       xHiDHiDHiy_ge);
+
+        gsl_blas_daxpy(delta * delta * d_Hi_i1j2 * d_Hiy_i, &xHi_col_j.vector,
+                       xHiDHiDHiy_gg);
+        gsl_blas_daxpy(d_Hi_i1j2 * d_Hiy_i, &xHi_col_j.vector, xHiDHiDHiy_ee);
+        gsl_blas_daxpy(delta * d_Hi_i1j2 * d_Hiy_i, &xHi_col_j.vector,
+                       xHiDHiDHiy_ge);
+      }
+    }
+  }
+
+  return;
 }
 
+void Calc_xHiDHiDHix(const gsl_vector *eval, const gsl_matrix *Hi,
+                     const gsl_matrix *xHi, const size_t i1, const size_t j1,
+                     const size_t i2, const size_t j2,
+                     gsl_matrix *xHiDHiDHix_gg, gsl_matrix *xHiDHiDHix_ee,
+                     gsl_matrix *xHiDHiDHix_ge) {
+  gsl_matrix_set_zero(xHiDHiDHix_gg);
+  gsl_matrix_set_zero(xHiDHiDHix_ee);
+  gsl_matrix_set_zero(xHiDHiDHix_ge);
+
+  size_t n_size = eval->size, d_size = Hi->size1, dc_size = xHi->size1;
+
+  double delta, d_Hi_i1i2, d_Hi_i1j2, d_Hi_j1i2, d_Hi_j1j2;
+
+  gsl_matrix *mat_dcdc = gsl_matrix_alloc(dc_size, dc_size);
+
+  for (size_t k = 0; k < n_size; k++) {
+    delta = gsl_vector_get(eval, k);
+
+    gsl_vector_const_view xHi_col_i1 =
+        gsl_matrix_const_column(xHi, k * d_size + i1);
+    gsl_vector_const_view xHi_col_j1 =
+        gsl_matrix_const_column(xHi, k * d_size + j1);
+    gsl_vector_const_view xHi_col_i2 =
+        gsl_matrix_const_column(xHi, k * d_size + i2);
+    gsl_vector_const_view xHi_col_j2 =
+        gsl_matrix_const_column(xHi, k * d_size + j2);
+
+    d_Hi_i1i2 = gsl_matrix_get(Hi, i1, k * d_size + i2);
+    d_Hi_i1j2 = gsl_matrix_get(Hi, i1, k * d_size + j2);
+    d_Hi_j1i2 = gsl_matrix_get(Hi, j1, k * d_size + i2);
+    d_Hi_j1j2 = gsl_matrix_get(Hi, j1, k * d_size + j2);
+
+    if (i1 == j1) {
+      gsl_matrix_set_zero(mat_dcdc);
+      gsl_blas_dger(d_Hi_j1i2, &xHi_col_i1.vector, &xHi_col_j2.vector,
+                    mat_dcdc);
+
+      gsl_matrix_add(xHiDHiDHix_ee, mat_dcdc);
+      gsl_matrix_scale(mat_dcdc, delta);
+      gsl_matrix_add(xHiDHiDHix_ge, mat_dcdc);
+      gsl_matrix_scale(mat_dcdc, delta);
+      gsl_matrix_add(xHiDHiDHix_gg, mat_dcdc);
+
+      if (i2 != j2) {
+        gsl_matrix_set_zero(mat_dcdc);
+        gsl_blas_dger(d_Hi_j1j2, &xHi_col_i1.vector, &xHi_col_i2.vector,
+                      mat_dcdc);
+
+        gsl_matrix_add(xHiDHiDHix_ee, mat_dcdc);
+        gsl_matrix_scale(mat_dcdc, delta);
+        gsl_matrix_add(xHiDHiDHix_ge, mat_dcdc);
+        gsl_matrix_scale(mat_dcdc, delta);
+        gsl_matrix_add(xHiDHiDHix_gg, mat_dcdc);
+      }
+    } else {
+      gsl_matrix_set_zero(mat_dcdc);
+      gsl_blas_dger(d_Hi_j1i2, &xHi_col_i1.vector, &xHi_col_j2.vector,
+                    mat_dcdc);
+
+      gsl_matrix_add(xHiDHiDHix_ee, mat_dcdc);
+      gsl_matrix_scale(mat_dcdc, delta);
+      gsl_matrix_add(xHiDHiDHix_ge, mat_dcdc);
+      gsl_matrix_scale(mat_dcdc, delta);
+      gsl_matrix_add(xHiDHiDHix_gg, mat_dcdc);
+
+      gsl_matrix_set_zero(mat_dcdc);
+      gsl_blas_dger(d_Hi_i1i2, &xHi_col_j1.vector, &xHi_col_j2.vector,
+                    mat_dcdc);
+
+      gsl_matrix_add(xHiDHiDHix_ee, mat_dcdc);
+      gsl_matrix_scale(mat_dcdc, delta);
+      gsl_matrix_add(xHiDHiDHix_ge, mat_dcdc);
+      gsl_matrix_scale(mat_dcdc, delta);
+      gsl_matrix_add(xHiDHiDHix_gg, mat_dcdc);
+
+      if (i2 != j2) {
+        gsl_matrix_set_zero(mat_dcdc);
+        gsl_blas_dger(d_Hi_j1j2, &xHi_col_i1.vector, &xHi_col_i2.vector,
+                      mat_dcdc);
+
+        gsl_matrix_add(xHiDHiDHix_ee, mat_dcdc);
+        gsl_matrix_scale(mat_dcdc, delta);
+        gsl_matrix_add(xHiDHiDHix_ge, mat_dcdc);
+        gsl_matrix_scale(mat_dcdc, delta);
+        gsl_matrix_add(xHiDHiDHix_gg, mat_dcdc);
+
+        gsl_matrix_set_zero(mat_dcdc);
+        gsl_blas_dger(d_Hi_i1j2, &xHi_col_j1.vector, &xHi_col_i2.vector,
+                      mat_dcdc);
+
+        gsl_matrix_add(xHiDHiDHix_ee, mat_dcdc);
+        gsl_matrix_scale(mat_dcdc, delta);
+        gsl_matrix_add(xHiDHiDHix_ge, mat_dcdc);
+        gsl_matrix_scale(mat_dcdc, delta);
+        gsl_matrix_add(xHiDHiDHix_gg, mat_dcdc);
+      }
+    }
+  }
+
+  gsl_matrix_free(mat_dcdc);
 
-void Calc_xHiDHiDHix (const gsl_vector *eval, const gsl_matrix *Hi,
-		      const gsl_matrix *xHi, const size_t i1, const size_t j1,
-		      const size_t i2, const size_t j2,
-		      gsl_matrix *xHiDHiDHix_gg, gsl_matrix *xHiDHiDHix_ee,
-		      gsl_matrix *xHiDHiDHix_ge) {
-	gsl_matrix_set_zero(xHiDHiDHix_gg);
-	gsl_matrix_set_zero(xHiDHiDHix_ee);
-	gsl_matrix_set_zero(xHiDHiDHix_ge);
-
-	size_t n_size=eval->size, d_size=Hi->size1, dc_size=xHi->size1;
-
-	double delta, d_Hi_i1i2, d_Hi_i1j2, d_Hi_j1i2, d_Hi_j1j2;
-
-	gsl_matrix *mat_dcdc=gsl_matrix_alloc (dc_size, dc_size);
-
-	for (size_t k=0; k<n_size; k++) {
-		delta=gsl_vector_get (eval, k);
-
-		gsl_vector_const_view xHi_col_i1=
-		  gsl_matrix_const_column (xHi, k*d_size+i1);
-		gsl_vector_const_view xHi_col_j1=
-		  gsl_matrix_const_column (xHi, k*d_size+j1);
-		gsl_vector_const_view xHi_col_i2=
-		  gsl_matrix_const_column (xHi, k*d_size+i2);
-		gsl_vector_const_view xHi_col_j2=
-		  gsl_matrix_const_column (xHi, k*d_size+j2);
-
-		d_Hi_i1i2=gsl_matrix_get (Hi, i1, k*d_size+i2);
-		d_Hi_i1j2=gsl_matrix_get (Hi, i1, k*d_size+j2);
-		d_Hi_j1i2=gsl_matrix_get (Hi, j1, k*d_size+i2);
-		d_Hi_j1j2=gsl_matrix_get (Hi, j1, k*d_size+j2);
-
-		if (i1==j1) {
-		  gsl_matrix_set_zero (mat_dcdc);
-		  gsl_blas_dger (d_Hi_j1i2, &xHi_col_i1.vector,
-				 &xHi_col_j2.vector, mat_dcdc);
-
-		  gsl_matrix_add(xHiDHiDHix_ee, mat_dcdc);
-		  gsl_matrix_scale(mat_dcdc, delta);
-		  gsl_matrix_add(xHiDHiDHix_ge, mat_dcdc);
-		  gsl_matrix_scale(mat_dcdc, delta);
-		  gsl_matrix_add(xHiDHiDHix_gg, mat_dcdc);
-		  
-		  if (i2!=j2) {
-		    gsl_matrix_set_zero (mat_dcdc);
-		    gsl_blas_dger (d_Hi_j1j2, &xHi_col_i1.vector,
-				   &xHi_col_i2.vector, mat_dcdc);
-		    
-		    gsl_matrix_add(xHiDHiDHix_ee, mat_dcdc);
-		    gsl_matrix_scale(mat_dcdc, delta);
-		    gsl_matrix_add(xHiDHiDHix_ge, mat_dcdc);
-		    gsl_matrix_scale(mat_dcdc, delta);
-		    gsl_matrix_add(xHiDHiDHix_gg, mat_dcdc);
-		  }
-		} else {
-		  gsl_matrix_set_zero (mat_dcdc);
-		  gsl_blas_dger (d_Hi_j1i2, &xHi_col_i1.vector,
-				 &xHi_col_j2.vector, mat_dcdc);
-		  
-		  gsl_matrix_add(xHiDHiDHix_ee, mat_dcdc);
-		  gsl_matrix_scale(mat_dcdc, delta);
-		  gsl_matrix_add(xHiDHiDHix_ge, mat_dcdc);
-		  gsl_matrix_scale(mat_dcdc, delta);
-		  gsl_matrix_add(xHiDHiDHix_gg, mat_dcdc);
-		  
-		  gsl_matrix_set_zero (mat_dcdc);
-		  gsl_blas_dger (d_Hi_i1i2, &xHi_col_j1.vector,
-				 &xHi_col_j2.vector, mat_dcdc);
-
-			gsl_matrix_add(xHiDHiDHix_ee, mat_dcdc);
-			gsl_matrix_scale(mat_dcdc, delta);
-			gsl_matrix_add(xHiDHiDHix_ge, mat_dcdc);
-			gsl_matrix_scale(mat_dcdc, delta);
-			gsl_matrix_add(xHiDHiDHix_gg, mat_dcdc);
-
-			if (i2!=j2) {
-			  gsl_matrix_set_zero (mat_dcdc);
-			  gsl_blas_dger (d_Hi_j1j2, &xHi_col_i1.vector,
-					 &xHi_col_i2.vector, mat_dcdc);
-			  
-			  gsl_matrix_add(xHiDHiDHix_ee, mat_dcdc);
-			  gsl_matrix_scale(mat_dcdc, delta);
-			  gsl_matrix_add(xHiDHiDHix_ge, mat_dcdc);
-			  gsl_matrix_scale(mat_dcdc, delta);
-			  gsl_matrix_add(xHiDHiDHix_gg, mat_dcdc);
-			  
-			  gsl_matrix_set_zero (mat_dcdc);
-			  gsl_blas_dger (d_Hi_i1j2, &xHi_col_j1.vector,
-					 &xHi_col_i2.vector, mat_dcdc);
-			  
-			  gsl_matrix_add(xHiDHiDHix_ee, mat_dcdc);
-			  gsl_matrix_scale(mat_dcdc, delta);
-			  gsl_matrix_add(xHiDHiDHix_ge, mat_dcdc);
-			  gsl_matrix_scale(mat_dcdc, delta);
-			  gsl_matrix_add(xHiDHiDHix_gg, mat_dcdc);
-			}
-		}
-	}
-
-	gsl_matrix_free(mat_dcdc);
-
-	return;
+  return;
 }
 
-void Calc_traceHiD (const gsl_vector *eval, const gsl_matrix *Hi,
-		    const size_t i, const size_t j, double &tHiD_g,
-		    double &tHiD_e) {
-	tHiD_g=0.0;
-	tHiD_e=0.0;
-
-	size_t n_size=eval->size, d_size=Hi->size1;
-	double delta, d;
-
-	for (size_t k=0; k<n_size; k++) {
-		delta=gsl_vector_get (eval, k);
-		d=gsl_matrix_get (Hi, j, k*d_size+i);
-
-		if (i==j) {
-			tHiD_g+=delta*d;
-			tHiD_e+=d;
-		} else {
-			tHiD_g+=delta*d*2.0;
-			tHiD_e+=d*2.0;
-		}
-	}
-
-	return;
+void Calc_traceHiD(const gsl_vector *eval, const gsl_matrix *Hi, const size_t i,
+                   const size_t j, double &tHiD_g, double &tHiD_e) {
+  tHiD_g = 0.0;
+  tHiD_e = 0.0;
+
+  size_t n_size = eval->size, d_size = Hi->size1;
+  double delta, d;
+
+  for (size_t k = 0; k < n_size; k++) {
+    delta = gsl_vector_get(eval, k);
+    d = gsl_matrix_get(Hi, j, k * d_size + i);
+
+    if (i == j) {
+      tHiD_g += delta * d;
+      tHiD_e += d;
+    } else {
+      tHiD_g += delta * d * 2.0;
+      tHiD_e += d * 2.0;
+    }
+  }
+
+  return;
 }
 
-void Calc_traceHiDHiD (const gsl_vector *eval, const gsl_matrix *Hi,
-		       const size_t i1, const size_t j1, const size_t i2,
-		       const size_t j2, double &tHiDHiD_gg, double &tHiDHiD_ee,
-		       double &tHiDHiD_ge) {
-	tHiDHiD_gg=0.0;
-	tHiDHiD_ee=0.0;
-	tHiDHiD_ge=0.0;
-
-	size_t n_size=eval->size, d_size=Hi->size1;
-	double delta, d_Hi_i1i2, d_Hi_i1j2, d_Hi_j1i2, d_Hi_j1j2;
-
-	for (size_t k=0; k<n_size; k++) {
-		delta=gsl_vector_get (eval, k);
-
-		d_Hi_i1i2=gsl_matrix_get (Hi, i1, k*d_size+i2);
-		d_Hi_i1j2=gsl_matrix_get (Hi, i1, k*d_size+j2);
-		d_Hi_j1i2=gsl_matrix_get (Hi, j1, k*d_size+i2);
-		d_Hi_j1j2=gsl_matrix_get (Hi, j1, k*d_size+j2);
-
-		if (i1==j1) {
-		  tHiDHiD_gg+=delta*delta*d_Hi_i1j2*d_Hi_j1i2;
-		  tHiDHiD_ee+=d_Hi_i1j2*d_Hi_j1i2;
-		  tHiDHiD_ge+=delta*d_Hi_i1j2*d_Hi_j1i2;
-		  
-		  if (i2!=j2) {
-		    tHiDHiD_gg+=delta*delta*d_Hi_i1i2*d_Hi_j1j2;
-		    tHiDHiD_ee+=d_Hi_i1i2*d_Hi_j1j2;
-		    tHiDHiD_ge+=delta*d_Hi_i1i2*d_Hi_j1j2;
-		  }
-		} else {
-		  tHiDHiD_gg+=delta*delta*(d_Hi_i1j2*d_Hi_j1i2+d_Hi_j1j2*
-					   d_Hi_i1i2);
-		  tHiDHiD_ee+=(d_Hi_i1j2*d_Hi_j1i2+d_Hi_j1j2*d_Hi_i1i2);
-		  tHiDHiD_ge+=delta*(d_Hi_i1j2*d_Hi_j1i2+d_Hi_j1j2*d_Hi_i1i2);
-		  
-		  if (i2!=j2) {
-		    tHiDHiD_gg+=delta*delta*(d_Hi_i1i2*d_Hi_j1j2+d_Hi_j1i2*
-					     d_Hi_i1j2);
-		    tHiDHiD_ee+=(d_Hi_i1i2*d_Hi_j1j2+d_Hi_j1i2*d_Hi_i1j2);
-		    tHiDHiD_ge+=delta*(d_Hi_i1i2*d_Hi_j1j2 +
-				       d_Hi_j1i2*d_Hi_i1j2);
-		  }
-		}
-	}
-
-	return;
+void Calc_traceHiDHiD(const gsl_vector *eval, const gsl_matrix *Hi,
+                      const size_t i1, const size_t j1, const size_t i2,
+                      const size_t j2, double &tHiDHiD_gg, double &tHiDHiD_ee,
+                      double &tHiDHiD_ge) {
+  tHiDHiD_gg = 0.0;
+  tHiDHiD_ee = 0.0;
+  tHiDHiD_ge = 0.0;
+
+  size_t n_size = eval->size, d_size = Hi->size1;
+  double delta, d_Hi_i1i2, d_Hi_i1j2, d_Hi_j1i2, d_Hi_j1j2;
+
+  for (size_t k = 0; k < n_size; k++) {
+    delta = gsl_vector_get(eval, k);
+
+    d_Hi_i1i2 = gsl_matrix_get(Hi, i1, k * d_size + i2);
+    d_Hi_i1j2 = gsl_matrix_get(Hi, i1, k * d_size + j2);
+    d_Hi_j1i2 = gsl_matrix_get(Hi, j1, k * d_size + i2);
+    d_Hi_j1j2 = gsl_matrix_get(Hi, j1, k * d_size + j2);
+
+    if (i1 == j1) {
+      tHiDHiD_gg += delta * delta * d_Hi_i1j2 * d_Hi_j1i2;
+      tHiDHiD_ee += d_Hi_i1j2 * d_Hi_j1i2;
+      tHiDHiD_ge += delta * d_Hi_i1j2 * d_Hi_j1i2;
+
+      if (i2 != j2) {
+        tHiDHiD_gg += delta * delta * d_Hi_i1i2 * d_Hi_j1j2;
+        tHiDHiD_ee += d_Hi_i1i2 * d_Hi_j1j2;
+        tHiDHiD_ge += delta * d_Hi_i1i2 * d_Hi_j1j2;
+      }
+    } else {
+      tHiDHiD_gg +=
+          delta * delta * (d_Hi_i1j2 * d_Hi_j1i2 + d_Hi_j1j2 * d_Hi_i1i2);
+      tHiDHiD_ee += (d_Hi_i1j2 * d_Hi_j1i2 + d_Hi_j1j2 * d_Hi_i1i2);
+      tHiDHiD_ge += delta * (d_Hi_i1j2 * d_Hi_j1i2 + d_Hi_j1j2 * d_Hi_i1i2);
+
+      if (i2 != j2) {
+        tHiDHiD_gg +=
+            delta * delta * (d_Hi_i1i2 * d_Hi_j1j2 + d_Hi_j1i2 * d_Hi_i1j2);
+        tHiDHiD_ee += (d_Hi_i1i2 * d_Hi_j1j2 + d_Hi_j1i2 * d_Hi_i1j2);
+        tHiDHiD_ge += delta * (d_Hi_i1i2 * d_Hi_j1j2 + d_Hi_j1i2 * d_Hi_i1j2);
+      }
+    }
+  }
+
+  return;
 }
 
 // trace(PD) = trace((Hi-HixQixHi)D)=trace(HiD) - trace(HixQixHiD)
-void Calc_tracePD (const gsl_vector *eval, const gsl_matrix *Qi,
-		   const gsl_matrix *Hi, const gsl_matrix *xHiDHix_all_g,
-		   const gsl_matrix *xHiDHix_all_e, const size_t i,
-		   const size_t j, double &tPD_g, double &tPD_e) {
-	size_t dc_size=Qi->size1, d_size=Hi->size1;
-	size_t v=GetIndex(i, j, d_size);
-
-	double d;
-
-	// Calculate the first part: trace(HiD).
-	Calc_traceHiD (eval, Hi, i, j, tPD_g, tPD_e);
-
-	// Calculate the second part: -trace(HixQixHiD).
-	for (size_t k=0; k<dc_size; k++) {
-		gsl_vector_const_view Qi_row=gsl_matrix_const_row (Qi, k);
-		gsl_vector_const_view xHiDHix_g_col =
-		  gsl_matrix_const_column (xHiDHix_all_g, v*dc_size+k);
-		gsl_vector_const_view xHiDHix_e_col =
-		  gsl_matrix_const_column (xHiDHix_all_e, v*dc_size+k);
-
-		gsl_blas_ddot(&Qi_row.vector, &xHiDHix_g_col.vector, &d);
-		tPD_g-=d;
-		gsl_blas_ddot(&Qi_row.vector, &xHiDHix_e_col.vector, &d);
-		tPD_e-=d;
-	}
-
-	return;
+void Calc_tracePD(const gsl_vector *eval, const gsl_matrix *Qi,
+                  const gsl_matrix *Hi, const gsl_matrix *xHiDHix_all_g,
+                  const gsl_matrix *xHiDHix_all_e, const size_t i,
+                  const size_t j, double &tPD_g, double &tPD_e) {
+  size_t dc_size = Qi->size1, d_size = Hi->size1;
+  size_t v = GetIndex(i, j, d_size);
+
+  double d;
+
+  // Calculate the first part: trace(HiD).
+  Calc_traceHiD(eval, Hi, i, j, tPD_g, tPD_e);
+
+  // Calculate the second part: -trace(HixQixHiD).
+  for (size_t k = 0; k < dc_size; k++) {
+    gsl_vector_const_view Qi_row = gsl_matrix_const_row(Qi, k);
+    gsl_vector_const_view xHiDHix_g_col =
+        gsl_matrix_const_column(xHiDHix_all_g, v * dc_size + k);
+    gsl_vector_const_view xHiDHix_e_col =
+        gsl_matrix_const_column(xHiDHix_all_e, v * dc_size + k);
+
+    gsl_blas_ddot(&Qi_row.vector, &xHiDHix_g_col.vector, &d);
+    tPD_g -= d;
+    gsl_blas_ddot(&Qi_row.vector, &xHiDHix_e_col.vector, &d);
+    tPD_e -= d;
+  }
+
+  return;
 }
 
 // trace(PDPD) = trace((Hi-HixQixHi)D(Hi-HixQixHi)D)
 //             = trace(HiDHiD) - trace(HixQixHiDHiD)
 //               - trace(HiDHixQixHiD) + trace(HixQixHiDHixQixHiD)
-void Calc_tracePDPD (const gsl_vector *eval, const gsl_matrix *Qi,
-		     const gsl_matrix *Hi, const gsl_matrix *xHi,
-		     const gsl_matrix *QixHiDHix_all_g,
-		     const gsl_matrix *QixHiDHix_all_e,
-		     const gsl_matrix *xHiDHiDHix_all_gg,
-		     const gsl_matrix *xHiDHiDHix_all_ee,
-		     const gsl_matrix *xHiDHiDHix_all_ge,
-		     const size_t i1, const size_t j1,
-		     const size_t i2, const size_t j2,
-		     double &tPDPD_gg, double &tPDPD_ee,
-		     double &tPDPD_ge) {
-	size_t dc_size=Qi->size1, d_size=Hi->size1;
-	size_t v_size=d_size*(d_size+1)/2;
-	size_t v1=GetIndex(i1, j1, d_size), v2=GetIndex(i2, j2, d_size);
-
-	double d;
-
-	// Calculate the first part: trace(HiDHiD).
-	Calc_traceHiDHiD (eval, Hi, i1, j1, i2, j2, tPDPD_gg, tPDPD_ee,
-			  tPDPD_ge);
-
-	// Calculate the second and third parts:
-	// -trace(HixQixHiDHiD) - trace(HiDHixQixHiD)
-	for (size_t i=0; i<dc_size; i++) {
-	  gsl_vector_const_view Qi_row=gsl_matrix_const_row (Qi, i);
-	  gsl_vector_const_view xHiDHiDHix_gg_col=
-	    gsl_matrix_const_column(xHiDHiDHix_all_gg,
-				    (v1*v_size+v2)*dc_size+i);
-	  gsl_vector_const_view xHiDHiDHix_ee_col =
-	    gsl_matrix_const_column(xHiDHiDHix_all_ee,
-				    (v1*v_size+v2)*dc_size+i);
-	  gsl_vector_const_view xHiDHiDHix_ge_col =
-	    gsl_matrix_const_column(xHiDHiDHix_all_ge,
-				    (v1*v_size+v2)*dc_size+i);
-	  
-	  gsl_blas_ddot(&Qi_row.vector, &xHiDHiDHix_gg_col.vector, &d);
-	  tPDPD_gg-=d*2.0;
-	  gsl_blas_ddot(&Qi_row.vector, &xHiDHiDHix_ee_col.vector, &d);
-	  tPDPD_ee-=d*2.0;
-	  gsl_blas_ddot(&Qi_row.vector, &xHiDHiDHix_ge_col.vector, &d);
-	  tPDPD_ge-=d*2.0;
-	}
-
-	// Calculate the fourth part: trace(HixQixHiDHixQixHiD).
-	for (size_t i=0; i<dc_size; i++) {
-
-	  gsl_vector_const_view QixHiDHix_g_fullrow1 =
-	    gsl_matrix_const_row (QixHiDHix_all_g, i);
-	  gsl_vector_const_view QixHiDHix_e_fullrow1 =
-	    gsl_matrix_const_row (QixHiDHix_all_e, i);
-	  gsl_vector_const_view QixHiDHix_g_row1 =
-	    gsl_vector_const_subvector (&QixHiDHix_g_fullrow1.vector,
-					v1*dc_size, dc_size);
-	  gsl_vector_const_view QixHiDHix_e_row1 =
-	    gsl_vector_const_subvector (&QixHiDHix_e_fullrow1.vector,
-					v1*dc_size, dc_size);
-	  
-	  gsl_vector_const_view QixHiDHix_g_col2 =
-	    gsl_matrix_const_column (QixHiDHix_all_g, v2*dc_size+i);
-	  gsl_vector_const_view QixHiDHix_e_col2 =
-	    gsl_matrix_const_column (QixHiDHix_all_e, v2*dc_size+i);
-	  
-	  gsl_blas_ddot(&QixHiDHix_g_row1.vector,&QixHiDHix_g_col2.vector,&d);
-	  tPDPD_gg+=d;
-	  gsl_blas_ddot(&QixHiDHix_e_row1.vector,&QixHiDHix_e_col2.vector,&d);
-	  tPDPD_ee+=d;
-	  gsl_blas_ddot(&QixHiDHix_g_row1.vector,&QixHiDHix_e_col2.vector,&d);
-	  tPDPD_ge+=d;
-	}
-
-	return;
+void Calc_tracePDPD(const gsl_vector *eval, const gsl_matrix *Qi,
+                    const gsl_matrix *Hi, const gsl_matrix *xHi,
+                    const gsl_matrix *QixHiDHix_all_g,
+                    const gsl_matrix *QixHiDHix_all_e,
+                    const gsl_matrix *xHiDHiDHix_all_gg,
+                    const gsl_matrix *xHiDHiDHix_all_ee,
+                    const gsl_matrix *xHiDHiDHix_all_ge, const size_t i1,
+                    const size_t j1, const size_t i2, const size_t j2,
+                    double &tPDPD_gg, double &tPDPD_ee, double &tPDPD_ge) {
+  size_t dc_size = Qi->size1, d_size = Hi->size1;
+  size_t v_size = d_size * (d_size + 1) / 2;
+  size_t v1 = GetIndex(i1, j1, d_size), v2 = GetIndex(i2, j2, d_size);
+
+  double d;
+
+  // Calculate the first part: trace(HiDHiD).
+  Calc_traceHiDHiD(eval, Hi, i1, j1, i2, j2, tPDPD_gg, tPDPD_ee, tPDPD_ge);
+
+  // Calculate the second and third parts:
+  // -trace(HixQixHiDHiD) - trace(HiDHixQixHiD)
+  for (size_t i = 0; i < dc_size; i++) {
+    gsl_vector_const_view Qi_row = gsl_matrix_const_row(Qi, i);
+    gsl_vector_const_view xHiDHiDHix_gg_col = gsl_matrix_const_column(
+        xHiDHiDHix_all_gg, (v1 * v_size + v2) * dc_size + i);
+    gsl_vector_const_view xHiDHiDHix_ee_col = gsl_matrix_const_column(
+        xHiDHiDHix_all_ee, (v1 * v_size + v2) * dc_size + i);
+    gsl_vector_const_view xHiDHiDHix_ge_col = gsl_matrix_const_column(
+        xHiDHiDHix_all_ge, (v1 * v_size + v2) * dc_size + i);
+
+    gsl_blas_ddot(&Qi_row.vector, &xHiDHiDHix_gg_col.vector, &d);
+    tPDPD_gg -= d * 2.0;
+    gsl_blas_ddot(&Qi_row.vector, &xHiDHiDHix_ee_col.vector, &d);
+    tPDPD_ee -= d * 2.0;
+    gsl_blas_ddot(&Qi_row.vector, &xHiDHiDHix_ge_col.vector, &d);
+    tPDPD_ge -= d * 2.0;
+  }
+
+  // Calculate the fourth part: trace(HixQixHiDHixQixHiD).
+  for (size_t i = 0; i < dc_size; i++) {
+
+    gsl_vector_const_view QixHiDHix_g_fullrow1 =
+        gsl_matrix_const_row(QixHiDHix_all_g, i);
+    gsl_vector_const_view QixHiDHix_e_fullrow1 =
+        gsl_matrix_const_row(QixHiDHix_all_e, i);
+    gsl_vector_const_view QixHiDHix_g_row1 = gsl_vector_const_subvector(
+        &QixHiDHix_g_fullrow1.vector, v1 * dc_size, dc_size);
+    gsl_vector_const_view QixHiDHix_e_row1 = gsl_vector_const_subvector(
+        &QixHiDHix_e_fullrow1.vector, v1 * dc_size, dc_size);
+
+    gsl_vector_const_view QixHiDHix_g_col2 =
+        gsl_matrix_const_column(QixHiDHix_all_g, v2 * dc_size + i);
+    gsl_vector_const_view QixHiDHix_e_col2 =
+        gsl_matrix_const_column(QixHiDHix_all_e, v2 * dc_size + i);
+
+    gsl_blas_ddot(&QixHiDHix_g_row1.vector, &QixHiDHix_g_col2.vector, &d);
+    tPDPD_gg += d;
+    gsl_blas_ddot(&QixHiDHix_e_row1.vector, &QixHiDHix_e_col2.vector, &d);
+    tPDPD_ee += d;
+    gsl_blas_ddot(&QixHiDHix_g_row1.vector, &QixHiDHix_e_col2.vector, &d);
+    tPDPD_ge += d;
+  }
+
+  return;
 }
 
 // Calculate (xHiDHiy) for every pair (i,j).
-void Calc_xHiDHiy_all (const gsl_vector *eval, const gsl_matrix *xHi,
-		       const gsl_matrix *Hiy, gsl_matrix *xHiDHiy_all_g,
-		       gsl_matrix *xHiDHiy_all_e) {
-	gsl_matrix_set_zero(xHiDHiy_all_g);
-	gsl_matrix_set_zero(xHiDHiy_all_e);
-
-	size_t d_size=Hiy->size1;
-	size_t v;
-
-	for (size_t i=0; i<d_size; i++) {
-	  for (size_t j=0; j<d_size; j++) {
-	    if (j<i) {continue;}
-	    v=GetIndex(i, j, d_size);
-	    
-	    gsl_vector_view xHiDHiy_g=gsl_matrix_column (xHiDHiy_all_g, v);
-	    gsl_vector_view xHiDHiy_e=gsl_matrix_column (xHiDHiy_all_e, v);
-	    
-	    Calc_xHiDHiy (eval, xHi, Hiy, i, j, &xHiDHiy_g.vector,
-			  &xHiDHiy_e.vector);
-	  }
-	}
-	return;
+void Calc_xHiDHiy_all(const gsl_vector *eval, const gsl_matrix *xHi,
+                      const gsl_matrix *Hiy, gsl_matrix *xHiDHiy_all_g,
+                      gsl_matrix *xHiDHiy_all_e) {
+  gsl_matrix_set_zero(xHiDHiy_all_g);
+  gsl_matrix_set_zero(xHiDHiy_all_e);
+
+  size_t d_size = Hiy->size1;
+  size_t v;
+
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = 0; j < d_size; j++) {
+      if (j < i) {
+        continue;
+      }
+      v = GetIndex(i, j, d_size);
+
+      gsl_vector_view xHiDHiy_g = gsl_matrix_column(xHiDHiy_all_g, v);
+      gsl_vector_view xHiDHiy_e = gsl_matrix_column(xHiDHiy_all_e, v);
+
+      Calc_xHiDHiy(eval, xHi, Hiy, i, j, &xHiDHiy_g.vector, &xHiDHiy_e.vector);
+    }
+  }
+  return;
 }
 
 // Calculate (xHiDHix) for every pair (i,j).
-void Calc_xHiDHix_all (const gsl_vector *eval, const gsl_matrix *xHi,
-		       gsl_matrix *xHiDHix_all_g, gsl_matrix *xHiDHix_all_e) {
+void Calc_xHiDHix_all(const gsl_vector *eval, const gsl_matrix *xHi,
+                      gsl_matrix *xHiDHix_all_g, gsl_matrix *xHiDHix_all_e) {
   gsl_matrix_set_zero(xHiDHix_all_g);
   gsl_matrix_set_zero(xHiDHix_all_e);
-  
-  size_t d_size=xHi->size2/eval->size, dc_size=xHi->size1;
+
+  size_t d_size = xHi->size2 / eval->size, dc_size = xHi->size1;
   size_t v;
-  
-  for (size_t i=0; i<d_size; i++) {
-    for (size_t j=0; j<d_size; j++) {
-      if (j<i) {continue;}
-      v=GetIndex(i, j, d_size);
-      
+
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = 0; j < d_size; j++) {
+      if (j < i) {
+        continue;
+      }
+      v = GetIndex(i, j, d_size);
+
       gsl_matrix_view xHiDHix_g =
-	gsl_matrix_submatrix (xHiDHix_all_g, 0, v*dc_size, dc_size, dc_size);
+          gsl_matrix_submatrix(xHiDHix_all_g, 0, v * dc_size, dc_size, dc_size);
       gsl_matrix_view xHiDHix_e =
-	gsl_matrix_submatrix (xHiDHix_all_e, 0, v*dc_size, dc_size, dc_size);
-      
-      Calc_xHiDHix (eval, xHi, i, j, &xHiDHix_g.matrix, &xHiDHix_e.matrix);
+          gsl_matrix_submatrix(xHiDHix_all_e, 0, v * dc_size, dc_size, dc_size);
+
+      Calc_xHiDHix(eval, xHi, i, j, &xHiDHix_g.matrix, &xHiDHix_e.matrix);
     }
   }
   return;
 }
 
 // Calculate (xHiDHiy) for every pair (i,j).
-void Calc_xHiDHiDHiy_all (const size_t v_size, const gsl_vector *eval,
-			  const gsl_matrix *Hi, const gsl_matrix *xHi,
-			  const gsl_matrix *Hiy, gsl_matrix *xHiDHiDHiy_all_gg,
-			  gsl_matrix *xHiDHiDHiy_all_ee,
-			  gsl_matrix *xHiDHiDHiy_all_ge) {
-	gsl_matrix_set_zero(xHiDHiDHiy_all_gg);
-	gsl_matrix_set_zero(xHiDHiDHiy_all_ee);
-	gsl_matrix_set_zero(xHiDHiDHiy_all_ge);
-
-	size_t d_size=Hiy->size1;
-	size_t v1, v2;
-
-	for (size_t i1=0; i1<d_size; i1++) {
-	  for (size_t j1=0; j1<d_size; j1++) {
-	    if (j1<i1) {continue;}
-	    v1=GetIndex(i1, j1, d_size);
-	    
-	    for (size_t i2=0; i2<d_size; i2++) {
-	      for (size_t j2=0; j2<d_size; j2++) {
-		if (j2<i2) {continue;}
-		v2=GetIndex(i2, j2, d_size);
-		
-		gsl_vector_view xHiDHiDHiy_gg =
-		  gsl_matrix_column (xHiDHiDHiy_all_gg, v1*v_size+v2);
-		gsl_vector_view xHiDHiDHiy_ee =
-		  gsl_matrix_column (xHiDHiDHiy_all_ee, v1*v_size+v2);
-		gsl_vector_view xHiDHiDHiy_ge =
-		  gsl_matrix_column (xHiDHiDHiy_all_ge, v1*v_size+v2);
-		
-		Calc_xHiDHiDHiy (eval, Hi, xHi, Hiy, i1, j1, i2, j2, &xHiDHiDHiy_gg.vector, &xHiDHiDHiy_ee.vector, &xHiDHiDHiy_ge.vector);
-	      }
-	    }
-	  }
-	}
-	return;
+void Calc_xHiDHiDHiy_all(const size_t v_size, const gsl_vector *eval,
+                         const gsl_matrix *Hi, const gsl_matrix *xHi,
+                         const gsl_matrix *Hiy, gsl_matrix *xHiDHiDHiy_all_gg,
+                         gsl_matrix *xHiDHiDHiy_all_ee,
+                         gsl_matrix *xHiDHiDHiy_all_ge) {
+  gsl_matrix_set_zero(xHiDHiDHiy_all_gg);
+  gsl_matrix_set_zero(xHiDHiDHiy_all_ee);
+  gsl_matrix_set_zero(xHiDHiDHiy_all_ge);
+
+  size_t d_size = Hiy->size1;
+  size_t v1, v2;
+
+  for (size_t i1 = 0; i1 < d_size; i1++) {
+    for (size_t j1 = 0; j1 < d_size; j1++) {
+      if (j1 < i1) {
+        continue;
+      }
+      v1 = GetIndex(i1, j1, d_size);
+
+      for (size_t i2 = 0; i2 < d_size; i2++) {
+        for (size_t j2 = 0; j2 < d_size; j2++) {
+          if (j2 < i2) {
+            continue;
+          }
+          v2 = GetIndex(i2, j2, d_size);
+
+          gsl_vector_view xHiDHiDHiy_gg =
+              gsl_matrix_column(xHiDHiDHiy_all_gg, v1 * v_size + v2);
+          gsl_vector_view xHiDHiDHiy_ee =
+              gsl_matrix_column(xHiDHiDHiy_all_ee, v1 * v_size + v2);
+          gsl_vector_view xHiDHiDHiy_ge =
+              gsl_matrix_column(xHiDHiDHiy_all_ge, v1 * v_size + v2);
+
+          Calc_xHiDHiDHiy(eval, Hi, xHi, Hiy, i1, j1, i2, j2,
+                          &xHiDHiDHiy_gg.vector, &xHiDHiDHiy_ee.vector,
+                          &xHiDHiDHiy_ge.vector);
+        }
+      }
+    }
+  }
+  return;
 }
 
 // Calculate (xHiDHix) for every pair (i,j).
-void Calc_xHiDHiDHix_all (const size_t v_size, const gsl_vector *eval,
-			  const gsl_matrix *Hi, const gsl_matrix *xHi,
-			  gsl_matrix *xHiDHiDHix_all_gg,
-			  gsl_matrix *xHiDHiDHix_all_ee,
-			  gsl_matrix *xHiDHiDHix_all_ge) {
-	gsl_matrix_set_zero(xHiDHiDHix_all_gg);
-	gsl_matrix_set_zero(xHiDHiDHix_all_ee);
-	gsl_matrix_set_zero(xHiDHiDHix_all_ge);
-
-	size_t d_size=xHi->size2/eval->size, dc_size=xHi->size1;
-	size_t v1, v2;
-
-	for (size_t i1=0; i1<d_size; i1++) {
-	  for (size_t j1=0; j1<d_size; j1++) {
-	    if (j1<i1) {continue;}
-	    v1=GetIndex(i1, j1, d_size);
-	    
-	    for (size_t i2=0; i2<d_size; i2++) {
-	      for (size_t j2=0; j2<d_size; j2++) {
-		if (j2<i2) {continue;}
-		v2=GetIndex(i2, j2, d_size);
-		
-		if (v2<v1) {continue;}
-		
-		gsl_matrix_view xHiDHiDHix_gg1 =
-		  gsl_matrix_submatrix (xHiDHiDHix_all_gg, 0,
-					(v1*v_size+v2)*dc_size,
-					dc_size, dc_size);
-		gsl_matrix_view xHiDHiDHix_ee1 =
-		  gsl_matrix_submatrix (xHiDHiDHix_all_ee, 0,
-					(v1*v_size+v2)*dc_size,
-					dc_size, dc_size);
-		gsl_matrix_view xHiDHiDHix_ge1 =
-		  gsl_matrix_submatrix (xHiDHiDHix_all_ge, 0,
-					(v1*v_size+v2)*dc_size,
-					dc_size, dc_size);
-		
-		Calc_xHiDHiDHix (eval, Hi, xHi, i1, j1, i2, j2,
-				 &xHiDHiDHix_gg1.matrix,
-				 &xHiDHiDHix_ee1.matrix,
-				 &xHiDHiDHix_ge1.matrix);
-		
-		if (v2!=v1) {
-		  gsl_matrix_view xHiDHiDHix_gg2 =
-		    gsl_matrix_submatrix (xHiDHiDHix_all_gg, 0,
-					  (v2*v_size+v1)*dc_size,
-					  dc_size, dc_size);
-		  gsl_matrix_view xHiDHiDHix_ee2 =
-		    gsl_matrix_submatrix (xHiDHiDHix_all_ee, 0,
-					  (v2*v_size+v1)*dc_size,
-					  dc_size, dc_size);
-		  gsl_matrix_view xHiDHiDHix_ge2 =
-		    gsl_matrix_submatrix (xHiDHiDHix_all_ge, 0,
-					  (v2*v_size+v1)*dc_size,
-					  dc_size, dc_size);
-
-		  gsl_matrix_memcpy (&xHiDHiDHix_gg2.matrix,
-				     &xHiDHiDHix_gg1.matrix);
-		  gsl_matrix_memcpy (&xHiDHiDHix_ee2.matrix,
-				     &xHiDHiDHix_ee1.matrix);
-		  gsl_matrix_memcpy (&xHiDHiDHix_ge2.matrix,
-				     &xHiDHiDHix_ge1.matrix);
-		}
-	      }
-	    }
-	  }
-	}
-
-	return;
+void Calc_xHiDHiDHix_all(const size_t v_size, const gsl_vector *eval,
+                         const gsl_matrix *Hi, const gsl_matrix *xHi,
+                         gsl_matrix *xHiDHiDHix_all_gg,
+                         gsl_matrix *xHiDHiDHix_all_ee,
+                         gsl_matrix *xHiDHiDHix_all_ge) {
+  gsl_matrix_set_zero(xHiDHiDHix_all_gg);
+  gsl_matrix_set_zero(xHiDHiDHix_all_ee);
+  gsl_matrix_set_zero(xHiDHiDHix_all_ge);
+
+  size_t d_size = xHi->size2 / eval->size, dc_size = xHi->size1;
+  size_t v1, v2;
+
+  for (size_t i1 = 0; i1 < d_size; i1++) {
+    for (size_t j1 = 0; j1 < d_size; j1++) {
+      if (j1 < i1) {
+        continue;
+      }
+      v1 = GetIndex(i1, j1, d_size);
+
+      for (size_t i2 = 0; i2 < d_size; i2++) {
+        for (size_t j2 = 0; j2 < d_size; j2++) {
+          if (j2 < i2) {
+            continue;
+          }
+          v2 = GetIndex(i2, j2, d_size);
+
+          if (v2 < v1) {
+            continue;
+          }
+
+          gsl_matrix_view xHiDHiDHix_gg1 = gsl_matrix_submatrix(
+              xHiDHiDHix_all_gg, 0, (v1 * v_size + v2) * dc_size, dc_size,
+              dc_size);
+          gsl_matrix_view xHiDHiDHix_ee1 = gsl_matrix_submatrix(
+              xHiDHiDHix_all_ee, 0, (v1 * v_size + v2) * dc_size, dc_size,
+              dc_size);
+          gsl_matrix_view xHiDHiDHix_ge1 = gsl_matrix_submatrix(
+              xHiDHiDHix_all_ge, 0, (v1 * v_size + v2) * dc_size, dc_size,
+              dc_size);
+
+          Calc_xHiDHiDHix(eval, Hi, xHi, i1, j1, i2, j2, &xHiDHiDHix_gg1.matrix,
+                          &xHiDHiDHix_ee1.matrix, &xHiDHiDHix_ge1.matrix);
+
+          if (v2 != v1) {
+            gsl_matrix_view xHiDHiDHix_gg2 = gsl_matrix_submatrix(
+                xHiDHiDHix_all_gg, 0, (v2 * v_size + v1) * dc_size, dc_size,
+                dc_size);
+            gsl_matrix_view xHiDHiDHix_ee2 = gsl_matrix_submatrix(
+                xHiDHiDHix_all_ee, 0, (v2 * v_size + v1) * dc_size, dc_size,
+                dc_size);
+            gsl_matrix_view xHiDHiDHix_ge2 = gsl_matrix_submatrix(
+                xHiDHiDHix_all_ge, 0, (v2 * v_size + v1) * dc_size, dc_size,
+                dc_size);
+
+            gsl_matrix_memcpy(&xHiDHiDHix_gg2.matrix, &xHiDHiDHix_gg1.matrix);
+            gsl_matrix_memcpy(&xHiDHiDHix_ee2.matrix, &xHiDHiDHix_ee1.matrix);
+            gsl_matrix_memcpy(&xHiDHiDHix_ge2.matrix, &xHiDHiDHix_ge1.matrix);
+          }
+        }
+      }
+    }
+  }
+
+  return;
 }
 
 // Calculate (xHiDHix)Qi(xHiy) for every pair (i,j).
-void Calc_xHiDHixQixHiy_all (const gsl_matrix *xHiDHix_all_g,
-			     const gsl_matrix *xHiDHix_all_e,
-			     const gsl_vector *QixHiy,
-			     gsl_matrix *xHiDHixQixHiy_all_g,
-			     gsl_matrix *xHiDHixQixHiy_all_e) {
-	size_t dc_size=xHiDHix_all_g->size1;
-	size_t v_size=xHiDHix_all_g->size2/dc_size;
-
-	for (size_t i=0; i<v_size; i++) {
-		gsl_matrix_const_view xHiDHix_g =
-		  gsl_matrix_const_submatrix (xHiDHix_all_g, 0, i*dc_size,
-					      dc_size, dc_size);
-		gsl_matrix_const_view xHiDHix_e =
-		  gsl_matrix_const_submatrix (xHiDHix_all_e, 0, i*dc_size,
-					      dc_size, dc_size);
-
-		gsl_vector_view xHiDHixQixHiy_g =
-		  gsl_matrix_column (xHiDHixQixHiy_all_g, i);
-		gsl_vector_view xHiDHixQixHiy_e =
-		  gsl_matrix_column (xHiDHixQixHiy_all_e, i);
-
-		gsl_blas_dgemv (CblasNoTrans, 1.0, &xHiDHix_g.matrix,
-				QixHiy, 0.0, &xHiDHixQixHiy_g.vector);
-		gsl_blas_dgemv (CblasNoTrans, 1.0, &xHiDHix_e.matrix,
-				QixHiy, 0.0, &xHiDHixQixHiy_e.vector);
-	}
-
-	return;
+void Calc_xHiDHixQixHiy_all(const gsl_matrix *xHiDHix_all_g,
+                            const gsl_matrix *xHiDHix_all_e,
+                            const gsl_vector *QixHiy,
+                            gsl_matrix *xHiDHixQixHiy_all_g,
+                            gsl_matrix *xHiDHixQixHiy_all_e) {
+  size_t dc_size = xHiDHix_all_g->size1;
+  size_t v_size = xHiDHix_all_g->size2 / dc_size;
+
+  for (size_t i = 0; i < v_size; i++) {
+    gsl_matrix_const_view xHiDHix_g = gsl_matrix_const_submatrix(
+        xHiDHix_all_g, 0, i * dc_size, dc_size, dc_size);
+    gsl_matrix_const_view xHiDHix_e = gsl_matrix_const_submatrix(
+        xHiDHix_all_e, 0, i * dc_size, dc_size, dc_size);
+
+    gsl_vector_view xHiDHixQixHiy_g = gsl_matrix_column(xHiDHixQixHiy_all_g, i);
+    gsl_vector_view xHiDHixQixHiy_e = gsl_matrix_column(xHiDHixQixHiy_all_e, i);
+
+    gsl_blas_dgemv(CblasNoTrans, 1.0, &xHiDHix_g.matrix, QixHiy, 0.0,
+                   &xHiDHixQixHiy_g.vector);
+    gsl_blas_dgemv(CblasNoTrans, 1.0, &xHiDHix_e.matrix, QixHiy, 0.0,
+                   &xHiDHixQixHiy_e.vector);
+  }
+
+  return;
 }
 
 // Calculate Qi(xHiDHiy) and Qi(xHiDHix)Qi(xHiy) for each pair of i,j (i<=j).
-void Calc_QiVec_all (const gsl_matrix *Qi, const gsl_matrix *vec_all_g,
-		     const gsl_matrix *vec_all_e, gsl_matrix *Qivec_all_g,
-		     gsl_matrix *Qivec_all_e) {
-	for (size_t i=0; i<vec_all_g->size2; i++) {
-	  gsl_vector_const_view vec_g=gsl_matrix_const_column (vec_all_g, i);
-	  gsl_vector_const_view vec_e=gsl_matrix_const_column (vec_all_e, i);
-	  
-	  gsl_vector_view Qivec_g=gsl_matrix_column (Qivec_all_g, i);
-	  gsl_vector_view Qivec_e=gsl_matrix_column (Qivec_all_e, i);
-	  
-	  gsl_blas_dgemv(CblasNoTrans,1.0,Qi,&vec_g.vector,0.0,
-			 &Qivec_g.vector);
-	  gsl_blas_dgemv(CblasNoTrans,1.0,Qi,&vec_e.vector,0.0,
-			 &Qivec_e.vector);
-	}
-
-	return;
+void Calc_QiVec_all(const gsl_matrix *Qi, const gsl_matrix *vec_all_g,
+                    const gsl_matrix *vec_all_e, gsl_matrix *Qivec_all_g,
+                    gsl_matrix *Qivec_all_e) {
+  for (size_t i = 0; i < vec_all_g->size2; i++) {
+    gsl_vector_const_view vec_g = gsl_matrix_const_column(vec_all_g, i);
+    gsl_vector_const_view vec_e = gsl_matrix_const_column(vec_all_e, i);
+
+    gsl_vector_view Qivec_g = gsl_matrix_column(Qivec_all_g, i);
+    gsl_vector_view Qivec_e = gsl_matrix_column(Qivec_all_e, i);
+
+    gsl_blas_dgemv(CblasNoTrans, 1.0, Qi, &vec_g.vector, 0.0, &Qivec_g.vector);
+    gsl_blas_dgemv(CblasNoTrans, 1.0, Qi, &vec_e.vector, 0.0, &Qivec_e.vector);
+  }
+
+  return;
 }
 
 // Calculate Qi(xHiDHix) for each pair of i,j (i<=j).
-void Calc_QiMat_all (const gsl_matrix *Qi, const gsl_matrix *mat_all_g,
-		     const gsl_matrix *mat_all_e, gsl_matrix *Qimat_all_g,
-		     gsl_matrix *Qimat_all_e) {
-	size_t dc_size=Qi->size1;
-	size_t v_size=mat_all_g->size2/mat_all_g->size1;
-
-	for (size_t i=0; i<v_size; i++) {
-		gsl_matrix_const_view mat_g =
-		  gsl_matrix_const_submatrix (mat_all_g, 0, i*dc_size,
-					      dc_size, dc_size);
-		gsl_matrix_const_view mat_e =
-		  gsl_matrix_const_submatrix (mat_all_e, 0, i*dc_size,
-					      dc_size, dc_size);
-
-		gsl_matrix_view Qimat_g =
-		  gsl_matrix_submatrix (Qimat_all_g, 0, i*dc_size, dc_size,
-					dc_size);
-		gsl_matrix_view Qimat_e =
-		  gsl_matrix_submatrix (Qimat_all_e, 0, i*dc_size, dc_size,
-					dc_size);
-
-		gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, 1.0, Qi,
-				&mat_g.matrix, 0.0, &Qimat_g.matrix);
-		gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, 1.0, Qi,
-				&mat_e.matrix, 0.0, &Qimat_e.matrix);
-	}
-
-	return;
+void Calc_QiMat_all(const gsl_matrix *Qi, const gsl_matrix *mat_all_g,
+                    const gsl_matrix *mat_all_e, gsl_matrix *Qimat_all_g,
+                    gsl_matrix *Qimat_all_e) {
+  size_t dc_size = Qi->size1;
+  size_t v_size = mat_all_g->size2 / mat_all_g->size1;
+
+  for (size_t i = 0; i < v_size; i++) {
+    gsl_matrix_const_view mat_g =
+        gsl_matrix_const_submatrix(mat_all_g, 0, i * dc_size, dc_size, dc_size);
+    gsl_matrix_const_view mat_e =
+        gsl_matrix_const_submatrix(mat_all_e, 0, i * dc_size, dc_size, dc_size);
+
+    gsl_matrix_view Qimat_g =
+        gsl_matrix_submatrix(Qimat_all_g, 0, i * dc_size, dc_size, dc_size);
+    gsl_matrix_view Qimat_e =
+        gsl_matrix_submatrix(Qimat_all_e, 0, i * dc_size, dc_size, dc_size);
+
+    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Qi, &mat_g.matrix, 0.0,
+                   &Qimat_g.matrix);
+    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Qi, &mat_e.matrix, 0.0,
+                   &Qimat_e.matrix);
+  }
+
+  return;
 }
 
 // Calculate yPDPy
 // yPDPy = y(Hi-HixQixHi)D(Hi-HixQixHi)y
 //       = ytHiDHiy - (yHix)Qi(xHiDHiy) - (yHiDHix)Qi(xHiy)
 //         + (yHix)Qi(xHiDHix)Qi(xtHiy)
-void Calc_yPDPy (const gsl_vector *eval, const gsl_matrix *Hiy,
-		 const gsl_vector *QixHiy, const gsl_matrix *xHiDHiy_all_g,
-		 const gsl_matrix *xHiDHiy_all_e,
-		 const gsl_matrix *xHiDHixQixHiy_all_g,
-		 const gsl_matrix *xHiDHixQixHiy_all_e,
-		 const size_t i, const size_t j,
-		 double &yPDPy_g, double &yPDPy_e) {
-	size_t d_size=Hiy->size1;
-	size_t v=GetIndex(i, j, d_size);
-
-	double d;
-
-	// First part: ytHiDHiy.
-	Calc_yHiDHiy (eval, Hiy, i, j, yPDPy_g, yPDPy_e);
-
-	// Second and third parts: -(yHix)Qi(xHiDHiy)-(yHiDHix)Qi(xHiy)
-	gsl_vector_const_view xHiDHiy_g =
-	  gsl_matrix_const_column (xHiDHiy_all_g, v);
-	gsl_vector_const_view xHiDHiy_e =
-	  gsl_matrix_const_column (xHiDHiy_all_e, v);
-
-	gsl_blas_ddot(QixHiy, &xHiDHiy_g.vector, &d);
-	yPDPy_g-=d*2.0;
-	gsl_blas_ddot(QixHiy, &xHiDHiy_e.vector, &d);
-	yPDPy_e-=d*2.0;
-
-	// Fourth part: +(yHix)Qi(xHiDHix)Qi(xHiy).
-	gsl_vector_const_view xHiDHixQixHiy_g =
-	  gsl_matrix_const_column (xHiDHixQixHiy_all_g, v);
-	gsl_vector_const_view xHiDHixQixHiy_e =
-	  gsl_matrix_const_column (xHiDHixQixHiy_all_e, v);
-
-	gsl_blas_ddot(QixHiy, &xHiDHixQixHiy_g.vector, &d);
-	yPDPy_g+=d;
-	gsl_blas_ddot(QixHiy, &xHiDHixQixHiy_e.vector, &d);
-	yPDPy_e+=d;
-
-	return;
+void Calc_yPDPy(const gsl_vector *eval, const gsl_matrix *Hiy,
+                const gsl_vector *QixHiy, const gsl_matrix *xHiDHiy_all_g,
+                const gsl_matrix *xHiDHiy_all_e,
+                const gsl_matrix *xHiDHixQixHiy_all_g,
+                const gsl_matrix *xHiDHixQixHiy_all_e, const size_t i,
+                const size_t j, double &yPDPy_g, double &yPDPy_e) {
+  size_t d_size = Hiy->size1;
+  size_t v = GetIndex(i, j, d_size);
+
+  double d;
+
+  // First part: ytHiDHiy.
+  Calc_yHiDHiy(eval, Hiy, i, j, yPDPy_g, yPDPy_e);
+
+  // Second and third parts: -(yHix)Qi(xHiDHiy)-(yHiDHix)Qi(xHiy)
+  gsl_vector_const_view xHiDHiy_g = gsl_matrix_const_column(xHiDHiy_all_g, v);
+  gsl_vector_const_view xHiDHiy_e = gsl_matrix_const_column(xHiDHiy_all_e, v);
+
+  gsl_blas_ddot(QixHiy, &xHiDHiy_g.vector, &d);
+  yPDPy_g -= d * 2.0;
+  gsl_blas_ddot(QixHiy, &xHiDHiy_e.vector, &d);
+  yPDPy_e -= d * 2.0;
+
+  // Fourth part: +(yHix)Qi(xHiDHix)Qi(xHiy).
+  gsl_vector_const_view xHiDHixQixHiy_g =
+      gsl_matrix_const_column(xHiDHixQixHiy_all_g, v);
+  gsl_vector_const_view xHiDHixQixHiy_e =
+      gsl_matrix_const_column(xHiDHixQixHiy_all_e, v);
+
+  gsl_blas_ddot(QixHiy, &xHiDHixQixHiy_g.vector, &d);
+  yPDPy_g += d;
+  gsl_blas_ddot(QixHiy, &xHiDHixQixHiy_e.vector, &d);
+  yPDPy_e += d;
+
+  return;
 }
 
 // calculate yPDPDPy = y(Hi-HixQixHi)D(Hi-HixQixHi)D(Hi-HixQixHi)y
@@ -1912,3445 +1897,3503 @@ void Calc_yPDPy (const gsl_vector *eval, const gsl_matrix *Hiy,
 //                     + (yHiDHix)Qi(xHiDHix)Qi(xHiy)
 //                     + (yHix)Qi(xHiDHiDHix)Qi(xHiy)
 //                     - (yHix)Qi(xHiDHix)Qi(xHiDHix)Qi(xHiy)
-void Calc_yPDPDPy (const gsl_vector *eval, const gsl_matrix *Hi,
-		   const gsl_matrix *xHi, const gsl_matrix *Hiy,
-		   const gsl_vector *QixHiy, const gsl_matrix *xHiDHiy_all_g,
-		   const gsl_matrix *xHiDHiy_all_e,
-		   const gsl_matrix *QixHiDHiy_all_g,
-		   const gsl_matrix *QixHiDHiy_all_e,
-		   const gsl_matrix *xHiDHixQixHiy_all_g,
-		   const gsl_matrix *xHiDHixQixHiy_all_e,
-		   const gsl_matrix *QixHiDHixQixHiy_all_g,
-		   const gsl_matrix *QixHiDHixQixHiy_all_e,
-		   const gsl_matrix *xHiDHiDHiy_all_gg,
-		   const gsl_matrix *xHiDHiDHiy_all_ee,
-		   const gsl_matrix *xHiDHiDHiy_all_ge,
-		   const gsl_matrix *xHiDHiDHix_all_gg,
-		   const gsl_matrix *xHiDHiDHix_all_ee,
-		   const gsl_matrix *xHiDHiDHix_all_ge,
-		   const size_t i1, const size_t j1, const size_t i2,
-		   const size_t j2, double &yPDPDPy_gg, double &yPDPDPy_ee,
-		   double &yPDPDPy_ge) {
-	size_t d_size=Hi->size1, dc_size=xHi->size1;
-	size_t v1=GetIndex(i1, j1, d_size), v2=GetIndex(i2, j2, d_size);
-	size_t v_size=d_size*(d_size+1)/2;
-
-	double d;
-
-	gsl_vector *xHiDHiDHixQixHiy=gsl_vector_alloc (dc_size);
-
-	// First part: yHiDHiDHiy.
-	Calc_yHiDHiDHiy (eval, Hi, Hiy, i1, j1, i2, j2, yPDPDPy_gg,
-			 yPDPDPy_ee, yPDPDPy_ge);
-
-	// Second and third parts:
-	// -(yHix)Qi(xHiDHiDHiy) - (yHiDHiDHix)Qi(xHiy).
-	gsl_vector_const_view xHiDHiDHiy_gg1 =
-	  gsl_matrix_const_column (xHiDHiDHiy_all_gg, v1*v_size+v2);
-	gsl_vector_const_view xHiDHiDHiy_ee1 =
-	  gsl_matrix_const_column (xHiDHiDHiy_all_ee, v1*v_size+v2);
-	gsl_vector_const_view xHiDHiDHiy_ge1 =
-	  gsl_matrix_const_column (xHiDHiDHiy_all_ge, v1*v_size+v2);
-
-	gsl_vector_const_view xHiDHiDHiy_gg2 =
-	  gsl_matrix_const_column (xHiDHiDHiy_all_gg, v2*v_size+v1);
-	gsl_vector_const_view xHiDHiDHiy_ee2 =
-	  gsl_matrix_const_column (xHiDHiDHiy_all_ee, v2*v_size+v1);
-	gsl_vector_const_view xHiDHiDHiy_ge2 =
-	  gsl_matrix_const_column (xHiDHiDHiy_all_ge, v2*v_size+v1);
-
-	gsl_blas_ddot(QixHiy, &xHiDHiDHiy_gg1.vector, &d);
-	yPDPDPy_gg-=d;
-	gsl_blas_ddot(QixHiy, &xHiDHiDHiy_ee1.vector, &d);
-	yPDPDPy_ee-=d;
-	gsl_blas_ddot(QixHiy, &xHiDHiDHiy_ge1.vector, &d);
-	yPDPDPy_ge-=d;
-
-	gsl_blas_ddot(QixHiy, &xHiDHiDHiy_gg2.vector, &d);
-	yPDPDPy_gg-=d;
-	gsl_blas_ddot(QixHiy, &xHiDHiDHiy_ee2.vector, &d);
-	yPDPDPy_ee-=d;
-	gsl_blas_ddot(QixHiy, &xHiDHiDHiy_ge2.vector, &d);
-	yPDPDPy_ge-=d;
-
-	// Fourth part: - (yHiDHix)Qi(xHiDHiy).
-	gsl_vector_const_view xHiDHiy_g1 =
-	  gsl_matrix_const_column (xHiDHiy_all_g, v1);
-	gsl_vector_const_view xHiDHiy_e1 =
-	  gsl_matrix_const_column (xHiDHiy_all_e, v1);
-	gsl_vector_const_view QixHiDHiy_g2 =
-	  gsl_matrix_const_column (QixHiDHiy_all_g, v2);
-	gsl_vector_const_view QixHiDHiy_e2 =
-	  gsl_matrix_const_column (QixHiDHiy_all_e, v2);
-
-	gsl_blas_ddot(&xHiDHiy_g1.vector, &QixHiDHiy_g2.vector, &d);
-	yPDPDPy_gg-=d;
-	gsl_blas_ddot(&xHiDHiy_e1.vector, &QixHiDHiy_e2.vector, &d);
-	yPDPDPy_ee-=d;
-	gsl_blas_ddot(&xHiDHiy_g1.vector, &QixHiDHiy_e2.vector, &d);
-	yPDPDPy_ge-=d;
-
-	// Fifth and sixth parts:
-	//   + (yHix)Qi(xHiDHix)Qi(xHiDHiy) +
-	//   (yHiDHix)Qi(xHiDHix)Qi(xHiy)
-	gsl_vector_const_view QixHiDHiy_g1 =
-	  gsl_matrix_const_column (QixHiDHiy_all_g, v1);
-	gsl_vector_const_view QixHiDHiy_e1 =
-	  gsl_matrix_const_column (QixHiDHiy_all_e, v1);
-
-	gsl_vector_const_view xHiDHixQixHiy_g1 =
-	  gsl_matrix_const_column (xHiDHixQixHiy_all_g, v1);
-	gsl_vector_const_view xHiDHixQixHiy_e1 =
-	  gsl_matrix_const_column (xHiDHixQixHiy_all_e, v1);
-	gsl_vector_const_view xHiDHixQixHiy_g2 =
-	  gsl_matrix_const_column (xHiDHixQixHiy_all_g, v2);
-	gsl_vector_const_view xHiDHixQixHiy_e2 =
-	  gsl_matrix_const_column (xHiDHixQixHiy_all_e, v2);
-
-	gsl_blas_ddot(&xHiDHixQixHiy_g1.vector, &QixHiDHiy_g2.vector, &d);
-	yPDPDPy_gg+=d;
-	gsl_blas_ddot(&xHiDHixQixHiy_g2.vector, &QixHiDHiy_g1.vector, &d);
-	yPDPDPy_gg+=d;
-
-	gsl_blas_ddot(&xHiDHixQixHiy_e1.vector, &QixHiDHiy_e2.vector, &d);
-	yPDPDPy_ee+=d;
-	gsl_blas_ddot(&xHiDHixQixHiy_e2.vector, &QixHiDHiy_e1.vector, &d);
-	yPDPDPy_ee+=d;
-
-	gsl_blas_ddot(&xHiDHixQixHiy_g1.vector, &QixHiDHiy_e2.vector, &d);
-	yPDPDPy_ge+=d;
-	gsl_blas_ddot(&xHiDHixQixHiy_e2.vector, &QixHiDHiy_g1.vector, &d);
-	yPDPDPy_ge+=d;
-
-	// Seventh part: + (yHix)Qi(xHiDHiDHix)Qi(xHiy)
-	gsl_matrix_const_view xHiDHiDHix_gg =
-	  gsl_matrix_const_submatrix (xHiDHiDHix_all_gg, 0,
-				      (v1*v_size+v2)*dc_size,
-				      dc_size, dc_size);
-	gsl_matrix_const_view xHiDHiDHix_ee =
-	  gsl_matrix_const_submatrix (xHiDHiDHix_all_ee, 0,
-				      (v1*v_size+v2)*dc_size,
-				      dc_size, dc_size);
-	gsl_matrix_const_view xHiDHiDHix_ge =
-	  gsl_matrix_const_submatrix (xHiDHiDHix_all_ge, 0,
-				      (v1*v_size+v2)*dc_size,
-				      dc_size, dc_size);
-
-	gsl_blas_dgemv (CblasNoTrans, 1.0, &xHiDHiDHix_gg.matrix,
-			QixHiy, 0.0, xHiDHiDHixQixHiy);
-	gsl_blas_ddot(xHiDHiDHixQixHiy, QixHiy, &d);
-	yPDPDPy_gg+=d;
-	gsl_blas_dgemv (CblasNoTrans, 1.0, &xHiDHiDHix_ee.matrix,
-			QixHiy, 0.0, xHiDHiDHixQixHiy);
-	gsl_blas_ddot(xHiDHiDHixQixHiy, QixHiy, &d);
-	yPDPDPy_ee+=d;
-	gsl_blas_dgemv (CblasNoTrans, 1.0, &xHiDHiDHix_ge.matrix,
-			QixHiy, 0.0, xHiDHiDHixQixHiy);
-	gsl_blas_ddot(xHiDHiDHixQixHiy, QixHiy, &d);
-	yPDPDPy_ge+=d;
-
-	// Eighth part: - (yHix)Qi(xHiDHix)Qi(xHiDHix)Qi(xHiy).
-	gsl_vector_const_view QixHiDHixQixHiy_g1 =
-	  gsl_matrix_const_column (QixHiDHixQixHiy_all_g, v1);
-	gsl_vector_const_view QixHiDHixQixHiy_e1 =
-	  gsl_matrix_const_column (QixHiDHixQixHiy_all_e, v1);
-
-	gsl_blas_ddot(&QixHiDHixQixHiy_g1.vector,&xHiDHixQixHiy_g2.vector,&d);
-	yPDPDPy_gg-=d;
-	gsl_blas_ddot(&QixHiDHixQixHiy_e1.vector,&xHiDHixQixHiy_e2.vector,&d);
-	yPDPDPy_ee-=d;
-	gsl_blas_ddot(&QixHiDHixQixHiy_g1.vector,&xHiDHixQixHiy_e2.vector,&d);
-	yPDPDPy_ge-=d;
-
-	// Free memory.
-	gsl_vector_free(xHiDHiDHixQixHiy);
-
-	return;
+void Calc_yPDPDPy(
+    const gsl_vector *eval, const gsl_matrix *Hi, const gsl_matrix *xHi,
+    const gsl_matrix *Hiy, const gsl_vector *QixHiy,
+    const gsl_matrix *xHiDHiy_all_g, const gsl_matrix *xHiDHiy_all_e,
+    const gsl_matrix *QixHiDHiy_all_g, const gsl_matrix *QixHiDHiy_all_e,
+    const gsl_matrix *xHiDHixQixHiy_all_g,
+    const gsl_matrix *xHiDHixQixHiy_all_e,
+    const gsl_matrix *QixHiDHixQixHiy_all_g,
+    const gsl_matrix *QixHiDHixQixHiy_all_e,
+    const gsl_matrix *xHiDHiDHiy_all_gg, const gsl_matrix *xHiDHiDHiy_all_ee,
+    const gsl_matrix *xHiDHiDHiy_all_ge, const gsl_matrix *xHiDHiDHix_all_gg,
+    const gsl_matrix *xHiDHiDHix_all_ee, const gsl_matrix *xHiDHiDHix_all_ge,
+    const size_t i1, const size_t j1, const size_t i2, const size_t j2,
+    double &yPDPDPy_gg, double &yPDPDPy_ee, double &yPDPDPy_ge) {
+  size_t d_size = Hi->size1, dc_size = xHi->size1;
+  size_t v1 = GetIndex(i1, j1, d_size), v2 = GetIndex(i2, j2, d_size);
+  size_t v_size = d_size * (d_size + 1) / 2;
+
+  double d;
+
+  gsl_vector *xHiDHiDHixQixHiy = gsl_vector_alloc(dc_size);
+
+  // First part: yHiDHiDHiy.
+  Calc_yHiDHiDHiy(eval, Hi, Hiy, i1, j1, i2, j2, yPDPDPy_gg, yPDPDPy_ee,
+                  yPDPDPy_ge);
+
+  // Second and third parts:
+  // -(yHix)Qi(xHiDHiDHiy) - (yHiDHiDHix)Qi(xHiy).
+  gsl_vector_const_view xHiDHiDHiy_gg1 =
+      gsl_matrix_const_column(xHiDHiDHiy_all_gg, v1 * v_size + v2);
+  gsl_vector_const_view xHiDHiDHiy_ee1 =
+      gsl_matrix_const_column(xHiDHiDHiy_all_ee, v1 * v_size + v2);
+  gsl_vector_const_view xHiDHiDHiy_ge1 =
+      gsl_matrix_const_column(xHiDHiDHiy_all_ge, v1 * v_size + v2);
+
+  gsl_vector_const_view xHiDHiDHiy_gg2 =
+      gsl_matrix_const_column(xHiDHiDHiy_all_gg, v2 * v_size + v1);
+  gsl_vector_const_view xHiDHiDHiy_ee2 =
+      gsl_matrix_const_column(xHiDHiDHiy_all_ee, v2 * v_size + v1);
+  gsl_vector_const_view xHiDHiDHiy_ge2 =
+      gsl_matrix_const_column(xHiDHiDHiy_all_ge, v2 * v_size + v1);
+
+  gsl_blas_ddot(QixHiy, &xHiDHiDHiy_gg1.vector, &d);
+  yPDPDPy_gg -= d;
+  gsl_blas_ddot(QixHiy, &xHiDHiDHiy_ee1.vector, &d);
+  yPDPDPy_ee -= d;
+  gsl_blas_ddot(QixHiy, &xHiDHiDHiy_ge1.vector, &d);
+  yPDPDPy_ge -= d;
+
+  gsl_blas_ddot(QixHiy, &xHiDHiDHiy_gg2.vector, &d);
+  yPDPDPy_gg -= d;
+  gsl_blas_ddot(QixHiy, &xHiDHiDHiy_ee2.vector, &d);
+  yPDPDPy_ee -= d;
+  gsl_blas_ddot(QixHiy, &xHiDHiDHiy_ge2.vector, &d);
+  yPDPDPy_ge -= d;
+
+  // Fourth part: - (yHiDHix)Qi(xHiDHiy).
+  gsl_vector_const_view xHiDHiy_g1 = gsl_matrix_const_column(xHiDHiy_all_g, v1);
+  gsl_vector_const_view xHiDHiy_e1 = gsl_matrix_const_column(xHiDHiy_all_e, v1);
+  gsl_vector_const_view QixHiDHiy_g2 =
+      gsl_matrix_const_column(QixHiDHiy_all_g, v2);
+  gsl_vector_const_view QixHiDHiy_e2 =
+      gsl_matrix_const_column(QixHiDHiy_all_e, v2);
+
+  gsl_blas_ddot(&xHiDHiy_g1.vector, &QixHiDHiy_g2.vector, &d);
+  yPDPDPy_gg -= d;
+  gsl_blas_ddot(&xHiDHiy_e1.vector, &QixHiDHiy_e2.vector, &d);
+  yPDPDPy_ee -= d;
+  gsl_blas_ddot(&xHiDHiy_g1.vector, &QixHiDHiy_e2.vector, &d);
+  yPDPDPy_ge -= d;
+
+  // Fifth and sixth parts:
+  //   + (yHix)Qi(xHiDHix)Qi(xHiDHiy) +
+  //   (yHiDHix)Qi(xHiDHix)Qi(xHiy)
+  gsl_vector_const_view QixHiDHiy_g1 =
+      gsl_matrix_const_column(QixHiDHiy_all_g, v1);
+  gsl_vector_const_view QixHiDHiy_e1 =
+      gsl_matrix_const_column(QixHiDHiy_all_e, v1);
+
+  gsl_vector_const_view xHiDHixQixHiy_g1 =
+      gsl_matrix_const_column(xHiDHixQixHiy_all_g, v1);
+  gsl_vector_const_view xHiDHixQixHiy_e1 =
+      gsl_matrix_const_column(xHiDHixQixHiy_all_e, v1);
+  gsl_vector_const_view xHiDHixQixHiy_g2 =
+      gsl_matrix_const_column(xHiDHixQixHiy_all_g, v2);
+  gsl_vector_const_view xHiDHixQixHiy_e2 =
+      gsl_matrix_const_column(xHiDHixQixHiy_all_e, v2);
+
+  gsl_blas_ddot(&xHiDHixQixHiy_g1.vector, &QixHiDHiy_g2.vector, &d);
+  yPDPDPy_gg += d;
+  gsl_blas_ddot(&xHiDHixQixHiy_g2.vector, &QixHiDHiy_g1.vector, &d);
+  yPDPDPy_gg += d;
+
+  gsl_blas_ddot(&xHiDHixQixHiy_e1.vector, &QixHiDHiy_e2.vector, &d);
+  yPDPDPy_ee += d;
+  gsl_blas_ddot(&xHiDHixQixHiy_e2.vector, &QixHiDHiy_e1.vector, &d);
+  yPDPDPy_ee += d;
+
+  gsl_blas_ddot(&xHiDHixQixHiy_g1.vector, &QixHiDHiy_e2.vector, &d);
+  yPDPDPy_ge += d;
+  gsl_blas_ddot(&xHiDHixQixHiy_e2.vector, &QixHiDHiy_g1.vector, &d);
+  yPDPDPy_ge += d;
+
+  // Seventh part: + (yHix)Qi(xHiDHiDHix)Qi(xHiy)
+  gsl_matrix_const_view xHiDHiDHix_gg = gsl_matrix_const_submatrix(
+      xHiDHiDHix_all_gg, 0, (v1 * v_size + v2) * dc_size, dc_size, dc_size);
+  gsl_matrix_const_view xHiDHiDHix_ee = gsl_matrix_const_submatrix(
+      xHiDHiDHix_all_ee, 0, (v1 * v_size + v2) * dc_size, dc_size, dc_size);
+  gsl_matrix_const_view xHiDHiDHix_ge = gsl_matrix_const_submatrix(
+      xHiDHiDHix_all_ge, 0, (v1 * v_size + v2) * dc_size, dc_size, dc_size);
+
+  gsl_blas_dgemv(CblasNoTrans, 1.0, &xHiDHiDHix_gg.matrix, QixHiy, 0.0,
+                 xHiDHiDHixQixHiy);
+  gsl_blas_ddot(xHiDHiDHixQixHiy, QixHiy, &d);
+  yPDPDPy_gg += d;
+  gsl_blas_dgemv(CblasNoTrans, 1.0, &xHiDHiDHix_ee.matrix, QixHiy, 0.0,
+                 xHiDHiDHixQixHiy);
+  gsl_blas_ddot(xHiDHiDHixQixHiy, QixHiy, &d);
+  yPDPDPy_ee += d;
+  gsl_blas_dgemv(CblasNoTrans, 1.0, &xHiDHiDHix_ge.matrix, QixHiy, 0.0,
+                 xHiDHiDHixQixHiy);
+  gsl_blas_ddot(xHiDHiDHixQixHiy, QixHiy, &d);
+  yPDPDPy_ge += d;
+
+  // Eighth part: - (yHix)Qi(xHiDHix)Qi(xHiDHix)Qi(xHiy).
+  gsl_vector_const_view QixHiDHixQixHiy_g1 =
+      gsl_matrix_const_column(QixHiDHixQixHiy_all_g, v1);
+  gsl_vector_const_view QixHiDHixQixHiy_e1 =
+      gsl_matrix_const_column(QixHiDHixQixHiy_all_e, v1);
+
+  gsl_blas_ddot(&QixHiDHixQixHiy_g1.vector, &xHiDHixQixHiy_g2.vector, &d);
+  yPDPDPy_gg -= d;
+  gsl_blas_ddot(&QixHiDHixQixHiy_e1.vector, &xHiDHixQixHiy_e2.vector, &d);
+  yPDPDPy_ee -= d;
+  gsl_blas_ddot(&QixHiDHixQixHiy_g1.vector, &xHiDHixQixHiy_e2.vector, &d);
+  yPDPDPy_ge -= d;
+
+  // Free memory.
+  gsl_vector_free(xHiDHiDHixQixHiy);
+
+  return;
 }
 
 // Calculate Edgeworth correctation factors for small samples notation
 // and method follows Thomas J. Rothenberg, Econometirca 1984; 52 (4)
 // M=xHiDHix
-void CalcCRT (const gsl_matrix *Hessian_inv, const gsl_matrix *Qi,
-	      const gsl_matrix *QixHiDHix_all_g,
-	      const gsl_matrix *QixHiDHix_all_e,
-	      const gsl_matrix *xHiDHiDHix_all_gg,
-	      const gsl_matrix *xHiDHiDHix_all_ee,
-	      const gsl_matrix *xHiDHiDHix_all_ge,
-	      const size_t d_size, double &crt_a,
-	      double &crt_b, double &crt_c) {
-	crt_a=0.0; crt_b=0.0; crt_c=0.0;
-
-	size_t dc_size=Qi->size1, v_size=Hessian_inv->size1/2;
-	size_t c_size=dc_size/d_size;
-	double h_gg, h_ge, h_ee, d, B=0.0, C=0.0, D=0.0;
-	double trCg1, trCe1, trCg2, trCe2, trB_gg, trB_ge, trB_ee;
-	double trCC_gg, trCC_ge, trCC_ee, trD_gg=0.0, trD_ge=0.0, trD_ee=0.0;
-
-	gsl_matrix *QiMQi_g1=gsl_matrix_alloc (dc_size, dc_size);
-	gsl_matrix *QiMQi_e1=gsl_matrix_alloc (dc_size, dc_size);
-	gsl_matrix *QiMQi_g2=gsl_matrix_alloc (dc_size, dc_size);
-	gsl_matrix *QiMQi_e2=gsl_matrix_alloc (dc_size, dc_size);
-
-	gsl_matrix *QiMQisQisi_g1=gsl_matrix_alloc (d_size, d_size);
-	gsl_matrix *QiMQisQisi_e1=gsl_matrix_alloc (d_size, d_size);
-	gsl_matrix *QiMQisQisi_g2=gsl_matrix_alloc (d_size, d_size);
-	gsl_matrix *QiMQisQisi_e2=gsl_matrix_alloc (d_size, d_size);
-
-	gsl_matrix *QiMQiMQi_gg=gsl_matrix_alloc (dc_size, dc_size);
-	gsl_matrix *QiMQiMQi_ge=gsl_matrix_alloc (dc_size, dc_size);
-	gsl_matrix *QiMQiMQi_ee=gsl_matrix_alloc (dc_size, dc_size);
-
-	gsl_matrix *QiMMQi_gg=gsl_matrix_alloc (dc_size, dc_size);
-	gsl_matrix *QiMMQi_ge=gsl_matrix_alloc (dc_size, dc_size);
-	gsl_matrix *QiMMQi_ee=gsl_matrix_alloc (dc_size, dc_size);
-
-	gsl_matrix *Qi_si=gsl_matrix_alloc (d_size, d_size);
-
-	gsl_matrix *M_dd=gsl_matrix_alloc (d_size, d_size);
-	gsl_matrix *M_dcdc=gsl_matrix_alloc (dc_size, dc_size);
-
-	// Invert Qi_sub to Qi_si.
-	gsl_matrix *Qi_sub=gsl_matrix_alloc (d_size, d_size);
-
-	gsl_matrix_const_view Qi_s =
-	  gsl_matrix_const_submatrix (Qi, (c_size-1)*d_size,
-				      (c_size-1)*d_size, d_size, d_size);
-
-	int sig;
-	gsl_permutation * pmt=gsl_permutation_alloc (d_size);
-
-	gsl_matrix_memcpy (Qi_sub, &Qi_s.matrix);
-	LUDecomp (Qi_sub, pmt, &sig);
-	LUInvert (Qi_sub, pmt, Qi_si);
-
-	gsl_permutation_free(pmt);
-	gsl_matrix_free(Qi_sub);
-
-	// Calculate correction factors.
-	for (size_t v1=0; v1<v_size; v1++) {
-	  
-	  // Calculate Qi(xHiDHix)Qi, and subpart of it.
-	  gsl_matrix_const_view QiM_g1 =
-	    gsl_matrix_const_submatrix (QixHiDHix_all_g, 0, v1*dc_size,
-					dc_size, dc_size);
-	  gsl_matrix_const_view QiM_e1 =
-	    gsl_matrix_const_submatrix (QixHiDHix_all_e, 0, v1*dc_size,
-					dc_size, dc_size);
-	  
-	  gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiM_g1.matrix,
-			 Qi, 0.0, QiMQi_g1);
-	  gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiM_e1.matrix,
-			 Qi, 0.0, QiMQi_e1);
-	  
-	  gsl_matrix_view QiMQi_g1_s =
-	    gsl_matrix_submatrix (QiMQi_g1, (c_size-1)*d_size,
-				  (c_size-1)*d_size, d_size, d_size);
-	  gsl_matrix_view QiMQi_e1_s =
-	    gsl_matrix_submatrix (QiMQi_e1, (c_size-1)*d_size,
-				  (c_size-1)*d_size, d_size, d_size);
-
-	  // Calculate trCg1 and trCe1.
-	  gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiMQi_g1_s.matrix,
-			 Qi_si, 0.0, QiMQisQisi_g1);
-	  trCg1=0.0;
-	  for (size_t k=0; k<d_size; k++) {
-	    trCg1-=gsl_matrix_get (QiMQisQisi_g1, k, k);
-	  }
-
-	  gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiMQi_e1_s.matrix,
-			 Qi_si, 0.0, QiMQisQisi_e1);
-	  trCe1=0.0;
-	  for (size_t k=0; k<d_size; k++) {
-	    trCe1-=gsl_matrix_get (QiMQisQisi_e1, k, k);
-	  }
-
-	  for (size_t v2=0; v2<v_size; v2++) {
-	    if (v2<v1) {continue;}
-	    
-	    // Calculate Qi(xHiDHix)Qi, and subpart of it.
-	    gsl_matrix_const_view QiM_g2 =
-	      gsl_matrix_const_submatrix (QixHiDHix_all_g, 0, v2*dc_size,
-					  dc_size, dc_size);
-	    gsl_matrix_const_view QiM_e2 =
-	      gsl_matrix_const_submatrix (QixHiDHix_all_e, 0, v2*dc_size,
-					  dc_size, dc_size);
-	    
-	    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiM_g2.matrix,
-			   Qi, 0.0, QiMQi_g2);
-	    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiM_e2.matrix,
-			   Qi, 0.0, QiMQi_e2);
-	    
-	    gsl_matrix_view QiMQi_g2_s =
-	      gsl_matrix_submatrix (QiMQi_g2, (c_size-1)*d_size,
-				    (c_size-1)*d_size, d_size, d_size);
-	    gsl_matrix_view QiMQi_e2_s =
-	      gsl_matrix_submatrix (QiMQi_e2, (c_size-1)*d_size,
-				    (c_size-1)*d_size, d_size, d_size);
-	    
-	    // Calculate trCg2 and trCe2.
-	    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0,
-			   &QiMQi_g2_s.matrix, Qi_si, 0.0, QiMQisQisi_g2);
-	    trCg2=0.0;
-	    for (size_t k=0; k<d_size; k++) {
-	      trCg2-=gsl_matrix_get (QiMQisQisi_g2, k, k);
-	    }
-	    
-	    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0,
-			   &QiMQi_e2_s.matrix, Qi_si, 0.0, QiMQisQisi_e2);
-	    trCe2=0.0;
-	    for (size_t k=0; k<d_size; k++) {
-	      trCe2-=gsl_matrix_get (QiMQisQisi_e2, k, k);
-	    }
-	    
-	    // Calculate trCC_gg, trCC_ge, trCC_ee.
-	    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0,
-			   QiMQisQisi_g1, QiMQisQisi_g2, 0.0, M_dd);
-	    trCC_gg=0.0;
-	    for (size_t k=0; k<d_size; k++) {
-	      trCC_gg+=gsl_matrix_get (M_dd, k, k);
-	    }
-	    
-	    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, QiMQisQisi_g1,
-			   QiMQisQisi_e2, 0.0, M_dd);
-	    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, QiMQisQisi_e1,
-			   QiMQisQisi_g2, 1.0, M_dd);
-	    trCC_ge=0.0;
-	    for (size_t k=0; k<d_size; k++) {
-	      trCC_ge+=gsl_matrix_get (M_dd, k, k);
-	    }
-	    
-	    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, QiMQisQisi_e1,
-			   QiMQisQisi_e2, 0.0, M_dd);
-	    trCC_ee=0.0;
-	    for (size_t k=0; k<d_size; k++) {
-	      trCC_ee+=gsl_matrix_get (M_dd, k, k);
-	    }
-	    
-	    // Calculate Qi(xHiDHix)Qi(xHiDHix)Qi, and subpart of it.
-	    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiM_g1.matrix,
-			   QiMQi_g2, 0.0, QiMQiMQi_gg);
-	    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiM_g1.matrix,
-			   QiMQi_e2, 0.0, QiMQiMQi_ge);
-	    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiM_e1.matrix,
-			   QiMQi_g2, 1.0, QiMQiMQi_ge);
-	    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiM_e1.matrix,
-			   QiMQi_e2, 0.0, QiMQiMQi_ee);
-	    
-	    gsl_matrix_view QiMQiMQi_gg_s =
-	      gsl_matrix_submatrix (QiMQiMQi_gg, (c_size-1)*d_size,
-				    (c_size-1)*d_size, d_size, d_size);
-	    gsl_matrix_view QiMQiMQi_ge_s =
-	      gsl_matrix_submatrix (QiMQiMQi_ge, (c_size-1)*d_size,
-				    (c_size-1)*d_size, d_size, d_size);
-	    gsl_matrix_view QiMQiMQi_ee_s =
-	      gsl_matrix_submatrix (QiMQiMQi_ee, (c_size-1)*d_size,
-				    (c_size-1)*d_size, d_size, d_size);
-	    
-	    // and part of trB_gg, trB_ge, trB_ee.
-	    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0,
-			   &QiMQiMQi_gg_s.matrix, Qi_si, 0.0, M_dd);
-	    trB_gg=0.0;
-	    for (size_t k=0; k<d_size; k++) {
-	      d=gsl_matrix_get (M_dd, k, k);
-	      trB_gg-=d;
-	    }
-	    
-	    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0,
-			   &QiMQiMQi_ge_s.matrix, Qi_si, 0.0, M_dd);
-	    trB_ge=0.0;
-	    for (size_t k=0; k<d_size; k++) {
-	      d=gsl_matrix_get (M_dd, k, k);
-	      trB_ge-=d;
-	    }
-	    
-	    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0,
-			   &QiMQiMQi_ee_s.matrix, Qi_si, 0.0, M_dd);
-	    trB_ee=0.0;
-	    for (size_t k=0; k<d_size; k++) {
-	      d=gsl_matrix_get (M_dd, k, k);
-	      trB_ee-=d;
-	    }
-	    
-	    // Calculate Qi(xHiDHiDHix)Qi, and subpart of it.
-	    gsl_matrix_const_view MM_gg =
-	      gsl_matrix_const_submatrix (xHiDHiDHix_all_gg, 0,
-					  (v1*v_size+v2)*dc_size, dc_size,
-					  dc_size);
-	    gsl_matrix_const_view MM_ge =
-	      gsl_matrix_const_submatrix (xHiDHiDHix_all_ge, 0,
-					  (v1*v_size+v2)*dc_size, dc_size,
-					  dc_size);
-	    gsl_matrix_const_view MM_ee =
-	      gsl_matrix_const_submatrix (xHiDHiDHix_all_ee, 0,
-					  (v1*v_size+v2)*dc_size, dc_size,
-					  dc_size);
-	    
-	    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Qi,
-			   &MM_gg.matrix, 0.0, M_dcdc);
-	    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, M_dcdc, Qi, 0.0,
-			   QiMMQi_gg);
-	    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Qi,
-			   &MM_ge.matrix, 0.0, M_dcdc);
-	    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, M_dcdc,
-			   Qi, 0.0, QiMMQi_ge);
-	    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Qi,
-			   &MM_ee.matrix, 0.0, M_dcdc);
-	    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, M_dcdc, Qi,
-			   0.0, QiMMQi_ee);
-	    
-	    gsl_matrix_view QiMMQi_gg_s =
-	      gsl_matrix_submatrix (QiMMQi_gg, (c_size-1)*d_size,
-				    (c_size-1)*d_size, d_size, d_size);
-	    gsl_matrix_view QiMMQi_ge_s =
-	      gsl_matrix_submatrix (QiMMQi_ge, (c_size-1)*d_size,
-				    (c_size-1)*d_size, d_size, d_size);
-	    gsl_matrix_view QiMMQi_ee_s =
-	      gsl_matrix_submatrix (QiMMQi_ee, (c_size-1)*d_size,
-				    (c_size-1)*d_size, d_size, d_size);
-	    
-	    // Calculate the other part of trB_gg, trB_ge, trB_ee.
-	    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0,
-			   &QiMMQi_gg_s.matrix, Qi_si, 0.0, M_dd);
-	    for (size_t k=0; k<d_size; k++) {
-	      trB_gg+=gsl_matrix_get (M_dd, k, k);
-	    }
-	    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0,
-			   &QiMMQi_ge_s.matrix, Qi_si, 0.0, M_dd);
-	    for (size_t k=0; k<d_size; k++) {
-	      trB_ge+=2.0*gsl_matrix_get (M_dd, k, k);
-	    }
-	    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0,
-			   &QiMMQi_ee_s.matrix, Qi_si, 0.0, M_dd);
-	    for (size_t k=0; k<d_size; k++) {
-	      trB_ee+=gsl_matrix_get (M_dd, k, k);
-	    }
-	    
-	    // Calculate trD_gg, trD_ge, trD_ee.
-	    trD_gg=2.0*trB_gg;
-	    trD_ge=2.0*trB_ge;
-	    trD_ee=2.0*trB_ee;
-	    
-	    //calculate B, C and D
-	    h_gg=-1.0*gsl_matrix_get (Hessian_inv, v1, v2);
-	    h_ge=-1.0*gsl_matrix_get (Hessian_inv, v1, v2+v_size);
-	    h_ee=-1.0*gsl_matrix_get (Hessian_inv, v1+v_size, v2+v_size);
-	    
-	    B+=h_gg*trB_gg+h_ge*trB_ge+h_ee*trB_ee;
-	    C+=h_gg*(trCC_gg+0.5*trCg1*trCg2) +
-	      h_ge*(trCC_ge+0.5*trCg1*trCe2+0.5*trCe1*trCg2) +
-	      h_ee*(trCC_ee+0.5*trCe1*trCe2);
-	    D+=h_gg*(trCC_gg+0.5*trD_gg) +
-	      h_ge*(trCC_ge+0.5*trD_ge) + h_ee*(trCC_ee+0.5*trD_ee);
-	    
-	    if (v1!=v2) {
-	      B+=h_gg*trB_gg+h_ge*trB_ge+h_ee*trB_ee;
-	      C+=h_gg*(trCC_gg+0.5*trCg1*trCg2) +
-		h_ge*(trCC_ge+0.5*trCg1*trCe2+0.5*trCe1*trCg2) +
-		h_ee*(trCC_ee+0.5*trCe1*trCe2);
-	      D+=h_gg*(trCC_gg+0.5*trD_gg) +
-		h_ge*(trCC_ge+0.5*trD_ge) +
-		h_ee*(trCC_ee+0.5*trD_ee);
-	    }
-	  }
-	}
-
-	// Calculate a, b, c from B C D.
-	crt_a=2.0*D-C;
-	crt_b=2.0*B;
-	crt_c=C;
-
-	// Free matrix memory.
-	gsl_matrix_free(QiMQi_g1);
-	gsl_matrix_free(QiMQi_e1);
-	gsl_matrix_free(QiMQi_g2);
-	gsl_matrix_free(QiMQi_e2);
-
-	gsl_matrix_free(QiMQisQisi_g1);
-	gsl_matrix_free(QiMQisQisi_e1);
-	gsl_matrix_free(QiMQisQisi_g2);
-	gsl_matrix_free(QiMQisQisi_e2);
-
-	gsl_matrix_free(QiMQiMQi_gg);
-	gsl_matrix_free(QiMQiMQi_ge);
-	gsl_matrix_free(QiMQiMQi_ee);
-
-	gsl_matrix_free(QiMMQi_gg);
-	gsl_matrix_free(QiMMQi_ge);
-	gsl_matrix_free(QiMMQi_ee);
-
-	gsl_matrix_free(Qi_si);
-
-	gsl_matrix_free(M_dd);
-	gsl_matrix_free(M_dcdc);
-
-	return;
+void CalcCRT(const gsl_matrix *Hessian_inv, const gsl_matrix *Qi,
+             const gsl_matrix *QixHiDHix_all_g,
+             const gsl_matrix *QixHiDHix_all_e,
+             const gsl_matrix *xHiDHiDHix_all_gg,
+             const gsl_matrix *xHiDHiDHix_all_ee,
+             const gsl_matrix *xHiDHiDHix_all_ge, const size_t d_size,
+             double &crt_a, double &crt_b, double &crt_c) {
+  crt_a = 0.0;
+  crt_b = 0.0;
+  crt_c = 0.0;
+
+  size_t dc_size = Qi->size1, v_size = Hessian_inv->size1 / 2;
+  size_t c_size = dc_size / d_size;
+  double h_gg, h_ge, h_ee, d, B = 0.0, C = 0.0, D = 0.0;
+  double trCg1, trCe1, trCg2, trCe2, trB_gg, trB_ge, trB_ee;
+  double trCC_gg, trCC_ge, trCC_ee, trD_gg = 0.0, trD_ge = 0.0, trD_ee = 0.0;
+
+  gsl_matrix *QiMQi_g1 = gsl_matrix_alloc(dc_size, dc_size);
+  gsl_matrix *QiMQi_e1 = gsl_matrix_alloc(dc_size, dc_size);
+  gsl_matrix *QiMQi_g2 = gsl_matrix_alloc(dc_size, dc_size);
+  gsl_matrix *QiMQi_e2 = gsl_matrix_alloc(dc_size, dc_size);
+
+  gsl_matrix *QiMQisQisi_g1 = gsl_matrix_alloc(d_size, d_size);
+  gsl_matrix *QiMQisQisi_e1 = gsl_matrix_alloc(d_size, d_size);
+  gsl_matrix *QiMQisQisi_g2 = gsl_matrix_alloc(d_size, d_size);
+  gsl_matrix *QiMQisQisi_e2 = gsl_matrix_alloc(d_size, d_size);
+
+  gsl_matrix *QiMQiMQi_gg = gsl_matrix_alloc(dc_size, dc_size);
+  gsl_matrix *QiMQiMQi_ge = gsl_matrix_alloc(dc_size, dc_size);
+  gsl_matrix *QiMQiMQi_ee = gsl_matrix_alloc(dc_size, dc_size);
+
+  gsl_matrix *QiMMQi_gg = gsl_matrix_alloc(dc_size, dc_size);
+  gsl_matrix *QiMMQi_ge = gsl_matrix_alloc(dc_size, dc_size);
+  gsl_matrix *QiMMQi_ee = gsl_matrix_alloc(dc_size, dc_size);
+
+  gsl_matrix *Qi_si = gsl_matrix_alloc(d_size, d_size);
+
+  gsl_matrix *M_dd = gsl_matrix_alloc(d_size, d_size);
+  gsl_matrix *M_dcdc = gsl_matrix_alloc(dc_size, dc_size);
+
+  // Invert Qi_sub to Qi_si.
+  gsl_matrix *Qi_sub = gsl_matrix_alloc(d_size, d_size);
+
+  gsl_matrix_const_view Qi_s = gsl_matrix_const_submatrix(
+      Qi, (c_size - 1) * d_size, (c_size - 1) * d_size, d_size, d_size);
+
+  int sig;
+  gsl_permutation *pmt = gsl_permutation_alloc(d_size);
+
+  gsl_matrix_memcpy(Qi_sub, &Qi_s.matrix);
+  LUDecomp(Qi_sub, pmt, &sig);
+  LUInvert(Qi_sub, pmt, Qi_si);
+
+  gsl_permutation_free(pmt);
+  gsl_matrix_free(Qi_sub);
+
+  // Calculate correction factors.
+  for (size_t v1 = 0; v1 < v_size; v1++) {
+
+    // Calculate Qi(xHiDHix)Qi, and subpart of it.
+    gsl_matrix_const_view QiM_g1 = gsl_matrix_const_submatrix(
+        QixHiDHix_all_g, 0, v1 * dc_size, dc_size, dc_size);
+    gsl_matrix_const_view QiM_e1 = gsl_matrix_const_submatrix(
+        QixHiDHix_all_e, 0, v1 * dc_size, dc_size, dc_size);
+
+    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiM_g1.matrix, Qi, 0.0,
+                   QiMQi_g1);
+    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiM_e1.matrix, Qi, 0.0,
+                   QiMQi_e1);
+
+    gsl_matrix_view QiMQi_g1_s = gsl_matrix_submatrix(
+        QiMQi_g1, (c_size - 1) * d_size, (c_size - 1) * d_size, d_size, d_size);
+    gsl_matrix_view QiMQi_e1_s = gsl_matrix_submatrix(
+        QiMQi_e1, (c_size - 1) * d_size, (c_size - 1) * d_size, d_size, d_size);
+
+    // Calculate trCg1 and trCe1.
+    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiMQi_g1_s.matrix, Qi_si,
+                   0.0, QiMQisQisi_g1);
+    trCg1 = 0.0;
+    for (size_t k = 0; k < d_size; k++) {
+      trCg1 -= gsl_matrix_get(QiMQisQisi_g1, k, k);
+    }
+
+    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiMQi_e1_s.matrix, Qi_si,
+                   0.0, QiMQisQisi_e1);
+    trCe1 = 0.0;
+    for (size_t k = 0; k < d_size; k++) {
+      trCe1 -= gsl_matrix_get(QiMQisQisi_e1, k, k);
+    }
+
+    for (size_t v2 = 0; v2 < v_size; v2++) {
+      if (v2 < v1) {
+        continue;
+      }
+
+      // Calculate Qi(xHiDHix)Qi, and subpart of it.
+      gsl_matrix_const_view QiM_g2 = gsl_matrix_const_submatrix(
+          QixHiDHix_all_g, 0, v2 * dc_size, dc_size, dc_size);
+      gsl_matrix_const_view QiM_e2 = gsl_matrix_const_submatrix(
+          QixHiDHix_all_e, 0, v2 * dc_size, dc_size, dc_size);
+
+      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiM_g2.matrix, Qi, 0.0,
+                     QiMQi_g2);
+      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiM_e2.matrix, Qi, 0.0,
+                     QiMQi_e2);
+
+      gsl_matrix_view QiMQi_g2_s =
+          gsl_matrix_submatrix(QiMQi_g2, (c_size - 1) * d_size,
+                               (c_size - 1) * d_size, d_size, d_size);
+      gsl_matrix_view QiMQi_e2_s =
+          gsl_matrix_submatrix(QiMQi_e2, (c_size - 1) * d_size,
+                               (c_size - 1) * d_size, d_size, d_size);
+
+      // Calculate trCg2 and trCe2.
+      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiMQi_g2_s.matrix, Qi_si,
+                     0.0, QiMQisQisi_g2);
+      trCg2 = 0.0;
+      for (size_t k = 0; k < d_size; k++) {
+        trCg2 -= gsl_matrix_get(QiMQisQisi_g2, k, k);
+      }
+
+      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiMQi_e2_s.matrix, Qi_si,
+                     0.0, QiMQisQisi_e2);
+      trCe2 = 0.0;
+      for (size_t k = 0; k < d_size; k++) {
+        trCe2 -= gsl_matrix_get(QiMQisQisi_e2, k, k);
+      }
+
+      // Calculate trCC_gg, trCC_ge, trCC_ee.
+      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, QiMQisQisi_g1,
+                     QiMQisQisi_g2, 0.0, M_dd);
+      trCC_gg = 0.0;
+      for (size_t k = 0; k < d_size; k++) {
+        trCC_gg += gsl_matrix_get(M_dd, k, k);
+      }
+
+      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, QiMQisQisi_g1,
+                     QiMQisQisi_e2, 0.0, M_dd);
+      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, QiMQisQisi_e1,
+                     QiMQisQisi_g2, 1.0, M_dd);
+      trCC_ge = 0.0;
+      for (size_t k = 0; k < d_size; k++) {
+        trCC_ge += gsl_matrix_get(M_dd, k, k);
+      }
+
+      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, QiMQisQisi_e1,
+                     QiMQisQisi_e2, 0.0, M_dd);
+      trCC_ee = 0.0;
+      for (size_t k = 0; k < d_size; k++) {
+        trCC_ee += gsl_matrix_get(M_dd, k, k);
+      }
+
+      // Calculate Qi(xHiDHix)Qi(xHiDHix)Qi, and subpart of it.
+      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiM_g1.matrix, QiMQi_g2,
+                     0.0, QiMQiMQi_gg);
+      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiM_g1.matrix, QiMQi_e2,
+                     0.0, QiMQiMQi_ge);
+      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiM_e1.matrix, QiMQi_g2,
+                     1.0, QiMQiMQi_ge);
+      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiM_e1.matrix, QiMQi_e2,
+                     0.0, QiMQiMQi_ee);
+
+      gsl_matrix_view QiMQiMQi_gg_s =
+          gsl_matrix_submatrix(QiMQiMQi_gg, (c_size - 1) * d_size,
+                               (c_size - 1) * d_size, d_size, d_size);
+      gsl_matrix_view QiMQiMQi_ge_s =
+          gsl_matrix_submatrix(QiMQiMQi_ge, (c_size - 1) * d_size,
+                               (c_size - 1) * d_size, d_size, d_size);
+      gsl_matrix_view QiMQiMQi_ee_s =
+          gsl_matrix_submatrix(QiMQiMQi_ee, (c_size - 1) * d_size,
+                               (c_size - 1) * d_size, d_size, d_size);
+
+      // and part of trB_gg, trB_ge, trB_ee.
+      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiMQiMQi_gg_s.matrix,
+                     Qi_si, 0.0, M_dd);
+      trB_gg = 0.0;
+      for (size_t k = 0; k < d_size; k++) {
+        d = gsl_matrix_get(M_dd, k, k);
+        trB_gg -= d;
+      }
+
+      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiMQiMQi_ge_s.matrix,
+                     Qi_si, 0.0, M_dd);
+      trB_ge = 0.0;
+      for (size_t k = 0; k < d_size; k++) {
+        d = gsl_matrix_get(M_dd, k, k);
+        trB_ge -= d;
+      }
+
+      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiMQiMQi_ee_s.matrix,
+                     Qi_si, 0.0, M_dd);
+      trB_ee = 0.0;
+      for (size_t k = 0; k < d_size; k++) {
+        d = gsl_matrix_get(M_dd, k, k);
+        trB_ee -= d;
+      }
+
+      // Calculate Qi(xHiDHiDHix)Qi, and subpart of it.
+      gsl_matrix_const_view MM_gg = gsl_matrix_const_submatrix(
+          xHiDHiDHix_all_gg, 0, (v1 * v_size + v2) * dc_size, dc_size, dc_size);
+      gsl_matrix_const_view MM_ge = gsl_matrix_const_submatrix(
+          xHiDHiDHix_all_ge, 0, (v1 * v_size + v2) * dc_size, dc_size, dc_size);
+      gsl_matrix_const_view MM_ee = gsl_matrix_const_submatrix(
+          xHiDHiDHix_all_ee, 0, (v1 * v_size + v2) * dc_size, dc_size, dc_size);
+
+      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Qi, &MM_gg.matrix, 0.0,
+                     M_dcdc);
+      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, M_dcdc, Qi, 0.0,
+                     QiMMQi_gg);
+      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Qi, &MM_ge.matrix, 0.0,
+                     M_dcdc);
+      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, M_dcdc, Qi, 0.0,
+                     QiMMQi_ge);
+      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Qi, &MM_ee.matrix, 0.0,
+                     M_dcdc);
+      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, M_dcdc, Qi, 0.0,
+                     QiMMQi_ee);
+
+      gsl_matrix_view QiMMQi_gg_s =
+          gsl_matrix_submatrix(QiMMQi_gg, (c_size - 1) * d_size,
+                               (c_size - 1) * d_size, d_size, d_size);
+      gsl_matrix_view QiMMQi_ge_s =
+          gsl_matrix_submatrix(QiMMQi_ge, (c_size - 1) * d_size,
+                               (c_size - 1) * d_size, d_size, d_size);
+      gsl_matrix_view QiMMQi_ee_s =
+          gsl_matrix_submatrix(QiMMQi_ee, (c_size - 1) * d_size,
+                               (c_size - 1) * d_size, d_size, d_size);
+
+      // Calculate the other part of trB_gg, trB_ge, trB_ee.
+      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiMMQi_gg_s.matrix,
+                     Qi_si, 0.0, M_dd);
+      for (size_t k = 0; k < d_size; k++) {
+        trB_gg += gsl_matrix_get(M_dd, k, k);
+      }
+      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiMMQi_ge_s.matrix,
+                     Qi_si, 0.0, M_dd);
+      for (size_t k = 0; k < d_size; k++) {
+        trB_ge += 2.0 * gsl_matrix_get(M_dd, k, k);
+      }
+      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &QiMMQi_ee_s.matrix,
+                     Qi_si, 0.0, M_dd);
+      for (size_t k = 0; k < d_size; k++) {
+        trB_ee += gsl_matrix_get(M_dd, k, k);
+      }
+
+      // Calculate trD_gg, trD_ge, trD_ee.
+      trD_gg = 2.0 * trB_gg;
+      trD_ge = 2.0 * trB_ge;
+      trD_ee = 2.0 * trB_ee;
+
+      // calculate B, C and D
+      h_gg = -1.0 * gsl_matrix_get(Hessian_inv, v1, v2);
+      h_ge = -1.0 * gsl_matrix_get(Hessian_inv, v1, v2 + v_size);
+      h_ee = -1.0 * gsl_matrix_get(Hessian_inv, v1 + v_size, v2 + v_size);
+
+      B += h_gg * trB_gg + h_ge * trB_ge + h_ee * trB_ee;
+      C += h_gg * (trCC_gg + 0.5 * trCg1 * trCg2) +
+           h_ge * (trCC_ge + 0.5 * trCg1 * trCe2 + 0.5 * trCe1 * trCg2) +
+           h_ee * (trCC_ee + 0.5 * trCe1 * trCe2);
+      D += h_gg * (trCC_gg + 0.5 * trD_gg) + h_ge * (trCC_ge + 0.5 * trD_ge) +
+           h_ee * (trCC_ee + 0.5 * trD_ee);
+
+      if (v1 != v2) {
+        B += h_gg * trB_gg + h_ge * trB_ge + h_ee * trB_ee;
+        C += h_gg * (trCC_gg + 0.5 * trCg1 * trCg2) +
+             h_ge * (trCC_ge + 0.5 * trCg1 * trCe2 + 0.5 * trCe1 * trCg2) +
+             h_ee * (trCC_ee + 0.5 * trCe1 * trCe2);
+        D += h_gg * (trCC_gg + 0.5 * trD_gg) + h_ge * (trCC_ge + 0.5 * trD_ge) +
+             h_ee * (trCC_ee + 0.5 * trD_ee);
+      }
+    }
+  }
+
+  // Calculate a, b, c from B C D.
+  crt_a = 2.0 * D - C;
+  crt_b = 2.0 * B;
+  crt_c = C;
+
+  // Free matrix memory.
+  gsl_matrix_free(QiMQi_g1);
+  gsl_matrix_free(QiMQi_e1);
+  gsl_matrix_free(QiMQi_g2);
+  gsl_matrix_free(QiMQi_e2);
+
+  gsl_matrix_free(QiMQisQisi_g1);
+  gsl_matrix_free(QiMQisQisi_e1);
+  gsl_matrix_free(QiMQisQisi_g2);
+  gsl_matrix_free(QiMQisQisi_e2);
+
+  gsl_matrix_free(QiMQiMQi_gg);
+  gsl_matrix_free(QiMQiMQi_ge);
+  gsl_matrix_free(QiMQiMQi_ee);
+
+  gsl_matrix_free(QiMMQi_gg);
+  gsl_matrix_free(QiMMQi_ge);
+  gsl_matrix_free(QiMMQi_ee);
+
+  gsl_matrix_free(Qi_si);
+
+  gsl_matrix_free(M_dd);
+  gsl_matrix_free(M_dcdc);
+
+  return;
 }
 
 // Calculate first-order and second-order derivatives.
-void CalcDev (const char func_name, const gsl_vector *eval,
-	      const gsl_matrix *Qi, const gsl_matrix *Hi,
-	      const gsl_matrix *xHi, const gsl_matrix *Hiy,
-	      const gsl_vector *QixHiy, gsl_vector *gradient,
-	      gsl_matrix *Hessian_inv, double &crt_a, double &crt_b,
-	      double &crt_c) {
-	if (func_name!='R' && func_name!='L' && func_name!='r' &&
-	    func_name!='l') {
-	  cout<<"func_name only takes 'R' or 'L': 'R' for " <<
-	    "log-restricted likelihood, 'L' for log-likelihood."<<endl;
-	  return;
-	}
-
-	size_t dc_size=Qi->size1, d_size=Hi->size1;
-	size_t c_size=dc_size/d_size;
-	size_t v_size=d_size*(d_size+1)/2;
-	size_t v1, v2;
-	double dev1_g, dev1_e, dev2_gg, dev2_ee, dev2_ge;
-
-	gsl_matrix *Hessian=gsl_matrix_alloc (v_size*2, v_size*2);
-
-	gsl_matrix *xHiDHiy_all_g=gsl_matrix_alloc (dc_size, v_size);
-	gsl_matrix *xHiDHiy_all_e=gsl_matrix_alloc (dc_size, v_size);
-	gsl_matrix *xHiDHix_all_g=gsl_matrix_alloc (dc_size, v_size*dc_size);
-	gsl_matrix *xHiDHix_all_e=gsl_matrix_alloc (dc_size, v_size*dc_size);
-	gsl_matrix *xHiDHixQixHiy_all_g=gsl_matrix_alloc (dc_size, v_size);
-	gsl_matrix *xHiDHixQixHiy_all_e=gsl_matrix_alloc (dc_size, v_size);
-
-	gsl_matrix *QixHiDHiy_all_g=gsl_matrix_alloc (dc_size, v_size);
-	gsl_matrix *QixHiDHiy_all_e=gsl_matrix_alloc (dc_size, v_size);
-	gsl_matrix *QixHiDHix_all_g=gsl_matrix_alloc (dc_size, v_size*dc_size);
-	gsl_matrix *QixHiDHix_all_e=gsl_matrix_alloc (dc_size, v_size*dc_size);
-	gsl_matrix *QixHiDHixQixHiy_all_g=gsl_matrix_alloc (dc_size, v_size);
-	gsl_matrix *QixHiDHixQixHiy_all_e=gsl_matrix_alloc (dc_size, v_size);
-
-	gsl_matrix *xHiDHiDHiy_all_gg =
-	  gsl_matrix_alloc (dc_size, v_size*v_size);
-	gsl_matrix *xHiDHiDHiy_all_ee =
-	  gsl_matrix_alloc (dc_size, v_size*v_size);
-	gsl_matrix *xHiDHiDHiy_all_ge =
-	  gsl_matrix_alloc (dc_size, v_size*v_size);
-	gsl_matrix *xHiDHiDHix_all_gg =
-	  gsl_matrix_alloc (dc_size, v_size*v_size*dc_size);
-	gsl_matrix *xHiDHiDHix_all_ee =
-	  gsl_matrix_alloc (dc_size, v_size*v_size*dc_size);
-	gsl_matrix *xHiDHiDHix_all_ge =
-	  gsl_matrix_alloc (dc_size, v_size*v_size*dc_size);
-
-	// Calculate xHiDHiy_all, xHiDHix_all and xHiDHixQixHiy_all.
-	Calc_xHiDHiy_all (eval, xHi, Hiy, xHiDHiy_all_g, xHiDHiy_all_e);
-	Calc_xHiDHix_all (eval, xHi, xHiDHix_all_g, xHiDHix_all_e);
-	Calc_xHiDHixQixHiy_all (xHiDHix_all_g, xHiDHix_all_e, QixHiy,
-				xHiDHixQixHiy_all_g, xHiDHixQixHiy_all_e);
-
-	Calc_xHiDHiDHiy_all (v_size, eval, Hi, xHi, Hiy, xHiDHiDHiy_all_gg,
-			     xHiDHiDHiy_all_ee, xHiDHiDHiy_all_ge);
-	Calc_xHiDHiDHix_all (v_size, eval, Hi, xHi, xHiDHiDHix_all_gg,
-			     xHiDHiDHix_all_ee, xHiDHiDHix_all_ge);
-
-	// Calculate QixHiDHiy_all, QixHiDHix_all and QixHiDHixQixHiy_all.
-	Calc_QiVec_all (Qi, xHiDHiy_all_g, xHiDHiy_all_e, QixHiDHiy_all_g,
-			QixHiDHiy_all_e);
-	Calc_QiVec_all (Qi, xHiDHixQixHiy_all_g, xHiDHixQixHiy_all_e,
-			QixHiDHixQixHiy_all_g, QixHiDHixQixHiy_all_e);
-	Calc_QiMat_all (Qi, xHiDHix_all_g, xHiDHix_all_e, QixHiDHix_all_g,
-			QixHiDHix_all_e);
-
-	double tHiD_g, tHiD_e, tPD_g, tPD_e, tHiDHiD_gg, tHiDHiD_ee;
-	double tHiDHiD_ge, tPDPD_gg, tPDPD_ee, tPDPD_ge;
-	double yPDPy_g, yPDPy_e, yPDPDPy_gg, yPDPDPy_ee, yPDPDPy_ge;
-
-	// Calculate gradient and Hessian for Vg.
-	for (size_t i1=0; i1<d_size; i1++) {
-	  for (size_t j1=0; j1<d_size; j1++) {
-	    if (j1<i1) {continue;}
-	    v1=GetIndex (i1, j1, d_size);
-	    
-	    Calc_yPDPy (eval, Hiy, QixHiy, xHiDHiy_all_g, xHiDHiy_all_e,
-			xHiDHixQixHiy_all_g, xHiDHixQixHiy_all_e, i1, j1,
-			yPDPy_g, yPDPy_e);
-	    
-	    if (func_name=='R' || func_name=='r') {
-	      Calc_tracePD (eval, Qi, Hi, xHiDHix_all_g, xHiDHix_all_e,
-			    i1, j1, tPD_g, tPD_e);
-
-	      dev1_g=-0.5*tPD_g+0.5*yPDPy_g;
-	      dev1_e=-0.5*tPD_e+0.5*yPDPy_e;
-	    } else {
-	      Calc_traceHiD (eval, Hi, i1, j1, tHiD_g, tHiD_e);
-	      
-	      dev1_g=-0.5*tHiD_g+0.5*yPDPy_g;
-	      dev1_e=-0.5*tHiD_e+0.5*yPDPy_e;
-	    }
-	    
-	    gsl_vector_set (gradient, v1, dev1_g);
-	    gsl_vector_set (gradient, v1+v_size, dev1_e);
-	    
-	    for (size_t i2=0; i2<d_size; i2++) {
-	      for (size_t j2=0; j2<d_size; j2++) {
-		if (j2<i2) {continue;}
-		v2=GetIndex (i2, j2, d_size);
-		
-		if (v2<v1) {continue;}
-
-		Calc_yPDPDPy (eval, Hi, xHi, Hiy, QixHiy, xHiDHiy_all_g,
-			      xHiDHiy_all_e, QixHiDHiy_all_g, QixHiDHiy_all_e,
-			      xHiDHixQixHiy_all_g, xHiDHixQixHiy_all_e,
-			      QixHiDHixQixHiy_all_g, QixHiDHixQixHiy_all_e,
-			      xHiDHiDHiy_all_gg, xHiDHiDHiy_all_ee,
-			      xHiDHiDHiy_all_ge, xHiDHiDHix_all_gg,
-			      xHiDHiDHix_all_ee, xHiDHiDHix_all_ge, i1, j1,
-			      i2, j2, yPDPDPy_gg, yPDPDPy_ee, yPDPDPy_ge);
-		
-		// AI for REML.
-		if (func_name=='R' || func_name=='r') {
-		  Calc_tracePDPD (eval, Qi, Hi, xHi, QixHiDHix_all_g,
-				  QixHiDHix_all_e, xHiDHiDHix_all_gg,
-				  xHiDHiDHix_all_ee, xHiDHiDHix_all_ge, i1, j1,
-				  i2, j2, tPDPD_gg, tPDPD_ee, tPDPD_ge);
-		  
-		  dev2_gg=0.5*tPDPD_gg-yPDPDPy_gg;
-		  dev2_ee=0.5*tPDPD_ee-yPDPDPy_ee;
-		  dev2_ge=0.5*tPDPD_ge-yPDPDPy_ge;
-		} else {
-		  Calc_traceHiDHiD (eval, Hi, i1, j1, i2, j2, tHiDHiD_gg,
-				    tHiDHiD_ee, tHiDHiD_ge);
-		  
-		  dev2_gg=0.5*tHiDHiD_gg-yPDPDPy_gg;
-		  dev2_ee=0.5*tHiDHiD_ee-yPDPDPy_ee;
-		  dev2_ge=0.5*tHiDHiD_ge-yPDPDPy_ge;
-		}
-		
-		// Set up Hessian.
-		gsl_matrix_set (Hessian, v1, v2, dev2_gg);
-		gsl_matrix_set (Hessian, v1+v_size, v2+v_size, dev2_ee);
-		gsl_matrix_set (Hessian, v1, v2+v_size, dev2_ge);
-		gsl_matrix_set (Hessian, v2+v_size, v1, dev2_ge);
-		
-		if (v1!=v2) {
-		  gsl_matrix_set (Hessian, v2, v1, dev2_gg);
-		  gsl_matrix_set (Hessian, v2+v_size, v1+v_size, dev2_ee);
-		  gsl_matrix_set (Hessian, v2, v1+v_size, dev2_ge);
-		  gsl_matrix_set (Hessian, v1+v_size, v2, dev2_ge);
-		}
-	      }
-	    }
-	  }
-	}
-	
-	// Invert Hessian.
-	int sig;
-	gsl_permutation * pmt=gsl_permutation_alloc (v_size*2);
-
-	LUDecomp (Hessian, pmt, &sig);
-	LUInvert (Hessian, pmt, Hessian_inv);
-	
-	gsl_permutation_free(pmt);
-	gsl_matrix_free(Hessian);
-
-	// Calculate Edgeworth correction factors after inverting
-	// Hessian.
-	if (c_size>1) {
-	  CalcCRT(Hessian_inv, Qi, QixHiDHix_all_g, QixHiDHix_all_e,
-		  xHiDHiDHix_all_gg, xHiDHiDHix_all_ee, xHiDHiDHix_all_ge,
-		  d_size, crt_a, crt_b, crt_c);
-	} else {
-	  crt_a=0.0; crt_b=0.0; crt_c=0.0;
-	}
-	
-	gsl_matrix_free(xHiDHiy_all_g);
-	gsl_matrix_free(xHiDHiy_all_e);
-	gsl_matrix_free(xHiDHix_all_g);
-	gsl_matrix_free(xHiDHix_all_e);
-	gsl_matrix_free(xHiDHixQixHiy_all_g);
-	gsl_matrix_free(xHiDHixQixHiy_all_e);
-
-	gsl_matrix_free(QixHiDHiy_all_g);
-	gsl_matrix_free(QixHiDHiy_all_e);
-	gsl_matrix_free(QixHiDHix_all_g);
-	gsl_matrix_free(QixHiDHix_all_e);
-	gsl_matrix_free(QixHiDHixQixHiy_all_g);
-	gsl_matrix_free(QixHiDHixQixHiy_all_e);
-
-	gsl_matrix_free(xHiDHiDHiy_all_gg);
-	gsl_matrix_free(xHiDHiDHiy_all_ee);
-	gsl_matrix_free(xHiDHiDHiy_all_ge);
-	gsl_matrix_free(xHiDHiDHix_all_gg);
-	gsl_matrix_free(xHiDHiDHix_all_ee);
-	gsl_matrix_free(xHiDHiDHix_all_ge);
-
-	return;
+void CalcDev(const char func_name, const gsl_vector *eval, const gsl_matrix *Qi,
+             const gsl_matrix *Hi, const gsl_matrix *xHi, const gsl_matrix *Hiy,
+             const gsl_vector *QixHiy, gsl_vector *gradient,
+             gsl_matrix *Hessian_inv, double &crt_a, double &crt_b,
+             double &crt_c) {
+  if (func_name != 'R' && func_name != 'L' && func_name != 'r' &&
+      func_name != 'l') {
+    cout << "func_name only takes 'R' or 'L': 'R' for "
+         << "log-restricted likelihood, 'L' for log-likelihood." << endl;
+    return;
+  }
+
+  size_t dc_size = Qi->size1, d_size = Hi->size1;
+  size_t c_size = dc_size / d_size;
+  size_t v_size = d_size * (d_size + 1) / 2;
+  size_t v1, v2;
+  double dev1_g, dev1_e, dev2_gg, dev2_ee, dev2_ge;
+
+  gsl_matrix *Hessian = gsl_matrix_alloc(v_size * 2, v_size * 2);
+
+  gsl_matrix *xHiDHiy_all_g = gsl_matrix_alloc(dc_size, v_size);
+  gsl_matrix *xHiDHiy_all_e = gsl_matrix_alloc(dc_size, v_size);
+  gsl_matrix *xHiDHix_all_g = gsl_matrix_alloc(dc_size, v_size * dc_size);
+  gsl_matrix *xHiDHix_all_e = gsl_matrix_alloc(dc_size, v_size * dc_size);
+  gsl_matrix *xHiDHixQixHiy_all_g = gsl_matrix_alloc(dc_size, v_size);
+  gsl_matrix *xHiDHixQixHiy_all_e = gsl_matrix_alloc(dc_size, v_size);
+
+  gsl_matrix *QixHiDHiy_all_g = gsl_matrix_alloc(dc_size, v_size);
+  gsl_matrix *QixHiDHiy_all_e = gsl_matrix_alloc(dc_size, v_size);
+  gsl_matrix *QixHiDHix_all_g = gsl_matrix_alloc(dc_size, v_size * dc_size);
+  gsl_matrix *QixHiDHix_all_e = gsl_matrix_alloc(dc_size, v_size * dc_size);
+  gsl_matrix *QixHiDHixQixHiy_all_g = gsl_matrix_alloc(dc_size, v_size);
+  gsl_matrix *QixHiDHixQixHiy_all_e = gsl_matrix_alloc(dc_size, v_size);
+
+  gsl_matrix *xHiDHiDHiy_all_gg = gsl_matrix_alloc(dc_size, v_size * v_size);
+  gsl_matrix *xHiDHiDHiy_all_ee = gsl_matrix_alloc(dc_size, v_size * v_size);
+  gsl_matrix *xHiDHiDHiy_all_ge = gsl_matrix_alloc(dc_size, v_size * v_size);
+  gsl_matrix *xHiDHiDHix_all_gg =
+      gsl_matrix_alloc(dc_size, v_size * v_size * dc_size);
+  gsl_matrix *xHiDHiDHix_all_ee =
+      gsl_matrix_alloc(dc_size, v_size * v_size * dc_size);
+  gsl_matrix *xHiDHiDHix_all_ge =
+      gsl_matrix_alloc(dc_size, v_size * v_size * dc_size);
+
+  // Calculate xHiDHiy_all, xHiDHix_all and xHiDHixQixHiy_all.
+  Calc_xHiDHiy_all(eval, xHi, Hiy, xHiDHiy_all_g, xHiDHiy_all_e);
+  Calc_xHiDHix_all(eval, xHi, xHiDHix_all_g, xHiDHix_all_e);
+  Calc_xHiDHixQixHiy_all(xHiDHix_all_g, xHiDHix_all_e, QixHiy,
+                         xHiDHixQixHiy_all_g, xHiDHixQixHiy_all_e);
+
+  Calc_xHiDHiDHiy_all(v_size, eval, Hi, xHi, Hiy, xHiDHiDHiy_all_gg,
+                      xHiDHiDHiy_all_ee, xHiDHiDHiy_all_ge);
+  Calc_xHiDHiDHix_all(v_size, eval, Hi, xHi, xHiDHiDHix_all_gg,
+                      xHiDHiDHix_all_ee, xHiDHiDHix_all_ge);
+
+  // Calculate QixHiDHiy_all, QixHiDHix_all and QixHiDHixQixHiy_all.
+  Calc_QiVec_all(Qi, xHiDHiy_all_g, xHiDHiy_all_e, QixHiDHiy_all_g,
+                 QixHiDHiy_all_e);
+  Calc_QiVec_all(Qi, xHiDHixQixHiy_all_g, xHiDHixQixHiy_all_e,
+                 QixHiDHixQixHiy_all_g, QixHiDHixQixHiy_all_e);
+  Calc_QiMat_all(Qi, xHiDHix_all_g, xHiDHix_all_e, QixHiDHix_all_g,
+                 QixHiDHix_all_e);
+
+  double tHiD_g, tHiD_e, tPD_g, tPD_e, tHiDHiD_gg, tHiDHiD_ee;
+  double tHiDHiD_ge, tPDPD_gg, tPDPD_ee, tPDPD_ge;
+  double yPDPy_g, yPDPy_e, yPDPDPy_gg, yPDPDPy_ee, yPDPDPy_ge;
+
+  // Calculate gradient and Hessian for Vg.
+  for (size_t i1 = 0; i1 < d_size; i1++) {
+    for (size_t j1 = 0; j1 < d_size; j1++) {
+      if (j1 < i1) {
+        continue;
+      }
+      v1 = GetIndex(i1, j1, d_size);
+
+      Calc_yPDPy(eval, Hiy, QixHiy, xHiDHiy_all_g, xHiDHiy_all_e,
+                 xHiDHixQixHiy_all_g, xHiDHixQixHiy_all_e, i1, j1, yPDPy_g,
+                 yPDPy_e);
+
+      if (func_name == 'R' || func_name == 'r') {
+        Calc_tracePD(eval, Qi, Hi, xHiDHix_all_g, xHiDHix_all_e, i1, j1, tPD_g,
+                     tPD_e);
+
+        dev1_g = -0.5 * tPD_g + 0.5 * yPDPy_g;
+        dev1_e = -0.5 * tPD_e + 0.5 * yPDPy_e;
+      } else {
+        Calc_traceHiD(eval, Hi, i1, j1, tHiD_g, tHiD_e);
+
+        dev1_g = -0.5 * tHiD_g + 0.5 * yPDPy_g;
+        dev1_e = -0.5 * tHiD_e + 0.5 * yPDPy_e;
+      }
+
+      gsl_vector_set(gradient, v1, dev1_g);
+      gsl_vector_set(gradient, v1 + v_size, dev1_e);
+
+      for (size_t i2 = 0; i2 < d_size; i2++) {
+        for (size_t j2 = 0; j2 < d_size; j2++) {
+          if (j2 < i2) {
+            continue;
+          }
+          v2 = GetIndex(i2, j2, d_size);
+
+          if (v2 < v1) {
+            continue;
+          }
+
+          Calc_yPDPDPy(eval, Hi, xHi, Hiy, QixHiy, xHiDHiy_all_g, xHiDHiy_all_e,
+                       QixHiDHiy_all_g, QixHiDHiy_all_e, xHiDHixQixHiy_all_g,
+                       xHiDHixQixHiy_all_e, QixHiDHixQixHiy_all_g,
+                       QixHiDHixQixHiy_all_e, xHiDHiDHiy_all_gg,
+                       xHiDHiDHiy_all_ee, xHiDHiDHiy_all_ge, xHiDHiDHix_all_gg,
+                       xHiDHiDHix_all_ee, xHiDHiDHix_all_ge, i1, j1, i2, j2,
+                       yPDPDPy_gg, yPDPDPy_ee, yPDPDPy_ge);
+
+          // AI for REML.
+          if (func_name == 'R' || func_name == 'r') {
+            Calc_tracePDPD(eval, Qi, Hi, xHi, QixHiDHix_all_g, QixHiDHix_all_e,
+                           xHiDHiDHix_all_gg, xHiDHiDHix_all_ee,
+                           xHiDHiDHix_all_ge, i1, j1, i2, j2, tPDPD_gg,
+                           tPDPD_ee, tPDPD_ge);
+
+            dev2_gg = 0.5 * tPDPD_gg - yPDPDPy_gg;
+            dev2_ee = 0.5 * tPDPD_ee - yPDPDPy_ee;
+            dev2_ge = 0.5 * tPDPD_ge - yPDPDPy_ge;
+          } else {
+            Calc_traceHiDHiD(eval, Hi, i1, j1, i2, j2, tHiDHiD_gg, tHiDHiD_ee,
+                             tHiDHiD_ge);
+
+            dev2_gg = 0.5 * tHiDHiD_gg - yPDPDPy_gg;
+            dev2_ee = 0.5 * tHiDHiD_ee - yPDPDPy_ee;
+            dev2_ge = 0.5 * tHiDHiD_ge - yPDPDPy_ge;
+          }
+
+          // Set up Hessian.
+          gsl_matrix_set(Hessian, v1, v2, dev2_gg);
+          gsl_matrix_set(Hessian, v1 + v_size, v2 + v_size, dev2_ee);
+          gsl_matrix_set(Hessian, v1, v2 + v_size, dev2_ge);
+          gsl_matrix_set(Hessian, v2 + v_size, v1, dev2_ge);
+
+          if (v1 != v2) {
+            gsl_matrix_set(Hessian, v2, v1, dev2_gg);
+            gsl_matrix_set(Hessian, v2 + v_size, v1 + v_size, dev2_ee);
+            gsl_matrix_set(Hessian, v2, v1 + v_size, dev2_ge);
+            gsl_matrix_set(Hessian, v1 + v_size, v2, dev2_ge);
+          }
+        }
+      }
+    }
+  }
+
+  // Invert Hessian.
+  int sig;
+  gsl_permutation *pmt = gsl_permutation_alloc(v_size * 2);
+
+  LUDecomp(Hessian, pmt, &sig);
+  LUInvert(Hessian, pmt, Hessian_inv);
+
+  gsl_permutation_free(pmt);
+  gsl_matrix_free(Hessian);
+
+  // Calculate Edgeworth correction factors after inverting
+  // Hessian.
+  if (c_size > 1) {
+    CalcCRT(Hessian_inv, Qi, QixHiDHix_all_g, QixHiDHix_all_e,
+            xHiDHiDHix_all_gg, xHiDHiDHix_all_ee, xHiDHiDHix_all_ge, d_size,
+            crt_a, crt_b, crt_c);
+  } else {
+    crt_a = 0.0;
+    crt_b = 0.0;
+    crt_c = 0.0;
+  }
+
+  gsl_matrix_free(xHiDHiy_all_g);
+  gsl_matrix_free(xHiDHiy_all_e);
+  gsl_matrix_free(xHiDHix_all_g);
+  gsl_matrix_free(xHiDHix_all_e);
+  gsl_matrix_free(xHiDHixQixHiy_all_g);
+  gsl_matrix_free(xHiDHixQixHiy_all_e);
+
+  gsl_matrix_free(QixHiDHiy_all_g);
+  gsl_matrix_free(QixHiDHiy_all_e);
+  gsl_matrix_free(QixHiDHix_all_g);
+  gsl_matrix_free(QixHiDHix_all_e);
+  gsl_matrix_free(QixHiDHixQixHiy_all_g);
+  gsl_matrix_free(QixHiDHixQixHiy_all_e);
+
+  gsl_matrix_free(xHiDHiDHiy_all_gg);
+  gsl_matrix_free(xHiDHiDHiy_all_ee);
+  gsl_matrix_free(xHiDHiDHiy_all_ge);
+  gsl_matrix_free(xHiDHiDHix_all_gg);
+  gsl_matrix_free(xHiDHiDHix_all_ee);
+  gsl_matrix_free(xHiDHiDHix_all_ge);
+
+  return;
 }
 
 // Update Vg, Ve.
-void UpdateVgVe (const gsl_matrix *Hessian_inv, const gsl_vector *gradient,
-		 const double step_scale, gsl_matrix *V_g, gsl_matrix *V_e) {
-	size_t v_size=gradient->size/2, d_size=V_g->size1;
-	size_t v;
+void UpdateVgVe(const gsl_matrix *Hessian_inv, const gsl_vector *gradient,
+                const double step_scale, gsl_matrix *V_g, gsl_matrix *V_e) {
+  size_t v_size = gradient->size / 2, d_size = V_g->size1;
+  size_t v;
 
-	gsl_vector *vec_v=gsl_vector_alloc (v_size*2);
+  gsl_vector *vec_v = gsl_vector_alloc(v_size * 2);
 
-	double d;
+  double d;
 
-	// Vectorize Vg and Ve.
-	for (size_t i=0; i<d_size; i++) {
-		for (size_t j=0; j<d_size; j++) {
-			if (j<i) {continue;}
-			v=GetIndex(i, j, d_size);
+  // Vectorize Vg and Ve.
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = 0; j < d_size; j++) {
+      if (j < i) {
+        continue;
+      }
+      v = GetIndex(i, j, d_size);
 
-			d=gsl_matrix_get (V_g, i, j);
-			gsl_vector_set (vec_v, v, d);
+      d = gsl_matrix_get(V_g, i, j);
+      gsl_vector_set(vec_v, v, d);
 
-			d=gsl_matrix_get (V_e, i, j);
-			gsl_vector_set (vec_v, v+v_size, d);
-		}
-	}
+      d = gsl_matrix_get(V_e, i, j);
+      gsl_vector_set(vec_v, v + v_size, d);
+    }
+  }
 
-	gsl_blas_dgemv (CblasNoTrans, -1.0*step_scale, Hessian_inv,
-			gradient, 1.0, vec_v);
+  gsl_blas_dgemv(CblasNoTrans, -1.0 * step_scale, Hessian_inv, gradient, 1.0,
+                 vec_v);
 
-	// Save Vg and Ve.
-	for (size_t i=0; i<d_size; i++) {
-		for (size_t j=0; j<d_size; j++) {
-			if (j<i) {continue;}
-			v=GetIndex(i, j, d_size);
+  // Save Vg and Ve.
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = 0; j < d_size; j++) {
+      if (j < i) {
+        continue;
+      }
+      v = GetIndex(i, j, d_size);
 
-			d=gsl_vector_get (vec_v, v);
-			gsl_matrix_set (V_g, i, j, d);
-			gsl_matrix_set (V_g, j, i, d);
+      d = gsl_vector_get(vec_v, v);
+      gsl_matrix_set(V_g, i, j, d);
+      gsl_matrix_set(V_g, j, i, d);
 
-			d=gsl_vector_get (vec_v, v+v_size);
-			gsl_matrix_set (V_e, i, j, d);
-			gsl_matrix_set (V_e, j, i, d);
-		}
-	}
+      d = gsl_vector_get(vec_v, v + v_size);
+      gsl_matrix_set(V_e, i, j, d);
+      gsl_matrix_set(V_e, j, i, d);
+    }
+  }
 
-	gsl_vector_free(vec_v);
+  gsl_vector_free(vec_v);
 
-	return;
+  return;
 }
 
-double MphNR (const char func_name, const size_t max_iter,
-	      const double max_prec, const gsl_vector *eval,
-	      const gsl_matrix *X, const gsl_matrix *Y, gsl_matrix *Hi_all,
-	      gsl_matrix *xHi_all, gsl_matrix *Hiy_all, gsl_matrix *V_g,
-	      gsl_matrix *V_e, gsl_matrix *Hessian_inv, double &crt_a,
-	      double &crt_b, double &crt_c) {
-	if (func_name!='R' && func_name!='L' && func_name!='r' &&
-	    func_name!='l') {
-	  cout<<"func_name only takes 'R' or 'L': 'R' for log-restricted "<<
-	    "likelihood, 'L' for log-likelihood."<<endl;
-	  return 0.0;
-	}
-	size_t n_size=eval->size, c_size=X->size1, d_size=Y->size1;
-	size_t dc_size=d_size*c_size;
-	size_t v_size=d_size*(d_size+1)/2;
-
-	double logdet_H, logdet_Q, yPy, logl_const;
-	double logl_old=0.0, logl_new=0.0, step_scale;
-	int sig;
-	size_t step_iter, flag_pd;
-
-	gsl_matrix *Vg_save=gsl_matrix_alloc (d_size, d_size);
-	gsl_matrix *Ve_save=gsl_matrix_alloc (d_size, d_size);
-	gsl_matrix *V_temp=gsl_matrix_alloc (d_size, d_size);
-	gsl_matrix *U_temp=gsl_matrix_alloc (d_size, d_size);
-	gsl_vector *D_temp=gsl_vector_alloc (d_size);
-	gsl_vector *xHiy=gsl_vector_alloc (dc_size);
-	gsl_vector *QixHiy=gsl_vector_alloc (dc_size);
-	gsl_matrix *Qi=gsl_matrix_alloc (dc_size, dc_size);
-	gsl_matrix *XXt=gsl_matrix_alloc (c_size, c_size);
-
-	gsl_vector *gradient=gsl_vector_alloc (v_size*2);
-
-	// Calculate |XXt| and (XXt)^{-1}.
-	gsl_blas_dsyrk (CblasUpper, CblasNoTrans, 1.0, X, 0.0, XXt);
-	for (size_t i=0; i<c_size; ++i) {
-	  for (size_t j=0; j<i; ++j) {
-	    gsl_matrix_set (XXt, i, j, gsl_matrix_get (XXt, j, i));
-	  }
-	}
-
-	gsl_permutation * pmt=gsl_permutation_alloc (c_size);
-	LUDecomp (XXt, pmt, &sig);
-	gsl_permutation_free (pmt);
-
-	// Calculate the constant for logl.
-	if (func_name=='R' || func_name=='r') {
-	  logl_const=-0.5*(double)(n_size-c_size) *
-	    (double)d_size*log(2.0*M_PI) +
-	    0.5*(double)d_size*LULndet (XXt);
-	} else {
-	  logl_const=-0.5*(double)n_size*(double)d_size*log(2.0*M_PI);
-	}
-	
-	// Optimization iterations.
-	for (size_t t=0; t<max_iter; t++) {
-		gsl_matrix_memcpy (Vg_save, V_g);
-		gsl_matrix_memcpy (Ve_save, V_e);
-
-		step_scale=1.0; step_iter=0;
-		do {
-		  gsl_matrix_memcpy (V_g, Vg_save);
-		  gsl_matrix_memcpy (V_e, Ve_save);
-		  
-		  // Update Vg, Ve, and invert Hessian.
-		  if (t!=0) {
-		    UpdateVgVe (Hessian_inv, gradient, step_scale, V_g, V_e);
-		  }
-		  
-		  // Check if both Vg and Ve are positive definite.
-		  flag_pd=1;
-		  gsl_matrix_memcpy (V_temp, V_e);
-		  EigenDecomp(V_temp, U_temp, D_temp, 0);
-		  for (size_t i=0; i<d_size; i++) {
-		    if (gsl_vector_get (D_temp, i)<=0) {flag_pd=0;}
-		  }
-		  gsl_matrix_memcpy (V_temp, V_g);
-		  EigenDecomp(V_temp, U_temp, D_temp, 0);
-		  for (size_t i=0; i<d_size; i++) {
-		    if (gsl_vector_get (D_temp, i)<=0) {flag_pd=0;}
-		  }
-		  
-		  // If flag_pd==1, continue to calculate quantities
-		  // and logl.
-		  if (flag_pd==1) {
-		    CalcHiQi(eval,X,V_g,V_e,Hi_all,Qi,logdet_H,logdet_Q);
-		    Calc_Hiy_all (Y, Hi_all, Hiy_all);
-		    Calc_xHi_all (X, Hi_all, xHi_all);
-		    
-		    // Calculate QixHiy and yPy.
-		    Calc_xHiy (Y, xHi_all, xHiy);
-		    gsl_blas_dgemv (CblasNoTrans, 1.0, Qi, xHiy, 0.0, QixHiy);
-		    
-		    gsl_blas_ddot (QixHiy, xHiy, &yPy);
-		    yPy=Calc_yHiy (Y, Hiy_all)-yPy;
-		    
-		    // Calculate log likelihood/restricted likelihood value.
-		    if (func_name=='R' || func_name=='r') {
-		      logl_new=logl_const-0.5*logdet_H-0.5*logdet_Q-0.5*yPy;
-		    } else {
-		      logl_new=logl_const-0.5*logdet_H-0.5*yPy;
-		    }
-		  }
-		  
-		  step_scale/=2.0;
-		  step_iter++;
-		  
-		} while ( (flag_pd==0 || logl_new<logl_old ||
-			   logl_new-logl_old>10 ) && step_iter<10 && t!=0);
-
-		// Terminate if change is small.
-		if (t!=0) {
-			if (logl_new<logl_old || flag_pd==0) {
-				gsl_matrix_memcpy (V_g, Vg_save);
-				gsl_matrix_memcpy (V_e, Ve_save);
-				break;
-			}
-
-			if (logl_new-logl_old<max_prec) {
-				break;
-			}
-		}
-
-		logl_old=logl_new;
-
-		CalcDev (func_name, eval, Qi, Hi_all, xHi_all, Hiy_all,
-			 QixHiy, gradient, Hessian_inv, crt_a, crt_b, crt_c);
-	}
-
-	// Mutiply Hessian_inv with -1.0.
-	// Now Hessian_inv is the variance matrix.
-	gsl_matrix_scale (Hessian_inv, -1.0);
-
-	gsl_matrix_free(Vg_save);
-	gsl_matrix_free(Ve_save);
-	gsl_matrix_free(V_temp);
-	gsl_matrix_free(U_temp);
-	gsl_vector_free(D_temp);
-	gsl_vector_free(xHiy);
-	gsl_vector_free(QixHiy);
-
-	gsl_matrix_free(Qi);
-	gsl_matrix_free(XXt);
-
-	gsl_vector_free(gradient);
-
-	return logl_new;
+double MphNR(const char func_name, const size_t max_iter, const double max_prec,
+             const gsl_vector *eval, const gsl_matrix *X, const gsl_matrix *Y,
+             gsl_matrix *Hi_all, gsl_matrix *xHi_all, gsl_matrix *Hiy_all,
+             gsl_matrix *V_g, gsl_matrix *V_e, gsl_matrix *Hessian_inv,
+             double &crt_a, double &crt_b, double &crt_c) {
+  if (func_name != 'R' && func_name != 'L' && func_name != 'r' &&
+      func_name != 'l') {
+    cout << "func_name only takes 'R' or 'L': 'R' for log-restricted "
+         << "likelihood, 'L' for log-likelihood." << endl;
+    return 0.0;
+  }
+  size_t n_size = eval->size, c_size = X->size1, d_size = Y->size1;
+  size_t dc_size = d_size * c_size;
+  size_t v_size = d_size * (d_size + 1) / 2;
+
+  double logdet_H, logdet_Q, yPy, logl_const;
+  double logl_old = 0.0, logl_new = 0.0, step_scale;
+  int sig;
+  size_t step_iter, flag_pd;
+
+  gsl_matrix *Vg_save = gsl_matrix_alloc(d_size, d_size);
+  gsl_matrix *Ve_save = gsl_matrix_alloc(d_size, d_size);
+  gsl_matrix *V_temp = gsl_matrix_alloc(d_size, d_size);
+  gsl_matrix *U_temp = gsl_matrix_alloc(d_size, d_size);
+  gsl_vector *D_temp = gsl_vector_alloc(d_size);
+  gsl_vector *xHiy = gsl_vector_alloc(dc_size);
+  gsl_vector *QixHiy = gsl_vector_alloc(dc_size);
+  gsl_matrix *Qi = gsl_matrix_alloc(dc_size, dc_size);
+  gsl_matrix *XXt = gsl_matrix_alloc(c_size, c_size);
+
+  gsl_vector *gradient = gsl_vector_alloc(v_size * 2);
+
+  // Calculate |XXt| and (XXt)^{-1}.
+  gsl_blas_dsyrk(CblasUpper, CblasNoTrans, 1.0, X, 0.0, XXt);
+  for (size_t i = 0; i < c_size; ++i) {
+    for (size_t j = 0; j < i; ++j) {
+      gsl_matrix_set(XXt, i, j, gsl_matrix_get(XXt, j, i));
+    }
+  }
+
+  gsl_permutation *pmt = gsl_permutation_alloc(c_size);
+  LUDecomp(XXt, pmt, &sig);
+  gsl_permutation_free(pmt);
+
+  // Calculate the constant for logl.
+  if (func_name == 'R' || func_name == 'r') {
+    logl_const =
+        -0.5 * (double)(n_size - c_size) * (double)d_size * log(2.0 * M_PI) +
+        0.5 * (double)d_size * LULndet(XXt);
+  } else {
+    logl_const = -0.5 * (double)n_size * (double)d_size * log(2.0 * M_PI);
+  }
+
+  // Optimization iterations.
+  for (size_t t = 0; t < max_iter; t++) {
+    gsl_matrix_memcpy(Vg_save, V_g);
+    gsl_matrix_memcpy(Ve_save, V_e);
+
+    step_scale = 1.0;
+    step_iter = 0;
+    do {
+      gsl_matrix_memcpy(V_g, Vg_save);
+      gsl_matrix_memcpy(V_e, Ve_save);
+
+      // Update Vg, Ve, and invert Hessian.
+      if (t != 0) {
+        UpdateVgVe(Hessian_inv, gradient, step_scale, V_g, V_e);
+      }
+
+      // Check if both Vg and Ve are positive definite.
+      flag_pd = 1;
+      gsl_matrix_memcpy(V_temp, V_e);
+      EigenDecomp(V_temp, U_temp, D_temp, 0);
+      for (size_t i = 0; i < d_size; i++) {
+        if (gsl_vector_get(D_temp, i) <= 0) {
+          flag_pd = 0;
+        }
+      }
+      gsl_matrix_memcpy(V_temp, V_g);
+      EigenDecomp(V_temp, U_temp, D_temp, 0);
+      for (size_t i = 0; i < d_size; i++) {
+        if (gsl_vector_get(D_temp, i) <= 0) {
+          flag_pd = 0;
+        }
+      }
+
+      // If flag_pd==1, continue to calculate quantities
+      // and logl.
+      if (flag_pd == 1) {
+        CalcHiQi(eval, X, V_g, V_e, Hi_all, Qi, logdet_H, logdet_Q);
+        Calc_Hiy_all(Y, Hi_all, Hiy_all);
+        Calc_xHi_all(X, Hi_all, xHi_all);
+
+        // Calculate QixHiy and yPy.
+        Calc_xHiy(Y, xHi_all, xHiy);
+        gsl_blas_dgemv(CblasNoTrans, 1.0, Qi, xHiy, 0.0, QixHiy);
+
+        gsl_blas_ddot(QixHiy, xHiy, &yPy);
+        yPy = Calc_yHiy(Y, Hiy_all) - yPy;
+
+        // Calculate log likelihood/restricted likelihood value.
+        if (func_name == 'R' || func_name == 'r') {
+          logl_new = logl_const - 0.5 * logdet_H - 0.5 * logdet_Q - 0.5 * yPy;
+        } else {
+          logl_new = logl_const - 0.5 * logdet_H - 0.5 * yPy;
+        }
+      }
+
+      step_scale /= 2.0;
+      step_iter++;
+
+    } while (
+        (flag_pd == 0 || logl_new < logl_old || logl_new - logl_old > 10) &&
+        step_iter < 10 && t != 0);
+
+    // Terminate if change is small.
+    if (t != 0) {
+      if (logl_new < logl_old || flag_pd == 0) {
+        gsl_matrix_memcpy(V_g, Vg_save);
+        gsl_matrix_memcpy(V_e, Ve_save);
+        break;
+      }
+
+      if (logl_new - logl_old < max_prec) {
+        break;
+      }
+    }
+
+    logl_old = logl_new;
+
+    CalcDev(func_name, eval, Qi, Hi_all, xHi_all, Hiy_all, QixHiy, gradient,
+            Hessian_inv, crt_a, crt_b, crt_c);
+  }
+
+  // Mutiply Hessian_inv with -1.0.
+  // Now Hessian_inv is the variance matrix.
+  gsl_matrix_scale(Hessian_inv, -1.0);
+
+  gsl_matrix_free(Vg_save);
+  gsl_matrix_free(Ve_save);
+  gsl_matrix_free(V_temp);
+  gsl_matrix_free(U_temp);
+  gsl_vector_free(D_temp);
+  gsl_vector_free(xHiy);
+  gsl_vector_free(QixHiy);
+
+  gsl_matrix_free(Qi);
+  gsl_matrix_free(XXt);
+
+  gsl_vector_free(gradient);
+
+  return logl_new;
 }
 
 // Initialize Vg, Ve and B.
 void MphInitial(const size_t em_iter, const double em_prec,
-		const size_t nr_iter, const double nr_prec,
-		const gsl_vector *eval, const gsl_matrix *X,
-		const gsl_matrix *Y, const double l_min, const double l_max,
-		const size_t n_region, gsl_matrix *V_g, gsl_matrix *V_e,
-		gsl_matrix *B) {
-  
-	gsl_matrix_set_zero (V_g);
-	gsl_matrix_set_zero (V_e);
-	gsl_matrix_set_zero (B);
-
-	size_t n_size=eval->size, c_size=X->size1, d_size=Y->size1;
-	double a, b, c;
-	double lambda, logl, vg, ve;
-
-	// Initialize the diagonal elements of Vg and Ve using univariate
-	// LMM and REML estimates.
-	gsl_matrix *Xt=gsl_matrix_alloc (n_size, c_size);
-	gsl_vector *beta_temp=gsl_vector_alloc(c_size);
-	gsl_vector *se_beta_temp=gsl_vector_alloc(c_size);
-
-	gsl_matrix_transpose_memcpy (Xt, X);
-
-	for (size_t i=0; i<d_size; i++) {
-	  gsl_vector_const_view Y_row=gsl_matrix_const_row (Y, i);
-	  CalcLambda ('R', eval, Xt, &Y_row.vector, l_min, l_max,
-		      n_region, lambda, logl);
-	  CalcLmmVgVeBeta (eval, Xt, &Y_row.vector, lambda, vg, ve,
-			   beta_temp, se_beta_temp);
-	  
-	  gsl_matrix_set(V_g, i, i, vg);
-	  gsl_matrix_set(V_e, i, i, ve);
-	}
-	
-	gsl_matrix_free (Xt);
-	gsl_vector_free (beta_temp);
-	gsl_vector_free (se_beta_temp);
-
-	// If number of phenotypes is above four, then obtain the off
-	// diagonal elements with two trait models.
-	if (d_size>4) {
-	  
-	  // First obtain good initial values.
-	  // Large matrices for EM.
-	  gsl_matrix *U_hat=gsl_matrix_alloc (2, n_size);
-	  gsl_matrix *E_hat=gsl_matrix_alloc (2, n_size);
-	  gsl_matrix *OmegaU=gsl_matrix_alloc (2, n_size);
-	  gsl_matrix *OmegaE=gsl_matrix_alloc (2, n_size);
-	  gsl_matrix *UltVehiY=gsl_matrix_alloc (2, n_size);
-	  gsl_matrix *UltVehiBX=gsl_matrix_alloc (2, n_size);
-	  gsl_matrix *UltVehiU=gsl_matrix_alloc (2, n_size);
-	  gsl_matrix *UltVehiE=gsl_matrix_alloc (2, n_size);
-	  
-	  // Large matrices for NR. Each dxd block is H_k^{-1}.
-	  gsl_matrix *Hi_all=gsl_matrix_alloc (2, 2*n_size);
-
-	  // Each column is H_k^{-1}y_k.
-	  gsl_matrix *Hiy_all=gsl_matrix_alloc (2, n_size);
-
-	  // Each dcxdc block is x_k\otimes H_k^{-1}.
-	  gsl_matrix *xHi_all=gsl_matrix_alloc (2*c_size, 2*n_size);
-	  gsl_matrix *Hessian=gsl_matrix_alloc (6, 6);
-	  
-	  // 2 by n matrix of Y.
-	  gsl_matrix *Y_sub=gsl_matrix_alloc (2, n_size);
-	  gsl_matrix *Vg_sub=gsl_matrix_alloc (2, 2);
-	  gsl_matrix *Ve_sub=gsl_matrix_alloc (2, 2);
-	  gsl_matrix *B_sub=gsl_matrix_alloc (2, c_size);
-	  
-	  for (size_t i=0; i<d_size; i++) {
-	    gsl_vector_view Y_sub1=gsl_matrix_row (Y_sub, 0);
-	    gsl_vector_const_view Y_1=gsl_matrix_const_row (Y, i);
-	    gsl_vector_memcpy (&Y_sub1.vector, &Y_1.vector);
-	    
-	    for (size_t j=i+1; j<d_size; j++) {
-	      gsl_vector_view Y_sub2=gsl_matrix_row (Y_sub, 1);
-	      gsl_vector_const_view Y_2=gsl_matrix_const_row (Y, j);
-	      gsl_vector_memcpy (&Y_sub2.vector, &Y_2.vector);
-	      
-	      gsl_matrix_set_zero (Vg_sub);
-	      gsl_matrix_set_zero (Ve_sub);
-	      gsl_matrix_set (Vg_sub, 0, 0, gsl_matrix_get (V_g, i, i));
-	      gsl_matrix_set (Ve_sub, 0, 0, gsl_matrix_get (V_e, i, i));
-	      gsl_matrix_set (Vg_sub, 1, 1, gsl_matrix_get (V_g, j, j));
-	      gsl_matrix_set (Ve_sub, 1, 1, gsl_matrix_get (V_e, j, j));
-	      
-	      logl=MphEM ('R', em_iter, em_prec, eval, X, Y_sub, U_hat,
-			  E_hat, OmegaU, OmegaE, UltVehiY, UltVehiBX,
-			  UltVehiU, UltVehiE, Vg_sub, Ve_sub, B_sub);
-	      logl=MphNR ('R', nr_iter, nr_prec, eval, X, Y_sub, Hi_all,
-			  xHi_all, Hiy_all, Vg_sub, Ve_sub, Hessian, a, b, c);
-	      
-	      gsl_matrix_set(V_g, i, j, gsl_matrix_get (Vg_sub, 0, 1));
-	      gsl_matrix_set(V_g, j, i, gsl_matrix_get (Vg_sub, 0, 1));
-	      
-	      gsl_matrix_set(V_e, i, j, ve=gsl_matrix_get (Ve_sub, 0, 1));
-	      gsl_matrix_set(V_e, j, i, ve=gsl_matrix_get (Ve_sub, 0, 1));
-	    }
-	  }
-	  
-	  // Free matrices.
-	  gsl_matrix_free(U_hat);
-	  gsl_matrix_free(E_hat);
-	  gsl_matrix_free(OmegaU);
-	  gsl_matrix_free(OmegaE);
-	  gsl_matrix_free(UltVehiY);
-	  gsl_matrix_free(UltVehiBX);
-	  gsl_matrix_free(UltVehiU);
-	  gsl_matrix_free(UltVehiE);
-	  
-	  gsl_matrix_free(Hi_all);
-	  gsl_matrix_free(Hiy_all);
-	  gsl_matrix_free(xHi_all);
-	  gsl_matrix_free(Hessian);
-	  
-	  gsl_matrix_free(Y_sub);
-	  gsl_matrix_free(Vg_sub);
-	  gsl_matrix_free(Ve_sub);
-	  gsl_matrix_free(B_sub);
-	}
-
-	// Calculate B hat using GSL estimate.
-	gsl_matrix *UltVehiY=gsl_matrix_alloc (d_size, n_size);
-
-	gsl_vector *D_l=gsl_vector_alloc (d_size);
-	gsl_matrix *UltVeh=gsl_matrix_alloc (d_size, d_size);
-	gsl_matrix *UltVehi=gsl_matrix_alloc (d_size, d_size);
-	gsl_matrix *Qi=gsl_matrix_alloc (d_size*c_size, d_size*c_size);
-	gsl_vector *XHiy=gsl_vector_alloc (d_size*c_size);
-	gsl_vector *beta=gsl_vector_alloc (d_size*c_size);
-
-	gsl_vector_set_zero (XHiy);
-
-	double logdet_Ve, logdet_Q, dl, d, delta, dx, dy;
-
-	// Eigen decomposition and calculate log|Ve|.
-	logdet_Ve=EigenProc (V_g, V_e, D_l, UltVeh, UltVehi);
-
-	// Calculate Qi and log|Q|.
-	logdet_Q=CalcQi (eval, D_l, X, Qi);
-
-	// Calculate UltVehiY.
-	gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,1.0,UltVehi,Y,0.0,UltVehiY);
-
-	//calculate XHiy
-	for (size_t i=0; i<d_size; i++) {
-	  dl=gsl_vector_get(D_l, i);
-	  
-	  for (size_t j=0; j<c_size; j++) {
-	    d=0.0;
-	    for (size_t k=0; k<n_size; k++) {
-	      delta=gsl_vector_get(eval, k);
-	      dx=gsl_matrix_get(X, j, k);
-	      dy=gsl_matrix_get(UltVehiY, i, k);
-	      d+=dy*dx/(delta*dl+1.0);
-	    }
-	    gsl_vector_set(XHiy, j*d_size+i, d);
-	  }
-	}
-
-	gsl_blas_dgemv(CblasNoTrans, 1.0, Qi, XHiy, 0.0, beta);
-
-	// Multiply beta by UltVeh and save to B.
-	for (size_t i=0; i<c_size; i++) {
-	  gsl_vector_view B_col=gsl_matrix_column (B, i);
-	  gsl_vector_view beta_sub=gsl_vector_subvector(beta,i*d_size,d_size);
-	  gsl_blas_dgemv(CblasTrans, 1.0, UltVeh, &beta_sub.vector, 0.0,
-			 &B_col.vector);
-	}
-
-	// Free memory.
-	gsl_matrix_free(UltVehiY);
-
-	gsl_vector_free(D_l);
-	gsl_matrix_free(UltVeh);
-	gsl_matrix_free(UltVehi);
-	gsl_matrix_free(Qi);
-	gsl_vector_free(XHiy);
-	gsl_vector_free(beta);
-
-	return;
+                const size_t nr_iter, const double nr_prec,
+                const gsl_vector *eval, const gsl_matrix *X,
+                const gsl_matrix *Y, const double l_min, const double l_max,
+                const size_t n_region, gsl_matrix *V_g, gsl_matrix *V_e,
+                gsl_matrix *B) {
+
+  gsl_matrix_set_zero(V_g);
+  gsl_matrix_set_zero(V_e);
+  gsl_matrix_set_zero(B);
+
+  size_t n_size = eval->size, c_size = X->size1, d_size = Y->size1;
+  double a, b, c;
+  double lambda, logl, vg, ve;
+
+  // Initialize the diagonal elements of Vg and Ve using univariate
+  // LMM and REML estimates.
+  gsl_matrix *Xt = gsl_matrix_alloc(n_size, c_size);
+  gsl_vector *beta_temp = gsl_vector_alloc(c_size);
+  gsl_vector *se_beta_temp = gsl_vector_alloc(c_size);
+
+  gsl_matrix_transpose_memcpy(Xt, X);
+
+  for (size_t i = 0; i < d_size; i++) {
+    gsl_vector_const_view Y_row = gsl_matrix_const_row(Y, i);
+    CalcLambda('R', eval, Xt, &Y_row.vector, l_min, l_max, n_region, lambda,
+               logl);
+    CalcLmmVgVeBeta(eval, Xt, &Y_row.vector, lambda, vg, ve, beta_temp,
+                    se_beta_temp);
+
+    gsl_matrix_set(V_g, i, i, vg);
+    gsl_matrix_set(V_e, i, i, ve);
+  }
+
+  gsl_matrix_free(Xt);
+  gsl_vector_free(beta_temp);
+  gsl_vector_free(se_beta_temp);
+
+  // If number of phenotypes is above four, then obtain the off
+  // diagonal elements with two trait models.
+  if (d_size > 4) {
+
+    // First obtain good initial values.
+    // Large matrices for EM.
+    gsl_matrix *U_hat = gsl_matrix_alloc(2, n_size);
+    gsl_matrix *E_hat = gsl_matrix_alloc(2, n_size);
+    gsl_matrix *OmegaU = gsl_matrix_alloc(2, n_size);
+    gsl_matrix *OmegaE = gsl_matrix_alloc(2, n_size);
+    gsl_matrix *UltVehiY = gsl_matrix_alloc(2, n_size);
+    gsl_matrix *UltVehiBX = gsl_matrix_alloc(2, n_size);
+    gsl_matrix *UltVehiU = gsl_matrix_alloc(2, n_size);
+    gsl_matrix *UltVehiE = gsl_matrix_alloc(2, n_size);
+
+    // Large matrices for NR. Each dxd block is H_k^{-1}.
+    gsl_matrix *Hi_all = gsl_matrix_alloc(2, 2 * n_size);
+
+    // Each column is H_k^{-1}y_k.
+    gsl_matrix *Hiy_all = gsl_matrix_alloc(2, n_size);
+
+    // Each dcxdc block is x_k\otimes H_k^{-1}.
+    gsl_matrix *xHi_all = gsl_matrix_alloc(2 * c_size, 2 * n_size);
+    gsl_matrix *Hessian = gsl_matrix_alloc(6, 6);
+
+    // 2 by n matrix of Y.
+    gsl_matrix *Y_sub = gsl_matrix_alloc(2, n_size);
+    gsl_matrix *Vg_sub = gsl_matrix_alloc(2, 2);
+    gsl_matrix *Ve_sub = gsl_matrix_alloc(2, 2);
+    gsl_matrix *B_sub = gsl_matrix_alloc(2, c_size);
+
+    for (size_t i = 0; i < d_size; i++) {
+      gsl_vector_view Y_sub1 = gsl_matrix_row(Y_sub, 0);
+      gsl_vector_const_view Y_1 = gsl_matrix_const_row(Y, i);
+      gsl_vector_memcpy(&Y_sub1.vector, &Y_1.vector);
+
+      for (size_t j = i + 1; j < d_size; j++) {
+        gsl_vector_view Y_sub2 = gsl_matrix_row(Y_sub, 1);
+        gsl_vector_const_view Y_2 = gsl_matrix_const_row(Y, j);
+        gsl_vector_memcpy(&Y_sub2.vector, &Y_2.vector);
+
+        gsl_matrix_set_zero(Vg_sub);
+        gsl_matrix_set_zero(Ve_sub);
+        gsl_matrix_set(Vg_sub, 0, 0, gsl_matrix_get(V_g, i, i));
+        gsl_matrix_set(Ve_sub, 0, 0, gsl_matrix_get(V_e, i, i));
+        gsl_matrix_set(Vg_sub, 1, 1, gsl_matrix_get(V_g, j, j));
+        gsl_matrix_set(Ve_sub, 1, 1, gsl_matrix_get(V_e, j, j));
+
+        logl = MphEM('R', em_iter, em_prec, eval, X, Y_sub, U_hat, E_hat,
+                     OmegaU, OmegaE, UltVehiY, UltVehiBX, UltVehiU, UltVehiE,
+                     Vg_sub, Ve_sub, B_sub);
+        logl = MphNR('R', nr_iter, nr_prec, eval, X, Y_sub, Hi_all, xHi_all,
+                     Hiy_all, Vg_sub, Ve_sub, Hessian, a, b, c);
+
+        gsl_matrix_set(V_g, i, j, gsl_matrix_get(Vg_sub, 0, 1));
+        gsl_matrix_set(V_g, j, i, gsl_matrix_get(Vg_sub, 0, 1));
+
+        gsl_matrix_set(V_e, i, j, ve = gsl_matrix_get(Ve_sub, 0, 1));
+        gsl_matrix_set(V_e, j, i, ve = gsl_matrix_get(Ve_sub, 0, 1));
+      }
+    }
+
+    // Free matrices.
+    gsl_matrix_free(U_hat);
+    gsl_matrix_free(E_hat);
+    gsl_matrix_free(OmegaU);
+    gsl_matrix_free(OmegaE);
+    gsl_matrix_free(UltVehiY);
+    gsl_matrix_free(UltVehiBX);
+    gsl_matrix_free(UltVehiU);
+    gsl_matrix_free(UltVehiE);
+
+    gsl_matrix_free(Hi_all);
+    gsl_matrix_free(Hiy_all);
+    gsl_matrix_free(xHi_all);
+    gsl_matrix_free(Hessian);
+
+    gsl_matrix_free(Y_sub);
+    gsl_matrix_free(Vg_sub);
+    gsl_matrix_free(Ve_sub);
+    gsl_matrix_free(B_sub);
+  }
+
+  // Calculate B hat using GSL estimate.
+  gsl_matrix *UltVehiY = gsl_matrix_alloc(d_size, n_size);
+
+  gsl_vector *D_l = gsl_vector_alloc(d_size);
+  gsl_matrix *UltVeh = gsl_matrix_alloc(d_size, d_size);
+  gsl_matrix *UltVehi = gsl_matrix_alloc(d_size, d_size);
+  gsl_matrix *Qi = gsl_matrix_alloc(d_size * c_size, d_size * c_size);
+  gsl_vector *XHiy = gsl_vector_alloc(d_size * c_size);
+  gsl_vector *beta = gsl_vector_alloc(d_size * c_size);
+
+  gsl_vector_set_zero(XHiy);
+
+  double logdet_Ve, logdet_Q, dl, d, delta, dx, dy;
+
+  // Eigen decomposition and calculate log|Ve|.
+  logdet_Ve = EigenProc(V_g, V_e, D_l, UltVeh, UltVehi);
+
+  // Calculate Qi and log|Q|.
+  logdet_Q = CalcQi(eval, D_l, X, Qi);
+
+  // Calculate UltVehiY.
+  gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, UltVehi, Y, 0.0, UltVehiY);
+
+  // calculate XHiy
+  for (size_t i = 0; i < d_size; i++) {
+    dl = gsl_vector_get(D_l, i);
+
+    for (size_t j = 0; j < c_size; j++) {
+      d = 0.0;
+      for (size_t k = 0; k < n_size; k++) {
+        delta = gsl_vector_get(eval, k);
+        dx = gsl_matrix_get(X, j, k);
+        dy = gsl_matrix_get(UltVehiY, i, k);
+        d += dy * dx / (delta * dl + 1.0);
+      }
+      gsl_vector_set(XHiy, j * d_size + i, d);
+    }
+  }
+
+  gsl_blas_dgemv(CblasNoTrans, 1.0, Qi, XHiy, 0.0, beta);
+
+  // Multiply beta by UltVeh and save to B.
+  for (size_t i = 0; i < c_size; i++) {
+    gsl_vector_view B_col = gsl_matrix_column(B, i);
+    gsl_vector_view beta_sub = gsl_vector_subvector(beta, i * d_size, d_size);
+    gsl_blas_dgemv(CblasTrans, 1.0, UltVeh, &beta_sub.vector, 0.0,
+                   &B_col.vector);
+  }
+
+  // Free memory.
+  gsl_matrix_free(UltVehiY);
+
+  gsl_vector_free(D_l);
+  gsl_matrix_free(UltVeh);
+  gsl_matrix_free(UltVehi);
+  gsl_matrix_free(Qi);
+  gsl_vector_free(XHiy);
+  gsl_vector_free(beta);
+
+  return;
 }
 
 // p-value correction
 // mode=1 Wald; mode=2 LRT; mode=3 SCORE;
-double PCRT (const size_t mode, const size_t d_size, const double p_value,
-	     const double crt_a, const double crt_b, const double crt_c) {
-	double p_crt=0.0, chisq_crt=0.0, q=(double)d_size;
-	double chisq=gsl_cdf_chisq_Qinv(p_value, (double)d_size );
-
-	if (mode==1) {
-		double a=crt_c/(2.0*q*(q+2.0));
-		double b=1.0+(crt_a+crt_b)/(2.0*q);
-		chisq_crt=(-1.0*b+sqrt(b*b+4.0*a*chisq))/(2.0*a);
-	} else if (mode==2) {
-		chisq_crt=chisq/(1.0+crt_a/(2.0*q) );
-	} else {
-		chisq_crt=chisq;
-	}
-
-	p_crt=gsl_cdf_chisq_Q (chisq_crt, (double)d_size );
-
-	return p_crt;
+double PCRT(const size_t mode, const size_t d_size, const double p_value,
+            const double crt_a, const double crt_b, const double crt_c) {
+  double p_crt = 0.0, chisq_crt = 0.0, q = (double)d_size;
+  double chisq = gsl_cdf_chisq_Qinv(p_value, (double)d_size);
+
+  if (mode == 1) {
+    double a = crt_c / (2.0 * q * (q + 2.0));
+    double b = 1.0 + (crt_a + crt_b) / (2.0 * q);
+    chisq_crt = (-1.0 * b + sqrt(b * b + 4.0 * a * chisq)) / (2.0 * a);
+  } else if (mode == 2) {
+    chisq_crt = chisq / (1.0 + crt_a / (2.0 * q));
+  } else {
+    chisq_crt = chisq;
+  }
+
+  p_crt = gsl_cdf_chisq_Q(chisq_crt, (double)d_size);
+
+  return p_crt;
 }
 
 // WJA added.
-void MVLMM::Analyzebgen (const gsl_matrix *U, const gsl_vector *eval,
-			 const gsl_matrix *UtW, const gsl_matrix *UtY) {
-	string file_bgen=file_oxford+".bgen";
-	ifstream infile (file_bgen.c_str(), ios::binary);
-	if (!infile) {
-	  cout<<"error reading bgen file:"<<file_bgen<<endl;
-	  return;
-	}
-
-	clock_t time_start=clock();
-	time_UtX=0; time_opt=0;
-
-	string line;
-
-	// Create a large matrix.
-	size_t msize=10000;
-	gsl_matrix *Xlarge=gsl_matrix_alloc (U->size1, msize);
-	gsl_matrix *UtXlarge=gsl_matrix_alloc (U->size1, msize);
-	gsl_matrix_set_zero(Xlarge);
-
-	double logl_H0=0.0, logl_H1=0.0, p_wald=0, p_lrt=0, p_score=0;
-	double crt_a, crt_b, crt_c;
-	int n_miss, c_phen;
-	double geno, x_mean;
-	size_t c=0;
-	size_t n_size=UtY->size1, d_size=UtY->size2, c_size=UtW->size2;
-
-	size_t dc_size=d_size*(c_size+1), v_size=d_size*(d_size+1)/2;
-
-	// Large matrices for EM.
-	gsl_matrix *U_hat=gsl_matrix_alloc (d_size, n_size);
-	gsl_matrix *E_hat=gsl_matrix_alloc (d_size, n_size);
-	gsl_matrix *OmegaU=gsl_matrix_alloc (d_size, n_size);
-	gsl_matrix *OmegaE=gsl_matrix_alloc (d_size, n_size);
-	gsl_matrix *UltVehiY=gsl_matrix_alloc (d_size, n_size);
-	gsl_matrix *UltVehiBX=gsl_matrix_alloc (d_size, n_size);
-	gsl_matrix *UltVehiU=gsl_matrix_alloc (d_size, n_size);
-	gsl_matrix *UltVehiE=gsl_matrix_alloc (d_size, n_size);
-
-	// Large matrices for NR. Each dxd block is H_k^{-1}.
-	gsl_matrix *Hi_all=gsl_matrix_alloc (d_size, d_size*n_size);
-
-	// Each column is H_k^{-1}y_k.
-	gsl_matrix *Hiy_all=gsl_matrix_alloc (d_size, n_size);
-
-	// Each dcxdc block is x_k\otimes H_k^{-1}.
-	gsl_matrix *xHi_all=gsl_matrix_alloc (dc_size, d_size*n_size);
-	gsl_matrix *Hessian=gsl_matrix_alloc (v_size*2, v_size*2);
-	gsl_vector *x=gsl_vector_alloc (n_size);
-	gsl_vector *x_miss=gsl_vector_alloc (n_size);
-
-	gsl_matrix *Y=gsl_matrix_alloc (d_size, n_size);
-	gsl_matrix *X=gsl_matrix_alloc (c_size+1, n_size);
-	gsl_matrix *V_g=gsl_matrix_alloc (d_size, d_size);
-	gsl_matrix *V_e=gsl_matrix_alloc (d_size, d_size);
-	gsl_matrix *B=gsl_matrix_alloc (d_size, c_size+1);
-	gsl_vector *beta=gsl_vector_alloc (d_size);
-	gsl_matrix *Vbeta=gsl_matrix_alloc (d_size, d_size);
-
-	// Null estimates for initial values.
-	gsl_matrix *V_g_null=gsl_matrix_alloc (d_size, d_size);
-	gsl_matrix *V_e_null=gsl_matrix_alloc (d_size, d_size);
-	gsl_matrix *B_null=gsl_matrix_alloc (d_size, c_size+1);
-	gsl_matrix *se_B_null=gsl_matrix_alloc (d_size, c_size);
-
-	gsl_matrix_view X_sub=gsl_matrix_submatrix (X, 0, 0, c_size, n_size);
-	gsl_matrix_view B_sub=gsl_matrix_submatrix (B, 0, 0, d_size, c_size);
-	gsl_matrix_view xHi_all_sub =
-	  gsl_matrix_submatrix (xHi_all, 0, 0, d_size*c_size, d_size*n_size);
-
-	gsl_matrix_transpose_memcpy (Y, UtY);
-
-	gsl_matrix_transpose_memcpy (&X_sub.matrix, UtW);
-
-	gsl_vector_view X_row=gsl_matrix_row(X, c_size);
-	gsl_vector_set_zero(&X_row.vector);
-	gsl_vector_view B_col=gsl_matrix_column(B, c_size);
-	gsl_vector_set_zero(&B_col.vector);
-
-	MphInitial(em_iter, em_prec, nr_iter, nr_prec, eval, &X_sub.matrix,
-		   Y, l_min, l_max, n_region, V_g, V_e, &B_sub.matrix);
-	logl_H0=MphEM ('R', em_iter, em_prec, eval, &X_sub.matrix, Y, U_hat,
-		       E_hat, OmegaU, OmegaE, UltVehiY, UltVehiBX, UltVehiU,
-		       UltVehiE, V_g, V_e, &B_sub.matrix);
-	logl_H0=MphNR ('R', nr_iter, nr_prec, eval, &X_sub.matrix, Y,
-		       Hi_all, &xHi_all_sub.matrix, Hiy_all, V_g, V_e,
-		       Hessian, crt_a, crt_b, crt_c);
-	MphCalcBeta (eval, &X_sub.matrix, Y, V_g, V_e, UltVehiY,
-		     &B_sub.matrix, se_B_null);
-
-	c=0;
-	Vg_remle_null.clear();
-	Ve_remle_null.clear();
-	for (size_t i=0; i<d_size; i++) {
-	  for (size_t j=i; j<d_size; j++) {
-	    Vg_remle_null.push_back(gsl_matrix_get (V_g, i, j) );
-	    Ve_remle_null.push_back(gsl_matrix_get (V_e, i, j) );
-	    VVg_remle_null.push_back(gsl_matrix_get (Hessian, c, c) );
-	    VVe_remle_null.push_back(gsl_matrix_get (Hessian, c+v_size,
-						     c+v_size) );
-	    c++;
-	  }
-	}
-	beta_remle_null.clear();
-	se_beta_remle_null.clear();
-	for (size_t i=0; i<se_B_null->size1; i++) {
-	  for (size_t j=0; j<se_B_null->size2; j++) {
-	    beta_remle_null.push_back(gsl_matrix_get(B, i, j) );
-	    se_beta_remle_null.push_back(gsl_matrix_get(se_B_null, i, j) );
-	  }
-	}
-	logl_remle_H0=logl_H0;
-
-	cout.setf(std::ios_base::fixed, std::ios_base::floatfield);
-	cout.precision(4);
-
-	cout<<"REMLE estimate for Vg in the null model: "<<endl;
-	for (size_t i=0; i<d_size; i++) {
-		for (size_t j=0; j<=i; j++) {
-			cout<<gsl_matrix_get(V_g, i, j)<<"\t";
-		}
-		cout<<endl;
-	}
-	cout<<"se(Vg): "<<endl;
-	for (size_t i=0; i<d_size; i++) {
-		for (size_t j=0; j<=i; j++) {
-			c=GetIndex(i, j, d_size);
-			cout<<sqrt(gsl_matrix_get(Hessian, c, c))<<"\t";
-		}
-		cout<<endl;
-	}
-	cout<<"REMLE estimate for Ve in the null model: "<<endl;
-	for (size_t i=0; i<d_size; i++) {
-		for (size_t j=0; j<=i; j++) {
-			cout<<gsl_matrix_get(V_e, i, j)<<"\t";
-		}
-		cout<<endl;
-	}
-	cout<<"se(Ve): "<<endl;
-	for (size_t i=0; i<d_size; i++) {
-	  for (size_t j=0; j<=i; j++) {
-	    c=GetIndex(i, j, d_size);
-	    cout<<sqrt(gsl_matrix_get(Hessian, c+v_size, c+v_size))<<"\t";
-	  }
-	  cout<<endl;
-	}
-	cout<<"REMLE likelihood = "<<logl_H0<<endl;
-
-
-	logl_H0=MphEM ('L', em_iter, em_prec, eval, &X_sub.matrix, Y, U_hat,
-		       E_hat, OmegaU, OmegaE, UltVehiY, UltVehiBX, UltVehiU,
-		       UltVehiE, V_g, V_e, &B_sub.matrix);
-	logl_H0=MphNR ('L', nr_iter, nr_prec, eval, &X_sub.matrix, Y,
-		       Hi_all, &xHi_all_sub.matrix, Hiy_all, V_g, V_e,
-		       Hessian, crt_a, crt_b, crt_c);
-	MphCalcBeta (eval, &X_sub.matrix, Y, V_g, V_e, UltVehiY,
-		     &B_sub.matrix, se_B_null);
-
-	c=0;
-	Vg_mle_null.clear();
-	Ve_mle_null.clear();
-	for (size_t i=0; i<d_size; i++) {
-	  for (size_t j=i; j<d_size; j++) {
-	    Vg_mle_null.push_back(gsl_matrix_get (V_g, i, j) );
-	    Ve_mle_null.push_back(gsl_matrix_get (V_e, i, j) );
-	    VVg_mle_null.push_back(gsl_matrix_get (Hessian, c, c) );
-	    VVe_mle_null.push_back(gsl_matrix_get(Hessian,c+v_size,c+v_size));
-	    c++;
-	  }
-	}
-	beta_mle_null.clear();
-	se_beta_mle_null.clear();
-	for (size_t i=0; i<se_B_null->size1; i++) {
-	  for (size_t j=0; j<se_B_null->size2; j++) {
-	    beta_mle_null.push_back(gsl_matrix_get(B, i, j) );
-	    se_beta_mle_null.push_back(gsl_matrix_get(se_B_null, i, j) );
-	  }
-	}
-	logl_mle_H0=logl_H0;
-
-	cout<<"MLE estimate for Vg in the null model: "<<endl;
-	for (size_t i=0; i<d_size; i++) {
-	  for (size_t j=0; j<=i; j++) {
-	    cout<<gsl_matrix_get(V_g, i, j)<<"\t";
-	  }
-	  cout<<endl;
-	}
-	cout<<"se(Vg): "<<endl;
-	for (size_t i=0; i<d_size; i++) {
-	  for (size_t j=0; j<=i; j++) {
-	    c=GetIndex(i, j, d_size);
-	    cout<<sqrt(gsl_matrix_get(Hessian, c, c))<<"\t";
-	  }
-	  cout<<endl;
-	}
-	cout<<"MLE estimate for Ve in the null model: "<<endl;
-	for (size_t i=0; i<d_size; i++) {
-	  for (size_t j=0; j<=i; j++) {
-	    cout<<gsl_matrix_get(V_e, i, j)<<"\t";
-	  }
-	  cout<<endl;
-	}
-	cout<<"se(Ve): "<<endl;
-	for (size_t i=0; i<d_size; i++) {
-	  for (size_t j=0; j<=i; j++) {
-	    c=GetIndex(i, j, d_size);
-	    cout<<sqrt(gsl_matrix_get(Hessian, c+v_size, c+v_size))<<"\t";
-	  }
-	  cout<<endl;
-	}
-	cout<<"MLE likelihood = "<<logl_H0<<endl;
-
-
-	vector<double> v_beta, v_Vg, v_Ve, v_Vbeta;
-	for (size_t i=0; i<d_size; i++) {
-	  v_beta.push_back(0.0);
-	}
-	for (size_t i=0; i<d_size; i++) {
-	  for (size_t j=i; j<d_size; j++) {
-	    v_Vg.push_back(0.0);
-	    v_Ve.push_back(0.0);
-	    v_Vbeta.push_back(0.0);
-	  }
-	}
-
-	gsl_matrix_memcpy (V_g_null, V_g);
-	gsl_matrix_memcpy (V_e_null, V_e);
-	gsl_matrix_memcpy (B_null, B);
-
-	// Read in header.
-	uint32_t bgen_snp_block_offset;
-	uint32_t bgen_header_length;
-	uint32_t bgen_nsamples;
-	uint32_t bgen_nsnps;
-	uint32_t bgen_flags;
-	infile.read(reinterpret_cast<char*>(&bgen_snp_block_offset),4);
-	infile.read(reinterpret_cast<char*>(&bgen_header_length),4);
-	bgen_snp_block_offset-=4;
-	infile.read(reinterpret_cast<char*>(&bgen_nsnps),4);
-	bgen_snp_block_offset-=4;
-	infile.read(reinterpret_cast<char*>(&bgen_nsamples),4);
-	bgen_snp_block_offset-=4;
-	infile.ignore(4+bgen_header_length-20);
-	bgen_snp_block_offset-=4+bgen_header_length-20;
-	infile.read(reinterpret_cast<char*>(&bgen_flags),4);
-	bgen_snp_block_offset-=4;
-	bool CompressedSNPBlocks=bgen_flags&0x1;
-
-	infile.ignore(bgen_snp_block_offset);
-
-	double bgen_geno_prob_AA, bgen_geno_prob_AB, bgen_geno_prob_BB;
-	double bgen_geno_prob_non_miss;
-
-	uint32_t bgen_N;
-	uint16_t bgen_LS;
-	uint16_t bgen_LR;
-	uint16_t bgen_LC;
-	uint32_t bgen_SNP_pos;
-	uint32_t bgen_LA;
-	std::string bgen_A_allele;
-	uint32_t bgen_LB;
-	std::string bgen_B_allele;
-	uint32_t bgen_P;
-	size_t unzipped_data_size;
-	string id;
-	string rs;
-	string chr;
-	std::cout<<"Warning: WJA hard coded SNP missingness threshold "<<
-	  "of 10%"<<std::endl;
-
-	// Start reading genotypes and analyze.
-	size_t csnp=0, t_last=0;
-	for (size_t t=0; t<indicator_snp.size(); ++t) {
-	  if (indicator_snp[t]==0) {continue;}
-	  t_last++;
-	}
-	for (size_t t=0; t<indicator_snp.size(); ++t) {
-	  if (t%d_pace==0 || t==(ns_total-1)) {
-	    ProgressBar ("Reading SNPs  ", t, ns_total-1);
-	  }
-	  if (indicator_snp[t]==0) {continue;}
-	  
-	  // Read SNP header.
-	  id.clear();
-	  rs.clear();
-	  chr.clear();
-	  bgen_A_allele.clear();
-	  bgen_B_allele.clear();
-	  
-	  infile.read(reinterpret_cast<char*>(&bgen_N),4);
-	  infile.read(reinterpret_cast<char*>(&bgen_LS),2);
-	  
-	  id.resize(bgen_LS);
-	  infile.read(&id[0], bgen_LS);
-	  
-	  infile.read(reinterpret_cast<char*>(&bgen_LR),2);
-	  rs.resize(bgen_LR);
-	  infile.read(&rs[0], bgen_LR);
-	  
-	  infile.read(reinterpret_cast<char*>(&bgen_LC),2);
-	  chr.resize(bgen_LC);
-	  infile.read(&chr[0], bgen_LC);
-	  
-	  infile.read(reinterpret_cast<char*>(&bgen_SNP_pos),4);
-	  
-	  infile.read(reinterpret_cast<char*>(&bgen_LA),4);
-	  bgen_A_allele.resize(bgen_LA);
-	  infile.read(&bgen_A_allele[0], bgen_LA);
-		
-	  infile.read(reinterpret_cast<char*>(&bgen_LB),4);
-	  bgen_B_allele.resize(bgen_LB);
-	  infile.read(&bgen_B_allele[0], bgen_LB);
-	  
-	  uint16_t unzipped_data[3*bgen_N];
-	  
-	  if (indicator_snp[t]==0) {
-	    if(CompressedSNPBlocks)
-	      infile.read(reinterpret_cast<char*>(&bgen_P),4);
-	    else
-	      bgen_P=6*bgen_N;
-	    
-	    infile.ignore(static_cast<size_t>(bgen_P));
-	    
-	    continue;
-	  }
-
-	  if(CompressedSNPBlocks) {
-
-	    infile.read(reinterpret_cast<char*>(&bgen_P),4);
-	    uint8_t zipped_data[bgen_P];
-	    
-	    unzipped_data_size=6*bgen_N;
-	    
-	    infile.read(reinterpret_cast<char*>(zipped_data),bgen_P);
-	    
-	    int result=uncompress(reinterpret_cast<Bytef*>(unzipped_data),
-	      reinterpret_cast<uLongf*>(&unzipped_data_size),
-	      reinterpret_cast<Bytef*>(zipped_data),
-              static_cast<uLong> (bgen_P));
-	    assert(result == Z_OK);
-	    
-	  } else {
-	    
-	    bgen_P=6*bgen_N;
-	    infile.read(reinterpret_cast<char*>(unzipped_data),bgen_P);
-	  }
-	  
-	  x_mean=0.0; c_phen=0; n_miss=0;
-	  gsl_vector_set_zero(x_miss);
-	  for (size_t i=0; i<bgen_N; ++i) {
-	    if (indicator_idv[i]==0) {continue;}
-	    
-	    bgen_geno_prob_AA =
-	      static_cast<double>(unzipped_data[i*3])/32768.0;
-	    bgen_geno_prob_AB =
-	      static_cast<double>(unzipped_data[i*3+1])/32768.0;
-	    bgen_geno_prob_BB =
-	      static_cast<double>(unzipped_data[i*3+2])/32768.0;
-	    
-	    // WJA.
-	    bgen_geno_prob_non_miss=bgen_geno_prob_AA +
-	      bgen_geno_prob_AB+bgen_geno_prob_BB;
-	    if (bgen_geno_prob_non_miss<0.9) {
-	      gsl_vector_set(x_miss, c_phen, 0.0);
-	      n_miss++;
-	    }
-	    else {
-	      
-	      bgen_geno_prob_AA/=bgen_geno_prob_non_miss;
-	      bgen_geno_prob_AB/=bgen_geno_prob_non_miss;
-	      bgen_geno_prob_BB/=bgen_geno_prob_non_miss;
-	      
-	      geno=2.0*bgen_geno_prob_BB+bgen_geno_prob_AB;
-	      
-	      gsl_vector_set(x, c_phen, geno);
-	      gsl_vector_set(x_miss, c_phen, 1.0);
-	      x_mean+=geno;
-	    }
-	    c_phen++;
-	  }
-	  
-	  x_mean/=static_cast<double>(ni_test-n_miss);
-	  
-	  for (size_t i=0; i<ni_test; ++i) {
-	    if (gsl_vector_get (x_miss, i)==0) {gsl_vector_set(x, i, x_mean);}
-	  }
-
-	  gsl_vector_view Xlarge_col=gsl_matrix_column (Xlarge, csnp%msize);
-	  gsl_vector_memcpy (&Xlarge_col.vector, x);
-	  csnp++;
-
-	  if (csnp%msize==0 || csnp==t_last ) {
-	    size_t l=0;
-	    if (csnp%msize==0) {l=msize;} else {l=csnp%msize;}
-	    
-	    gsl_matrix_view Xlarge_sub =
-	      gsl_matrix_submatrix(Xlarge, 0, 0, Xlarge->size1, l);
-	    gsl_matrix_view UtXlarge_sub =
-	      gsl_matrix_submatrix(UtXlarge, 0, 0, UtXlarge->size1, l);
-	    
-	    time_start=clock();
-	    eigenlib_dgemm ("T", "N", 1.0, U, &Xlarge_sub.matrix, 0.0,
-			    &UtXlarge_sub.matrix);
-	    time_UtX+=(clock()-time_start)/(double(CLOCKS_PER_SEC)*60.0);
-	    
-	    gsl_matrix_set_zero (Xlarge);
-	    
-	    for (size_t i=0; i<l; i++) {
-	      gsl_vector_view UtXlarge_col=gsl_matrix_column (UtXlarge, i);
-	      gsl_vector_memcpy (&X_row.vector, &UtXlarge_col.vector);
-	      
-	      // Initial values.
-	      gsl_matrix_memcpy (V_g, V_g_null);
-	      gsl_matrix_memcpy (V_e, V_e_null);
-	      gsl_matrix_memcpy (B, B_null);
-	      
-	      time_start=clock();
-		    
-	      // 3 is before 1.
-	      if (a_mode==3 || a_mode==4) {
-		p_score=MphCalcP (eval, &X_row.vector, &X_sub.matrix, Y,
-				  V_g_null, V_e_null, UltVehiY, beta, Vbeta);
-		if (p_score<p_nr && crt==1) {
-		  logl_H1=MphNR ('R', 1, nr_prec*10, eval, X, Y, Hi_all,
-				 xHi_all, Hiy_all, V_g, V_e, Hessian, crt_a,
-				 crt_b, crt_c);
-		  p_score=PCRT (3, d_size, p_score, crt_a, crt_b, crt_c);
-		}
-	      }
-	      
-	      if (a_mode==2 || a_mode==4) {
-		logl_H1=MphEM ('L', em_iter/10, em_prec*10, eval, X, Y,
-			       U_hat, E_hat, OmegaU, OmegaE, UltVehiY,
-			       UltVehiBX, UltVehiU, UltVehiE, V_g, V_e, B);
-		
-		// Calculate beta and Vbeta.
-		p_lrt=MphCalcP (eval, &X_row.vector, &X_sub.matrix, Y, V_g,
-				V_e, UltVehiY, beta, Vbeta);
-		p_lrt=gsl_cdf_chisq_Q (2.0*(logl_H1-logl_H0), (double)d_size );
-		
-		if (p_lrt<p_nr) {
-		  logl_H1=MphNR ('L', nr_iter/10, nr_prec*10, eval, X, Y,
-				 Hi_all, xHi_all, Hiy_all, V_g, V_e, Hessian,
-				 crt_a, crt_b, crt_c);
-		  
-		  // Calculate beta and Vbeta.
-		  p_lrt=MphCalcP (eval, &X_row.vector, &X_sub.matrix, Y, V_g,
-				  V_e, UltVehiY, beta, Vbeta);
-		  p_lrt=gsl_cdf_chisq_Q (2.0*(logl_H1-logl_H0),
-					 (double)d_size );
-		  
-		  if (crt==1) {
-		    p_lrt=PCRT (2, d_size, p_lrt, crt_a, crt_b, crt_c);
-		  }
-		}
-	      }
-	      
-	      if (a_mode==1 || a_mode==4) {
-		logl_H1=MphEM ('R', em_iter/10, em_prec*10, eval, X, Y, U_hat,
-			       E_hat, OmegaU, OmegaE, UltVehiY, UltVehiBX,
-			       UltVehiU, UltVehiE, V_g, V_e, B);
-		p_wald=MphCalcP (eval, &X_row.vector, &X_sub.matrix, Y, V_g,
-				 V_e, UltVehiY, beta, Vbeta);
-		
-		if (p_wald<p_nr) {
-		  logl_H1=MphNR ('R', nr_iter/10, nr_prec*10, eval, X, Y,
-				 Hi_all, xHi_all, Hiy_all, V_g, V_e, Hessian,
-				 crt_a, crt_b, crt_c);
-		  p_wald=MphCalcP (eval, &X_row.vector, &X_sub.matrix, Y,
-				   V_g, V_e, UltVehiY, beta, Vbeta);
-		  
-		  if (crt==1) {
-		    p_wald=PCRT (1, d_size, p_wald, crt_a, crt_b, crt_c);
-		  }
-		}
-	      }
-
-	      time_opt+=(clock()-time_start)/(double(CLOCKS_PER_SEC)*60.0);
-
-	      // Store summary data.
-	      for (size_t i=0; i<d_size; i++) {
-		v_beta[i]=gsl_vector_get (beta, i);
-	      }
-	      
-	      c=0;
-	      for (size_t i=0; i<d_size; i++) {
-		for (size_t j=i; j<d_size; j++) {
-		  v_Vg[c]=gsl_matrix_get (V_g, i, j);
-		  v_Ve[c]=gsl_matrix_get (V_e, i, j);
-		  v_Vbeta[c]=gsl_matrix_get (Vbeta, i, j);
-		  c++;
-		}
-	      }
-	      
-	      MPHSUMSTAT SNPs={v_beta, p_wald, p_lrt, p_score, v_Vg, v_Ve,
-			       v_Vbeta};
-	      sumStat.push_back(SNPs);
-	    }
-	  }
-	}
-	cout<<endl;
-
-	infile.close();
-	infile.clear();
-
-	gsl_matrix_free(U_hat);
-	gsl_matrix_free(E_hat);
-	gsl_matrix_free(OmegaU);
-	gsl_matrix_free(OmegaE);
-	gsl_matrix_free(UltVehiY);
-	gsl_matrix_free(UltVehiBX);
-	gsl_matrix_free(UltVehiU);
-	gsl_matrix_free(UltVehiE);
-
-	gsl_matrix_free(Hi_all);
-	gsl_matrix_free(Hiy_all);
-	gsl_matrix_free(xHi_all);
-	gsl_matrix_free(Hessian);
-
-	gsl_vector_free(x);
-	gsl_vector_free(x_miss);
-
-	gsl_matrix_free(Y);
-	gsl_matrix_free(X);
-	gsl_matrix_free(V_g);
-	gsl_matrix_free(V_e);
-	gsl_matrix_free(B);
-	gsl_vector_free(beta);
-	gsl_matrix_free(Vbeta);
-
-	gsl_matrix_free(V_g_null);
-	gsl_matrix_free(V_e_null);
-	gsl_matrix_free(B_null);
-	gsl_matrix_free(se_B_null);
-
-	gsl_matrix_free(Xlarge);
-	gsl_matrix_free(UtXlarge);
-
-	return;
+void MVLMM::Analyzebgen(const gsl_matrix *U, const gsl_vector *eval,
+                        const gsl_matrix *UtW, const gsl_matrix *UtY) {
+  string file_bgen = file_oxford + ".bgen";
+  ifstream infile(file_bgen.c_str(), ios::binary);
+  if (!infile) {
+    cout << "error reading bgen file:" << file_bgen << endl;
+    return;
+  }
+
+  clock_t time_start = clock();
+  time_UtX = 0;
+  time_opt = 0;
+
+  string line;
+
+  // Create a large matrix.
+  size_t msize = 10000;
+  gsl_matrix *Xlarge = gsl_matrix_alloc(U->size1, msize);
+  gsl_matrix *UtXlarge = gsl_matrix_alloc(U->size1, msize);
+  gsl_matrix_set_zero(Xlarge);
+
+  double logl_H0 = 0.0, logl_H1 = 0.0, p_wald = 0, p_lrt = 0, p_score = 0;
+  double crt_a, crt_b, crt_c;
+  int n_miss, c_phen;
+  double geno, x_mean;
+  size_t c = 0;
+  size_t n_size = UtY->size1, d_size = UtY->size2, c_size = UtW->size2;
+
+  size_t dc_size = d_size * (c_size + 1), v_size = d_size * (d_size + 1) / 2;
+
+  // Large matrices for EM.
+  gsl_matrix *U_hat = gsl_matrix_alloc(d_size, n_size);
+  gsl_matrix *E_hat = gsl_matrix_alloc(d_size, n_size);
+  gsl_matrix *OmegaU = gsl_matrix_alloc(d_size, n_size);
+  gsl_matrix *OmegaE = gsl_matrix_alloc(d_size, n_size);
+  gsl_matrix *UltVehiY = gsl_matrix_alloc(d_size, n_size);
+  gsl_matrix *UltVehiBX = gsl_matrix_alloc(d_size, n_size);
+  gsl_matrix *UltVehiU = gsl_matrix_alloc(d_size, n_size);
+  gsl_matrix *UltVehiE = gsl_matrix_alloc(d_size, n_size);
+
+  // Large matrices for NR. Each dxd block is H_k^{-1}.
+  gsl_matrix *Hi_all = gsl_matrix_alloc(d_size, d_size * n_size);
+
+  // Each column is H_k^{-1}y_k.
+  gsl_matrix *Hiy_all = gsl_matrix_alloc(d_size, n_size);
+
+  // Each dcxdc block is x_k\otimes H_k^{-1}.
+  gsl_matrix *xHi_all = gsl_matrix_alloc(dc_size, d_size * n_size);
+  gsl_matrix *Hessian = gsl_matrix_alloc(v_size * 2, v_size * 2);
+  gsl_vector *x = gsl_vector_alloc(n_size);
+  gsl_vector *x_miss = gsl_vector_alloc(n_size);
+
+  gsl_matrix *Y = gsl_matrix_alloc(d_size, n_size);
+  gsl_matrix *X = gsl_matrix_alloc(c_size + 1, n_size);
+  gsl_matrix *V_g = gsl_matrix_alloc(d_size, d_size);
+  gsl_matrix *V_e = gsl_matrix_alloc(d_size, d_size);
+  gsl_matrix *B = gsl_matrix_alloc(d_size, c_size + 1);
+  gsl_vector *beta = gsl_vector_alloc(d_size);
+  gsl_matrix *Vbeta = gsl_matrix_alloc(d_size, d_size);
+
+  // Null estimates for initial values.
+  gsl_matrix *V_g_null = gsl_matrix_alloc(d_size, d_size);
+  gsl_matrix *V_e_null = gsl_matrix_alloc(d_size, d_size);
+  gsl_matrix *B_null = gsl_matrix_alloc(d_size, c_size + 1);
+  gsl_matrix *se_B_null = gsl_matrix_alloc(d_size, c_size);
+
+  gsl_matrix_view X_sub = gsl_matrix_submatrix(X, 0, 0, c_size, n_size);
+  gsl_matrix_view B_sub = gsl_matrix_submatrix(B, 0, 0, d_size, c_size);
+  gsl_matrix_view xHi_all_sub =
+      gsl_matrix_submatrix(xHi_all, 0, 0, d_size * c_size, d_size * n_size);
+
+  gsl_matrix_transpose_memcpy(Y, UtY);
+
+  gsl_matrix_transpose_memcpy(&X_sub.matrix, UtW);
+
+  gsl_vector_view X_row = gsl_matrix_row(X, c_size);
+  gsl_vector_set_zero(&X_row.vector);
+  gsl_vector_view B_col = gsl_matrix_column(B, c_size);
+  gsl_vector_set_zero(&B_col.vector);
+
+  MphInitial(em_iter, em_prec, nr_iter, nr_prec, eval, &X_sub.matrix, Y, l_min,
+             l_max, n_region, V_g, V_e, &B_sub.matrix);
+  logl_H0 = MphEM('R', em_iter, em_prec, eval, &X_sub.matrix, Y, U_hat, E_hat,
+                  OmegaU, OmegaE, UltVehiY, UltVehiBX, UltVehiU, UltVehiE, V_g,
+                  V_e, &B_sub.matrix);
+  logl_H0 = MphNR('R', nr_iter, nr_prec, eval, &X_sub.matrix, Y, Hi_all,
+                  &xHi_all_sub.matrix, Hiy_all, V_g, V_e, Hessian, crt_a, crt_b,
+                  crt_c);
+  MphCalcBeta(eval, &X_sub.matrix, Y, V_g, V_e, UltVehiY, &B_sub.matrix,
+              se_B_null);
+
+  c = 0;
+  Vg_remle_null.clear();
+  Ve_remle_null.clear();
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = i; j < d_size; j++) {
+      Vg_remle_null.push_back(gsl_matrix_get(V_g, i, j));
+      Ve_remle_null.push_back(gsl_matrix_get(V_e, i, j));
+      VVg_remle_null.push_back(gsl_matrix_get(Hessian, c, c));
+      VVe_remle_null.push_back(gsl_matrix_get(Hessian, c + v_size, c + v_size));
+      c++;
+    }
+  }
+  beta_remle_null.clear();
+  se_beta_remle_null.clear();
+  for (size_t i = 0; i < se_B_null->size1; i++) {
+    for (size_t j = 0; j < se_B_null->size2; j++) {
+      beta_remle_null.push_back(gsl_matrix_get(B, i, j));
+      se_beta_remle_null.push_back(gsl_matrix_get(se_B_null, i, j));
+    }
+  }
+  logl_remle_H0 = logl_H0;
+
+  cout.setf(std::ios_base::fixed, std::ios_base::floatfield);
+  cout.precision(4);
+
+  cout << "REMLE estimate for Vg in the null model: " << endl;
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = 0; j <= i; j++) {
+      cout << gsl_matrix_get(V_g, i, j) << "\t";
+    }
+    cout << endl;
+  }
+  cout << "se(Vg): " << endl;
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = 0; j <= i; j++) {
+      c = GetIndex(i, j, d_size);
+      cout << sqrt(gsl_matrix_get(Hessian, c, c)) << "\t";
+    }
+    cout << endl;
+  }
+  cout << "REMLE estimate for Ve in the null model: " << endl;
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = 0; j <= i; j++) {
+      cout << gsl_matrix_get(V_e, i, j) << "\t";
+    }
+    cout << endl;
+  }
+  cout << "se(Ve): " << endl;
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = 0; j <= i; j++) {
+      c = GetIndex(i, j, d_size);
+      cout << sqrt(gsl_matrix_get(Hessian, c + v_size, c + v_size)) << "\t";
+    }
+    cout << endl;
+  }
+  cout << "REMLE likelihood = " << logl_H0 << endl;
+
+  logl_H0 = MphEM('L', em_iter, em_prec, eval, &X_sub.matrix, Y, U_hat, E_hat,
+                  OmegaU, OmegaE, UltVehiY, UltVehiBX, UltVehiU, UltVehiE, V_g,
+                  V_e, &B_sub.matrix);
+  logl_H0 = MphNR('L', nr_iter, nr_prec, eval, &X_sub.matrix, Y, Hi_all,
+                  &xHi_all_sub.matrix, Hiy_all, V_g, V_e, Hessian, crt_a, crt_b,
+                  crt_c);
+  MphCalcBeta(eval, &X_sub.matrix, Y, V_g, V_e, UltVehiY, &B_sub.matrix,
+              se_B_null);
+
+  c = 0;
+  Vg_mle_null.clear();
+  Ve_mle_null.clear();
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = i; j < d_size; j++) {
+      Vg_mle_null.push_back(gsl_matrix_get(V_g, i, j));
+      Ve_mle_null.push_back(gsl_matrix_get(V_e, i, j));
+      VVg_mle_null.push_back(gsl_matrix_get(Hessian, c, c));
+      VVe_mle_null.push_back(gsl_matrix_get(Hessian, c + v_size, c + v_size));
+      c++;
+    }
+  }
+  beta_mle_null.clear();
+  se_beta_mle_null.clear();
+  for (size_t i = 0; i < se_B_null->size1; i++) {
+    for (size_t j = 0; j < se_B_null->size2; j++) {
+      beta_mle_null.push_back(gsl_matrix_get(B, i, j));
+      se_beta_mle_null.push_back(gsl_matrix_get(se_B_null, i, j));
+    }
+  }
+  logl_mle_H0 = logl_H0;
+
+  cout << "MLE estimate for Vg in the null model: " << endl;
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = 0; j <= i; j++) {
+      cout << gsl_matrix_get(V_g, i, j) << "\t";
+    }
+    cout << endl;
+  }
+  cout << "se(Vg): " << endl;
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = 0; j <= i; j++) {
+      c = GetIndex(i, j, d_size);
+      cout << sqrt(gsl_matrix_get(Hessian, c, c)) << "\t";
+    }
+    cout << endl;
+  }
+  cout << "MLE estimate for Ve in the null model: " << endl;
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = 0; j <= i; j++) {
+      cout << gsl_matrix_get(V_e, i, j) << "\t";
+    }
+    cout << endl;
+  }
+  cout << "se(Ve): " << endl;
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = 0; j <= i; j++) {
+      c = GetIndex(i, j, d_size);
+      cout << sqrt(gsl_matrix_get(Hessian, c + v_size, c + v_size)) << "\t";
+    }
+    cout << endl;
+  }
+  cout << "MLE likelihood = " << logl_H0 << endl;
+
+  vector<double> v_beta, v_Vg, v_Ve, v_Vbeta;
+  for (size_t i = 0; i < d_size; i++) {
+    v_beta.push_back(0.0);
+  }
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = i; j < d_size; j++) {
+      v_Vg.push_back(0.0);
+      v_Ve.push_back(0.0);
+      v_Vbeta.push_back(0.0);
+    }
+  }
+
+  gsl_matrix_memcpy(V_g_null, V_g);
+  gsl_matrix_memcpy(V_e_null, V_e);
+  gsl_matrix_memcpy(B_null, B);
+
+  // Read in header.
+  uint32_t bgen_snp_block_offset;
+  uint32_t bgen_header_length;
+  uint32_t bgen_nsamples;
+  uint32_t bgen_nsnps;
+  uint32_t bgen_flags;
+  infile.read(reinterpret_cast<char *>(&bgen_snp_block_offset), 4);
+  infile.read(reinterpret_cast<char *>(&bgen_header_length), 4);
+  bgen_snp_block_offset -= 4;
+  infile.read(reinterpret_cast<char *>(&bgen_nsnps), 4);
+  bgen_snp_block_offset -= 4;
+  infile.read(reinterpret_cast<char *>(&bgen_nsamples), 4);
+  bgen_snp_block_offset -= 4;
+  infile.ignore(4 + bgen_header_length - 20);
+  bgen_snp_block_offset -= 4 + bgen_header_length - 20;
+  infile.read(reinterpret_cast<char *>(&bgen_flags), 4);
+  bgen_snp_block_offset -= 4;
+  bool CompressedSNPBlocks = bgen_flags & 0x1;
+
+  infile.ignore(bgen_snp_block_offset);
+
+  double bgen_geno_prob_AA, bgen_geno_prob_AB, bgen_geno_prob_BB;
+  double bgen_geno_prob_non_miss;
+
+  uint32_t bgen_N;
+  uint16_t bgen_LS;
+  uint16_t bgen_LR;
+  uint16_t bgen_LC;
+  uint32_t bgen_SNP_pos;
+  uint32_t bgen_LA;
+  std::string bgen_A_allele;
+  uint32_t bgen_LB;
+  std::string bgen_B_allele;
+  uint32_t bgen_P;
+  size_t unzipped_data_size;
+  string id;
+  string rs;
+  string chr;
+  std::cout << "Warning: WJA hard coded SNP missingness threshold "
+            << "of 10%" << std::endl;
+
+  // Start reading genotypes and analyze.
+  size_t csnp = 0, t_last = 0;
+  for (size_t t = 0; t < indicator_snp.size(); ++t) {
+    if (indicator_snp[t] == 0) {
+      continue;
+    }
+    t_last++;
+  }
+  for (size_t t = 0; t < indicator_snp.size(); ++t) {
+    if (t % d_pace == 0 || t == (ns_total - 1)) {
+      ProgressBar("Reading SNPs  ", t, ns_total - 1);
+    }
+    if (indicator_snp[t] == 0) {
+      continue;
+    }
+
+    // Read SNP header.
+    id.clear();
+    rs.clear();
+    chr.clear();
+    bgen_A_allele.clear();
+    bgen_B_allele.clear();
+
+    infile.read(reinterpret_cast<char *>(&bgen_N), 4);
+    infile.read(reinterpret_cast<char *>(&bgen_LS), 2);
+
+    id.resize(bgen_LS);
+    infile.read(&id[0], bgen_LS);
+
+    infile.read(reinterpret_cast<char *>(&bgen_LR), 2);
+    rs.resize(bgen_LR);
+    infile.read(&rs[0], bgen_LR);
+
+    infile.read(reinterpret_cast<char *>(&bgen_LC), 2);
+    chr.resize(bgen_LC);
+    infile.read(&chr[0], bgen_LC);
+
+    infile.read(reinterpret_cast<char *>(&bgen_SNP_pos), 4);
+
+    infile.read(reinterpret_cast<char *>(&bgen_LA), 4);
+    bgen_A_allele.resize(bgen_LA);
+    infile.read(&bgen_A_allele[0], bgen_LA);
+
+    infile.read(reinterpret_cast<char *>(&bgen_LB), 4);
+    bgen_B_allele.resize(bgen_LB);
+    infile.read(&bgen_B_allele[0], bgen_LB);
+
+    uint16_t unzipped_data[3 * bgen_N];
+
+    if (indicator_snp[t] == 0) {
+      if (CompressedSNPBlocks)
+        infile.read(reinterpret_cast<char *>(&bgen_P), 4);
+      else
+        bgen_P = 6 * bgen_N;
+
+      infile.ignore(static_cast<size_t>(bgen_P));
+
+      continue;
+    }
+
+    if (CompressedSNPBlocks) {
+
+      infile.read(reinterpret_cast<char *>(&bgen_P), 4);
+      uint8_t zipped_data[bgen_P];
+
+      unzipped_data_size = 6 * bgen_N;
+
+      infile.read(reinterpret_cast<char *>(zipped_data), bgen_P);
+
+      int result = uncompress(reinterpret_cast<Bytef *>(unzipped_data),
+                              reinterpret_cast<uLongf *>(&unzipped_data_size),
+                              reinterpret_cast<Bytef *>(zipped_data),
+                              static_cast<uLong>(bgen_P));
+      assert(result == Z_OK);
+
+    } else {
+
+      bgen_P = 6 * bgen_N;
+      infile.read(reinterpret_cast<char *>(unzipped_data), bgen_P);
+    }
+
+    x_mean = 0.0;
+    c_phen = 0;
+    n_miss = 0;
+    gsl_vector_set_zero(x_miss);
+    for (size_t i = 0; i < bgen_N; ++i) {
+      if (indicator_idv[i] == 0) {
+        continue;
+      }
+
+      bgen_geno_prob_AA = static_cast<double>(unzipped_data[i * 3]) / 32768.0;
+      bgen_geno_prob_AB =
+          static_cast<double>(unzipped_data[i * 3 + 1]) / 32768.0;
+      bgen_geno_prob_BB =
+          static_cast<double>(unzipped_data[i * 3 + 2]) / 32768.0;
+
+      // WJA.
+      bgen_geno_prob_non_miss =
+          bgen_geno_prob_AA + bgen_geno_prob_AB + bgen_geno_prob_BB;
+      if (bgen_geno_prob_non_miss < 0.9) {
+        gsl_vector_set(x_miss, c_phen, 0.0);
+        n_miss++;
+      } else {
+
+        bgen_geno_prob_AA /= bgen_geno_prob_non_miss;
+        bgen_geno_prob_AB /= bgen_geno_prob_non_miss;
+        bgen_geno_prob_BB /= bgen_geno_prob_non_miss;
+
+        geno = 2.0 * bgen_geno_prob_BB + bgen_geno_prob_AB;
+
+        gsl_vector_set(x, c_phen, geno);
+        gsl_vector_set(x_miss, c_phen, 1.0);
+        x_mean += geno;
+      }
+      c_phen++;
+    }
+
+    x_mean /= static_cast<double>(ni_test - n_miss);
+
+    for (size_t i = 0; i < ni_test; ++i) {
+      if (gsl_vector_get(x_miss, i) == 0) {
+        gsl_vector_set(x, i, x_mean);
+      }
+    }
+
+    gsl_vector_view Xlarge_col = gsl_matrix_column(Xlarge, csnp % msize);
+    gsl_vector_memcpy(&Xlarge_col.vector, x);
+    csnp++;
+
+    if (csnp % msize == 0 || csnp == t_last) {
+      size_t l = 0;
+      if (csnp % msize == 0) {
+        l = msize;
+      } else {
+        l = csnp % msize;
+      }
+
+      gsl_matrix_view Xlarge_sub =
+          gsl_matrix_submatrix(Xlarge, 0, 0, Xlarge->size1, l);
+      gsl_matrix_view UtXlarge_sub =
+          gsl_matrix_submatrix(UtXlarge, 0, 0, UtXlarge->size1, l);
+
+      time_start = clock();
+      eigenlib_dgemm("T", "N", 1.0, U, &Xlarge_sub.matrix, 0.0,
+                     &UtXlarge_sub.matrix);
+      time_UtX += (clock() - time_start) / (double(CLOCKS_PER_SEC) * 60.0);
+
+      gsl_matrix_set_zero(Xlarge);
+
+      for (size_t i = 0; i < l; i++) {
+        gsl_vector_view UtXlarge_col = gsl_matrix_column(UtXlarge, i);
+        gsl_vector_memcpy(&X_row.vector, &UtXlarge_col.vector);
+
+        // Initial values.
+        gsl_matrix_memcpy(V_g, V_g_null);
+        gsl_matrix_memcpy(V_e, V_e_null);
+        gsl_matrix_memcpy(B, B_null);
+
+        time_start = clock();
+
+        // 3 is before 1.
+        if (a_mode == 3 || a_mode == 4) {
+          p_score = MphCalcP(eval, &X_row.vector, &X_sub.matrix, Y, V_g_null,
+                             V_e_null, UltVehiY, beta, Vbeta);
+          if (p_score < p_nr && crt == 1) {
+            logl_H1 = MphNR('R', 1, nr_prec * 10, eval, X, Y, Hi_all, xHi_all,
+                            Hiy_all, V_g, V_e, Hessian, crt_a, crt_b, crt_c);
+            p_score = PCRT(3, d_size, p_score, crt_a, crt_b, crt_c);
+          }
+        }
+
+        if (a_mode == 2 || a_mode == 4) {
+          logl_H1 = MphEM('L', em_iter / 10, em_prec * 10, eval, X, Y, U_hat,
+                          E_hat, OmegaU, OmegaE, UltVehiY, UltVehiBX, UltVehiU,
+                          UltVehiE, V_g, V_e, B);
+
+          // Calculate beta and Vbeta.
+          p_lrt = MphCalcP(eval, &X_row.vector, &X_sub.matrix, Y, V_g, V_e,
+                           UltVehiY, beta, Vbeta);
+          p_lrt = gsl_cdf_chisq_Q(2.0 * (logl_H1 - logl_H0), (double)d_size);
+
+          if (p_lrt < p_nr) {
+            logl_H1 =
+                MphNR('L', nr_iter / 10, nr_prec * 10, eval, X, Y, Hi_all,
+                      xHi_all, Hiy_all, V_g, V_e, Hessian, crt_a, crt_b, crt_c);
+
+            // Calculate beta and Vbeta.
+            p_lrt = MphCalcP(eval, &X_row.vector, &X_sub.matrix, Y, V_g, V_e,
+                             UltVehiY, beta, Vbeta);
+            p_lrt = gsl_cdf_chisq_Q(2.0 * (logl_H1 - logl_H0), (double)d_size);
+
+            if (crt == 1) {
+              p_lrt = PCRT(2, d_size, p_lrt, crt_a, crt_b, crt_c);
+            }
+          }
+        }
+
+        if (a_mode == 1 || a_mode == 4) {
+          logl_H1 = MphEM('R', em_iter / 10, em_prec * 10, eval, X, Y, U_hat,
+                          E_hat, OmegaU, OmegaE, UltVehiY, UltVehiBX, UltVehiU,
+                          UltVehiE, V_g, V_e, B);
+          p_wald = MphCalcP(eval, &X_row.vector, &X_sub.matrix, Y, V_g, V_e,
+                            UltVehiY, beta, Vbeta);
+
+          if (p_wald < p_nr) {
+            logl_H1 =
+                MphNR('R', nr_iter / 10, nr_prec * 10, eval, X, Y, Hi_all,
+                      xHi_all, Hiy_all, V_g, V_e, Hessian, crt_a, crt_b, crt_c);
+            p_wald = MphCalcP(eval, &X_row.vector, &X_sub.matrix, Y, V_g, V_e,
+                              UltVehiY, beta, Vbeta);
+
+            if (crt == 1) {
+              p_wald = PCRT(1, d_size, p_wald, crt_a, crt_b, crt_c);
+            }
+          }
+        }
+
+        time_opt += (clock() - time_start) / (double(CLOCKS_PER_SEC) * 60.0);
+
+        // Store summary data.
+        for (size_t i = 0; i < d_size; i++) {
+          v_beta[i] = gsl_vector_get(beta, i);
+        }
+
+        c = 0;
+        for (size_t i = 0; i < d_size; i++) {
+          for (size_t j = i; j < d_size; j++) {
+            v_Vg[c] = gsl_matrix_get(V_g, i, j);
+            v_Ve[c] = gsl_matrix_get(V_e, i, j);
+            v_Vbeta[c] = gsl_matrix_get(Vbeta, i, j);
+            c++;
+          }
+        }
+
+        MPHSUMSTAT SNPs = {v_beta, p_wald, p_lrt, p_score, v_Vg, v_Ve, v_Vbeta};
+        sumStat.push_back(SNPs);
+      }
+    }
+  }
+  cout << endl;
+
+  infile.close();
+  infile.clear();
+
+  gsl_matrix_free(U_hat);
+  gsl_matrix_free(E_hat);
+  gsl_matrix_free(OmegaU);
+  gsl_matrix_free(OmegaE);
+  gsl_matrix_free(UltVehiY);
+  gsl_matrix_free(UltVehiBX);
+  gsl_matrix_free(UltVehiU);
+  gsl_matrix_free(UltVehiE);
+
+  gsl_matrix_free(Hi_all);
+  gsl_matrix_free(Hiy_all);
+  gsl_matrix_free(xHi_all);
+  gsl_matrix_free(Hessian);
+
+  gsl_vector_free(x);
+  gsl_vector_free(x_miss);
+
+  gsl_matrix_free(Y);
+  gsl_matrix_free(X);
+  gsl_matrix_free(V_g);
+  gsl_matrix_free(V_e);
+  gsl_matrix_free(B);
+  gsl_vector_free(beta);
+  gsl_matrix_free(Vbeta);
+
+  gsl_matrix_free(V_g_null);
+  gsl_matrix_free(V_e_null);
+  gsl_matrix_free(B_null);
+  gsl_matrix_free(se_B_null);
+
+  gsl_matrix_free(Xlarge);
+  gsl_matrix_free(UtXlarge);
+
+  return;
 }
 
-void MVLMM::AnalyzeBimbam (const gsl_matrix *U, const gsl_vector *eval,
-			   const gsl_matrix *UtW, const gsl_matrix *UtY) {
-	igzstream infile (file_geno.c_str(), igzstream::in);
-	if (!infile) {
-	  cout<<"error reading genotype file:"<<file_geno<<endl;
-	  return;
-	}
-
-	clock_t time_start=clock();
-	time_UtX=0; time_opt=0;
-
-	string line;
-	char *ch_ptr;
-
-	double logl_H0=0.0, logl_H1=0.0, p_wald=0, p_lrt=0, p_score=0;
-	double crt_a, crt_b, crt_c;
-	int n_miss, c_phen;
-	double geno, x_mean;
-	size_t c=0;
-	size_t n_size=UtY->size1, d_size=UtY->size2, c_size=UtW->size2;
-
-	size_t dc_size=d_size*(c_size+1), v_size=d_size*(d_size+1)/2;
-
-	// Create a large matrix.
-	size_t msize=10000;
-	gsl_matrix *Xlarge=gsl_matrix_alloc (U->size1, msize);
-	gsl_matrix *UtXlarge=gsl_matrix_alloc (U->size1, msize);
-	gsl_matrix_set_zero(Xlarge);
-
-	// Large matrices for EM.
-	gsl_matrix *U_hat=gsl_matrix_alloc (d_size, n_size);
-	gsl_matrix *E_hat=gsl_matrix_alloc (d_size, n_size);
-	gsl_matrix *OmegaU=gsl_matrix_alloc (d_size, n_size);
-	gsl_matrix *OmegaE=gsl_matrix_alloc (d_size, n_size);
-	gsl_matrix *UltVehiY=gsl_matrix_alloc (d_size, n_size);
-	gsl_matrix *UltVehiBX=gsl_matrix_alloc (d_size, n_size);
-	gsl_matrix *UltVehiU=gsl_matrix_alloc (d_size, n_size);
-	gsl_matrix *UltVehiE=gsl_matrix_alloc (d_size, n_size);
-
-	// Large matrices for NR.
-	// Each dxd block is H_k^{-1}.
-	gsl_matrix *Hi_all=gsl_matrix_alloc (d_size, d_size*n_size);
-
-	// Each column is H_k^{-1}y_k.
-	gsl_matrix *Hiy_all=gsl_matrix_alloc (d_size, n_size);
-
-	// Each dcxdc block is x_k \otimes H_k^{-1}.
-	gsl_matrix *xHi_all=gsl_matrix_alloc (dc_size, d_size*n_size);
-	gsl_matrix *Hessian=gsl_matrix_alloc (v_size*2, v_size*2);
-
-	gsl_vector *x=gsl_vector_alloc (n_size);
-	gsl_vector *x_miss=gsl_vector_alloc (n_size);
-
-	gsl_matrix *Y=gsl_matrix_alloc (d_size, n_size);
-	gsl_matrix *X=gsl_matrix_alloc (c_size+1, n_size);
-	gsl_matrix *V_g=gsl_matrix_alloc (d_size, d_size);
-	gsl_matrix *V_e=gsl_matrix_alloc (d_size, d_size);
-	gsl_matrix *B=gsl_matrix_alloc (d_size, c_size+1);
-	gsl_vector *beta=gsl_vector_alloc (d_size);
-	gsl_matrix *Vbeta=gsl_matrix_alloc (d_size, d_size);
-
-	// Null estimates for initial values.
-	gsl_matrix *V_g_null=gsl_matrix_alloc (d_size, d_size);
-	gsl_matrix *V_e_null=gsl_matrix_alloc (d_size, d_size);
-	gsl_matrix *B_null=gsl_matrix_alloc (d_size, c_size+1);
-	gsl_matrix *se_B_null=gsl_matrix_alloc (d_size, c_size);
-
-	gsl_matrix_view X_sub=gsl_matrix_submatrix (X, 0, 0, c_size, n_size);
-	gsl_matrix_view B_sub=gsl_matrix_submatrix (B, 0, 0, d_size, c_size);
-	gsl_matrix_view xHi_all_sub =
-	  gsl_matrix_submatrix (xHi_all, 0, 0, d_size*c_size, d_size*n_size);
-
-	gsl_matrix_transpose_memcpy (Y, UtY);
-
-	gsl_matrix_transpose_memcpy (&X_sub.matrix, UtW);
-
-	gsl_vector_view X_row=gsl_matrix_row(X, c_size);
-	gsl_vector_set_zero(&X_row.vector);
-	gsl_vector_view B_col=gsl_matrix_column(B, c_size);
-	gsl_vector_set_zero(&B_col.vector);
-
-	MphInitial(em_iter, em_prec, nr_iter, nr_prec, eval, &X_sub.matrix,
-		   Y, l_min, l_max, n_region, V_g, V_e, &B_sub.matrix);
-	logl_H0=MphEM ('R', em_iter, em_prec, eval, &X_sub.matrix, Y, U_hat,
-		       E_hat, OmegaU, OmegaE, UltVehiY, UltVehiBX, UltVehiU,
-		       UltVehiE, V_g, V_e, &B_sub.matrix);
-	logl_H0=MphNR ('R', nr_iter, nr_prec, eval, &X_sub.matrix, Y, Hi_all,
-		       &xHi_all_sub.matrix, Hiy_all, V_g, V_e, Hessian,
-		       crt_a, crt_b, crt_c);
-	MphCalcBeta (eval, &X_sub.matrix, Y, V_g, V_e, UltVehiY,
-		     &B_sub.matrix, se_B_null);
-
-	c=0;
-	Vg_remle_null.clear();
-	Ve_remle_null.clear();
-	for (size_t i=0; i<d_size; i++) {
-	  for (size_t j=i; j<d_size; j++) {
-	    Vg_remle_null.push_back(gsl_matrix_get (V_g, i, j) );
-	    Ve_remle_null.push_back(gsl_matrix_get (V_e, i, j) );
-	    VVg_remle_null.push_back(gsl_matrix_get (Hessian, c, c) );
-	    VVe_remle_null.push_back(gsl_matrix_get (Hessian, c+v_size,
-						     c+v_size) );
-	    c++;
-	  }
-	}
-	beta_remle_null.clear();
-	se_beta_remle_null.clear();
-	for (size_t i=0; i<se_B_null->size1; i++) {
-	  for (size_t j=0; j<se_B_null->size2; j++) {
-	    beta_remle_null.push_back(gsl_matrix_get(B, i, j) );
-	    se_beta_remle_null.push_back(gsl_matrix_get(se_B_null, i, j) );
-	  }
-	}
-	logl_remle_H0=logl_H0;
-
-	cout.setf(std::ios_base::fixed, std::ios_base::floatfield);
-	cout.precision(4);
-
-	cout<<"REMLE estimate for Vg in the null model: "<<endl;
-	for (size_t i=0; i<d_size; i++) {
-	  for (size_t j=0; j<=i; j++) {
-	    cout<<gsl_matrix_get(V_g, i, j)<<"\t";
-	  }
-	  cout<<endl;
-	}
-	cout<<"se(Vg): "<<endl;
-	for (size_t i=0; i<d_size; i++) {
-	  for (size_t j=0; j<=i; j++) {
-	    c=GetIndex(i, j, d_size);
-	    cout<<sqrt(gsl_matrix_get(Hessian, c, c))<<"\t";
-	  }
-	  cout<<endl;
-	}
-	cout<<"REMLE estimate for Ve in the null model: "<<endl;
-	for (size_t i=0; i<d_size; i++) {
-	  for (size_t j=0; j<=i; j++) {
-	    cout<<gsl_matrix_get(V_e, i, j)<<"\t";
-	  }
-	  cout<<endl;
-	}
-	cout<<"se(Ve): "<<endl;
-	for (size_t i=0; i<d_size; i++) {
-	  for (size_t j=0; j<=i; j++) {
-	    c=GetIndex(i, j, d_size);
-	    cout<<sqrt(gsl_matrix_get(Hessian, c+v_size, c+v_size))<<"\t";
-	  }
-	  cout<<endl;
-	}
-	cout<<"REMLE likelihood = "<<logl_H0<<endl;
-
-	logl_H0=MphEM ('L', em_iter, em_prec, eval, &X_sub.matrix, Y, U_hat,
-		       E_hat, OmegaU, OmegaE, UltVehiY, UltVehiBX, UltVehiU,
-		       UltVehiE, V_g, V_e, &B_sub.matrix);
-	logl_H0=MphNR ('L', nr_iter, nr_prec, eval, &X_sub.matrix, Y,
-		       Hi_all, &xHi_all_sub.matrix, Hiy_all, V_g, V_e,
-		       Hessian, crt_a, crt_b, crt_c);
-	MphCalcBeta (eval, &X_sub.matrix, Y, V_g, V_e, UltVehiY,
-		     &B_sub.matrix, se_B_null);
-
-	c=0;
-	Vg_mle_null.clear();
-	Ve_mle_null.clear();
-	for (size_t i=0; i<d_size; i++) {
-	  for (size_t j=i; j<d_size; j++) {
-	    Vg_mle_null.push_back(gsl_matrix_get (V_g, i, j) );
-	    Ve_mle_null.push_back(gsl_matrix_get (V_e, i, j) );
-	    VVg_mle_null.push_back(gsl_matrix_get (Hessian, c, c) );
-	    VVe_mle_null.push_back(gsl_matrix_get(Hessian,c+v_size,c+v_size));
-	    c++;
-	  }
-	}
-	beta_mle_null.clear();
-	se_beta_mle_null.clear();
-	for (size_t i=0; i<se_B_null->size1; i++) {
-	  for (size_t j=0; j<se_B_null->size2; j++) {
-	    beta_mle_null.push_back(gsl_matrix_get(B, i, j) );
-	    se_beta_mle_null.push_back(gsl_matrix_get(se_B_null, i, j) );
-	  }
-	}
-	logl_mle_H0=logl_H0;
-
-	cout<<"MLE estimate for Vg in the null model: "<<endl;
-	for (size_t i=0; i<d_size; i++) {
-	  for (size_t j=0; j<=i; j++) {
-	    cout<<gsl_matrix_get(V_g, i, j)<<"\t";
-	  }
-	  cout<<endl;
-	}
-	cout<<"se(Vg): "<<endl;
-	for (size_t i=0; i<d_size; i++) {
-	  for (size_t j=0; j<=i; j++) {
-	    c=GetIndex(i, j, d_size);
-	    cout<<sqrt(gsl_matrix_get(Hessian, c, c))<<"\t";
-	  }
-	  cout<<endl;
-	}
-	cout<<"MLE estimate for Ve in the null model: "<<endl;
-	for (size_t i=0; i<d_size; i++) {
-	  for (size_t j=0; j<=i; j++) {
-	    cout<<gsl_matrix_get(V_e, i, j)<<"\t";
-	  }
-	  cout<<endl;
-	}
-	cout<<"se(Ve): "<<endl;
-	for (size_t i=0; i<d_size; i++) {
-	  for (size_t j=0; j<=i; j++) {
-	    c=GetIndex(i, j, d_size);
-	    cout<<sqrt(gsl_matrix_get(Hessian, c+v_size, c+v_size))<<"\t";
-	  }
-	  cout<<endl;
-	}
-	cout<<"MLE likelihood = "<<logl_H0<<endl;
-
-	vector<double> v_beta, v_Vg, v_Ve, v_Vbeta;
-	for (size_t i=0; i<d_size; i++) {
-	  v_beta.push_back(0.0);
-	}
-	for (size_t i=0; i<d_size; i++) {
-	  for (size_t j=i; j<d_size; j++) {
-	    v_Vg.push_back(0.0);
-	    v_Ve.push_back(0.0);
-	    v_Vbeta.push_back(0.0);
-	  }
-	}
-
-	gsl_matrix_memcpy (V_g_null, V_g);
-	gsl_matrix_memcpy (V_e_null, V_e);
-	gsl_matrix_memcpy (B_null, B);
-
-	// Start reading genotypes and analyze.
-	size_t csnp=0, t_last=0;
-	for (size_t t=0; t<indicator_snp.size(); ++t) {
-	  if (indicator_snp[t]==0) {continue;}
-	  t_last++;
-	}
-	for (size_t t=0; t<indicator_snp.size(); ++t) {
-	  !safeGetline(infile, line).eof();
-	  if (t%d_pace==0 || t==(ns_total-1)) {
-	    ProgressBar ("Reading SNPs  ", t, ns_total-1);
-	  }
-	  if (indicator_snp[t]==0) {continue;}
-	  
-	  ch_ptr=strtok ((char *)line.c_str(), " , \t");
-	  ch_ptr=strtok (NULL, " , \t");
-	  ch_ptr=strtok (NULL, " , \t");
-	  
-	  x_mean=0.0; c_phen=0; n_miss=0;
-	  gsl_vector_set_zero(x_miss);
-	  for (size_t i=0; i<ni_total; ++i) {
-	    ch_ptr=strtok (NULL, " , \t");
-	    if (indicator_idv[i]==0) {continue;}
-	    
-	    if (strcmp(ch_ptr, "NA")==0) {
-	      gsl_vector_set(x_miss, c_phen, 0.0);
-	      n_miss++;
-	    }
-	    else {
-	      geno=atof(ch_ptr);
-	      
-	      gsl_vector_set(x, c_phen, geno);
-	      gsl_vector_set(x_miss, c_phen, 1.0);
-	      x_mean+=geno;
-	    }
-	    c_phen++;
-	  }
-	  
-	  x_mean/=(double)(ni_test-n_miss);
-	  
-	  for (size_t i=0; i<ni_test; ++i) {
-	    if (gsl_vector_get (x_miss, i)==0) {gsl_vector_set(x, i, x_mean);}
-	    geno=gsl_vector_get(x, i);
-	  }
-
-	  gsl_vector_view Xlarge_col=gsl_matrix_column (Xlarge, csnp%msize);
-	  gsl_vector_memcpy (&Xlarge_col.vector, x);
-	  csnp++;
-	  
-	  if (csnp%msize==0 || csnp==t_last ) {
-	    size_t l=0;
-	    if (csnp%msize==0) {l=msize;} else {l=csnp%msize;}
-	    
-	    gsl_matrix_view Xlarge_sub =
-	      gsl_matrix_submatrix(Xlarge, 0, 0, Xlarge->size1, l);
-	    gsl_matrix_view UtXlarge_sub =
-	      gsl_matrix_submatrix(UtXlarge, 0, 0, UtXlarge->size1, l);
-	    
-	    time_start=clock();
-	    eigenlib_dgemm ("T", "N", 1.0, U, &Xlarge_sub.matrix, 0.0,
-			    &UtXlarge_sub.matrix);
-	    time_UtX+=(clock()-time_start)/(double(CLOCKS_PER_SEC)*60.0);
-	    
-	    gsl_matrix_set_zero (Xlarge);
-	    
-	    for (size_t i=0; i<l; i++) {
-	      gsl_vector_view UtXlarge_col=gsl_matrix_column (UtXlarge, i);
-	      gsl_vector_memcpy (&X_row.vector, &UtXlarge_col.vector);
-	      
-	      // Initial values.
-	      gsl_matrix_memcpy (V_g, V_g_null);
-	      gsl_matrix_memcpy (V_e, V_e_null);
-	      gsl_matrix_memcpy (B, B_null);
-	      
-	      time_start=clock();
-	      
-	      // 3 is before 1.
-	      if (a_mode==3 || a_mode==4) {
-		p_score=MphCalcP (eval, &X_row.vector, &X_sub.matrix, Y,
-				  V_g_null, V_e_null, UltVehiY, beta, Vbeta);
-		if (p_score<p_nr && crt==1) {
-		  logl_H1=MphNR ('R', 1, nr_prec*10, eval, X, Y, Hi_all,
-				 xHi_all, Hiy_all, V_g, V_e, Hessian, crt_a,
-				 crt_b, crt_c);
-		  p_score=PCRT (3, d_size, p_score, crt_a, crt_b, crt_c);
-		}
-	      }
-
-	      if (a_mode==2 || a_mode==4) {
-		logl_H1=MphEM ('L', em_iter/10, em_prec*10, eval, X, Y,
-			       U_hat, E_hat, OmegaU, OmegaE, UltVehiY,
-			       UltVehiBX, UltVehiU, UltVehiE, V_g, V_e, B);
-		
-		// Calculate beta and Vbeta.
-		p_lrt=MphCalcP (eval, &X_row.vector, &X_sub.matrix, Y,
-				V_g, V_e, UltVehiY, beta, Vbeta);
-		p_lrt=gsl_cdf_chisq_Q (2.0*(logl_H1-logl_H0), (double)d_size );
-		
-		if (p_lrt<p_nr) {
-		  logl_H1=MphNR ('L', nr_iter/10, nr_prec*10, eval, X, Y,
-				 Hi_all, xHi_all, Hiy_all, V_g, V_e, Hessian,
-				 crt_a, crt_b, crt_c);
-		  
-		  // Calculate beta and Vbeta.
-		  p_lrt=MphCalcP (eval, &X_row.vector, &X_sub.matrix, Y,
-				  V_g, V_e, UltVehiY, beta, Vbeta);
-		  p_lrt=gsl_cdf_chisq_Q (2.0*(logl_H1-logl_H0),
-					 (double)d_size );
-
-		  if (crt==1) {
-		    p_lrt=PCRT (2, d_size, p_lrt, crt_a, crt_b, crt_c);
-		  }
-		}
-	      }
-	      
-	      if (a_mode==1 || a_mode==4) {
-		logl_H1=MphEM ('R', em_iter/10, em_prec*10, eval, X, Y,
-			       U_hat, E_hat, OmegaU, OmegaE, UltVehiY,
-			       UltVehiBX, UltVehiU, UltVehiE, V_g, V_e, B);
-		p_wald=MphCalcP (eval, &X_row.vector, &X_sub.matrix, Y, V_g,
-				 V_e, UltVehiY, beta, Vbeta);
-		
-		if (p_wald<p_nr) {
-		  logl_H1=MphNR ('R', nr_iter/10, nr_prec*10, eval, X, Y,
-				 Hi_all, xHi_all, Hiy_all, V_g, V_e,
-				 Hessian, crt_a, crt_b, crt_c);
-		  p_wald=MphCalcP (eval, &X_row.vector, &X_sub.matrix, Y,
-				   V_g, V_e, UltVehiY, beta, Vbeta);
-		  
-		  if (crt==1) {
-		    p_wald=PCRT (1, d_size, p_wald, crt_a, crt_b, crt_c);
-		  }
-		}
-	      }
-	      
-	      time_opt+=(clock()-time_start)/(double(CLOCKS_PER_SEC)*60.0);
-
-	      // Store summary data.
-	      for (size_t i=0; i<d_size; i++) {
-		v_beta[i]=gsl_vector_get (beta, i);
-	      }
-	      
-	      c=0;
-	      for (size_t i=0; i<d_size; i++) {
-		for (size_t j=i; j<d_size; j++) {
-		  v_Vg[c]=gsl_matrix_get (V_g, i, j);
-		  v_Ve[c]=gsl_matrix_get (V_e, i, j);
-		  v_Vbeta[c]=gsl_matrix_get (Vbeta, i, j);
-		  c++;
-		}
-	      }
-	      
-	      MPHSUMSTAT SNPs={v_beta, p_wald, p_lrt, p_score, v_Vg,
-			       v_Ve, v_Vbeta};
-	      sumStat.push_back(SNPs);
-	    }
-	  }
-	}
-	cout<<endl;
-
-	infile.close();
-	infile.clear();
-
-	gsl_matrix_free(U_hat);
-	gsl_matrix_free(E_hat);
-	gsl_matrix_free(OmegaU);
-	gsl_matrix_free(OmegaE);
-	gsl_matrix_free(UltVehiY);
-	gsl_matrix_free(UltVehiBX);
-	gsl_matrix_free(UltVehiU);
-	gsl_matrix_free(UltVehiE);
-
-	gsl_matrix_free(Hi_all);
-	gsl_matrix_free(Hiy_all);
-	gsl_matrix_free(xHi_all);
-	gsl_matrix_free(Hessian);
-
-	gsl_vector_free(x);
-	gsl_vector_free(x_miss);
-
-	gsl_matrix_free(Y);
-	gsl_matrix_free(X);
-	gsl_matrix_free(V_g);
-	gsl_matrix_free(V_e);
-	gsl_matrix_free(B);
-	gsl_vector_free(beta);
-	gsl_matrix_free(Vbeta);
-
-	gsl_matrix_free(V_g_null);
-	gsl_matrix_free(V_e_null);
-	gsl_matrix_free(B_null);
-	gsl_matrix_free(se_B_null);
-
-	gsl_matrix_free(Xlarge);
-	gsl_matrix_free(UtXlarge);
-
-	return;
+void MVLMM::AnalyzeBimbam(const gsl_matrix *U, const gsl_vector *eval,
+                          const gsl_matrix *UtW, const gsl_matrix *UtY) {
+  igzstream infile(file_geno.c_str(), igzstream::in);
+  if (!infile) {
+    cout << "error reading genotype file:" << file_geno << endl;
+    return;
+  }
+
+  clock_t time_start = clock();
+  time_UtX = 0;
+  time_opt = 0;
+
+  string line;
+  char *ch_ptr;
+
+  double logl_H0 = 0.0, logl_H1 = 0.0, p_wald = 0, p_lrt = 0, p_score = 0;
+  double crt_a, crt_b, crt_c;
+  int n_miss, c_phen;
+  double geno, x_mean;
+  size_t c = 0;
+  size_t n_size = UtY->size1, d_size = UtY->size2, c_size = UtW->size2;
+
+  size_t dc_size = d_size * (c_size + 1), v_size = d_size * (d_size + 1) / 2;
+
+  // Create a large matrix.
+  size_t msize = 10000;
+  gsl_matrix *Xlarge = gsl_matrix_alloc(U->size1, msize);
+  gsl_matrix *UtXlarge = gsl_matrix_alloc(U->size1, msize);
+  gsl_matrix_set_zero(Xlarge);
+
+  // Large matrices for EM.
+  gsl_matrix *U_hat = gsl_matrix_alloc(d_size, n_size);
+  gsl_matrix *E_hat = gsl_matrix_alloc(d_size, n_size);
+  gsl_matrix *OmegaU = gsl_matrix_alloc(d_size, n_size);
+  gsl_matrix *OmegaE = gsl_matrix_alloc(d_size, n_size);
+  gsl_matrix *UltVehiY = gsl_matrix_alloc(d_size, n_size);
+  gsl_matrix *UltVehiBX = gsl_matrix_alloc(d_size, n_size);
+  gsl_matrix *UltVehiU = gsl_matrix_alloc(d_size, n_size);
+  gsl_matrix *UltVehiE = gsl_matrix_alloc(d_size, n_size);
+
+  // Large matrices for NR.
+  // Each dxd block is H_k^{-1}.
+  gsl_matrix *Hi_all = gsl_matrix_alloc(d_size, d_size * n_size);
+
+  // Each column is H_k^{-1}y_k.
+  gsl_matrix *Hiy_all = gsl_matrix_alloc(d_size, n_size);
+
+  // Each dcxdc block is x_k \otimes H_k^{-1}.
+  gsl_matrix *xHi_all = gsl_matrix_alloc(dc_size, d_size * n_size);
+  gsl_matrix *Hessian = gsl_matrix_alloc(v_size * 2, v_size * 2);
+
+  gsl_vector *x = gsl_vector_alloc(n_size);
+  gsl_vector *x_miss = gsl_vector_alloc(n_size);
+
+  gsl_matrix *Y = gsl_matrix_alloc(d_size, n_size);
+  gsl_matrix *X = gsl_matrix_alloc(c_size + 1, n_size);
+  gsl_matrix *V_g = gsl_matrix_alloc(d_size, d_size);
+  gsl_matrix *V_e = gsl_matrix_alloc(d_size, d_size);
+  gsl_matrix *B = gsl_matrix_alloc(d_size, c_size + 1);
+  gsl_vector *beta = gsl_vector_alloc(d_size);
+  gsl_matrix *Vbeta = gsl_matrix_alloc(d_size, d_size);
+
+  // Null estimates for initial values.
+  gsl_matrix *V_g_null = gsl_matrix_alloc(d_size, d_size);
+  gsl_matrix *V_e_null = gsl_matrix_alloc(d_size, d_size);
+  gsl_matrix *B_null = gsl_matrix_alloc(d_size, c_size + 1);
+  gsl_matrix *se_B_null = gsl_matrix_alloc(d_size, c_size);
+
+  gsl_matrix_view X_sub = gsl_matrix_submatrix(X, 0, 0, c_size, n_size);
+  gsl_matrix_view B_sub = gsl_matrix_submatrix(B, 0, 0, d_size, c_size);
+  gsl_matrix_view xHi_all_sub =
+      gsl_matrix_submatrix(xHi_all, 0, 0, d_size * c_size, d_size * n_size);
+
+  gsl_matrix_transpose_memcpy(Y, UtY);
+
+  gsl_matrix_transpose_memcpy(&X_sub.matrix, UtW);
+
+  gsl_vector_view X_row = gsl_matrix_row(X, c_size);
+  gsl_vector_set_zero(&X_row.vector);
+  gsl_vector_view B_col = gsl_matrix_column(B, c_size);
+  gsl_vector_set_zero(&B_col.vector);
+
+  MphInitial(em_iter, em_prec, nr_iter, nr_prec, eval, &X_sub.matrix, Y, l_min,
+             l_max, n_region, V_g, V_e, &B_sub.matrix);
+  logl_H0 = MphEM('R', em_iter, em_prec, eval, &X_sub.matrix, Y, U_hat, E_hat,
+                  OmegaU, OmegaE, UltVehiY, UltVehiBX, UltVehiU, UltVehiE, V_g,
+                  V_e, &B_sub.matrix);
+  logl_H0 = MphNR('R', nr_iter, nr_prec, eval, &X_sub.matrix, Y, Hi_all,
+                  &xHi_all_sub.matrix, Hiy_all, V_g, V_e, Hessian, crt_a, crt_b,
+                  crt_c);
+  MphCalcBeta(eval, &X_sub.matrix, Y, V_g, V_e, UltVehiY, &B_sub.matrix,
+              se_B_null);
+
+  c = 0;
+  Vg_remle_null.clear();
+  Ve_remle_null.clear();
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = i; j < d_size; j++) {
+      Vg_remle_null.push_back(gsl_matrix_get(V_g, i, j));
+      Ve_remle_null.push_back(gsl_matrix_get(V_e, i, j));
+      VVg_remle_null.push_back(gsl_matrix_get(Hessian, c, c));
+      VVe_remle_null.push_back(gsl_matrix_get(Hessian, c + v_size, c + v_size));
+      c++;
+    }
+  }
+  beta_remle_null.clear();
+  se_beta_remle_null.clear();
+  for (size_t i = 0; i < se_B_null->size1; i++) {
+    for (size_t j = 0; j < se_B_null->size2; j++) {
+      beta_remle_null.push_back(gsl_matrix_get(B, i, j));
+      se_beta_remle_null.push_back(gsl_matrix_get(se_B_null, i, j));
+    }
+  }
+  logl_remle_H0 = logl_H0;
+
+  cout.setf(std::ios_base::fixed, std::ios_base::floatfield);
+  cout.precision(4);
+
+  cout << "REMLE estimate for Vg in the null model: " << endl;
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = 0; j <= i; j++) {
+      cout << gsl_matrix_get(V_g, i, j) << "\t";
+    }
+    cout << endl;
+  }
+  cout << "se(Vg): " << endl;
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = 0; j <= i; j++) {
+      c = GetIndex(i, j, d_size);
+      cout << sqrt(gsl_matrix_get(Hessian, c, c)) << "\t";
+    }
+    cout << endl;
+  }
+  cout << "REMLE estimate for Ve in the null model: " << endl;
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = 0; j <= i; j++) {
+      cout << gsl_matrix_get(V_e, i, j) << "\t";
+    }
+    cout << endl;
+  }
+  cout << "se(Ve): " << endl;
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = 0; j <= i; j++) {
+      c = GetIndex(i, j, d_size);
+      cout << sqrt(gsl_matrix_get(Hessian, c + v_size, c + v_size)) << "\t";
+    }
+    cout << endl;
+  }
+  cout << "REMLE likelihood = " << logl_H0 << endl;
+
+  logl_H0 = MphEM('L', em_iter, em_prec, eval, &X_sub.matrix, Y, U_hat, E_hat,
+                  OmegaU, OmegaE, UltVehiY, UltVehiBX, UltVehiU, UltVehiE, V_g,
+                  V_e, &B_sub.matrix);
+  logl_H0 = MphNR('L', nr_iter, nr_prec, eval, &X_sub.matrix, Y, Hi_all,
+                  &xHi_all_sub.matrix, Hiy_all, V_g, V_e, Hessian, crt_a, crt_b,
+                  crt_c);
+  MphCalcBeta(eval, &X_sub.matrix, Y, V_g, V_e, UltVehiY, &B_sub.matrix,
+              se_B_null);
+
+  c = 0;
+  Vg_mle_null.clear();
+  Ve_mle_null.clear();
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = i; j < d_size; j++) {
+      Vg_mle_null.push_back(gsl_matrix_get(V_g, i, j));
+      Ve_mle_null.push_back(gsl_matrix_get(V_e, i, j));
+      VVg_mle_null.push_back(gsl_matrix_get(Hessian, c, c));
+      VVe_mle_null.push_back(gsl_matrix_get(Hessian, c + v_size, c + v_size));
+      c++;
+    }
+  }
+  beta_mle_null.clear();
+  se_beta_mle_null.clear();
+  for (size_t i = 0; i < se_B_null->size1; i++) {
+    for (size_t j = 0; j < se_B_null->size2; j++) {
+      beta_mle_null.push_back(gsl_matrix_get(B, i, j));
+      se_beta_mle_null.push_back(gsl_matrix_get(se_B_null, i, j));
+    }
+  }
+  logl_mle_H0 = logl_H0;
+
+  cout << "MLE estimate for Vg in the null model: " << endl;
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = 0; j <= i; j++) {
+      cout << gsl_matrix_get(V_g, i, j) << "\t";
+    }
+    cout << endl;
+  }
+  cout << "se(Vg): " << endl;
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = 0; j <= i; j++) {
+      c = GetIndex(i, j, d_size);
+      cout << sqrt(gsl_matrix_get(Hessian, c, c)) << "\t";
+    }
+    cout << endl;
+  }
+  cout << "MLE estimate for Ve in the null model: " << endl;
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = 0; j <= i; j++) {
+      cout << gsl_matrix_get(V_e, i, j) << "\t";
+    }
+    cout << endl;
+  }
+  cout << "se(Ve): " << endl;
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = 0; j <= i; j++) {
+      c = GetIndex(i, j, d_size);
+      cout << sqrt(gsl_matrix_get(Hessian, c + v_size, c + v_size)) << "\t";
+    }
+    cout << endl;
+  }
+  cout << "MLE likelihood = " << logl_H0 << endl;
+
+  vector<double> v_beta, v_Vg, v_Ve, v_Vbeta;
+  for (size_t i = 0; i < d_size; i++) {
+    v_beta.push_back(0.0);
+  }
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = i; j < d_size; j++) {
+      v_Vg.push_back(0.0);
+      v_Ve.push_back(0.0);
+      v_Vbeta.push_back(0.0);
+    }
+  }
+
+  gsl_matrix_memcpy(V_g_null, V_g);
+  gsl_matrix_memcpy(V_e_null, V_e);
+  gsl_matrix_memcpy(B_null, B);
+
+  // Start reading genotypes and analyze.
+  size_t csnp = 0, t_last = 0;
+  for (size_t t = 0; t < indicator_snp.size(); ++t) {
+    if (indicator_snp[t] == 0) {
+      continue;
+    }
+    t_last++;
+  }
+  for (size_t t = 0; t < indicator_snp.size(); ++t) {
+    !safeGetline(infile, line).eof();
+    if (t % d_pace == 0 || t == (ns_total - 1)) {
+      ProgressBar("Reading SNPs  ", t, ns_total - 1);
+    }
+    if (indicator_snp[t] == 0) {
+      continue;
+    }
+
+    ch_ptr = strtok((char *)line.c_str(), " , \t");
+    ch_ptr = strtok(NULL, " , \t");
+    ch_ptr = strtok(NULL, " , \t");
+
+    x_mean = 0.0;
+    c_phen = 0;
+    n_miss = 0;
+    gsl_vector_set_zero(x_miss);
+    for (size_t i = 0; i < ni_total; ++i) {
+      ch_ptr = strtok(NULL, " , \t");
+      if (indicator_idv[i] == 0) {
+        continue;
+      }
+
+      if (strcmp(ch_ptr, "NA") == 0) {
+        gsl_vector_set(x_miss, c_phen, 0.0);
+        n_miss++;
+      } else {
+        geno = atof(ch_ptr);
+
+        gsl_vector_set(x, c_phen, geno);
+        gsl_vector_set(x_miss, c_phen, 1.0);
+        x_mean += geno;
+      }
+      c_phen++;
+    }
+
+    x_mean /= (double)(ni_test - n_miss);
+
+    for (size_t i = 0; i < ni_test; ++i) {
+      if (gsl_vector_get(x_miss, i) == 0) {
+        gsl_vector_set(x, i, x_mean);
+      }
+      geno = gsl_vector_get(x, i);
+    }
+
+    gsl_vector_view Xlarge_col = gsl_matrix_column(Xlarge, csnp % msize);
+    gsl_vector_memcpy(&Xlarge_col.vector, x);
+    csnp++;
+
+    if (csnp % msize == 0 || csnp == t_last) {
+      size_t l = 0;
+      if (csnp % msize == 0) {
+        l = msize;
+      } else {
+        l = csnp % msize;
+      }
+
+      gsl_matrix_view Xlarge_sub =
+          gsl_matrix_submatrix(Xlarge, 0, 0, Xlarge->size1, l);
+      gsl_matrix_view UtXlarge_sub =
+          gsl_matrix_submatrix(UtXlarge, 0, 0, UtXlarge->size1, l);
+
+      time_start = clock();
+      eigenlib_dgemm("T", "N", 1.0, U, &Xlarge_sub.matrix, 0.0,
+                     &UtXlarge_sub.matrix);
+      time_UtX += (clock() - time_start) / (double(CLOCKS_PER_SEC) * 60.0);
+
+      gsl_matrix_set_zero(Xlarge);
+
+      for (size_t i = 0; i < l; i++) {
+        gsl_vector_view UtXlarge_col = gsl_matrix_column(UtXlarge, i);
+        gsl_vector_memcpy(&X_row.vector, &UtXlarge_col.vector);
+
+        // Initial values.
+        gsl_matrix_memcpy(V_g, V_g_null);
+        gsl_matrix_memcpy(V_e, V_e_null);
+        gsl_matrix_memcpy(B, B_null);
+
+        time_start = clock();
+
+        // 3 is before 1.
+        if (a_mode == 3 || a_mode == 4) {
+          p_score = MphCalcP(eval, &X_row.vector, &X_sub.matrix, Y, V_g_null,
+                             V_e_null, UltVehiY, beta, Vbeta);
+          if (p_score < p_nr && crt == 1) {
+            logl_H1 = MphNR('R', 1, nr_prec * 10, eval, X, Y, Hi_all, xHi_all,
+                            Hiy_all, V_g, V_e, Hessian, crt_a, crt_b, crt_c);
+            p_score = PCRT(3, d_size, p_score, crt_a, crt_b, crt_c);
+          }
+        }
+
+        if (a_mode == 2 || a_mode == 4) {
+          logl_H1 = MphEM('L', em_iter / 10, em_prec * 10, eval, X, Y, U_hat,
+                          E_hat, OmegaU, OmegaE, UltVehiY, UltVehiBX, UltVehiU,
+                          UltVehiE, V_g, V_e, B);
+
+          // Calculate beta and Vbeta.
+          p_lrt = MphCalcP(eval, &X_row.vector, &X_sub.matrix, Y, V_g, V_e,
+                           UltVehiY, beta, Vbeta);
+          p_lrt = gsl_cdf_chisq_Q(2.0 * (logl_H1 - logl_H0), (double)d_size);
+
+          if (p_lrt < p_nr) {
+            logl_H1 =
+                MphNR('L', nr_iter / 10, nr_prec * 10, eval, X, Y, Hi_all,
+                      xHi_all, Hiy_all, V_g, V_e, Hessian, crt_a, crt_b, crt_c);
+
+            // Calculate beta and Vbeta.
+            p_lrt = MphCalcP(eval, &X_row.vector, &X_sub.matrix, Y, V_g, V_e,
+                             UltVehiY, beta, Vbeta);
+            p_lrt = gsl_cdf_chisq_Q(2.0 * (logl_H1 - logl_H0), (double)d_size);
+
+            if (crt == 1) {
+              p_lrt = PCRT(2, d_size, p_lrt, crt_a, crt_b, crt_c);
+            }
+          }
+        }
+
+        if (a_mode == 1 || a_mode == 4) {
+          logl_H1 = MphEM('R', em_iter / 10, em_prec * 10, eval, X, Y, U_hat,
+                          E_hat, OmegaU, OmegaE, UltVehiY, UltVehiBX, UltVehiU,
+                          UltVehiE, V_g, V_e, B);
+          p_wald = MphCalcP(eval, &X_row.vector, &X_sub.matrix, Y, V_g, V_e,
+                            UltVehiY, beta, Vbeta);
+
+          if (p_wald < p_nr) {
+            logl_H1 =
+                MphNR('R', nr_iter / 10, nr_prec * 10, eval, X, Y, Hi_all,
+                      xHi_all, Hiy_all, V_g, V_e, Hessian, crt_a, crt_b, crt_c);
+            p_wald = MphCalcP(eval, &X_row.vector, &X_sub.matrix, Y, V_g, V_e,
+                              UltVehiY, beta, Vbeta);
+
+            if (crt == 1) {
+              p_wald = PCRT(1, d_size, p_wald, crt_a, crt_b, crt_c);
+            }
+          }
+        }
+
+        time_opt += (clock() - time_start) / (double(CLOCKS_PER_SEC) * 60.0);
+
+        // Store summary data.
+        for (size_t i = 0; i < d_size; i++) {
+          v_beta[i] = gsl_vector_get(beta, i);
+        }
+
+        c = 0;
+        for (size_t i = 0; i < d_size; i++) {
+          for (size_t j = i; j < d_size; j++) {
+            v_Vg[c] = gsl_matrix_get(V_g, i, j);
+            v_Ve[c] = gsl_matrix_get(V_e, i, j);
+            v_Vbeta[c] = gsl_matrix_get(Vbeta, i, j);
+            c++;
+          }
+        }
+
+        MPHSUMSTAT SNPs = {v_beta, p_wald, p_lrt, p_score, v_Vg, v_Ve, v_Vbeta};
+        sumStat.push_back(SNPs);
+      }
+    }
+  }
+  cout << endl;
+
+  infile.close();
+  infile.clear();
+
+  gsl_matrix_free(U_hat);
+  gsl_matrix_free(E_hat);
+  gsl_matrix_free(OmegaU);
+  gsl_matrix_free(OmegaE);
+  gsl_matrix_free(UltVehiY);
+  gsl_matrix_free(UltVehiBX);
+  gsl_matrix_free(UltVehiU);
+  gsl_matrix_free(UltVehiE);
+
+  gsl_matrix_free(Hi_all);
+  gsl_matrix_free(Hiy_all);
+  gsl_matrix_free(xHi_all);
+  gsl_matrix_free(Hessian);
+
+  gsl_vector_free(x);
+  gsl_vector_free(x_miss);
+
+  gsl_matrix_free(Y);
+  gsl_matrix_free(X);
+  gsl_matrix_free(V_g);
+  gsl_matrix_free(V_e);
+  gsl_matrix_free(B);
+  gsl_vector_free(beta);
+  gsl_matrix_free(Vbeta);
+
+  gsl_matrix_free(V_g_null);
+  gsl_matrix_free(V_e_null);
+  gsl_matrix_free(B_null);
+  gsl_matrix_free(se_B_null);
+
+  gsl_matrix_free(Xlarge);
+  gsl_matrix_free(UtXlarge);
+
+  return;
 }
 
-void MVLMM::AnalyzePlink (const gsl_matrix *U, const gsl_vector *eval,
-			  const gsl_matrix *UtW, const gsl_matrix *UtY) {
-	string file_bed=file_bfile+".bed";
-	ifstream infile (file_bed.c_str(), ios::binary);
-	if (!infile) {cout<<"error reading bed file:"<<file_bed<<endl; return;}
-
-	clock_t time_start=clock();
-	time_UtX=0; time_opt=0;
-
-	char ch[1];
-	bitset<8> b;
-
-	double logl_H0=0.0, logl_H1=0.0, p_wald=0, p_lrt=0, p_score=0;
-	double crt_a, crt_b, crt_c;
-	int n_bit, n_miss, ci_total, ci_test;
-	double geno, x_mean;
-	size_t c=0;
-	size_t n_size=UtY->size1, d_size=UtY->size2, c_size=UtW->size2;
-	size_t dc_size=d_size*(c_size+1), v_size=d_size*(d_size+1)/2;
-
-	// Create a large matrix.
-	size_t msize=10000;
-	gsl_matrix *Xlarge=gsl_matrix_alloc (U->size1, msize);
-	gsl_matrix *UtXlarge=gsl_matrix_alloc (U->size1, msize);
-	gsl_matrix_set_zero(Xlarge);
-
-	// Large matrices for EM.
-	gsl_matrix *U_hat=gsl_matrix_alloc (d_size, n_size);
-	gsl_matrix *E_hat=gsl_matrix_alloc (d_size, n_size);
-	gsl_matrix *OmegaU=gsl_matrix_alloc (d_size, n_size);
-	gsl_matrix *OmegaE=gsl_matrix_alloc (d_size, n_size);
-	gsl_matrix *UltVehiY=gsl_matrix_alloc (d_size, n_size);
-	gsl_matrix *UltVehiBX=gsl_matrix_alloc (d_size, n_size);
-	gsl_matrix *UltVehiU=gsl_matrix_alloc (d_size, n_size);
-	gsl_matrix *UltVehiE=gsl_matrix_alloc (d_size, n_size);
-
-	// Large matrices for NR.
-	// Each dxd block is H_k^{-1}.
-	gsl_matrix *Hi_all=gsl_matrix_alloc (d_size, d_size*n_size);
-
-	// Each column is H_k^{-1}y_k.
-	gsl_matrix *Hiy_all=gsl_matrix_alloc (d_size, n_size);
-
-	// Each dcxdc block is x_k\otimes H_k^{-1}.
-	gsl_matrix *xHi_all=gsl_matrix_alloc (dc_size, d_size*n_size);
-	
-	gsl_matrix *Hessian=gsl_matrix_alloc (v_size*2, v_size*2);
-
-	gsl_vector *x=gsl_vector_alloc (n_size);
-
-	gsl_matrix *Y=gsl_matrix_alloc (d_size, n_size);
-	gsl_matrix *X=gsl_matrix_alloc (c_size+1, n_size);
-	gsl_matrix *V_g=gsl_matrix_alloc (d_size, d_size);
-	gsl_matrix *V_e=gsl_matrix_alloc (d_size, d_size);
-	gsl_matrix *B=gsl_matrix_alloc (d_size, c_size+1);
-	gsl_vector *beta=gsl_vector_alloc (d_size);
-	gsl_matrix *Vbeta=gsl_matrix_alloc (d_size, d_size);
-
-	// Null estimates for initial values.
-	gsl_matrix *V_g_null=gsl_matrix_alloc (d_size, d_size);
-	gsl_matrix *V_e_null=gsl_matrix_alloc (d_size, d_size);
-	gsl_matrix *B_null=gsl_matrix_alloc (d_size, c_size+1);
-	gsl_matrix *se_B_null=gsl_matrix_alloc (d_size, c_size);
-
-	gsl_matrix_view X_sub=gsl_matrix_submatrix (X, 0, 0, c_size, n_size);
-	gsl_matrix_view B_sub=gsl_matrix_submatrix (B, 0, 0, d_size, c_size);
-	gsl_matrix_view xHi_all_sub =
-	  gsl_matrix_submatrix (xHi_all, 0, 0, d_size*c_size, d_size*n_size);
-
-	gsl_matrix_transpose_memcpy (Y, UtY);
-	gsl_matrix_transpose_memcpy (&X_sub.matrix, UtW);
-
-	gsl_vector_view X_row=gsl_matrix_row(X, c_size);
-	gsl_vector_set_zero(&X_row.vector);
-	gsl_vector_view B_col=gsl_matrix_column(B, c_size);
-	gsl_vector_set_zero(&B_col.vector);
-
-	MphInitial(em_iter, em_prec, nr_iter, nr_prec, eval, &X_sub.matrix,
-		   Y, l_min, l_max, n_region, V_g, V_e, &B_sub.matrix);
-
-	logl_H0=MphEM ('R', em_iter, em_prec, eval, &X_sub.matrix, Y, U_hat,
-		       E_hat, OmegaU, OmegaE, UltVehiY, UltVehiBX, UltVehiU,
-		       UltVehiE, V_g, V_e, &B_sub.matrix);
-	logl_H0=MphNR ('R', nr_iter, nr_prec, eval, &X_sub.matrix, Y, Hi_all,
-		       &xHi_all_sub.matrix, Hiy_all, V_g, V_e, Hessian,
-		       crt_a, crt_b, crt_c);
-	MphCalcBeta (eval, &X_sub.matrix, Y, V_g, V_e, UltVehiY,
-		     &B_sub.matrix, se_B_null);
-
-	c=0;
-	Vg_remle_null.clear();
-	Ve_remle_null.clear();
-	for (size_t i=0; i<d_size; i++) {
-	  for (size_t j=i; j<d_size; j++) {
-	    Vg_remle_null.push_back(gsl_matrix_get (V_g, i, j) );
-	    Ve_remle_null.push_back(gsl_matrix_get (V_e, i, j) );
-	    VVg_remle_null.push_back(gsl_matrix_get (Hessian, c, c) );
-	    VVe_remle_null.push_back(gsl_matrix_get(Hessian,c+v_size,
-						    c+v_size));
-	    c++;
-	  }
-	}
-	beta_remle_null.clear();
-	se_beta_remle_null.clear();
-	for (size_t i=0; i<se_B_null->size1; i++) {
-	  for (size_t j=0; j<se_B_null->size2; j++) {
-	    beta_remle_null.push_back(gsl_matrix_get(B, i, j) );
-	    se_beta_remle_null.push_back(gsl_matrix_get(se_B_null, i, j) );
-	  }
-	}
-	logl_remle_H0=logl_H0;
-
-	cout.setf(std::ios_base::fixed, std::ios_base::floatfield);
-	cout.precision(4);
-	cout<<"REMLE estimate for Vg in the null model: "<<endl;
-	for (size_t i=0; i<d_size; i++) {
-	  for (size_t j=0; j<=i; j++) {
-	    cout<<gsl_matrix_get(V_g, i, j)<<"\t";
-	  }
-	  cout<<endl;
-	}
-	cout<<"se(Vg): "<<endl;
-	for (size_t i=0; i<d_size; i++) {
-	  for (size_t j=0; j<=i; j++) {
-	    c=GetIndex(i, j, d_size);
-	    cout<<sqrt(gsl_matrix_get(Hessian, c, c))<<"\t";
-	  }
-	  cout<<endl;
-	}
-	cout<<"REMLE estimate for Ve in the null model: "<<endl;
-	for (size_t i=0; i<d_size; i++) {
-	  for (size_t j=0; j<=i; j++) {
-	    cout<<gsl_matrix_get(V_e, i, j)<<"\t";
-	  }
-	  cout<<endl;
-	}
-	cout<<"se(Ve): "<<endl;
-	for (size_t i=0; i<d_size; i++) {
-	  for (size_t j=0; j<=i; j++) {
-	    c=GetIndex(i, j, d_size);
-	    cout<<sqrt(gsl_matrix_get(Hessian, c+v_size, c+v_size))<<"\t";
-	  }
-		cout<<endl;
-	}
-	cout<<"REMLE likelihood = "<<logl_H0<<endl;
-
-	logl_H0=MphEM ('L', em_iter, em_prec, eval, &X_sub.matrix, Y,
-		       U_hat, E_hat, OmegaU, OmegaE, UltVehiY, UltVehiBX,
-		       UltVehiU, UltVehiE, V_g, V_e, &B_sub.matrix);
-	logl_H0=MphNR ('L', nr_iter, nr_prec, eval, &X_sub.matrix, Y,
-		       Hi_all, &xHi_all_sub.matrix, Hiy_all, V_g, V_e,
-		       Hessian, crt_a, crt_b, crt_c);
-	MphCalcBeta (eval, &X_sub.matrix, Y, V_g, V_e, UltVehiY,
-		     &B_sub.matrix, se_B_null);
-
-	c=0;
-	Vg_mle_null.clear();
-	Ve_mle_null.clear();
-	for (size_t i=0; i<d_size; i++) {
-	  for (size_t j=i; j<d_size; j++) {
-	    Vg_mle_null.push_back(gsl_matrix_get (V_g, i, j) );
-	    Ve_mle_null.push_back(gsl_matrix_get (V_e, i, j) );
-	    VVg_mle_null.push_back(gsl_matrix_get (Hessian, c, c) );
-	    VVe_mle_null.push_back(gsl_matrix_get(Hessian,c+v_size,c+v_size));
-	    c++;
-	  }
-	}
-	beta_mle_null.clear();
-	se_beta_mle_null.clear();
-	for (size_t i=0; i<se_B_null->size1; i++) {
-	  for (size_t j=0; j<se_B_null->size2; j++) {
-	    beta_mle_null.push_back(gsl_matrix_get(B, i, j) );
-	    se_beta_mle_null.push_back(gsl_matrix_get(se_B_null, i, j) );
-	  }
-	}
-	logl_mle_H0=logl_H0;
-
-	cout<<"MLE estimate for Vg in the null model: "<<endl;
-	for (size_t i=0; i<d_size; i++) {
-	  for (size_t j=0; j<=i; j++) {
-	    cout<<gsl_matrix_get(V_g, i, j)<<"\t";
-	  }
-	  cout<<endl;
-	}
-	cout<<"se(Vg): "<<endl;
-	for (size_t i=0; i<d_size; i++) {
-	  for (size_t j=0; j<=i; j++) {
-	    c=GetIndex(i, j, d_size);
-	    cout<<sqrt(gsl_matrix_get(Hessian, c, c))<<"\t";
-	  }
-	  cout<<endl;
-	}
-	cout<<"MLE estimate for Ve in the null model: "<<endl;
-	for (size_t i=0; i<d_size; i++) {
-	  for (size_t j=0; j<=i; j++) {
-	    cout<<gsl_matrix_get(V_e, i, j)<<"\t";
-	  }
-	  cout<<endl;
-	}
-	cout<<"se(Ve): "<<endl;
-	for (size_t i=0; i<d_size; i++) {
-	  for (size_t j=0; j<=i; j++) {
-	    c=GetIndex(i, j, d_size);
-	    cout<<sqrt(gsl_matrix_get(Hessian, c+v_size, c+v_size))<<"\t";
-	  }
-	  cout<<endl;
-	}
-	cout<<"MLE likelihood = "<<logl_H0<<endl;
-	
-	vector<double> v_beta, v_Vg, v_Ve, v_Vbeta;
-	for (size_t i=0; i<d_size; i++) {
-	  v_beta.push_back(0.0);
-	}
-	for (size_t i=0; i<d_size; i++) {
-	  for (size_t j=i; j<d_size; j++) {
-	    v_Vg.push_back(0.0);
-	    v_Ve.push_back(0.0);
-	    v_Vbeta.push_back(0.0);
-	  }
-	}
-	
-	gsl_matrix_memcpy (V_g_null, V_g);
-	gsl_matrix_memcpy (V_e_null, V_e);
-	gsl_matrix_memcpy (B_null, B);
-	
-	// Start reading genotypes and analyze.
-	// Calculate n_bit and c, the number of bit for each snp.
-	if (ni_total%4==0) {n_bit=ni_total/4;}
-	else {n_bit=ni_total/4+1; }
-
-	// Print the first three magic numbers.
-	for (int i=0; i<3; ++i) {
-	  infile.read(ch,1);
-	  b=ch[0];
-	}
-
-	size_t csnp=0, t_last=0;
-	for (size_t t=0; t<indicator_snp.size(); ++t) {
-	  if (indicator_snp[t]==0) {continue;}
-	  t_last++;
-	}
-	for (vector<SNPINFO>::size_type t=0; t<snpInfo.size(); ++t) {
-	  if (t%d_pace==0 || t==snpInfo.size()-1) {
-	    ProgressBar ("Reading SNPs  ", t, snpInfo.size()-1);
-	  }
-	  if (indicator_snp[t]==0) {continue;}
-
-	  // n_bit, and 3 is the number of magic numbers.
-	  infile.seekg(t*n_bit+3);		
-
-	  //read genotypes
-	  x_mean=0.0;	n_miss=0; ci_total=0; ci_test=0;
-	  for (int i=0; i<n_bit; ++i) {
-	    infile.read(ch,1);
-	    b=ch[0];
-
-	    // Minor allele homozygous: 2.0; major: 0.0;
-	    for (size_t j=0; j<4; ++j) {                
-	      if ((i==(n_bit-1)) && ci_total==(int)ni_total) {break;}
-	      if (indicator_idv[ci_total]==0) {ci_total++; continue;}
-	      
-	      if (b[2*j]==0) {
-		if (b[2*j+1]==0) {gsl_vector_set(x, ci_test, 2); x_mean+=2.0; }
-		else {gsl_vector_set(x, ci_test, 1); x_mean+=1.0; }
-	      }
-	      else {
-		if (b[2*j+1]==1) {gsl_vector_set(x, ci_test, 0); }
-		else {gsl_vector_set(x, ci_test, -9); n_miss++; }
-	      }
-	      
-	      ci_total++;
-	      ci_test++;
-			}
-	  }
-	  
-	  x_mean/=(double)(ni_test-n_miss);
-	  
-	  for (size_t i=0; i<ni_test; ++i) {
-	    geno=gsl_vector_get(x,i);
-	    if (geno==-9) {gsl_vector_set(x, i, x_mean); geno=x_mean;}
-	  }
-
-	  gsl_vector_view Xlarge_col=gsl_matrix_column (Xlarge, csnp%msize);
-	  gsl_vector_memcpy (&Xlarge_col.vector, x);
-	  csnp++;
-	  
-	  if (csnp%msize==0 || csnp==t_last ) {
-	    size_t l=0;
-	    if (csnp%msize==0) {l=msize;} else {l=csnp%msize;}
-	    
-	    gsl_matrix_view Xlarge_sub =
-	      gsl_matrix_submatrix(Xlarge, 0, 0, Xlarge->size1, l);
-	    gsl_matrix_view UtXlarge_sub =
-	      gsl_matrix_submatrix(UtXlarge, 0, 0, UtXlarge->size1, l);
-	    
-	    time_start=clock();
-	    eigenlib_dgemm ("T", "N", 1.0, U, &Xlarge_sub.matrix, 0.0,
-			    &UtXlarge_sub.matrix);
-	    time_UtX+=(clock()-time_start)/(double(CLOCKS_PER_SEC)*60.0);
-	    
-	    gsl_matrix_set_zero (Xlarge);
-	    
-	    for (size_t i=0; i<l; i++) {
-	      gsl_vector_view UtXlarge_col=gsl_matrix_column (UtXlarge, i);
-	      gsl_vector_memcpy (&X_row.vector, &UtXlarge_col.vector);
-	      
-	      // Initial values.
-	      gsl_matrix_memcpy (V_g, V_g_null);
-	      gsl_matrix_memcpy (V_e, V_e_null);
-	      gsl_matrix_memcpy (B, B_null);
-	      
-	      time_start=clock();
-	      
-	      // 3 is before 1.
-	      if (a_mode==3 || a_mode==4) {
-		p_score=MphCalcP (eval, &X_row.vector, &X_sub.matrix, Y,
-				  V_g_null, V_e_null, UltVehiY, beta, Vbeta);
-		
-		if (p_score<p_nr && crt==1) {
-		  logl_H1=MphNR ('R', 1, nr_prec*10, eval, X, Y, Hi_all,
-				 xHi_all, Hiy_all, V_g, V_e, Hessian, crt_a,
-				 crt_b, crt_c);
-		  p_score=PCRT (3, d_size, p_score, crt_a, crt_b, crt_c);
-		}
-	      }
-	      
-	      if (a_mode==2 || a_mode==4) {
-		logl_H1=MphEM ('L', em_iter/10, em_prec*10, eval, X, Y,
-			       U_hat, E_hat, OmegaU, OmegaE, UltVehiY,
-			       UltVehiBX, UltVehiU, UltVehiE, V_g, V_e, B);
-		
-		// Calculate beta and Vbeta.
-		p_lrt=MphCalcP (eval, &X_row.vector, &X_sub.matrix, Y, V_g,
-				V_e, UltVehiY, beta, Vbeta);
-		p_lrt=gsl_cdf_chisq_Q (2.0*(logl_H1-logl_H0), (double)d_size );
-		
-		if (p_lrt<p_nr) {
-		  logl_H1=MphNR ('L', nr_iter/10, nr_prec*10, eval, X, Y,
-				 Hi_all, xHi_all, Hiy_all, V_g, V_e, Hessian,
-				 crt_a, crt_b, crt_c);
-		  
-		  // Calculate beta and Vbeta.
-		  p_lrt=MphCalcP (eval, &X_row.vector, &X_sub.matrix, Y, V_g,
-				  V_e, UltVehiY, beta, Vbeta);
-		  p_lrt=gsl_cdf_chisq_Q (2.0*(logl_H1-logl_H0),
-					 (double)d_size );
-		  if (crt==1) {
-		    p_lrt=PCRT (2, d_size, p_lrt, crt_a, crt_b, crt_c);
-		  }
-		}
-	      }
-	      
-	      if (a_mode==1 || a_mode==4) {
-		logl_H1=MphEM ('R', em_iter/10, em_prec*10, eval, X, Y,
-			       U_hat, E_hat, OmegaU, OmegaE, UltVehiY,
-			       UltVehiBX, UltVehiU, UltVehiE, V_g, V_e, B);
-		p_wald=MphCalcP (eval, &X_row.vector, &X_sub.matrix, Y, V_g,
-				 V_e, UltVehiY, beta, Vbeta);
-		
-		if (p_wald<p_nr) {
-		  logl_H1=MphNR ('R', nr_iter/10, nr_prec*10, eval, X, Y,
-				 Hi_all, xHi_all, Hiy_all, V_g, V_e, Hessian,
-				 crt_a, crt_b, crt_c);
-		  p_wald=MphCalcP (eval, &X_row.vector, &X_sub.matrix, Y,
-				   V_g, V_e, UltVehiY, beta, Vbeta);
-		  
-		  if (crt==1) {
-		    p_wald=PCRT (1, d_size, p_wald, crt_a, crt_b, crt_c);
-		  }
-		}
-	      }
-	      
-	      time_opt+=(clock()-time_start)/(double(CLOCKS_PER_SEC)*60.0);
-
-	      // Store summary data.
-	      for (size_t i=0; i<d_size; i++) {
-		v_beta[i]=gsl_vector_get (beta, i);
-	      }
-	      
-	      c=0;
-	      for (size_t i=0; i<d_size; i++) {
-		for (size_t j=i; j<d_size; j++) {
-		  v_Vg[c]=gsl_matrix_get (V_g, i, j);
-		  v_Ve[c]=gsl_matrix_get (V_e, i, j);
-		  v_Vbeta[c]=gsl_matrix_get (Vbeta, i, j);
-		  c++;
-		}
-	      }
-	      
-	      MPHSUMSTAT SNPs={v_beta, p_wald, p_lrt, p_score, v_Vg,
-			       v_Ve, v_Vbeta};
-	      sumStat.push_back(SNPs);
-	    }
-	  }
-	}
-	cout<<endl;
-
-	infile.close();
-	infile.clear();
-
-	gsl_matrix_free(U_hat);
-	gsl_matrix_free(E_hat);
-	gsl_matrix_free(OmegaU);
-	gsl_matrix_free(OmegaE);
-	gsl_matrix_free(UltVehiY);
-	gsl_matrix_free(UltVehiBX);
-	gsl_matrix_free(UltVehiU);
-	gsl_matrix_free(UltVehiE);
-
-	gsl_matrix_free(Hi_all);
-	gsl_matrix_free(Hiy_all);
-	gsl_matrix_free(xHi_all);
-	gsl_matrix_free(Hessian);
-
-	gsl_vector_free(x);
-
-	gsl_matrix_free(Y);
-	gsl_matrix_free(X);
-	gsl_matrix_free(V_g);
-	gsl_matrix_free(V_e);
-	gsl_matrix_free(B);
-	gsl_vector_free(beta);
-	gsl_matrix_free(Vbeta);
-
-	gsl_matrix_free(V_g_null);
-	gsl_matrix_free(V_e_null);
-	gsl_matrix_free(B_null);
-	gsl_matrix_free(se_B_null);
-
-	gsl_matrix_free(Xlarge);
-	gsl_matrix_free(UtXlarge);
-
-	return;
+void MVLMM::AnalyzePlink(const gsl_matrix *U, const gsl_vector *eval,
+                         const gsl_matrix *UtW, const gsl_matrix *UtY) {
+  string file_bed = file_bfile + ".bed";
+  ifstream infile(file_bed.c_str(), ios::binary);
+  if (!infile) {
+    cout << "error reading bed file:" << file_bed << endl;
+    return;
+  }
+
+  clock_t time_start = clock();
+  time_UtX = 0;
+  time_opt = 0;
+
+  char ch[1];
+  bitset<8> b;
+
+  double logl_H0 = 0.0, logl_H1 = 0.0, p_wald = 0, p_lrt = 0, p_score = 0;
+  double crt_a, crt_b, crt_c;
+  int n_bit, n_miss, ci_total, ci_test;
+  double geno, x_mean;
+  size_t c = 0;
+  size_t n_size = UtY->size1, d_size = UtY->size2, c_size = UtW->size2;
+  size_t dc_size = d_size * (c_size + 1), v_size = d_size * (d_size + 1) / 2;
+
+  // Create a large matrix.
+  size_t msize = 10000;
+  gsl_matrix *Xlarge = gsl_matrix_alloc(U->size1, msize);
+  gsl_matrix *UtXlarge = gsl_matrix_alloc(U->size1, msize);
+  gsl_matrix_set_zero(Xlarge);
+
+  // Large matrices for EM.
+  gsl_matrix *U_hat = gsl_matrix_alloc(d_size, n_size);
+  gsl_matrix *E_hat = gsl_matrix_alloc(d_size, n_size);
+  gsl_matrix *OmegaU = gsl_matrix_alloc(d_size, n_size);
+  gsl_matrix *OmegaE = gsl_matrix_alloc(d_size, n_size);
+  gsl_matrix *UltVehiY = gsl_matrix_alloc(d_size, n_size);
+  gsl_matrix *UltVehiBX = gsl_matrix_alloc(d_size, n_size);
+  gsl_matrix *UltVehiU = gsl_matrix_alloc(d_size, n_size);
+  gsl_matrix *UltVehiE = gsl_matrix_alloc(d_size, n_size);
+
+  // Large matrices for NR.
+  // Each dxd block is H_k^{-1}.
+  gsl_matrix *Hi_all = gsl_matrix_alloc(d_size, d_size * n_size);
+
+  // Each column is H_k^{-1}y_k.
+  gsl_matrix *Hiy_all = gsl_matrix_alloc(d_size, n_size);
+
+  // Each dcxdc block is x_k\otimes H_k^{-1}.
+  gsl_matrix *xHi_all = gsl_matrix_alloc(dc_size, d_size * n_size);
+
+  gsl_matrix *Hessian = gsl_matrix_alloc(v_size * 2, v_size * 2);
+
+  gsl_vector *x = gsl_vector_alloc(n_size);
+
+  gsl_matrix *Y = gsl_matrix_alloc(d_size, n_size);
+  gsl_matrix *X = gsl_matrix_alloc(c_size + 1, n_size);
+  gsl_matrix *V_g = gsl_matrix_alloc(d_size, d_size);
+  gsl_matrix *V_e = gsl_matrix_alloc(d_size, d_size);
+  gsl_matrix *B = gsl_matrix_alloc(d_size, c_size + 1);
+  gsl_vector *beta = gsl_vector_alloc(d_size);
+  gsl_matrix *Vbeta = gsl_matrix_alloc(d_size, d_size);
+
+  // Null estimates for initial values.
+  gsl_matrix *V_g_null = gsl_matrix_alloc(d_size, d_size);
+  gsl_matrix *V_e_null = gsl_matrix_alloc(d_size, d_size);
+  gsl_matrix *B_null = gsl_matrix_alloc(d_size, c_size + 1);
+  gsl_matrix *se_B_null = gsl_matrix_alloc(d_size, c_size);
+
+  gsl_matrix_view X_sub = gsl_matrix_submatrix(X, 0, 0, c_size, n_size);
+  gsl_matrix_view B_sub = gsl_matrix_submatrix(B, 0, 0, d_size, c_size);
+  gsl_matrix_view xHi_all_sub =
+      gsl_matrix_submatrix(xHi_all, 0, 0, d_size * c_size, d_size * n_size);
+
+  gsl_matrix_transpose_memcpy(Y, UtY);
+  gsl_matrix_transpose_memcpy(&X_sub.matrix, UtW);
+
+  gsl_vector_view X_row = gsl_matrix_row(X, c_size);
+  gsl_vector_set_zero(&X_row.vector);
+  gsl_vector_view B_col = gsl_matrix_column(B, c_size);
+  gsl_vector_set_zero(&B_col.vector);
+
+  MphInitial(em_iter, em_prec, nr_iter, nr_prec, eval, &X_sub.matrix, Y, l_min,
+             l_max, n_region, V_g, V_e, &B_sub.matrix);
+
+  logl_H0 = MphEM('R', em_iter, em_prec, eval, &X_sub.matrix, Y, U_hat, E_hat,
+                  OmegaU, OmegaE, UltVehiY, UltVehiBX, UltVehiU, UltVehiE, V_g,
+                  V_e, &B_sub.matrix);
+  logl_H0 = MphNR('R', nr_iter, nr_prec, eval, &X_sub.matrix, Y, Hi_all,
+                  &xHi_all_sub.matrix, Hiy_all, V_g, V_e, Hessian, crt_a, crt_b,
+                  crt_c);
+  MphCalcBeta(eval, &X_sub.matrix, Y, V_g, V_e, UltVehiY, &B_sub.matrix,
+              se_B_null);
+
+  c = 0;
+  Vg_remle_null.clear();
+  Ve_remle_null.clear();
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = i; j < d_size; j++) {
+      Vg_remle_null.push_back(gsl_matrix_get(V_g, i, j));
+      Ve_remle_null.push_back(gsl_matrix_get(V_e, i, j));
+      VVg_remle_null.push_back(gsl_matrix_get(Hessian, c, c));
+      VVe_remle_null.push_back(gsl_matrix_get(Hessian, c + v_size, c + v_size));
+      c++;
+    }
+  }
+  beta_remle_null.clear();
+  se_beta_remle_null.clear();
+  for (size_t i = 0; i < se_B_null->size1; i++) {
+    for (size_t j = 0; j < se_B_null->size2; j++) {
+      beta_remle_null.push_back(gsl_matrix_get(B, i, j));
+      se_beta_remle_null.push_back(gsl_matrix_get(se_B_null, i, j));
+    }
+  }
+  logl_remle_H0 = logl_H0;
+
+  cout.setf(std::ios_base::fixed, std::ios_base::floatfield);
+  cout.precision(4);
+  cout << "REMLE estimate for Vg in the null model: " << endl;
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = 0; j <= i; j++) {
+      cout << gsl_matrix_get(V_g, i, j) << "\t";
+    }
+    cout << endl;
+  }
+  cout << "se(Vg): " << endl;
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = 0; j <= i; j++) {
+      c = GetIndex(i, j, d_size);
+      cout << sqrt(gsl_matrix_get(Hessian, c, c)) << "\t";
+    }
+    cout << endl;
+  }
+  cout << "REMLE estimate for Ve in the null model: " << endl;
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = 0; j <= i; j++) {
+      cout << gsl_matrix_get(V_e, i, j) << "\t";
+    }
+    cout << endl;
+  }
+  cout << "se(Ve): " << endl;
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = 0; j <= i; j++) {
+      c = GetIndex(i, j, d_size);
+      cout << sqrt(gsl_matrix_get(Hessian, c + v_size, c + v_size)) << "\t";
+    }
+    cout << endl;
+  }
+  cout << "REMLE likelihood = " << logl_H0 << endl;
+
+  logl_H0 = MphEM('L', em_iter, em_prec, eval, &X_sub.matrix, Y, U_hat, E_hat,
+                  OmegaU, OmegaE, UltVehiY, UltVehiBX, UltVehiU, UltVehiE, V_g,
+                  V_e, &B_sub.matrix);
+  logl_H0 = MphNR('L', nr_iter, nr_prec, eval, &X_sub.matrix, Y, Hi_all,
+                  &xHi_all_sub.matrix, Hiy_all, V_g, V_e, Hessian, crt_a, crt_b,
+                  crt_c);
+  MphCalcBeta(eval, &X_sub.matrix, Y, V_g, V_e, UltVehiY, &B_sub.matrix,
+              se_B_null);
+
+  c = 0;
+  Vg_mle_null.clear();
+  Ve_mle_null.clear();
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = i; j < d_size; j++) {
+      Vg_mle_null.push_back(gsl_matrix_get(V_g, i, j));
+      Ve_mle_null.push_back(gsl_matrix_get(V_e, i, j));
+      VVg_mle_null.push_back(gsl_matrix_get(Hessian, c, c));
+      VVe_mle_null.push_back(gsl_matrix_get(Hessian, c + v_size, c + v_size));
+      c++;
+    }
+  }
+  beta_mle_null.clear();
+  se_beta_mle_null.clear();
+  for (size_t i = 0; i < se_B_null->size1; i++) {
+    for (size_t j = 0; j < se_B_null->size2; j++) {
+      beta_mle_null.push_back(gsl_matrix_get(B, i, j));
+      se_beta_mle_null.push_back(gsl_matrix_get(se_B_null, i, j));
+    }
+  }
+  logl_mle_H0 = logl_H0;
+
+  cout << "MLE estimate for Vg in the null model: " << endl;
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = 0; j <= i; j++) {
+      cout << gsl_matrix_get(V_g, i, j) << "\t";
+    }
+    cout << endl;
+  }
+  cout << "se(Vg): " << endl;
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = 0; j <= i; j++) {
+      c = GetIndex(i, j, d_size);
+      cout << sqrt(gsl_matrix_get(Hessian, c, c)) << "\t";
+    }
+    cout << endl;
+  }
+  cout << "MLE estimate for Ve in the null model: " << endl;
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = 0; j <= i; j++) {
+      cout << gsl_matrix_get(V_e, i, j) << "\t";
+    }
+    cout << endl;
+  }
+  cout << "se(Ve): " << endl;
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = 0; j <= i; j++) {
+      c = GetIndex(i, j, d_size);
+      cout << sqrt(gsl_matrix_get(Hessian, c + v_size, c + v_size)) << "\t";
+    }
+    cout << endl;
+  }
+  cout << "MLE likelihood = " << logl_H0 << endl;
+
+  vector<double> v_beta, v_Vg, v_Ve, v_Vbeta;
+  for (size_t i = 0; i < d_size; i++) {
+    v_beta.push_back(0.0);
+  }
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = i; j < d_size; j++) {
+      v_Vg.push_back(0.0);
+      v_Ve.push_back(0.0);
+      v_Vbeta.push_back(0.0);
+    }
+  }
+
+  gsl_matrix_memcpy(V_g_null, V_g);
+  gsl_matrix_memcpy(V_e_null, V_e);
+  gsl_matrix_memcpy(B_null, B);
+
+  // Start reading genotypes and analyze.
+  // Calculate n_bit and c, the number of bit for each snp.
+  if (ni_total % 4 == 0) {
+    n_bit = ni_total / 4;
+  } else {
+    n_bit = ni_total / 4 + 1;
+  }
+
+  // Print the first three magic numbers.
+  for (int i = 0; i < 3; ++i) {
+    infile.read(ch, 1);
+    b = ch[0];
+  }
+
+  size_t csnp = 0, t_last = 0;
+  for (size_t t = 0; t < indicator_snp.size(); ++t) {
+    if (indicator_snp[t] == 0) {
+      continue;
+    }
+    t_last++;
+  }
+  for (vector<SNPINFO>::size_type t = 0; t < snpInfo.size(); ++t) {
+    if (t % d_pace == 0 || t == snpInfo.size() - 1) {
+      ProgressBar("Reading SNPs  ", t, snpInfo.size() - 1);
+    }
+    if (indicator_snp[t] == 0) {
+      continue;
+    }
+
+    // n_bit, and 3 is the number of magic numbers.
+    infile.seekg(t * n_bit + 3);
+
+    // read genotypes
+    x_mean = 0.0;
+    n_miss = 0;
+    ci_total = 0;
+    ci_test = 0;
+    for (int i = 0; i < n_bit; ++i) {
+      infile.read(ch, 1);
+      b = ch[0];
+
+      // Minor allele homozygous: 2.0; major: 0.0;
+      for (size_t j = 0; j < 4; ++j) {
+        if ((i == (n_bit - 1)) && ci_total == (int)ni_total) {
+          break;
+        }
+        if (indicator_idv[ci_total] == 0) {
+          ci_total++;
+          continue;
+        }
+
+        if (b[2 * j] == 0) {
+          if (b[2 * j + 1] == 0) {
+            gsl_vector_set(x, ci_test, 2);
+            x_mean += 2.0;
+          } else {
+            gsl_vector_set(x, ci_test, 1);
+            x_mean += 1.0;
+          }
+        } else {
+          if (b[2 * j + 1] == 1) {
+            gsl_vector_set(x, ci_test, 0);
+          } else {
+            gsl_vector_set(x, ci_test, -9);
+            n_miss++;
+          }
+        }
+
+        ci_total++;
+        ci_test++;
+      }
+    }
+
+    x_mean /= (double)(ni_test - n_miss);
+
+    for (size_t i = 0; i < ni_test; ++i) {
+      geno = gsl_vector_get(x, i);
+      if (geno == -9) {
+        gsl_vector_set(x, i, x_mean);
+        geno = x_mean;
+      }
+    }
+
+    gsl_vector_view Xlarge_col = gsl_matrix_column(Xlarge, csnp % msize);
+    gsl_vector_memcpy(&Xlarge_col.vector, x);
+    csnp++;
+
+    if (csnp % msize == 0 || csnp == t_last) {
+      size_t l = 0;
+      if (csnp % msize == 0) {
+        l = msize;
+      } else {
+        l = csnp % msize;
+      }
+
+      gsl_matrix_view Xlarge_sub =
+          gsl_matrix_submatrix(Xlarge, 0, 0, Xlarge->size1, l);
+      gsl_matrix_view UtXlarge_sub =
+          gsl_matrix_submatrix(UtXlarge, 0, 0, UtXlarge->size1, l);
+
+      time_start = clock();
+      eigenlib_dgemm("T", "N", 1.0, U, &Xlarge_sub.matrix, 0.0,
+                     &UtXlarge_sub.matrix);
+      time_UtX += (clock() - time_start) / (double(CLOCKS_PER_SEC) * 60.0);
+
+      gsl_matrix_set_zero(Xlarge);
+
+      for (size_t i = 0; i < l; i++) {
+        gsl_vector_view UtXlarge_col = gsl_matrix_column(UtXlarge, i);
+        gsl_vector_memcpy(&X_row.vector, &UtXlarge_col.vector);
+
+        // Initial values.
+        gsl_matrix_memcpy(V_g, V_g_null);
+        gsl_matrix_memcpy(V_e, V_e_null);
+        gsl_matrix_memcpy(B, B_null);
+
+        time_start = clock();
+
+        // 3 is before 1.
+        if (a_mode == 3 || a_mode == 4) {
+          p_score = MphCalcP(eval, &X_row.vector, &X_sub.matrix, Y, V_g_null,
+                             V_e_null, UltVehiY, beta, Vbeta);
+
+          if (p_score < p_nr && crt == 1) {
+            logl_H1 = MphNR('R', 1, nr_prec * 10, eval, X, Y, Hi_all, xHi_all,
+                            Hiy_all, V_g, V_e, Hessian, crt_a, crt_b, crt_c);
+            p_score = PCRT(3, d_size, p_score, crt_a, crt_b, crt_c);
+          }
+        }
+
+        if (a_mode == 2 || a_mode == 4) {
+          logl_H1 = MphEM('L', em_iter / 10, em_prec * 10, eval, X, Y, U_hat,
+                          E_hat, OmegaU, OmegaE, UltVehiY, UltVehiBX, UltVehiU,
+                          UltVehiE, V_g, V_e, B);
+
+          // Calculate beta and Vbeta.
+          p_lrt = MphCalcP(eval, &X_row.vector, &X_sub.matrix, Y, V_g, V_e,
+                           UltVehiY, beta, Vbeta);
+          p_lrt = gsl_cdf_chisq_Q(2.0 * (logl_H1 - logl_H0), (double)d_size);
+
+          if (p_lrt < p_nr) {
+            logl_H1 =
+                MphNR('L', nr_iter / 10, nr_prec * 10, eval, X, Y, Hi_all,
+                      xHi_all, Hiy_all, V_g, V_e, Hessian, crt_a, crt_b, crt_c);
+
+            // Calculate beta and Vbeta.
+            p_lrt = MphCalcP(eval, &X_row.vector, &X_sub.matrix, Y, V_g, V_e,
+                             UltVehiY, beta, Vbeta);
+            p_lrt = gsl_cdf_chisq_Q(2.0 * (logl_H1 - logl_H0), (double)d_size);
+            if (crt == 1) {
+              p_lrt = PCRT(2, d_size, p_lrt, crt_a, crt_b, crt_c);
+            }
+          }
+        }
+
+        if (a_mode == 1 || a_mode == 4) {
+          logl_H1 = MphEM('R', em_iter / 10, em_prec * 10, eval, X, Y, U_hat,
+                          E_hat, OmegaU, OmegaE, UltVehiY, UltVehiBX, UltVehiU,
+                          UltVehiE, V_g, V_e, B);
+          p_wald = MphCalcP(eval, &X_row.vector, &X_sub.matrix, Y, V_g, V_e,
+                            UltVehiY, beta, Vbeta);
+
+          if (p_wald < p_nr) {
+            logl_H1 =
+                MphNR('R', nr_iter / 10, nr_prec * 10, eval, X, Y, Hi_all,
+                      xHi_all, Hiy_all, V_g, V_e, Hessian, crt_a, crt_b, crt_c);
+            p_wald = MphCalcP(eval, &X_row.vector, &X_sub.matrix, Y, V_g, V_e,
+                              UltVehiY, beta, Vbeta);
+
+            if (crt == 1) {
+              p_wald = PCRT(1, d_size, p_wald, crt_a, crt_b, crt_c);
+            }
+          }
+        }
+
+        time_opt += (clock() - time_start) / (double(CLOCKS_PER_SEC) * 60.0);
+
+        // Store summary data.
+        for (size_t i = 0; i < d_size; i++) {
+          v_beta[i] = gsl_vector_get(beta, i);
+        }
+
+        c = 0;
+        for (size_t i = 0; i < d_size; i++) {
+          for (size_t j = i; j < d_size; j++) {
+            v_Vg[c] = gsl_matrix_get(V_g, i, j);
+            v_Ve[c] = gsl_matrix_get(V_e, i, j);
+            v_Vbeta[c] = gsl_matrix_get(Vbeta, i, j);
+            c++;
+          }
+        }
+
+        MPHSUMSTAT SNPs = {v_beta, p_wald, p_lrt, p_score, v_Vg, v_Ve, v_Vbeta};
+        sumStat.push_back(SNPs);
+      }
+    }
+  }
+  cout << endl;
+
+  infile.close();
+  infile.clear();
+
+  gsl_matrix_free(U_hat);
+  gsl_matrix_free(E_hat);
+  gsl_matrix_free(OmegaU);
+  gsl_matrix_free(OmegaE);
+  gsl_matrix_free(UltVehiY);
+  gsl_matrix_free(UltVehiBX);
+  gsl_matrix_free(UltVehiU);
+  gsl_matrix_free(UltVehiE);
+
+  gsl_matrix_free(Hi_all);
+  gsl_matrix_free(Hiy_all);
+  gsl_matrix_free(xHi_all);
+  gsl_matrix_free(Hessian);
+
+  gsl_vector_free(x);
+
+  gsl_matrix_free(Y);
+  gsl_matrix_free(X);
+  gsl_matrix_free(V_g);
+  gsl_matrix_free(V_e);
+  gsl_matrix_free(B);
+  gsl_vector_free(beta);
+  gsl_matrix_free(Vbeta);
+
+  gsl_matrix_free(V_g_null);
+  gsl_matrix_free(V_e_null);
+  gsl_matrix_free(B_null);
+  gsl_matrix_free(se_B_null);
+
+  gsl_matrix_free(Xlarge);
+  gsl_matrix_free(UtXlarge);
+
+  return;
 }
 
 // Calculate Vg, Ve, B, se(B) in the null mvLMM model.
 // Both B and se_B are d by c matrices.
-void CalcMvLmmVgVeBeta (const gsl_vector *eval, const gsl_matrix *UtW,
-			const gsl_matrix *UtY, const size_t em_iter,
-			const size_t nr_iter, const double em_prec,
-			const double nr_prec, const double l_min,
-			const double l_max, const size_t n_region,
-			gsl_matrix *V_g, gsl_matrix *V_e, gsl_matrix *B,
-			gsl_matrix *se_B) {
-	size_t n_size=UtY->size1, d_size=UtY->size2, c_size=UtW->size2;
-	size_t dc_size=d_size*c_size, v_size=d_size*(d_size+1)/2;
-
-	double logl, crt_a, crt_b, crt_c;
-
-	// Large matrices for EM.
-	gsl_matrix *U_hat=gsl_matrix_alloc (d_size, n_size);
-	gsl_matrix *E_hat=gsl_matrix_alloc (d_size, n_size);
-	gsl_matrix *OmegaU=gsl_matrix_alloc (d_size, n_size);
-	gsl_matrix *OmegaE=gsl_matrix_alloc (d_size, n_size);
-	gsl_matrix *UltVehiY=gsl_matrix_alloc (d_size, n_size);
-	gsl_matrix *UltVehiBX=gsl_matrix_alloc (d_size, n_size);
-	gsl_matrix *UltVehiU=gsl_matrix_alloc (d_size, n_size);
-	gsl_matrix *UltVehiE=gsl_matrix_alloc (d_size, n_size);
-
-	// Large matrices for NR.
-	// Each dxd block is H_k^{-1}.
-	gsl_matrix *Hi_all=gsl_matrix_alloc (d_size, d_size*n_size);
-	
-	// Each column is H_k^{-1}y_k.
-	gsl_matrix *Hiy_all=gsl_matrix_alloc (d_size, n_size);		
-
-	// Each dcxdc block is x_k\otimes H_k^{-1}.
-	gsl_matrix *xHi_all=gsl_matrix_alloc (dc_size, d_size*n_size);	
-	gsl_matrix *Hessian=gsl_matrix_alloc (v_size*2, v_size*2);
-
-	// Transpose matrices.
-	gsl_matrix *Y=gsl_matrix_alloc (d_size, n_size);
-	gsl_matrix *W=gsl_matrix_alloc (c_size, n_size);
-	gsl_matrix_transpose_memcpy (Y, UtY);
-	gsl_matrix_transpose_memcpy (W, UtW);
-
-	// Initial, EM, NR, and calculate B.
-	MphInitial(em_iter, em_prec, nr_iter, nr_prec, eval, W, Y, 
-		   l_min, l_max, n_region, V_g, V_e, B);
-	logl=MphEM ('R', em_iter, em_prec, eval, W, Y, U_hat, E_hat, 
-		    OmegaU, OmegaE, UltVehiY, UltVehiBX, UltVehiU, 
-		    UltVehiE, V_g, V_e, B);
-	logl=MphNR ('R', nr_iter, nr_prec, eval, W, Y, Hi_all, xHi_all, 
-		    Hiy_all, V_g, V_e, Hessian, crt_a, crt_b, crt_c);
-	MphCalcBeta (eval, W, Y, V_g, V_e, UltVehiY, B, se_B);
-
-	// Free matrices.
-	gsl_matrix_free(U_hat);
-	gsl_matrix_free(E_hat);
-	gsl_matrix_free(OmegaU);
-	gsl_matrix_free(OmegaE);
-	gsl_matrix_free(UltVehiY);
-	gsl_matrix_free(UltVehiBX);
-	gsl_matrix_free(UltVehiU);
-	gsl_matrix_free(UltVehiE);
-
-	gsl_matrix_free(Hi_all);
-	gsl_matrix_free(Hiy_all);
-	gsl_matrix_free(xHi_all);
-	gsl_matrix_free(Hessian);
-
-	gsl_matrix_free(Y);
-	gsl_matrix_free(W);
-
-	return;
+void CalcMvLmmVgVeBeta(const gsl_vector *eval, const gsl_matrix *UtW,
+                       const gsl_matrix *UtY, const size_t em_iter,
+                       const size_t nr_iter, const double em_prec,
+                       const double nr_prec, const double l_min,
+                       const double l_max, const size_t n_region,
+                       gsl_matrix *V_g, gsl_matrix *V_e, gsl_matrix *B,
+                       gsl_matrix *se_B) {
+  size_t n_size = UtY->size1, d_size = UtY->size2, c_size = UtW->size2;
+  size_t dc_size = d_size * c_size, v_size = d_size * (d_size + 1) / 2;
+
+  double logl, crt_a, crt_b, crt_c;
+
+  // Large matrices for EM.
+  gsl_matrix *U_hat = gsl_matrix_alloc(d_size, n_size);
+  gsl_matrix *E_hat = gsl_matrix_alloc(d_size, n_size);
+  gsl_matrix *OmegaU = gsl_matrix_alloc(d_size, n_size);
+  gsl_matrix *OmegaE = gsl_matrix_alloc(d_size, n_size);
+  gsl_matrix *UltVehiY = gsl_matrix_alloc(d_size, n_size);
+  gsl_matrix *UltVehiBX = gsl_matrix_alloc(d_size, n_size);
+  gsl_matrix *UltVehiU = gsl_matrix_alloc(d_size, n_size);
+  gsl_matrix *UltVehiE = gsl_matrix_alloc(d_size, n_size);
+
+  // Large matrices for NR.
+  // Each dxd block is H_k^{-1}.
+  gsl_matrix *Hi_all = gsl_matrix_alloc(d_size, d_size * n_size);
+
+  // Each column is H_k^{-1}y_k.
+  gsl_matrix *Hiy_all = gsl_matrix_alloc(d_size, n_size);
+
+  // Each dcxdc block is x_k\otimes H_k^{-1}.
+  gsl_matrix *xHi_all = gsl_matrix_alloc(dc_size, d_size * n_size);
+  gsl_matrix *Hessian = gsl_matrix_alloc(v_size * 2, v_size * 2);
+
+  // Transpose matrices.
+  gsl_matrix *Y = gsl_matrix_alloc(d_size, n_size);
+  gsl_matrix *W = gsl_matrix_alloc(c_size, n_size);
+  gsl_matrix_transpose_memcpy(Y, UtY);
+  gsl_matrix_transpose_memcpy(W, UtW);
+
+  // Initial, EM, NR, and calculate B.
+  MphInitial(em_iter, em_prec, nr_iter, nr_prec, eval, W, Y, l_min, l_max,
+             n_region, V_g, V_e, B);
+  logl = MphEM('R', em_iter, em_prec, eval, W, Y, U_hat, E_hat, OmegaU, OmegaE,
+               UltVehiY, UltVehiBX, UltVehiU, UltVehiE, V_g, V_e, B);
+  logl = MphNR('R', nr_iter, nr_prec, eval, W, Y, Hi_all, xHi_all, Hiy_all, V_g,
+               V_e, Hessian, crt_a, crt_b, crt_c);
+  MphCalcBeta(eval, W, Y, V_g, V_e, UltVehiY, B, se_B);
+
+  // Free matrices.
+  gsl_matrix_free(U_hat);
+  gsl_matrix_free(E_hat);
+  gsl_matrix_free(OmegaU);
+  gsl_matrix_free(OmegaE);
+  gsl_matrix_free(UltVehiY);
+  gsl_matrix_free(UltVehiBX);
+  gsl_matrix_free(UltVehiU);
+  gsl_matrix_free(UltVehiE);
+
+  gsl_matrix_free(Hi_all);
+  gsl_matrix_free(Hiy_all);
+  gsl_matrix_free(xHi_all);
+  gsl_matrix_free(Hessian);
+
+  gsl_matrix_free(Y);
+  gsl_matrix_free(W);
+
+  return;
 }
 
-void MVLMM::AnalyzeBimbamGXE (const gsl_matrix *U, const gsl_vector *eval, 
-			      const gsl_matrix *UtW, const gsl_matrix *UtY, 
-			      const gsl_vector *env) {
-	igzstream infile (file_geno.c_str(), igzstream::in);
-	if (!infile) {
-	  cout<<"error reading genotype file:"<<file_geno<<endl; 
-	  return;
-	}
-
-	clock_t time_start=clock();
-	time_UtX=0; time_opt=0;
-
-	string line;
-	char *ch_ptr;
-
-	double logl_H0=0.0, logl_H1=0.0, p_wald=0, p_lrt=0, p_score=0;
-	double crt_a, crt_b, crt_c;
-	int n_miss, c_phen;
-	double geno, x_mean;
-	size_t c=0;
-	size_t n_size=UtY->size1, d_size=UtY->size2, c_size=UtW->size2+2;
-	size_t dc_size=d_size*(c_size+1), v_size=d_size*(d_size+1)/2;
-
-	// Large matrices for EM.
-	gsl_matrix *U_hat=gsl_matrix_alloc (d_size, n_size);
-	gsl_matrix *E_hat=gsl_matrix_alloc (d_size, n_size);
-	gsl_matrix *OmegaU=gsl_matrix_alloc (d_size, n_size);
-	gsl_matrix *OmegaE=gsl_matrix_alloc (d_size, n_size);
-	gsl_matrix *UltVehiY=gsl_matrix_alloc (d_size, n_size);
-	gsl_matrix *UltVehiBX=gsl_matrix_alloc (d_size, n_size);
-	gsl_matrix *UltVehiU=gsl_matrix_alloc (d_size, n_size);
-	gsl_matrix *UltVehiE=gsl_matrix_alloc (d_size, n_size);
-
-	// Large matrices for NR.
-	// Each dxd block is H_k^{-1}.
-	gsl_matrix *Hi_all=gsl_matrix_alloc (d_size, d_size*n_size);
-
-	// Each column is H_k^{-1}y_k.
-	gsl_matrix *Hiy_all=gsl_matrix_alloc (d_size, n_size);
-
-	// Each dcxdc block is x_k\otimes H_k^{-1}.
-	gsl_matrix *xHi_all=gsl_matrix_alloc (dc_size, d_size*n_size);
-	gsl_matrix *Hessian=gsl_matrix_alloc (v_size*2, v_size*2);
-
-	gsl_vector *x=gsl_vector_alloc (n_size);
-	gsl_vector *x_miss=gsl_vector_alloc (n_size);
-
-	gsl_matrix *Y=gsl_matrix_alloc (d_size, n_size);
-	gsl_matrix *X=gsl_matrix_alloc (c_size+1, n_size);
-	gsl_matrix *V_g=gsl_matrix_alloc (d_size, d_size);
-	gsl_matrix *V_e=gsl_matrix_alloc (d_size, d_size);
-	gsl_matrix *B=gsl_matrix_alloc (d_size, c_size+1);
-	gsl_vector *beta=gsl_vector_alloc (d_size);
-	gsl_matrix *Vbeta=gsl_matrix_alloc (d_size, d_size);
-
-	// Null estimates for initial values; including env but not
-	// including x.
-	gsl_matrix *V_g_null=gsl_matrix_alloc (d_size, d_size);
-	gsl_matrix *V_e_null=gsl_matrix_alloc (d_size, d_size);
-	gsl_matrix *B_null=gsl_matrix_alloc (d_size, c_size+1);
-	gsl_matrix *se_B_null1=gsl_matrix_alloc (d_size, c_size-1);
-	gsl_matrix *se_B_null2=gsl_matrix_alloc (d_size, c_size);
-
-	gsl_matrix_view X_sub1=gsl_matrix_submatrix(X,0,0,c_size-1,n_size);
-	gsl_matrix_view B_sub1=gsl_matrix_submatrix(B,0,0,d_size,c_size-1);
-	gsl_matrix_view xHi_all_sub1=
-	  gsl_matrix_submatrix(xHi_all,0,0,d_size*(c_size-1),d_size*n_size);
-
-	gsl_matrix_view X_sub2=gsl_matrix_submatrix (X, 0, 0, c_size, n_size);
-	gsl_matrix_view B_sub2=gsl_matrix_submatrix (B, 0, 0, d_size, c_size);
-	gsl_matrix_view xHi_all_sub2=
-	  gsl_matrix_submatrix (xHi_all, 0, 0, d_size*c_size, d_size*n_size);
-
-	gsl_matrix_transpose_memcpy (Y, UtY);
-
-	gsl_matrix_view X_sub0=gsl_matrix_submatrix(X,0,0,c_size-2,n_size);
-	gsl_matrix_transpose_memcpy (&X_sub0.matrix, UtW);
-	gsl_vector_view X_row0=gsl_matrix_row(X, c_size-2);
-	gsl_blas_dgemv (CblasTrans, 1.0, U, env, 0.0, &X_row0.vector);
-
-	gsl_vector_view X_row1=gsl_matrix_row(X, c_size-1);
-	gsl_vector_set_zero(&X_row1.vector);
-	gsl_vector_view X_row2=gsl_matrix_row(X, c_size);
-	gsl_vector_set_zero(&X_row2.vector);
-
-	gsl_vector_view B_col1=gsl_matrix_column(B, c_size-1);
-	gsl_vector_set_zero(&B_col1.vector);
-	gsl_vector_view B_col2=gsl_matrix_column(B, c_size);
-	gsl_vector_set_zero(&B_col2.vector);
-
-	MphInitial(em_iter, em_prec, nr_iter, nr_prec, eval, &X_sub1.matrix, 
-		   Y, l_min, l_max, n_region, V_g, V_e, &B_sub1.matrix);
-	logl_H0=MphEM ('R', em_iter, em_prec, eval, &X_sub1.matrix, Y, 
-		       U_hat, E_hat, OmegaU, OmegaE, UltVehiY, UltVehiBX, 
-		       UltVehiU, UltVehiE, V_g, V_e, &B_sub1.matrix);
-	logl_H0=MphNR ('R', nr_iter, nr_prec, eval, &X_sub1.matrix, Y, 
-		       Hi_all, &xHi_all_sub1.matrix, Hiy_all, V_g, V_e, 
-		       Hessian, crt_a, crt_b, crt_c);
-	MphCalcBeta (eval, &X_sub1.matrix, Y, V_g, V_e, UltVehiY, 
-		     &B_sub1.matrix, se_B_null1);
-
-	c=0;
-	Vg_remle_null.clear();
-	Ve_remle_null.clear();
-	for (size_t i=0; i<d_size; i++) {
-	  for (size_t j=i; j<d_size; j++) {
-	    Vg_remle_null.push_back(gsl_matrix_get (V_g, i, j) );
-	    Ve_remle_null.push_back(gsl_matrix_get (V_e, i, j) );
-	    VVg_remle_null.push_back(gsl_matrix_get (Hessian, c, c) );
-	    VVe_remle_null.push_back(gsl_matrix_get(Hessian,c+v_size,
-						    c+v_size));
-	    c++;
-	  }
-	}
-	beta_remle_null.clear();
-	se_beta_remle_null.clear();
-	for (size_t i=0; i<se_B_null1->size1; i++) {
-	  for (size_t j=0; j<se_B_null1->size2; j++) {
-	    beta_remle_null.push_back(gsl_matrix_get(B, i, j) );
-	    se_beta_remle_null.push_back(gsl_matrix_get(se_B_null1, i, j) );
-	  }
-	}
-	logl_remle_H0=logl_H0;
-
-	cout.setf(std::ios_base::fixed, std::ios_base::floatfield);
-	cout.precision(4);
-
-	cout<<"REMLE estimate for Vg in the null model: "<<endl;
-	for (size_t i=0; i<d_size; i++) {
-	  for (size_t j=0; j<=i; j++) {
-	    cout<<gsl_matrix_get(V_g, i, j)<<"\t";
-	  }
-	  cout<<endl;
-	}
-	cout<<"se(Vg): "<<endl;
-	for (size_t i=0; i<d_size; i++) {
-	  for (size_t j=0; j<=i; j++) {
-	    c=GetIndex(i, j, d_size);
-	    cout<<sqrt(gsl_matrix_get(Hessian, c, c))<<"\t";
-	  }
-	  cout<<endl;
-	}
-	cout<<"REMLE estimate for Ve in the null model: "<<endl;
-	for (size_t i=0; i<d_size; i++) {
-	  for (size_t j=0; j<=i; j++) {
-	    cout<<gsl_matrix_get(V_e, i, j)<<"\t";
-	  }
-	  cout<<endl;
-	}
-	cout<<"se(Ve): "<<endl;
-	for (size_t i=0; i<d_size; i++) {
-	  for (size_t j=0; j<=i; j++) {
-	    c=GetIndex(i, j, d_size);
-	    cout<<sqrt(gsl_matrix_get(Hessian, c+v_size, c+v_size))<<"\t";
-	  }
-	  cout<<endl;
-	}
-	cout<<"REMLE likelihood = "<<logl_H0<<endl;
-	
-	logl_H0=MphEM ('L', em_iter, em_prec, eval, &X_sub1.matrix, Y, U_hat, 
-		       E_hat, OmegaU, OmegaE, UltVehiY, UltVehiBX, UltVehiU, 
-		       UltVehiE, V_g, V_e, &B_sub1.matrix);
-	logl_H0=MphNR ('L', nr_iter, nr_prec, eval, &X_sub1.matrix, Y, 
-		       Hi_all, &xHi_all_sub1.matrix, Hiy_all, V_g, V_e, 
-		       Hessian, crt_a, crt_b, crt_c);
-	MphCalcBeta (eval, &X_sub1.matrix, Y, V_g, V_e, UltVehiY, 
-		     &B_sub1.matrix, se_B_null1);
-
-	c=0;
-	Vg_mle_null.clear();
-	Ve_mle_null.clear();
-	for (size_t i=0; i<d_size; i++) {
-	  for (size_t j=i; j<d_size; j++) {
-	    Vg_mle_null.push_back(gsl_matrix_get (V_g, i, j) );
-	    Ve_mle_null.push_back(gsl_matrix_get (V_e, i, j) );
-	    VVg_mle_null.push_back(gsl_matrix_get (Hessian, c, c) );
-	    VVe_mle_null.push_back(gsl_matrix_get(Hessian,c+v_size,c+v_size));
-	    c++;
-	  }
-	}
-	beta_mle_null.clear();
-	se_beta_mle_null.clear();
-	for (size_t i=0; i<se_B_null1->size1; i++) {
-	  for (size_t j=0; j<se_B_null1->size2; j++) {
-	    beta_mle_null.push_back(gsl_matrix_get(B, i, j) );
-	    se_beta_mle_null.push_back(gsl_matrix_get(se_B_null1, i, j) );
-	  }
-	}
-	logl_mle_H0=logl_H0;
-
-	cout<<"MLE estimate for Vg in the null model: "<<endl;
-	for (size_t i=0; i<d_size; i++) {
-	  for (size_t j=0; j<=i; j++) {
-	    cout<<gsl_matrix_get(V_g, i, j)<<"\t";
-	  }
-	  cout<<endl;
-	}
-	cout<<"se(Vg): "<<endl;
-	for (size_t i=0; i<d_size; i++) {
-	  for (size_t j=0; j<=i; j++) {
-	    c=GetIndex(i, j, d_size);
-	    cout<<sqrt(gsl_matrix_get(Hessian, c, c))<<"\t";
-	  }
-	  cout<<endl;
-	}
-	cout<<"MLE estimate for Ve in the null model: "<<endl;
-	for (size_t i=0; i<d_size; i++) {
-	  for (size_t j=0; j<=i; j++) {
-	    cout<<gsl_matrix_get(V_e, i, j)<<"\t";
-	  }
-	  cout<<endl;
-	}
-	cout<<"se(Ve): "<<endl;
-	for (size_t i=0; i<d_size; i++) {
-	  for (size_t j=0; j<=i; j++) {
-	    c=GetIndex(i, j, d_size);
-	    cout<<sqrt(gsl_matrix_get(Hessian, c+v_size, c+v_size))<<"\t";
-	  }
-	  cout<<endl;
-	}
-	cout<<"MLE likelihood = "<<logl_H0<<endl;
-	
-	vector<double> v_beta, v_Vg, v_Ve, v_Vbeta;
-	for (size_t i=0; i<d_size; i++) {
-	  v_beta.push_back(0.0);
-	}
-	for (size_t i=0; i<d_size; i++) {
-	  for (size_t j=i; j<d_size; j++) {
-	    v_Vg.push_back(0.0);
-	    v_Ve.push_back(0.0);
-	    v_Vbeta.push_back(0.0);
-	  }
-	}
-
-	gsl_matrix_memcpy (V_g_null, V_g);
-	gsl_matrix_memcpy (V_e_null, V_e);
-	gsl_matrix_memcpy (B_null, B);
-
-	// Start reading genotypes and analyze.
-	for (size_t t=0; t<indicator_snp.size(); ++t) {
-	  !safeGetline(infile, line).eof();
-	  if (t%d_pace==0 || t==(ns_total-1)) {
-	    ProgressBar ("Reading SNPs  ", t, ns_total-1);
-	  }
-	  if (indicator_snp[t]==0) {continue;}
-	  
-	  ch_ptr=strtok ((char *)line.c_str(), " , \t");
-	  ch_ptr=strtok (NULL, " , \t");
-	  ch_ptr=strtok (NULL, " , \t");
-	  
-	  x_mean=0.0; c_phen=0; n_miss=0;
-	  gsl_vector_set_zero(x_miss);
-	  for (size_t i=0; i<ni_total; ++i) {
-	    ch_ptr=strtok (NULL, " , \t");
-	    if (indicator_idv[i]==0) {continue;}
-	    
-	    if (strcmp(ch_ptr, "NA")==0) {
-	      gsl_vector_set(x_miss, c_phen, 0.0); 
-	      n_miss++;
-	    }
-	    else {
-	      geno=atof(ch_ptr);
-	      
-	      gsl_vector_set(x, c_phen, geno);
-	      gsl_vector_set(x_miss, c_phen, 1.0);
-	      x_mean+=geno;
-	    }
-	    c_phen++;
-	  }
-	  
-	  x_mean/=(double)(ni_test-n_miss);
-	  
-	  for (size_t i=0; i<ni_test; ++i) {
-	    if (gsl_vector_get (x_miss, i)==0) {gsl_vector_set(x, i, x_mean);}
-	    geno=gsl_vector_get(x, i);
-	    if (x_mean>1) {
-	      gsl_vector_set(x, i, 2-geno);
-	    }
-	  }
-	  
-	  // Calculate statistics.
-	  time_start=clock();
-	  gsl_blas_dgemv (CblasTrans, 1.0, U, x, 0.0, &X_row1.vector);
-	  gsl_vector_mul (x, env);
-	  gsl_blas_dgemv (CblasTrans, 1.0, U, x, 0.0, &X_row2.vector);
-	  time_UtX+=(clock()-time_start)/(double(CLOCKS_PER_SEC)*60.0);
-	  
-		//initial values
-		gsl_matrix_memcpy (V_g, V_g_null);
-		gsl_matrix_memcpy (V_e, V_e_null);
-		gsl_matrix_memcpy (B, B_null);
-
-		if (a_mode==2 || a_mode==3 || a_mode==4) {
-		  if (a_mode==3 || a_mode==4) {
-		    logl_H0=MphEM ('R', em_iter/10, em_prec*10, eval, 
-				   &X_sub2.matrix, Y, U_hat, E_hat, OmegaU, 
-				   OmegaE, UltVehiY, UltVehiBX, UltVehiU, 
-				   UltVehiE, V_g, V_e, &B_sub2.matrix);
-		    logl_H0=MphNR ('R', nr_iter/10, nr_prec*10, eval, 
-				   &X_sub2.matrix, Y, Hi_all, 
-				   &xHi_all_sub2.matrix, Hiy_all, V_g, V_e, 
-				   Hessian, crt_a, crt_b, crt_c);
-		    MphCalcBeta (eval, &X_sub2.matrix, Y, V_g, V_e, UltVehiY, 
-				 &B_sub2.matrix, se_B_null2);
-		  }
-
-		  if (a_mode==2 || a_mode==4) {
-		    logl_H0=MphEM ('L', em_iter/10, em_prec*10, eval, 
-				   &X_sub2.matrix, Y, U_hat, E_hat, OmegaU, 
-				   OmegaE, UltVehiY, UltVehiBX, UltVehiU, 
-				   UltVehiE, V_g, V_e, &B_sub2.matrix);
-		    logl_H0=MphNR ('L', nr_iter/10, nr_prec*10, eval, 
-				   &X_sub2.matrix, Y, Hi_all, 
-				   &xHi_all_sub2.matrix, Hiy_all, V_g, V_e, 
-				   Hessian, crt_a, crt_b, crt_c);
-		    MphCalcBeta (eval, &X_sub2.matrix, Y, V_g, V_e, UltVehiY, 
-				 &B_sub2.matrix, se_B_null2);
-		  }
-		}
-
-		time_start=clock();
-
-		// 3 is before 1.
-		if (a_mode==3 || a_mode==4) {
-		  p_score=MphCalcP (eval, &X_row2.vector, &X_sub2.matrix, Y, 
-				    V_g_null, V_e_null, UltVehiY, beta, Vbeta);
-		  if (p_score<p_nr && crt==1) {
-		    logl_H1=MphNR ('R', 1, nr_prec*10, eval, X, Y, Hi_all, 
-				   xHi_all, Hiy_all, V_g, V_e, Hessian, 
-				   crt_a, crt_b, crt_c);
-		    p_score=PCRT (3, d_size, p_score, crt_a, crt_b, crt_c);
-		  }
-		}
-		
-		if (a_mode==2 || a_mode==4) {
-		  logl_H1=MphEM ('L', em_iter/10, em_prec*10, eval, X, Y, 
-				 U_hat, E_hat, OmegaU, OmegaE, UltVehiY, 
-				 UltVehiBX, UltVehiU, UltVehiE, V_g, V_e, B);
-
-		  // Calculate beta and Vbeta.
-		  p_lrt=MphCalcP (eval, &X_row2.vector, &X_sub2.matrix, Y, 
-				  V_g, V_e, UltVehiY, beta, Vbeta);
-		  p_lrt=gsl_cdf_chisq_Q(2.0*(logl_H1-logl_H0),(double)d_size);
-		  
-		  if (p_lrt<p_nr) {
-		    logl_H1=MphNR ('L', nr_iter/10, nr_prec*10, eval, X, Y, 
-				   Hi_all, xHi_all, Hiy_all, V_g, V_e, 
-				   Hessian, crt_a, crt_b, crt_c);
-		    
-		    // Calculate beta and Vbeta.
-		    p_lrt=MphCalcP (eval, &X_row2.vector, &X_sub2.matrix, Y, 
-				    V_g, V_e, UltVehiY, beta, Vbeta);
-		    p_lrt=gsl_cdf_chisq_Q(2.0*(logl_H1-logl_H0),
-					  (double)d_size );
-		    
-		    if (crt==1) {
-		      p_lrt=PCRT (2, d_size, p_lrt, crt_a, crt_b, crt_c);
-		    }
-		  }
-		}
-		
-		if (a_mode==1 || a_mode==4) {
-		  logl_H1=MphEM ('R', em_iter/10, em_prec*10, eval, X, Y, 
-				 U_hat, E_hat, OmegaU, OmegaE, UltVehiY, 
-				 UltVehiBX, UltVehiU, UltVehiE, V_g, V_e, B);
-		  p_wald=MphCalcP (eval, &X_row2.vector, &X_sub2.matrix, Y, 
-				   V_g, V_e, UltVehiY, beta, Vbeta);
-		  
-		  if (p_wald<p_nr) {
-		    logl_H1=MphNR ('R', nr_iter/10, nr_prec*10, eval, X, Y, 
-				   Hi_all, xHi_all, Hiy_all, V_g, V_e, 
-				   Hessian, crt_a, crt_b, crt_c);
-		    p_wald=MphCalcP (eval, &X_row2.vector, &X_sub2.matrix, Y, 
-				     V_g, V_e, UltVehiY, beta, Vbeta);
-		    
-		    if (crt==1) {
-		      p_wald=PCRT (1, d_size, p_wald, crt_a, crt_b, crt_c);
-		    }
-		  }
-		}
-
-		if (x_mean>1) {gsl_vector_scale(beta, -1.0);}
-
-		time_opt+=(clock()-time_start)/(double(CLOCKS_PER_SEC)*60.0);
-
-		// Store summary data.
-		for (size_t i=0; i<d_size; i++) {
-		  v_beta[i]=gsl_vector_get (beta, i);
-		}
-
-		c=0;
-		for (size_t i=0; i<d_size; i++) {
-		  for (size_t j=i; j<d_size; j++) {
-		    v_Vg[c]=gsl_matrix_get (V_g, i, j);
-		    v_Ve[c]=gsl_matrix_get (V_e, i, j);
-		    v_Vbeta[c]=gsl_matrix_get (Vbeta, i, j);
-		    c++;
-		  }
-		}
-
-		MPHSUMSTAT SNPs={v_beta, p_wald, p_lrt, p_score, v_Vg, 
-				 v_Ve, v_Vbeta};
-		sumStat.push_back(SNPs);
-	}
-	cout<<endl;
-
-
-	infile.close();
-	infile.clear();
-
-	gsl_matrix_free(U_hat);
-	gsl_matrix_free(E_hat);
-	gsl_matrix_free(OmegaU);
-	gsl_matrix_free(OmegaE);
-	gsl_matrix_free(UltVehiY);
-	gsl_matrix_free(UltVehiBX);
-	gsl_matrix_free(UltVehiU);
-	gsl_matrix_free(UltVehiE);
-
-	gsl_matrix_free(Hi_all);
-	gsl_matrix_free(Hiy_all);
-	gsl_matrix_free(xHi_all);
-	gsl_matrix_free(Hessian);
-
-	gsl_vector_free(x);
-	gsl_vector_free(x_miss);
-
-	gsl_matrix_free(Y);
-	gsl_matrix_free(X);
-	gsl_matrix_free(V_g);
-	gsl_matrix_free(V_e);
-	gsl_matrix_free(B);
-	gsl_vector_free(beta);
-	gsl_matrix_free(Vbeta);
-
-	gsl_matrix_free(V_g_null);
-	gsl_matrix_free(V_e_null);
-	gsl_matrix_free(B_null);
-	gsl_matrix_free(se_B_null1);
-	gsl_matrix_free(se_B_null2);
-
-	return;
+void MVLMM::AnalyzeBimbamGXE(const gsl_matrix *U, const gsl_vector *eval,
+                             const gsl_matrix *UtW, const gsl_matrix *UtY,
+                             const gsl_vector *env) {
+  igzstream infile(file_geno.c_str(), igzstream::in);
+  if (!infile) {
+    cout << "error reading genotype file:" << file_geno << endl;
+    return;
+  }
+
+  clock_t time_start = clock();
+  time_UtX = 0;
+  time_opt = 0;
+
+  string line;
+  char *ch_ptr;
+
+  double logl_H0 = 0.0, logl_H1 = 0.0, p_wald = 0, p_lrt = 0, p_score = 0;
+  double crt_a, crt_b, crt_c;
+  int n_miss, c_phen;
+  double geno, x_mean;
+  size_t c = 0;
+  size_t n_size = UtY->size1, d_size = UtY->size2, c_size = UtW->size2 + 2;
+  size_t dc_size = d_size * (c_size + 1), v_size = d_size * (d_size + 1) / 2;
+
+  // Large matrices for EM.
+  gsl_matrix *U_hat = gsl_matrix_alloc(d_size, n_size);
+  gsl_matrix *E_hat = gsl_matrix_alloc(d_size, n_size);
+  gsl_matrix *OmegaU = gsl_matrix_alloc(d_size, n_size);
+  gsl_matrix *OmegaE = gsl_matrix_alloc(d_size, n_size);
+  gsl_matrix *UltVehiY = gsl_matrix_alloc(d_size, n_size);
+  gsl_matrix *UltVehiBX = gsl_matrix_alloc(d_size, n_size);
+  gsl_matrix *UltVehiU = gsl_matrix_alloc(d_size, n_size);
+  gsl_matrix *UltVehiE = gsl_matrix_alloc(d_size, n_size);
+
+  // Large matrices for NR.
+  // Each dxd block is H_k^{-1}.
+  gsl_matrix *Hi_all = gsl_matrix_alloc(d_size, d_size * n_size);
+
+  // Each column is H_k^{-1}y_k.
+  gsl_matrix *Hiy_all = gsl_matrix_alloc(d_size, n_size);
+
+  // Each dcxdc block is x_k\otimes H_k^{-1}.
+  gsl_matrix *xHi_all = gsl_matrix_alloc(dc_size, d_size * n_size);
+  gsl_matrix *Hessian = gsl_matrix_alloc(v_size * 2, v_size * 2);
+
+  gsl_vector *x = gsl_vector_alloc(n_size);
+  gsl_vector *x_miss = gsl_vector_alloc(n_size);
+
+  gsl_matrix *Y = gsl_matrix_alloc(d_size, n_size);
+  gsl_matrix *X = gsl_matrix_alloc(c_size + 1, n_size);
+  gsl_matrix *V_g = gsl_matrix_alloc(d_size, d_size);
+  gsl_matrix *V_e = gsl_matrix_alloc(d_size, d_size);
+  gsl_matrix *B = gsl_matrix_alloc(d_size, c_size + 1);
+  gsl_vector *beta = gsl_vector_alloc(d_size);
+  gsl_matrix *Vbeta = gsl_matrix_alloc(d_size, d_size);
+
+  // Null estimates for initial values; including env but not
+  // including x.
+  gsl_matrix *V_g_null = gsl_matrix_alloc(d_size, d_size);
+  gsl_matrix *V_e_null = gsl_matrix_alloc(d_size, d_size);
+  gsl_matrix *B_null = gsl_matrix_alloc(d_size, c_size + 1);
+  gsl_matrix *se_B_null1 = gsl_matrix_alloc(d_size, c_size - 1);
+  gsl_matrix *se_B_null2 = gsl_matrix_alloc(d_size, c_size);
+
+  gsl_matrix_view X_sub1 = gsl_matrix_submatrix(X, 0, 0, c_size - 1, n_size);
+  gsl_matrix_view B_sub1 = gsl_matrix_submatrix(B, 0, 0, d_size, c_size - 1);
+  gsl_matrix_view xHi_all_sub1 = gsl_matrix_submatrix(
+      xHi_all, 0, 0, d_size * (c_size - 1), d_size * n_size);
+
+  gsl_matrix_view X_sub2 = gsl_matrix_submatrix(X, 0, 0, c_size, n_size);
+  gsl_matrix_view B_sub2 = gsl_matrix_submatrix(B, 0, 0, d_size, c_size);
+  gsl_matrix_view xHi_all_sub2 =
+      gsl_matrix_submatrix(xHi_all, 0, 0, d_size * c_size, d_size * n_size);
+
+  gsl_matrix_transpose_memcpy(Y, UtY);
+
+  gsl_matrix_view X_sub0 = gsl_matrix_submatrix(X, 0, 0, c_size - 2, n_size);
+  gsl_matrix_transpose_memcpy(&X_sub0.matrix, UtW);
+  gsl_vector_view X_row0 = gsl_matrix_row(X, c_size - 2);
+  gsl_blas_dgemv(CblasTrans, 1.0, U, env, 0.0, &X_row0.vector);
+
+  gsl_vector_view X_row1 = gsl_matrix_row(X, c_size - 1);
+  gsl_vector_set_zero(&X_row1.vector);
+  gsl_vector_view X_row2 = gsl_matrix_row(X, c_size);
+  gsl_vector_set_zero(&X_row2.vector);
+
+  gsl_vector_view B_col1 = gsl_matrix_column(B, c_size - 1);
+  gsl_vector_set_zero(&B_col1.vector);
+  gsl_vector_view B_col2 = gsl_matrix_column(B, c_size);
+  gsl_vector_set_zero(&B_col2.vector);
+
+  MphInitial(em_iter, em_prec, nr_iter, nr_prec, eval, &X_sub1.matrix, Y, l_min,
+             l_max, n_region, V_g, V_e, &B_sub1.matrix);
+  logl_H0 = MphEM('R', em_iter, em_prec, eval, &X_sub1.matrix, Y, U_hat, E_hat,
+                  OmegaU, OmegaE, UltVehiY, UltVehiBX, UltVehiU, UltVehiE, V_g,
+                  V_e, &B_sub1.matrix);
+  logl_H0 = MphNR('R', nr_iter, nr_prec, eval, &X_sub1.matrix, Y, Hi_all,
+                  &xHi_all_sub1.matrix, Hiy_all, V_g, V_e, Hessian, crt_a,
+                  crt_b, crt_c);
+  MphCalcBeta(eval, &X_sub1.matrix, Y, V_g, V_e, UltVehiY, &B_sub1.matrix,
+              se_B_null1);
+
+  c = 0;
+  Vg_remle_null.clear();
+  Ve_remle_null.clear();
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = i; j < d_size; j++) {
+      Vg_remle_null.push_back(gsl_matrix_get(V_g, i, j));
+      Ve_remle_null.push_back(gsl_matrix_get(V_e, i, j));
+      VVg_remle_null.push_back(gsl_matrix_get(Hessian, c, c));
+      VVe_remle_null.push_back(gsl_matrix_get(Hessian, c + v_size, c + v_size));
+      c++;
+    }
+  }
+  beta_remle_null.clear();
+  se_beta_remle_null.clear();
+  for (size_t i = 0; i < se_B_null1->size1; i++) {
+    for (size_t j = 0; j < se_B_null1->size2; j++) {
+      beta_remle_null.push_back(gsl_matrix_get(B, i, j));
+      se_beta_remle_null.push_back(gsl_matrix_get(se_B_null1, i, j));
+    }
+  }
+  logl_remle_H0 = logl_H0;
+
+  cout.setf(std::ios_base::fixed, std::ios_base::floatfield);
+  cout.precision(4);
+
+  cout << "REMLE estimate for Vg in the null model: " << endl;
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = 0; j <= i; j++) {
+      cout << gsl_matrix_get(V_g, i, j) << "\t";
+    }
+    cout << endl;
+  }
+  cout << "se(Vg): " << endl;
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = 0; j <= i; j++) {
+      c = GetIndex(i, j, d_size);
+      cout << sqrt(gsl_matrix_get(Hessian, c, c)) << "\t";
+    }
+    cout << endl;
+  }
+  cout << "REMLE estimate for Ve in the null model: " << endl;
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = 0; j <= i; j++) {
+      cout << gsl_matrix_get(V_e, i, j) << "\t";
+    }
+    cout << endl;
+  }
+  cout << "se(Ve): " << endl;
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = 0; j <= i; j++) {
+      c = GetIndex(i, j, d_size);
+      cout << sqrt(gsl_matrix_get(Hessian, c + v_size, c + v_size)) << "\t";
+    }
+    cout << endl;
+  }
+  cout << "REMLE likelihood = " << logl_H0 << endl;
+
+  logl_H0 = MphEM('L', em_iter, em_prec, eval, &X_sub1.matrix, Y, U_hat, E_hat,
+                  OmegaU, OmegaE, UltVehiY, UltVehiBX, UltVehiU, UltVehiE, V_g,
+                  V_e, &B_sub1.matrix);
+  logl_H0 = MphNR('L', nr_iter, nr_prec, eval, &X_sub1.matrix, Y, Hi_all,
+                  &xHi_all_sub1.matrix, Hiy_all, V_g, V_e, Hessian, crt_a,
+                  crt_b, crt_c);
+  MphCalcBeta(eval, &X_sub1.matrix, Y, V_g, V_e, UltVehiY, &B_sub1.matrix,
+              se_B_null1);
+
+  c = 0;
+  Vg_mle_null.clear();
+  Ve_mle_null.clear();
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = i; j < d_size; j++) {
+      Vg_mle_null.push_back(gsl_matrix_get(V_g, i, j));
+      Ve_mle_null.push_back(gsl_matrix_get(V_e, i, j));
+      VVg_mle_null.push_back(gsl_matrix_get(Hessian, c, c));
+      VVe_mle_null.push_back(gsl_matrix_get(Hessian, c + v_size, c + v_size));
+      c++;
+    }
+  }
+  beta_mle_null.clear();
+  se_beta_mle_null.clear();
+  for (size_t i = 0; i < se_B_null1->size1; i++) {
+    for (size_t j = 0; j < se_B_null1->size2; j++) {
+      beta_mle_null.push_back(gsl_matrix_get(B, i, j));
+      se_beta_mle_null.push_back(gsl_matrix_get(se_B_null1, i, j));
+    }
+  }
+  logl_mle_H0 = logl_H0;
+
+  cout << "MLE estimate for Vg in the null model: " << endl;
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = 0; j <= i; j++) {
+      cout << gsl_matrix_get(V_g, i, j) << "\t";
+    }
+    cout << endl;
+  }
+  cout << "se(Vg): " << endl;
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = 0; j <= i; j++) {
+      c = GetIndex(i, j, d_size);
+      cout << sqrt(gsl_matrix_get(Hessian, c, c)) << "\t";
+    }
+    cout << endl;
+  }
+  cout << "MLE estimate for Ve in the null model: " << endl;
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = 0; j <= i; j++) {
+      cout << gsl_matrix_get(V_e, i, j) << "\t";
+    }
+    cout << endl;
+  }
+  cout << "se(Ve): " << endl;
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = 0; j <= i; j++) {
+      c = GetIndex(i, j, d_size);
+      cout << sqrt(gsl_matrix_get(Hessian, c + v_size, c + v_size)) << "\t";
+    }
+    cout << endl;
+  }
+  cout << "MLE likelihood = " << logl_H0 << endl;
+
+  vector<double> v_beta, v_Vg, v_Ve, v_Vbeta;
+  for (size_t i = 0; i < d_size; i++) {
+    v_beta.push_back(0.0);
+  }
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = i; j < d_size; j++) {
+      v_Vg.push_back(0.0);
+      v_Ve.push_back(0.0);
+      v_Vbeta.push_back(0.0);
+    }
+  }
+
+  gsl_matrix_memcpy(V_g_null, V_g);
+  gsl_matrix_memcpy(V_e_null, V_e);
+  gsl_matrix_memcpy(B_null, B);
+
+  // Start reading genotypes and analyze.
+  for (size_t t = 0; t < indicator_snp.size(); ++t) {
+    !safeGetline(infile, line).eof();
+    if (t % d_pace == 0 || t == (ns_total - 1)) {
+      ProgressBar("Reading SNPs  ", t, ns_total - 1);
+    }
+    if (indicator_snp[t] == 0) {
+      continue;
+    }
+
+    ch_ptr = strtok((char *)line.c_str(), " , \t");
+    ch_ptr = strtok(NULL, " , \t");
+    ch_ptr = strtok(NULL, " , \t");
+
+    x_mean = 0.0;
+    c_phen = 0;
+    n_miss = 0;
+    gsl_vector_set_zero(x_miss);
+    for (size_t i = 0; i < ni_total; ++i) {
+      ch_ptr = strtok(NULL, " , \t");
+      if (indicator_idv[i] == 0) {
+        continue;
+      }
+
+      if (strcmp(ch_ptr, "NA") == 0) {
+        gsl_vector_set(x_miss, c_phen, 0.0);
+        n_miss++;
+      } else {
+        geno = atof(ch_ptr);
+
+        gsl_vector_set(x, c_phen, geno);
+        gsl_vector_set(x_miss, c_phen, 1.0);
+        x_mean += geno;
+      }
+      c_phen++;
+    }
+
+    x_mean /= (double)(ni_test - n_miss);
+
+    for (size_t i = 0; i < ni_test; ++i) {
+      if (gsl_vector_get(x_miss, i) == 0) {
+        gsl_vector_set(x, i, x_mean);
+      }
+      geno = gsl_vector_get(x, i);
+      if (x_mean > 1) {
+        gsl_vector_set(x, i, 2 - geno);
+      }
+    }
+
+    // Calculate statistics.
+    time_start = clock();
+    gsl_blas_dgemv(CblasTrans, 1.0, U, x, 0.0, &X_row1.vector);
+    gsl_vector_mul(x, env);
+    gsl_blas_dgemv(CblasTrans, 1.0, U, x, 0.0, &X_row2.vector);
+    time_UtX += (clock() - time_start) / (double(CLOCKS_PER_SEC) * 60.0);
+
+    // initial values
+    gsl_matrix_memcpy(V_g, V_g_null);
+    gsl_matrix_memcpy(V_e, V_e_null);
+    gsl_matrix_memcpy(B, B_null);
+
+    if (a_mode == 2 || a_mode == 3 || a_mode == 4) {
+      if (a_mode == 3 || a_mode == 4) {
+        logl_H0 = MphEM('R', em_iter / 10, em_prec * 10, eval, &X_sub2.matrix,
+                        Y, U_hat, E_hat, OmegaU, OmegaE, UltVehiY, UltVehiBX,
+                        UltVehiU, UltVehiE, V_g, V_e, &B_sub2.matrix);
+        logl_H0 = MphNR('R', nr_iter / 10, nr_prec * 10, eval, &X_sub2.matrix,
+                        Y, Hi_all, &xHi_all_sub2.matrix, Hiy_all, V_g, V_e,
+                        Hessian, crt_a, crt_b, crt_c);
+        MphCalcBeta(eval, &X_sub2.matrix, Y, V_g, V_e, UltVehiY, &B_sub2.matrix,
+                    se_B_null2);
+      }
+
+      if (a_mode == 2 || a_mode == 4) {
+        logl_H0 = MphEM('L', em_iter / 10, em_prec * 10, eval, &X_sub2.matrix,
+                        Y, U_hat, E_hat, OmegaU, OmegaE, UltVehiY, UltVehiBX,
+                        UltVehiU, UltVehiE, V_g, V_e, &B_sub2.matrix);
+        logl_H0 = MphNR('L', nr_iter / 10, nr_prec * 10, eval, &X_sub2.matrix,
+                        Y, Hi_all, &xHi_all_sub2.matrix, Hiy_all, V_g, V_e,
+                        Hessian, crt_a, crt_b, crt_c);
+        MphCalcBeta(eval, &X_sub2.matrix, Y, V_g, V_e, UltVehiY, &B_sub2.matrix,
+                    se_B_null2);
+      }
+    }
+
+    time_start = clock();
+
+    // 3 is before 1.
+    if (a_mode == 3 || a_mode == 4) {
+      p_score = MphCalcP(eval, &X_row2.vector, &X_sub2.matrix, Y, V_g_null,
+                         V_e_null, UltVehiY, beta, Vbeta);
+      if (p_score < p_nr && crt == 1) {
+        logl_H1 = MphNR('R', 1, nr_prec * 10, eval, X, Y, Hi_all, xHi_all,
+                        Hiy_all, V_g, V_e, Hessian, crt_a, crt_b, crt_c);
+        p_score = PCRT(3, d_size, p_score, crt_a, crt_b, crt_c);
+      }
+    }
+
+    if (a_mode == 2 || a_mode == 4) {
+      logl_H1 = MphEM('L', em_iter / 10, em_prec * 10, eval, X, Y, U_hat, E_hat,
+                      OmegaU, OmegaE, UltVehiY, UltVehiBX, UltVehiU, UltVehiE,
+                      V_g, V_e, B);
+
+      // Calculate beta and Vbeta.
+      p_lrt = MphCalcP(eval, &X_row2.vector, &X_sub2.matrix, Y, V_g, V_e,
+                       UltVehiY, beta, Vbeta);
+      p_lrt = gsl_cdf_chisq_Q(2.0 * (logl_H1 - logl_H0), (double)d_size);
+
+      if (p_lrt < p_nr) {
+        logl_H1 =
+            MphNR('L', nr_iter / 10, nr_prec * 10, eval, X, Y, Hi_all, xHi_all,
+                  Hiy_all, V_g, V_e, Hessian, crt_a, crt_b, crt_c);
+
+        // Calculate beta and Vbeta.
+        p_lrt = MphCalcP(eval, &X_row2.vector, &X_sub2.matrix, Y, V_g, V_e,
+                         UltVehiY, beta, Vbeta);
+        p_lrt = gsl_cdf_chisq_Q(2.0 * (logl_H1 - logl_H0), (double)d_size);
+
+        if (crt == 1) {
+          p_lrt = PCRT(2, d_size, p_lrt, crt_a, crt_b, crt_c);
+        }
+      }
+    }
+
+    if (a_mode == 1 || a_mode == 4) {
+      logl_H1 = MphEM('R', em_iter / 10, em_prec * 10, eval, X, Y, U_hat, E_hat,
+                      OmegaU, OmegaE, UltVehiY, UltVehiBX, UltVehiU, UltVehiE,
+                      V_g, V_e, B);
+      p_wald = MphCalcP(eval, &X_row2.vector, &X_sub2.matrix, Y, V_g, V_e,
+                        UltVehiY, beta, Vbeta);
+
+      if (p_wald < p_nr) {
+        logl_H1 =
+            MphNR('R', nr_iter / 10, nr_prec * 10, eval, X, Y, Hi_all, xHi_all,
+                  Hiy_all, V_g, V_e, Hessian, crt_a, crt_b, crt_c);
+        p_wald = MphCalcP(eval, &X_row2.vector, &X_sub2.matrix, Y, V_g, V_e,
+                          UltVehiY, beta, Vbeta);
+
+        if (crt == 1) {
+          p_wald = PCRT(1, d_size, p_wald, crt_a, crt_b, crt_c);
+        }
+      }
+    }
+
+    if (x_mean > 1) {
+      gsl_vector_scale(beta, -1.0);
+    }
+
+    time_opt += (clock() - time_start) / (double(CLOCKS_PER_SEC) * 60.0);
+
+    // Store summary data.
+    for (size_t i = 0; i < d_size; i++) {
+      v_beta[i] = gsl_vector_get(beta, i);
+    }
+
+    c = 0;
+    for (size_t i = 0; i < d_size; i++) {
+      for (size_t j = i; j < d_size; j++) {
+        v_Vg[c] = gsl_matrix_get(V_g, i, j);
+        v_Ve[c] = gsl_matrix_get(V_e, i, j);
+        v_Vbeta[c] = gsl_matrix_get(Vbeta, i, j);
+        c++;
+      }
+    }
+
+    MPHSUMSTAT SNPs = {v_beta, p_wald, p_lrt, p_score, v_Vg, v_Ve, v_Vbeta};
+    sumStat.push_back(SNPs);
+  }
+  cout << endl;
+
+  infile.close();
+  infile.clear();
+
+  gsl_matrix_free(U_hat);
+  gsl_matrix_free(E_hat);
+  gsl_matrix_free(OmegaU);
+  gsl_matrix_free(OmegaE);
+  gsl_matrix_free(UltVehiY);
+  gsl_matrix_free(UltVehiBX);
+  gsl_matrix_free(UltVehiU);
+  gsl_matrix_free(UltVehiE);
+
+  gsl_matrix_free(Hi_all);
+  gsl_matrix_free(Hiy_all);
+  gsl_matrix_free(xHi_all);
+  gsl_matrix_free(Hessian);
+
+  gsl_vector_free(x);
+  gsl_vector_free(x_miss);
+
+  gsl_matrix_free(Y);
+  gsl_matrix_free(X);
+  gsl_matrix_free(V_g);
+  gsl_matrix_free(V_e);
+  gsl_matrix_free(B);
+  gsl_vector_free(beta);
+  gsl_matrix_free(Vbeta);
+
+  gsl_matrix_free(V_g_null);
+  gsl_matrix_free(V_e_null);
+  gsl_matrix_free(B_null);
+  gsl_matrix_free(se_B_null1);
+  gsl_matrix_free(se_B_null2);
+
+  return;
 }
 
-void MVLMM::AnalyzePlinkGXE (const gsl_matrix *U, const gsl_vector *eval, 
-			     const gsl_matrix *UtW, const gsl_matrix *UtY, 
-			     const gsl_vector *env) {
-	string file_bed=file_bfile+".bed";
-	ifstream infile (file_bed.c_str(), ios::binary);
-	if (!infile) {
-	  cout<<"error reading bed file:"<<file_bed<<endl; 
-	  return;
-	}
-
-	clock_t time_start=clock();
-	time_UtX=0; time_opt=0;
-
-	char ch[1];
-	bitset<8> b;
-
-	double logl_H0=0.0, logl_H1=0.0, p_wald=0, p_lrt=0, p_score=0;
-	double crt_a, crt_b, crt_c;
-	int n_bit, n_miss, ci_total, ci_test;
-	double geno, x_mean;
-	size_t c=0;
-	size_t n_size=UtY->size1, d_size=UtY->size2, c_size=UtW->size2+2;
-	size_t dc_size=d_size*(c_size+1), v_size=d_size*(d_size+1)/2;
-
-	// Large matrices for EM.
-	gsl_matrix *U_hat=gsl_matrix_alloc (d_size, n_size);
-	gsl_matrix *E_hat=gsl_matrix_alloc (d_size, n_size);
-	gsl_matrix *OmegaU=gsl_matrix_alloc (d_size, n_size);
-	gsl_matrix *OmegaE=gsl_matrix_alloc (d_size, n_size);
-	gsl_matrix *UltVehiY=gsl_matrix_alloc (d_size, n_size);
-	gsl_matrix *UltVehiBX=gsl_matrix_alloc (d_size, n_size);
-	gsl_matrix *UltVehiU=gsl_matrix_alloc (d_size, n_size);
-	gsl_matrix *UltVehiE=gsl_matrix_alloc (d_size, n_size);
-
-	// Large matrices for NR.
-	// Each dxd block is H_k^{-1}.
-	gsl_matrix *Hi_all=gsl_matrix_alloc (d_size, d_size*n_size);
-	
-	// Each column is H_k^{-1}y_k
-	gsl_matrix *Hiy_all=gsl_matrix_alloc (d_size, n_size);
-	
-	// Each dcxdc block is x_k\otimes H_k^{-1}.
-	gsl_matrix *xHi_all=gsl_matrix_alloc (dc_size, d_size*n_size);	
-	gsl_matrix *Hessian=gsl_matrix_alloc (v_size*2, v_size*2);
-
-	gsl_vector *x=gsl_vector_alloc (n_size);
-
-	gsl_matrix *Y=gsl_matrix_alloc (d_size, n_size);
-	gsl_matrix *X=gsl_matrix_alloc (c_size+1, n_size);
-	gsl_matrix *V_g=gsl_matrix_alloc (d_size, d_size);
-	gsl_matrix *V_e=gsl_matrix_alloc (d_size, d_size);
-	gsl_matrix *B=gsl_matrix_alloc (d_size, c_size+1);
-	gsl_vector *beta=gsl_vector_alloc (d_size);
-	gsl_matrix *Vbeta=gsl_matrix_alloc (d_size, d_size);
-
-	// Null estimates for initial values.
-	gsl_matrix *V_g_null=gsl_matrix_alloc (d_size, d_size);
-	gsl_matrix *V_e_null=gsl_matrix_alloc (d_size, d_size);
-	gsl_matrix *B_null=gsl_matrix_alloc (d_size, c_size+1);
-	gsl_matrix *se_B_null1=gsl_matrix_alloc (d_size, c_size-1);
-	gsl_matrix *se_B_null2=gsl_matrix_alloc (d_size, c_size);
-
-	gsl_matrix_view X_sub1=gsl_matrix_submatrix(X,0,0,c_size-1,n_size);
-	gsl_matrix_view B_sub1=gsl_matrix_submatrix(B,0,0,d_size,c_size-1);
-	gsl_matrix_view xHi_all_sub1=
-	  gsl_matrix_submatrix(xHi_all,0,0,d_size*(c_size-1),d_size*n_size);
-
-	gsl_matrix_view X_sub2=gsl_matrix_submatrix (X, 0, 0, c_size, n_size);
-	gsl_matrix_view B_sub2=gsl_matrix_submatrix (B, 0, 0, d_size, c_size);
-	gsl_matrix_view xHi_all_sub2=
-	  gsl_matrix_submatrix (xHi_all, 0, 0, d_size*c_size, d_size*n_size);
-
-	gsl_matrix_transpose_memcpy (Y, UtY);
-
-	gsl_matrix_view X_sub0=gsl_matrix_submatrix(X,0,0,c_size-2,n_size);
-	gsl_matrix_transpose_memcpy (&X_sub0.matrix, UtW);
-	gsl_vector_view X_row0=gsl_matrix_row(X, c_size-2);
-	gsl_blas_dgemv (CblasTrans, 1.0, U, env, 0.0, &X_row0.vector);
-
-	gsl_vector_view X_row1=gsl_matrix_row(X, c_size-1);
-	gsl_vector_set_zero(&X_row1.vector);
-	gsl_vector_view X_row2=gsl_matrix_row(X, c_size);
-	gsl_vector_set_zero(&X_row2.vector);
-
-	gsl_vector_view B_col1=gsl_matrix_column(B, c_size-1);
-	gsl_vector_set_zero(&B_col1.vector);
-	gsl_vector_view B_col2=gsl_matrix_column(B, c_size);
-	gsl_vector_set_zero(&B_col2.vector);
-
-	MphInitial(em_iter, em_prec, nr_iter, nr_prec, eval, &X_sub1.matrix, 
-		   Y, l_min, l_max, n_region, V_g, V_e, &B_sub1.matrix);
-
-	logl_H0=MphEM ('R', em_iter, em_prec, eval, &X_sub1.matrix, Y, U_hat, 
-		       E_hat, OmegaU, OmegaE, UltVehiY, UltVehiBX, UltVehiU, 
-		       UltVehiE, V_g, V_e, &B_sub1.matrix);
-	logl_H0=MphNR ('R', nr_iter, nr_prec, eval, &X_sub1.matrix, Y, 
-		       Hi_all, &xHi_all_sub1.matrix, Hiy_all, V_g, V_e, 
-		       Hessian, crt_a, crt_b, crt_c);
-	MphCalcBeta (eval, &X_sub1.matrix, Y, V_g, V_e, UltVehiY, 
-		     &B_sub1.matrix, se_B_null1);
-
-	c=0;
-	Vg_remle_null.clear();
-	Ve_remle_null.clear();
-	for (size_t i=0; i<d_size; i++) {
-	  for (size_t j=i; j<d_size; j++) {
-	    Vg_remle_null.push_back(gsl_matrix_get (V_g, i, j) );
-	    Ve_remle_null.push_back(gsl_matrix_get (V_e, i, j) );
-	    VVg_remle_null.push_back(gsl_matrix_get (Hessian, c, c) );
-	    VVe_remle_null.push_back(gsl_matrix_get(Hessian,c+v_size,
-						    c+v_size));
-	    c++;
-	  }
-	}
-	beta_remle_null.clear();
-	se_beta_remle_null.clear();
-	for (size_t i=0; i<se_B_null1->size1; i++) {
-	  for (size_t j=0; j<se_B_null1->size2; j++) {
-	    beta_remle_null.push_back(gsl_matrix_get(B, i, j) );
-	    se_beta_remle_null.push_back(gsl_matrix_get(se_B_null1, i, j) );
-	  }
-	}
-	logl_remle_H0=logl_H0;
-
-	cout.setf(std::ios_base::fixed, std::ios_base::floatfield);
-	cout.precision(4);
-	cout<<"REMLE estimate for Vg in the null model: "<<endl;
-	for (size_t i=0; i<d_size; i++) {
-	  for (size_t j=0; j<=i; j++) {
-	    cout<<gsl_matrix_get(V_g, i, j)<<"\t";
-	  }
-	  cout<<endl;
-	}
-	cout<<"se(Vg): "<<endl;
-	for (size_t i=0; i<d_size; i++) {
-	  for (size_t j=0; j<=i; j++) {
-	    c=GetIndex(i, j, d_size);
-	    cout<<sqrt(gsl_matrix_get(Hessian, c, c))<<"\t";
-	  }
-	  cout<<endl;
-	}
-	cout<<"REMLE estimate for Ve in the null model: "<<endl;
-	for (size_t i=0; i<d_size; i++) {
-	  for (size_t j=0; j<=i; j++) {
-	    cout<<gsl_matrix_get(V_e, i, j)<<"\t";
-	  }
-	  cout<<endl;
-	}
-	cout<<"se(Ve): "<<endl;
-	for (size_t i=0; i<d_size; i++) {
-	  for (size_t j=0; j<=i; j++) {
-	    c=GetIndex(i, j, d_size);
-	    cout<<sqrt(gsl_matrix_get(Hessian, c+v_size, c+v_size))<<"\t";
-	  }
-	  cout<<endl;
-	}
-	cout<<"REMLE likelihood = "<<logl_H0<<endl;
-
-	logl_H0=MphEM ('L', em_iter, em_prec, eval, &X_sub1.matrix, Y, 
-		       U_hat, E_hat, OmegaU, OmegaE, UltVehiY, UltVehiBX, 
-		       UltVehiU, UltVehiE, V_g, V_e, &B_sub1.matrix);
-	logl_H0=MphNR ('L', nr_iter, nr_prec, eval, &X_sub1.matrix, Y, 
-		       Hi_all, &xHi_all_sub1.matrix, Hiy_all, V_g, V_e, 
-		       Hessian, crt_a, crt_b, crt_c);
-	MphCalcBeta (eval, &X_sub1.matrix, Y, V_g, V_e, UltVehiY, 
-		     &B_sub1.matrix, se_B_null1);
-
-	c=0;
-	Vg_mle_null.clear();
-	Ve_mle_null.clear();
-	for (size_t i=0; i<d_size; i++) {
-	  for (size_t j=i; j<d_size; j++) {
-	    Vg_mle_null.push_back(gsl_matrix_get (V_g, i, j) );
-	    Ve_mle_null.push_back(gsl_matrix_get (V_e, i, j) );
-	    VVg_mle_null.push_back(gsl_matrix_get (Hessian, c, c) );
-	    VVe_mle_null.push_back(gsl_matrix_get(Hessian,c+v_size,c+v_size));
-	    c++;
-	  }
-	}
-	beta_mle_null.clear();
-	se_beta_mle_null.clear();
-	for (size_t i=0; i<se_B_null1->size1; i++) {
-	  for (size_t j=0; j<se_B_null1->size2; j++) {
-	    beta_mle_null.push_back(gsl_matrix_get(B, i, j) );
-	    se_beta_mle_null.push_back(gsl_matrix_get(se_B_null1, i, j) );
-	  }
-	}
-	logl_mle_H0=logl_H0;
-
-	cout<<"MLE estimate for Vg in the null model: "<<endl;
-	for (size_t i=0; i<d_size; i++) {
-	  for (size_t j=0; j<=i; j++) {
-	    cout<<gsl_matrix_get(V_g, i, j)<<"\t";
-	  }
-	  cout<<endl;
-	}
-	cout<<"se(Vg): "<<endl;
-	for (size_t i=0; i<d_size; i++) {
-	  for (size_t j=0; j<=i; j++) {
-	    c=GetIndex(i, j, d_size);
-	    cout<<sqrt(gsl_matrix_get(Hessian, c, c))<<"\t";
-	  }
-	  cout<<endl;
-	}
-	cout<<"MLE estimate for Ve in the null model: "<<endl;
-	for (size_t i=0; i<d_size; i++) {
-	  for (size_t j=0; j<=i; j++) {
-	    cout<<gsl_matrix_get(V_e, i, j)<<"\t";
-	  }
-	  cout<<endl;
-	}
-	cout<<"se(Ve): "<<endl;
-	for (size_t i=0; i<d_size; i++) {
-	  for (size_t j=0; j<=i; j++) {
-	    c=GetIndex(i, j, d_size);
-	    cout<<sqrt(gsl_matrix_get(Hessian, c+v_size, c+v_size))<<"\t";
-	  }
-	  cout<<endl;
-	}
-	cout<<"MLE likelihood = "<<logl_H0<<endl;
-
-	vector<double> v_beta, v_Vg, v_Ve, v_Vbeta;
-	for (size_t i=0; i<d_size; i++) {
-	  v_beta.push_back(0.0);
-	}
-	for (size_t i=0; i<d_size; i++) {
-	  for (size_t j=i; j<d_size; j++) {
-	    v_Vg.push_back(0.0);
-	    v_Ve.push_back(0.0);
-	    v_Vbeta.push_back(0.0);
-	  }
-	}
-
-	gsl_matrix_memcpy (V_g_null, V_g);
-	gsl_matrix_memcpy (V_e_null, V_e);
-	gsl_matrix_memcpy (B_null, B);
-
-	// Start reading genotypes and analyze.
-	// Calculate n_bit and c, the number of bit for each SNP.
-	if (ni_total%4==0) {n_bit=ni_total/4;}
-	else {n_bit=ni_total/4+1; }
-
-	// Print the first three magic numbers.
-	for (int i=0; i<3; ++i) {
-	  infile.read(ch,1);
-	  b=ch[0];
-	}
-
-	for (vector<SNPINFO>::size_type t=0; t<snpInfo.size(); ++t) {
-	  if (t%d_pace==0 || t==snpInfo.size()-1) {
-	    ProgressBar ("Reading SNPs  ", t, snpInfo.size()-1);
-	  }
-	  if (indicator_snp[t]==0) {continue;}
-	  
-	  // n_bit, and 3 is the number of magic numbers.
-	  infile.seekg(t*n_bit+3);
-
-	  // Read genotypes.
-	  x_mean=0.0;	n_miss=0; ci_total=0; ci_test=0;
-	  for (int i=0; i<n_bit; ++i) {
-	    infile.read(ch,1);
-	    b=ch[0];
-
-	    // Minor allele homozygous: 2.0; major: 0.0.
-	    for (size_t j=0; j<4; ++j) {                
-	      
-	      if ((i==(n_bit-1)) && ci_total==(int)ni_total) {break;}
-	      if (indicator_idv[ci_total]==0) {ci_total++; continue;}
-	      
-	      if (b[2*j]==0) {
-		if (b[2*j+1]==0) {gsl_vector_set(x, ci_test, 2); x_mean+=2.0; }
-		else {gsl_vector_set(x, ci_test, 1); x_mean+=1.0; }
-	      }
-	      else {
-		if (b[2*j+1]==1) {gsl_vector_set(x, ci_test, 0); }
-		else {gsl_vector_set(x, ci_test, -9); n_miss++; }
-	      }
-	      
-	      ci_total++;
-	      ci_test++;
-	    }
-	  }
-	  
-	  x_mean/=(double)(ni_test-n_miss);
-	  
-	  for (size_t i=0; i<ni_test; ++i) {
-	    geno=gsl_vector_get(x,i);
-	    if (geno==-9) {gsl_vector_set(x, i, x_mean); geno=x_mean;}
-	    if (x_mean>1) {
-	      gsl_vector_set(x, i, 2-geno);
-	    }
-	  }
-	  
-	  // Calculate statistics.
-	  time_start=clock();
-	  gsl_blas_dgemv (CblasTrans, 1.0, U, x, 0.0, &X_row1.vector);
-	  gsl_vector_mul (x, env);
-	  gsl_blas_dgemv (CblasTrans, 1.0, U, x, 0.0, &X_row2.vector);
-	  time_UtX+=(clock()-time_start)/(double(CLOCKS_PER_SEC)*60.0);
-	  
-	  // Initial values.
-	  gsl_matrix_memcpy (V_g, V_g_null);
-	  gsl_matrix_memcpy (V_e, V_e_null);
-	  gsl_matrix_memcpy (B, B_null);
-	  
-	  if (a_mode==2 || a_mode==3 || a_mode==4) {
-	    if (a_mode==3 || a_mode==4) {
-	      logl_H0=MphEM ('R', em_iter/10, em_prec*10, eval, 
-			     &X_sub2.matrix, Y, U_hat, E_hat, OmegaU, OmegaE, 
-			     UltVehiY, UltVehiBX, UltVehiU, UltVehiE, V_g, 
-			     V_e, &B_sub2.matrix);
-	      logl_H0=MphNR ('R', nr_iter/10, nr_prec*10, eval, 
-			     &X_sub2.matrix, Y, Hi_all, &xHi_all_sub2.matrix, 
-			     Hiy_all, V_g, V_e, Hessian, crt_a, crt_b, crt_c);
-	      MphCalcBeta (eval, &X_sub2.matrix, Y, V_g, V_e, UltVehiY, 
-			   &B_sub2.matrix, se_B_null2);
-	    }
-	    
-	    if (a_mode==2 || a_mode==4) {
-	      logl_H0=MphEM ('L', em_iter/10, em_prec*10, eval, 
-			     &X_sub2.matrix, Y, U_hat, E_hat, OmegaU, OmegaE, 
-			     UltVehiY, UltVehiBX, UltVehiU, UltVehiE, V_g, 
-			     V_e, &B_sub2.matrix);
-	      logl_H0=MphNR ('L', nr_iter/10, nr_prec*10, eval, 
-			     &X_sub2.matrix, Y, Hi_all, &xHi_all_sub2.matrix, 
-			     Hiy_all, V_g, V_e, Hessian, crt_a, crt_b, crt_c);
-	      MphCalcBeta (eval, &X_sub2.matrix, Y, V_g, V_e, UltVehiY, 
-			   &B_sub2.matrix, se_B_null2);
-	    }
-	  }
-	  
-	  time_start=clock();
-	  
-	  // 3 is before 1.
-	  if (a_mode==3 || a_mode==4) {
-	    p_score=MphCalcP (eval, &X_row2.vector, &X_sub2.matrix, Y, 
-			      V_g_null, V_e_null, UltVehiY, beta, Vbeta);
-	    
-	    if (p_score<p_nr && crt==1) {
-	      logl_H1=MphNR ('R', 1, nr_prec*10, eval, X, Y, Hi_all, xHi_all, 
-			     Hiy_all, V_g, V_e, Hessian, crt_a, crt_b, crt_c);
-	      p_score=PCRT (3, d_size, p_score, crt_a, crt_b, crt_c);
-	    }
-	  }
-	  
-	  if (a_mode==2 || a_mode==4) {
-	    logl_H1=MphEM ('L', em_iter/10, em_prec*10, eval, X, Y, U_hat, 
-			   E_hat, OmegaU, OmegaE, UltVehiY, UltVehiBX, 
-			   UltVehiU, UltVehiE, V_g, V_e, B);
-
-	    // Calculate beta and Vbeta.
-	    p_lrt=MphCalcP (eval, &X_row2.vector, &X_sub2.matrix, Y, V_g, 
-			    V_e, UltVehiY, beta, Vbeta);
-	    p_lrt=gsl_cdf_chisq_Q (2.0*(logl_H1-logl_H0), (double)d_size );
-	    
-	    if (p_lrt<p_nr) {
-	      logl_H1=MphNR ('L', nr_iter/10, nr_prec*10, eval, X, Y, Hi_all, 
-			     xHi_all, Hiy_all, V_g, V_e, Hessian, crt_a, 
-			     crt_b, crt_c);
-	      
-	      // Calculate beta and Vbeta.
-	      p_lrt=MphCalcP (eval, &X_row2.vector, &X_sub2.matrix, Y, V_g, 
-			      V_e, UltVehiY, beta, Vbeta);
-	      p_lrt=gsl_cdf_chisq_Q (2.0*(logl_H1-logl_H0), (double)d_size );
-	      if (crt==1) {
-		p_lrt=PCRT (2, d_size, p_lrt, crt_a, crt_b, crt_c);
-	      }
-	    }
-	  }
-	  
-	  if (a_mode==1 || a_mode==4) {
-	    logl_H1=MphEM ('R', em_iter/10, em_prec*10, eval, X, Y, U_hat, 
-			   E_hat, OmegaU, OmegaE, UltVehiY, UltVehiBX, 
-			   UltVehiU, UltVehiE, V_g, V_e, B);
-	    p_wald=MphCalcP (eval, &X_row2.vector, &X_sub2.matrix, Y, V_g, 
-			     V_e, UltVehiY, beta, Vbeta);
-	    
-	    if (p_wald<p_nr) {
-	      logl_H1=MphNR ('R', nr_iter/10, nr_prec*10, eval, X, Y, Hi_all, 
-			     xHi_all, Hiy_all, V_g, V_e, Hessian, crt_a, 
-			     crt_b, crt_c);
-	      p_wald=MphCalcP (eval, &X_row2.vector, &X_sub2.matrix, Y, V_g, 
-			       V_e, UltVehiY, beta, Vbeta);
-	      
-	      if (crt==1) {
-		p_wald=PCRT (1, d_size, p_wald, crt_a, crt_b, crt_c);
-	      }
-	    }
-	  }
-	  
-	  if (x_mean>1) {gsl_vector_scale(beta, -1.0);}
-
-	  time_opt+=(clock()-time_start)/(double(CLOCKS_PER_SEC)*60.0);
-
-	  // Store summary data.
-	  for (size_t i=0; i<d_size; i++) {
-	    v_beta[i]=gsl_vector_get (beta, i);
-	  }
-	  
-	  c=0;
-	  for (size_t i=0; i<d_size; i++) {
-	    for (size_t j=i; j<d_size; j++) {
-	      v_Vg[c]=gsl_matrix_get (V_g, i, j);
-	      v_Ve[c]=gsl_matrix_get (V_e, i, j);
-	      v_Vbeta[c]=gsl_matrix_get (Vbeta, i, j);
-	      c++;
-	    }
-	  }
-	  
-	  MPHSUMSTAT SNPs={v_beta, p_wald, p_lrt, p_score, 
-			   v_Vg, v_Ve, v_Vbeta};
-	  sumStat.push_back(SNPs);
-	}
-	cout<<endl;
-
-	infile.close();
-	infile.clear();
-
-	gsl_matrix_free(U_hat);
-	gsl_matrix_free(E_hat);
-	gsl_matrix_free(OmegaU);
-	gsl_matrix_free(OmegaE);
-	gsl_matrix_free(UltVehiY);
-	gsl_matrix_free(UltVehiBX);
-	gsl_matrix_free(UltVehiU);
-	gsl_matrix_free(UltVehiE);
-
-	gsl_matrix_free(Hi_all);
-	gsl_matrix_free(Hiy_all);
-	gsl_matrix_free(xHi_all);
-	gsl_matrix_free(Hessian);
-
-	gsl_vector_free(x);
-
-	gsl_matrix_free(Y);
-	gsl_matrix_free(X);
-	gsl_matrix_free(V_g);
-	gsl_matrix_free(V_e);
-	gsl_matrix_free(B);
-	gsl_vector_free(beta);
-	gsl_matrix_free(Vbeta);
-
-	gsl_matrix_free(V_g_null);
-	gsl_matrix_free(V_e_null);
-	gsl_matrix_free(B_null);
-	gsl_matrix_free(se_B_null1);
-	gsl_matrix_free(se_B_null2);
-
-	return;
+void MVLMM::AnalyzePlinkGXE(const gsl_matrix *U, const gsl_vector *eval,
+                            const gsl_matrix *UtW, const gsl_matrix *UtY,
+                            const gsl_vector *env) {
+  string file_bed = file_bfile + ".bed";
+  ifstream infile(file_bed.c_str(), ios::binary);
+  if (!infile) {
+    cout << "error reading bed file:" << file_bed << endl;
+    return;
+  }
+
+  clock_t time_start = clock();
+  time_UtX = 0;
+  time_opt = 0;
+
+  char ch[1];
+  bitset<8> b;
+
+  double logl_H0 = 0.0, logl_H1 = 0.0, p_wald = 0, p_lrt = 0, p_score = 0;
+  double crt_a, crt_b, crt_c;
+  int n_bit, n_miss, ci_total, ci_test;
+  double geno, x_mean;
+  size_t c = 0;
+  size_t n_size = UtY->size1, d_size = UtY->size2, c_size = UtW->size2 + 2;
+  size_t dc_size = d_size * (c_size + 1), v_size = d_size * (d_size + 1) / 2;
+
+  // Large matrices for EM.
+  gsl_matrix *U_hat = gsl_matrix_alloc(d_size, n_size);
+  gsl_matrix *E_hat = gsl_matrix_alloc(d_size, n_size);
+  gsl_matrix *OmegaU = gsl_matrix_alloc(d_size, n_size);
+  gsl_matrix *OmegaE = gsl_matrix_alloc(d_size, n_size);
+  gsl_matrix *UltVehiY = gsl_matrix_alloc(d_size, n_size);
+  gsl_matrix *UltVehiBX = gsl_matrix_alloc(d_size, n_size);
+  gsl_matrix *UltVehiU = gsl_matrix_alloc(d_size, n_size);
+  gsl_matrix *UltVehiE = gsl_matrix_alloc(d_size, n_size);
+
+  // Large matrices for NR.
+  // Each dxd block is H_k^{-1}.
+  gsl_matrix *Hi_all = gsl_matrix_alloc(d_size, d_size * n_size);
+
+  // Each column is H_k^{-1}y_k
+  gsl_matrix *Hiy_all = gsl_matrix_alloc(d_size, n_size);
+
+  // Each dcxdc block is x_k\otimes H_k^{-1}.
+  gsl_matrix *xHi_all = gsl_matrix_alloc(dc_size, d_size * n_size);
+  gsl_matrix *Hessian = gsl_matrix_alloc(v_size * 2, v_size * 2);
+
+  gsl_vector *x = gsl_vector_alloc(n_size);
+
+  gsl_matrix *Y = gsl_matrix_alloc(d_size, n_size);
+  gsl_matrix *X = gsl_matrix_alloc(c_size + 1, n_size);
+  gsl_matrix *V_g = gsl_matrix_alloc(d_size, d_size);
+  gsl_matrix *V_e = gsl_matrix_alloc(d_size, d_size);
+  gsl_matrix *B = gsl_matrix_alloc(d_size, c_size + 1);
+  gsl_vector *beta = gsl_vector_alloc(d_size);
+  gsl_matrix *Vbeta = gsl_matrix_alloc(d_size, d_size);
+
+  // Null estimates for initial values.
+  gsl_matrix *V_g_null = gsl_matrix_alloc(d_size, d_size);
+  gsl_matrix *V_e_null = gsl_matrix_alloc(d_size, d_size);
+  gsl_matrix *B_null = gsl_matrix_alloc(d_size, c_size + 1);
+  gsl_matrix *se_B_null1 = gsl_matrix_alloc(d_size, c_size - 1);
+  gsl_matrix *se_B_null2 = gsl_matrix_alloc(d_size, c_size);
+
+  gsl_matrix_view X_sub1 = gsl_matrix_submatrix(X, 0, 0, c_size - 1, n_size);
+  gsl_matrix_view B_sub1 = gsl_matrix_submatrix(B, 0, 0, d_size, c_size - 1);
+  gsl_matrix_view xHi_all_sub1 = gsl_matrix_submatrix(
+      xHi_all, 0, 0, d_size * (c_size - 1), d_size * n_size);
+
+  gsl_matrix_view X_sub2 = gsl_matrix_submatrix(X, 0, 0, c_size, n_size);
+  gsl_matrix_view B_sub2 = gsl_matrix_submatrix(B, 0, 0, d_size, c_size);
+  gsl_matrix_view xHi_all_sub2 =
+      gsl_matrix_submatrix(xHi_all, 0, 0, d_size * c_size, d_size * n_size);
+
+  gsl_matrix_transpose_memcpy(Y, UtY);
+
+  gsl_matrix_view X_sub0 = gsl_matrix_submatrix(X, 0, 0, c_size - 2, n_size);
+  gsl_matrix_transpose_memcpy(&X_sub0.matrix, UtW);
+  gsl_vector_view X_row0 = gsl_matrix_row(X, c_size - 2);
+  gsl_blas_dgemv(CblasTrans, 1.0, U, env, 0.0, &X_row0.vector);
+
+  gsl_vector_view X_row1 = gsl_matrix_row(X, c_size - 1);
+  gsl_vector_set_zero(&X_row1.vector);
+  gsl_vector_view X_row2 = gsl_matrix_row(X, c_size);
+  gsl_vector_set_zero(&X_row2.vector);
+
+  gsl_vector_view B_col1 = gsl_matrix_column(B, c_size - 1);
+  gsl_vector_set_zero(&B_col1.vector);
+  gsl_vector_view B_col2 = gsl_matrix_column(B, c_size);
+  gsl_vector_set_zero(&B_col2.vector);
+
+  MphInitial(em_iter, em_prec, nr_iter, nr_prec, eval, &X_sub1.matrix, Y, l_min,
+             l_max, n_region, V_g, V_e, &B_sub1.matrix);
+
+  logl_H0 = MphEM('R', em_iter, em_prec, eval, &X_sub1.matrix, Y, U_hat, E_hat,
+                  OmegaU, OmegaE, UltVehiY, UltVehiBX, UltVehiU, UltVehiE, V_g,
+                  V_e, &B_sub1.matrix);
+  logl_H0 = MphNR('R', nr_iter, nr_prec, eval, &X_sub1.matrix, Y, Hi_all,
+                  &xHi_all_sub1.matrix, Hiy_all, V_g, V_e, Hessian, crt_a,
+                  crt_b, crt_c);
+  MphCalcBeta(eval, &X_sub1.matrix, Y, V_g, V_e, UltVehiY, &B_sub1.matrix,
+              se_B_null1);
+
+  c = 0;
+  Vg_remle_null.clear();
+  Ve_remle_null.clear();
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = i; j < d_size; j++) {
+      Vg_remle_null.push_back(gsl_matrix_get(V_g, i, j));
+      Ve_remle_null.push_back(gsl_matrix_get(V_e, i, j));
+      VVg_remle_null.push_back(gsl_matrix_get(Hessian, c, c));
+      VVe_remle_null.push_back(gsl_matrix_get(Hessian, c + v_size, c + v_size));
+      c++;
+    }
+  }
+  beta_remle_null.clear();
+  se_beta_remle_null.clear();
+  for (size_t i = 0; i < se_B_null1->size1; i++) {
+    for (size_t j = 0; j < se_B_null1->size2; j++) {
+      beta_remle_null.push_back(gsl_matrix_get(B, i, j));
+      se_beta_remle_null.push_back(gsl_matrix_get(se_B_null1, i, j));
+    }
+  }
+  logl_remle_H0 = logl_H0;
+
+  cout.setf(std::ios_base::fixed, std::ios_base::floatfield);
+  cout.precision(4);
+  cout << "REMLE estimate for Vg in the null model: " << endl;
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = 0; j <= i; j++) {
+      cout << gsl_matrix_get(V_g, i, j) << "\t";
+    }
+    cout << endl;
+  }
+  cout << "se(Vg): " << endl;
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = 0; j <= i; j++) {
+      c = GetIndex(i, j, d_size);
+      cout << sqrt(gsl_matrix_get(Hessian, c, c)) << "\t";
+    }
+    cout << endl;
+  }
+  cout << "REMLE estimate for Ve in the null model: " << endl;
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = 0; j <= i; j++) {
+      cout << gsl_matrix_get(V_e, i, j) << "\t";
+    }
+    cout << endl;
+  }
+  cout << "se(Ve): " << endl;
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = 0; j <= i; j++) {
+      c = GetIndex(i, j, d_size);
+      cout << sqrt(gsl_matrix_get(Hessian, c + v_size, c + v_size)) << "\t";
+    }
+    cout << endl;
+  }
+  cout << "REMLE likelihood = " << logl_H0 << endl;
+
+  logl_H0 = MphEM('L', em_iter, em_prec, eval, &X_sub1.matrix, Y, U_hat, E_hat,
+                  OmegaU, OmegaE, UltVehiY, UltVehiBX, UltVehiU, UltVehiE, V_g,
+                  V_e, &B_sub1.matrix);
+  logl_H0 = MphNR('L', nr_iter, nr_prec, eval, &X_sub1.matrix, Y, Hi_all,
+                  &xHi_all_sub1.matrix, Hiy_all, V_g, V_e, Hessian, crt_a,
+                  crt_b, crt_c);
+  MphCalcBeta(eval, &X_sub1.matrix, Y, V_g, V_e, UltVehiY, &B_sub1.matrix,
+              se_B_null1);
+
+  c = 0;
+  Vg_mle_null.clear();
+  Ve_mle_null.clear();
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = i; j < d_size; j++) {
+      Vg_mle_null.push_back(gsl_matrix_get(V_g, i, j));
+      Ve_mle_null.push_back(gsl_matrix_get(V_e, i, j));
+      VVg_mle_null.push_back(gsl_matrix_get(Hessian, c, c));
+      VVe_mle_null.push_back(gsl_matrix_get(Hessian, c + v_size, c + v_size));
+      c++;
+    }
+  }
+  beta_mle_null.clear();
+  se_beta_mle_null.clear();
+  for (size_t i = 0; i < se_B_null1->size1; i++) {
+    for (size_t j = 0; j < se_B_null1->size2; j++) {
+      beta_mle_null.push_back(gsl_matrix_get(B, i, j));
+      se_beta_mle_null.push_back(gsl_matrix_get(se_B_null1, i, j));
+    }
+  }
+  logl_mle_H0 = logl_H0;
+
+  cout << "MLE estimate for Vg in the null model: " << endl;
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = 0; j <= i; j++) {
+      cout << gsl_matrix_get(V_g, i, j) << "\t";
+    }
+    cout << endl;
+  }
+  cout << "se(Vg): " << endl;
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = 0; j <= i; j++) {
+      c = GetIndex(i, j, d_size);
+      cout << sqrt(gsl_matrix_get(Hessian, c, c)) << "\t";
+    }
+    cout << endl;
+  }
+  cout << "MLE estimate for Ve in the null model: " << endl;
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = 0; j <= i; j++) {
+      cout << gsl_matrix_get(V_e, i, j) << "\t";
+    }
+    cout << endl;
+  }
+  cout << "se(Ve): " << endl;
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = 0; j <= i; j++) {
+      c = GetIndex(i, j, d_size);
+      cout << sqrt(gsl_matrix_get(Hessian, c + v_size, c + v_size)) << "\t";
+    }
+    cout << endl;
+  }
+  cout << "MLE likelihood = " << logl_H0 << endl;
+
+  vector<double> v_beta, v_Vg, v_Ve, v_Vbeta;
+  for (size_t i = 0; i < d_size; i++) {
+    v_beta.push_back(0.0);
+  }
+  for (size_t i = 0; i < d_size; i++) {
+    for (size_t j = i; j < d_size; j++) {
+      v_Vg.push_back(0.0);
+      v_Ve.push_back(0.0);
+      v_Vbeta.push_back(0.0);
+    }
+  }
+
+  gsl_matrix_memcpy(V_g_null, V_g);
+  gsl_matrix_memcpy(V_e_null, V_e);
+  gsl_matrix_memcpy(B_null, B);
+
+  // Start reading genotypes and analyze.
+  // Calculate n_bit and c, the number of bit for each SNP.
+  if (ni_total % 4 == 0) {
+    n_bit = ni_total / 4;
+  } else {
+    n_bit = ni_total / 4 + 1;
+  }
+
+  // Print the first three magic numbers.
+  for (int i = 0; i < 3; ++i) {
+    infile.read(ch, 1);
+    b = ch[0];
+  }
+
+  for (vector<SNPINFO>::size_type t = 0; t < snpInfo.size(); ++t) {
+    if (t % d_pace == 0 || t == snpInfo.size() - 1) {
+      ProgressBar("Reading SNPs  ", t, snpInfo.size() - 1);
+    }
+    if (indicator_snp[t] == 0) {
+      continue;
+    }
+
+    // n_bit, and 3 is the number of magic numbers.
+    infile.seekg(t * n_bit + 3);
+
+    // Read genotypes.
+    x_mean = 0.0;
+    n_miss = 0;
+    ci_total = 0;
+    ci_test = 0;
+    for (int i = 0; i < n_bit; ++i) {
+      infile.read(ch, 1);
+      b = ch[0];
+
+      // Minor allele homozygous: 2.0; major: 0.0.
+      for (size_t j = 0; j < 4; ++j) {
+
+        if ((i == (n_bit - 1)) && ci_total == (int)ni_total) {
+          break;
+        }
+        if (indicator_idv[ci_total] == 0) {
+          ci_total++;
+          continue;
+        }
+
+        if (b[2 * j] == 0) {
+          if (b[2 * j + 1] == 0) {
+            gsl_vector_set(x, ci_test, 2);
+            x_mean += 2.0;
+          } else {
+            gsl_vector_set(x, ci_test, 1);
+            x_mean += 1.0;
+          }
+        } else {
+          if (b[2 * j + 1] == 1) {
+            gsl_vector_set(x, ci_test, 0);
+          } else {
+            gsl_vector_set(x, ci_test, -9);
+            n_miss++;
+          }
+        }
+
+        ci_total++;
+        ci_test++;
+      }
+    }
+
+    x_mean /= (double)(ni_test - n_miss);
+
+    for (size_t i = 0; i < ni_test; ++i) {
+      geno = gsl_vector_get(x, i);
+      if (geno == -9) {
+        gsl_vector_set(x, i, x_mean);
+        geno = x_mean;
+      }
+      if (x_mean > 1) {
+        gsl_vector_set(x, i, 2 - geno);
+      }
+    }
+
+    // Calculate statistics.
+    time_start = clock();
+    gsl_blas_dgemv(CblasTrans, 1.0, U, x, 0.0, &X_row1.vector);
+    gsl_vector_mul(x, env);
+    gsl_blas_dgemv(CblasTrans, 1.0, U, x, 0.0, &X_row2.vector);
+    time_UtX += (clock() - time_start) / (double(CLOCKS_PER_SEC) * 60.0);
+
+    // Initial values.
+    gsl_matrix_memcpy(V_g, V_g_null);
+    gsl_matrix_memcpy(V_e, V_e_null);
+    gsl_matrix_memcpy(B, B_null);
+
+    if (a_mode == 2 || a_mode == 3 || a_mode == 4) {
+      if (a_mode == 3 || a_mode == 4) {
+        logl_H0 = MphEM('R', em_iter / 10, em_prec * 10, eval, &X_sub2.matrix,
+                        Y, U_hat, E_hat, OmegaU, OmegaE, UltVehiY, UltVehiBX,
+                        UltVehiU, UltVehiE, V_g, V_e, &B_sub2.matrix);
+        logl_H0 = MphNR('R', nr_iter / 10, nr_prec * 10, eval, &X_sub2.matrix,
+                        Y, Hi_all, &xHi_all_sub2.matrix, Hiy_all, V_g, V_e,
+                        Hessian, crt_a, crt_b, crt_c);
+        MphCalcBeta(eval, &X_sub2.matrix, Y, V_g, V_e, UltVehiY, &B_sub2.matrix,
+                    se_B_null2);
+      }
+
+      if (a_mode == 2 || a_mode == 4) {
+        logl_H0 = MphEM('L', em_iter / 10, em_prec * 10, eval, &X_sub2.matrix,
+                        Y, U_hat, E_hat, OmegaU, OmegaE, UltVehiY, UltVehiBX,
+                        UltVehiU, UltVehiE, V_g, V_e, &B_sub2.matrix);
+        logl_H0 = MphNR('L', nr_iter / 10, nr_prec * 10, eval, &X_sub2.matrix,
+                        Y, Hi_all, &xHi_all_sub2.matrix, Hiy_all, V_g, V_e,
+                        Hessian, crt_a, crt_b, crt_c);
+        MphCalcBeta(eval, &X_sub2.matrix, Y, V_g, V_e, UltVehiY, &B_sub2.matrix,
+                    se_B_null2);
+      }
+    }
+
+    time_start = clock();
+
+    // 3 is before 1.
+    if (a_mode == 3 || a_mode == 4) {
+      p_score = MphCalcP(eval, &X_row2.vector, &X_sub2.matrix, Y, V_g_null,
+                         V_e_null, UltVehiY, beta, Vbeta);
+
+      if (p_score < p_nr && crt == 1) {
+        logl_H1 = MphNR('R', 1, nr_prec * 10, eval, X, Y, Hi_all, xHi_all,
+                        Hiy_all, V_g, V_e, Hessian, crt_a, crt_b, crt_c);
+        p_score = PCRT(3, d_size, p_score, crt_a, crt_b, crt_c);
+      }
+    }
+
+    if (a_mode == 2 || a_mode == 4) {
+      logl_H1 = MphEM('L', em_iter / 10, em_prec * 10, eval, X, Y, U_hat, E_hat,
+                      OmegaU, OmegaE, UltVehiY, UltVehiBX, UltVehiU, UltVehiE,
+                      V_g, V_e, B);
+
+      // Calculate beta and Vbeta.
+      p_lrt = MphCalcP(eval, &X_row2.vector, &X_sub2.matrix, Y, V_g, V_e,
+                       UltVehiY, beta, Vbeta);
+      p_lrt = gsl_cdf_chisq_Q(2.0 * (logl_H1 - logl_H0), (double)d_size);
+
+      if (p_lrt < p_nr) {
+        logl_H1 =
+            MphNR('L', nr_iter / 10, nr_prec * 10, eval, X, Y, Hi_all, xHi_all,
+                  Hiy_all, V_g, V_e, Hessian, crt_a, crt_b, crt_c);
+
+        // Calculate beta and Vbeta.
+        p_lrt = MphCalcP(eval, &X_row2.vector, &X_sub2.matrix, Y, V_g, V_e,
+                         UltVehiY, beta, Vbeta);
+        p_lrt = gsl_cdf_chisq_Q(2.0 * (logl_H1 - logl_H0), (double)d_size);
+        if (crt == 1) {
+          p_lrt = PCRT(2, d_size, p_lrt, crt_a, crt_b, crt_c);
+        }
+      }
+    }
+
+    if (a_mode == 1 || a_mode == 4) {
+      logl_H1 = MphEM('R', em_iter / 10, em_prec * 10, eval, X, Y, U_hat, E_hat,
+                      OmegaU, OmegaE, UltVehiY, UltVehiBX, UltVehiU, UltVehiE,
+                      V_g, V_e, B);
+      p_wald = MphCalcP(eval, &X_row2.vector, &X_sub2.matrix, Y, V_g, V_e,
+                        UltVehiY, beta, Vbeta);
+
+      if (p_wald < p_nr) {
+        logl_H1 =
+            MphNR('R', nr_iter / 10, nr_prec * 10, eval, X, Y, Hi_all, xHi_all,
+                  Hiy_all, V_g, V_e, Hessian, crt_a, crt_b, crt_c);
+        p_wald = MphCalcP(eval, &X_row2.vector, &X_sub2.matrix, Y, V_g, V_e,
+                          UltVehiY, beta, Vbeta);
+
+        if (crt == 1) {
+          p_wald = PCRT(1, d_size, p_wald, crt_a, crt_b, crt_c);
+        }
+      }
+    }
+
+    if (x_mean > 1) {
+      gsl_vector_scale(beta, -1.0);
+    }
+
+    time_opt += (clock() - time_start) / (double(CLOCKS_PER_SEC) * 60.0);
+
+    // Store summary data.
+    for (size_t i = 0; i < d_size; i++) {
+      v_beta[i] = gsl_vector_get(beta, i);
+    }
+
+    c = 0;
+    for (size_t i = 0; i < d_size; i++) {
+      for (size_t j = i; j < d_size; j++) {
+        v_Vg[c] = gsl_matrix_get(V_g, i, j);
+        v_Ve[c] = gsl_matrix_get(V_e, i, j);
+        v_Vbeta[c] = gsl_matrix_get(Vbeta, i, j);
+        c++;
+      }
+    }
+
+    MPHSUMSTAT SNPs = {v_beta, p_wald, p_lrt, p_score, v_Vg, v_Ve, v_Vbeta};
+    sumStat.push_back(SNPs);
+  }
+  cout << endl;
+
+  infile.close();
+  infile.clear();
+
+  gsl_matrix_free(U_hat);
+  gsl_matrix_free(E_hat);
+  gsl_matrix_free(OmegaU);
+  gsl_matrix_free(OmegaE);
+  gsl_matrix_free(UltVehiY);
+  gsl_matrix_free(UltVehiBX);
+  gsl_matrix_free(UltVehiU);
+  gsl_matrix_free(UltVehiE);
+
+  gsl_matrix_free(Hi_all);
+  gsl_matrix_free(Hiy_all);
+  gsl_matrix_free(xHi_all);
+  gsl_matrix_free(Hessian);
+
+  gsl_vector_free(x);
+
+  gsl_matrix_free(Y);
+  gsl_matrix_free(X);
+  gsl_matrix_free(V_g);
+  gsl_matrix_free(V_e);
+  gsl_matrix_free(B);
+  gsl_vector_free(beta);
+  gsl_matrix_free(Vbeta);
+
+  gsl_matrix_free(V_g_null);
+  gsl_matrix_free(V_e_null);
+  gsl_matrix_free(B_null);
+  gsl_matrix_free(se_B_null1);
+  gsl_matrix_free(se_B_null2);
+
+  return;
 }