#include "iqlaestimator.h"


IQLAEstimator::IQLAEstimator(vector<Family *> & fam_list, vector<Genotype *> & complete_geno_list, vector<Genotype *> & geno_list, vector<Missing *> & missing_list, map<int, double> & hap_list, int size)
:fam_list(fam_list), complete_geno_list(complete_geno_list), geno_list(geno_list), missing_list(missing_list), hap_freq_table(hap_list), loci_num(size){
                     
  sample_size=0;

  for(int i=0;i<fam_list.size();i++)
    sample_size+=fam_list[i]->N_typed;

  init_freq_table();
  compute_count();
  geno_missing_patten();
  pair_missing_pattern();
  
  flag_pd = 1;
  pvalue = 0;
}


void IQLAEstimator::init_freq_table(){
     
  int tot;
  if(hap_freq_table.size()==0){
    tot = 1<<loci_num;
    for(int i=0;i<tot;i++)
      hap_freq_table[i] = 1.0/double(tot);
  }
  else{
    tot = hap_freq_table.size();
    map<int, double>::iterator iter=hap_freq_table.begin();
    while(iter!=hap_freq_table.end()){
      iter->second = 1.0/double(tot);
      iter++;
    }
  }
  
  geno_freq_table.clear();
  map<int, double>::iterator iter1=hap_freq_table.begin();
  map<int, double>::iterator iter2;
  while(iter1!=hap_freq_table.end()){
    iter2 = iter1;
    while(iter2!=hap_freq_table.end()){
      int h1 = iter1->first;
      int h2 = iter2->first;

      if(h1 == h2)
        geno_freq_table[pair<int, int>(h1,h2)] = iter1->second*iter2->second;
      else
        geno_freq_table[pair<int, int>(h1,h2)] = 2*iter1->second*iter2->second;

      iter2++;
    }
    iter1++;
  }
}


void IQLAEstimator::reset_freq_table(map<int,double> & hap_freq){
     
  hap_freq_table.clear();
  hap_freq_table = hap_freq;
  
  geno_freq_table.clear();
  map<int, double>::iterator iter1=hap_freq_table.begin();
  map<int, double>::iterator iter2;
  while(iter1!=hap_freq_table.end()){
    iter2 = iter1;
    while(iter2!=hap_freq_table.end()){
      int h1 = iter1->first;
      int h2 = iter2->first;

      if(h1 == h2)
        geno_freq_table[pair<int, int>(h1,h2)] = iter1->second*iter2->second;
      else
        geno_freq_table[pair<int, int>(h1,h2)] = 2*iter1->second*iter2->second;

      iter2++;
    }
    iter1++;
  }
}


void IQLAEstimator::show_incom_geno_freq(){
     
  for(int i=0;i<geno_list.size();i++){
    int *gcode = geno_list[i]->get_gcode();
    printf(" ");
    for(int j=0;j<loci_num;j++){
      if(gcode[j]==-1)
	    printf("*");
      else
	    printf("%d", gcode[j]);
      if(j!=loci_num-1)
	    printf("-");
    }

    double freq = 0;
    const vector<pair<int, int> > hv = geno_list[i]->get_hap_vec();
    for(int j=0;j<hv.size();j++)
      freq+=geno_freq_table[hv[j]];
    
    printf(" %6d ", geno_list[i]->get_num());
    printf(" %.4f \n", freq);
  }
  printf("\n");
}
    

void IQLAEstimator::show_hap_freq(){     

  fprintf(outfp, "Tag SNP haplotype frequency estimates in full sample\n");
  fprintf(outfp, "Haplotype\tFrequency\n");
  map<int, double>::iterator iter = hap_freq_table.begin();
  while(iter!=hap_freq_table.end()){
    if(iter->second<0.00005){
      iter++;
      continue;
    }
    translate_hap(outfp,iter->first,loci_num);
    fprintf(outfp, " \t%.4f\n", iter->second);
    iter++;
  }
  fprintf(outfp, "\n");
}


void IQLAEstimator::show_hap_freq(map<int, int> &trans_map, vector<char> &a0v, vector<char> &a1v){
     
  fprintf(outfp, "Tag SNP haplotype frequency estimates in full sample\n");
  fprintf(outfp, "Haplotype\tFrequency\n");
  map<int, double>::iterator iter = hap_freq_table.begin();
  while(iter!=hap_freq_table.end()){
    if(iter->second<0.00005){
      iter++;
      continue;
    }
    translate_hap(outfp,trans_map[iter->first],loci_num,a0v,a1v);
    if(loci_num==2)
      fprintf(outfp, "\t");
    fprintf(outfp, " \t%.4f\n", iter->second);
    iter++;
  }
  fprintf(outfp, "\n");
}


void IQLAEstimator::show_geno_freq(){
     
  map<pair<int, int>, double>::iterator giter = geno_freq_table.begin();
  while(giter!=geno_freq_table.end()){
    if(giter->second<0.00005){
      giter++;
      continue;
    }
    printf("(%2d,%2d) ", (giter->first).first,(giter->first).second);
    printf("   %.4f\n", giter->second);
    giter++;
  }
  printf("\n");
}


void IQLAEstimator::compute_count(){
     
  int row = hap_freq_table.size();
  int col = geno_freq_table.size();

  count = new int*[row-1];
  for(int i=0;i<row-1;i++){
    count[i] = new int[col];
    memset(count[i],0,col*sizeof(int));
  }

  map<int, int> hap_map;
  map<int, double>::iterator iter=hap_freq_table.begin();
  int loc = 0;
  while(iter!=hap_freq_table.end()){
    hap_map[iter->first] = loc;
    loc++;
    iter++;
  }

  loc = 0;
  map<pair<int, int>, double>::iterator giter=geno_freq_table.begin();
  while(giter!=geno_freq_table.end()){
    int h1=(giter->first).first;
    int h2=(giter->first).second;
    int loc1 = hap_map[h1];
    int loc2 = hap_map[h2];

    if(loc1!=row-1)
      count[loc1][loc]++;
    if(loc2!=row-1)
      count[loc2][loc]++;

    loc++;
    giter++;
  }

  hap_map.clear();
}


void IQLAEstimator::geno_missing_patten(){
     
  for(int i=0;i<geno_list.size();i++){
    int *a=geno_list[i]->get_gcode();
    for(int j=0;j<missing_list.size();j++){
      int flag=1;
      int *b=missing_list[j]->get_gcode();
      for(int k=0;k<loci_num;k++){
        if((a[k]==-1 && b[k]!=-1) || (b[k]==-1 && a[k]!=-1)){
          flag = 0;
          break;
        }
      }
      if(flag==0)
        continue;

      geno_missing_map[i] = j;
      break;
    }
  }
  if(geno_missing_map.size()!=geno_list.size()){
    printf("geno_missing_map assignment is wrong. Please check.\n");
    exit(1);
  }
}

  
void IQLAEstimator::show_test()
{
  fprintf(outfp, "ATRIUM test\n");
  fprintf(outfp, "ATRIUM statistic value = %.6f\t pvalue = %g\n\n", test,pvalue);
}


void IQLAEstimator::update_hap_freq(){
     
  map<int, double>::iterator iter = hap_freq_table.begin();
  while(iter!=hap_freq_table.end()){
    iter->second = 0;
    iter++;
  }
    
  map<pair<int, int>, double>::iterator giter = geno_freq_table.begin();
  
  while(giter!= geno_freq_table.end()){
    pair<int, int> conf = giter->first;
    if(conf.first==conf.second)
      hap_freq_table[conf.first] += geno_freq_table[conf];
    else{
      hap_freq_table[conf.first] += .5*geno_freq_table[conf];
      hap_freq_table[conf.second] += .5*geno_freq_table[conf];
    }
    giter++;
  }
}


void IQLAEstimator::update_geno_freq(){
     
  map<pair<int, int>, double>::iterator giter = geno_freq_table.begin();
  while(giter!=geno_freq_table.end()){
    giter->second = 0;
    giter++;
  }

  giter = geno_freq_table.begin();
  while(giter!=geno_freq_table.end()){
    pair<int, int> conf = giter->first;
    int k1 = conf.first;
    int k2 = conf.second;
    if(k1==k2)
      giter->second = hap_freq_table[k1]*hap_freq_table[k2];
    else
      giter->second = 2*hap_freq_table[k1]*hap_freq_table[k2];
    
    giter++;
  }
}


IQLAEstimator::~IQLAEstimator(){
                                
  int row = hap_freq_table.size();

  for(int k=0;k<row-1;k++)
    delete[] count[k];
  delete[] count;

  hap_freq_table.clear();
  geno_freq_table.clear();
  geno_missing_map.clear();
}


// find all possible pair of missing pattern
void IQLAEstimator::pair_missing_pattern(){

  int ind_1, ind_2;
  int dim_1, dim_2;
  int cat_1, cat_2;
  int mis_cat_1, mis_cat_2;
  map<pair<int, int>, double>::iterator iterk;
  map<pair<int, int>, double>::iterator iter1;
  map<pair<int, int>, double>::iterator iter2;
          
  for(int fam=0;fam<fam_list.size();fam++){
    if(fam_list[fam]->N_typed>0){
      for(int i=0;i<fam_list[fam]->N;i++){
        cat_1 = fam_list[fam]->cat[i];
        mis_cat_1 = fam_list[fam]->mis_cat[i];
        dim_1 = missing_list[mis_cat_1]->dim;
        if(dim_1 > 0 && geno_list[cat_1]->info==1){
          ind_1 = fam_list[fam]->fam_member[i][0];

          for(int j=i+1;j<fam_list[fam]->N;j++){
            cat_2 = fam_list[fam]->cat[j];
            mis_cat_2 = fam_list[fam]->mis_cat[j];
            dim_2 = missing_list[mis_cat_2]->dim;
            if(dim_2 > 0 && geno_list[cat_2]->info==1){
              ind_2 = fam_list[fam]->fam_member[j][0];
              iterk = (fam_list[fam]->kinship_coeff).find(pair<int, int>(ind_1,ind_2));
              if(iterk == (fam_list[fam]->kinship_coeff).end()){
                printf("No IBD coefficient between individual %d and individual %d from family %d. Please check...\n\n", ind_1,ind_2,fam_list[fam]->fam_id);
                exit(1);
              }

              if(iterk->second>1e-8){ // i is related to j
                if(pair_missing_v.find(mis_cat_1)==pair_missing_v.end())
                  pair_missing_v[mis_cat_1] = 0;
                if(pair_missing_v.find(mis_cat_2)==pair_missing_v.end())
                  pair_missing_v[mis_cat_2] = 0;
                int min = mis_cat_1<=mis_cat_2 ? mis_cat_1 : mis_cat_2;
                int max = mis_cat_1+mis_cat_2-min;
                if(fam_list[fam]->IBD1_coeff[pair<int, int>(ind_1,ind_2)]>1e-8){
                  if(pair_missing_IBD1.find(pair<int, int>(min,max))==pair_missing_IBD1.end())
                    pair_missing_IBD1[pair<int, int>(min,max)] = 0;
                }
                if(fam_list[fam]->IBD2_coeff[pair<int, int>(ind_1,ind_2)]>1e-8){
                  if(pair_missing_IBD2.find(pair<int, int>(min,max))==pair_missing_IBD2.end())
                    pair_missing_IBD2[pair<int, int>(min,max)] = 0;
                }
              }  // i, j related
            }  // j is typed
          }  // complete j
        }  // i is typed
      }  // complete i
    }
  }
}


void IQLAEstimator::allocate(){

  map<pair<int, int>, double>::iterator giter=geno_freq_table.begin();
  int loc = 0;
  while(giter!=geno_freq_table.end()){
    hap_pair_map[giter->first] = loc;
    loc++;
    giter++;
  }

  int h_size = hap_freq_table.size();
  int col = geno_freq_table.size();

  rst = new double*[h_size-1];
  for(int i=0;i<h_size-1;i++){
    rst[i] = new double[col];
    memset(rst[i],0,col*sizeof(double));
  }  // record derivative for each pair of haplotypes

  freq = new double[h_size];
  memset(freq,0,h_size*sizeof(double));

  int complete_geno = complete_geno_list.size();
  L = new double*[complete_geno];
  for(int i=0;i<complete_geno;i++){
    L[i] = new double[complete_geno];
    memset(L[i],0,complete_geno*sizeof(double));
  }

  int v_size = pair_missing_v.size();
  if(v_size>0){
    V = new double **[v_size];
    map<int, int>::iterator pi=pair_missing_v.begin();
    for(int i=0;i<v_size;i++){
      pi->second = i;
      int mis = pi->first;
      int dim_1 = missing_list[mis]->dim;
      V[i] = new double*[dim_1];
      for(int j=0;j<dim_1;j++){
        V[i][j] = new double[complete_geno];
        memset(V[i][j],0,complete_geno*sizeof(double));
      }
      pi++;
    }
  }  

  int IBD1_size = pair_missing_IBD1.size();
  int IBD2_size = pair_missing_IBD2.size();
  if(IBD1_size>0){
    mat_cov_1 = new double **[IBD1_size];
    map<pair<int, int>, int>::iterator pmi=pair_missing_IBD1.begin();
    for(int i=0;i<IBD1_size;i++){
      pmi->second = i;
      pair<int, int> mispair = pmi->first;
      int dim_1 = missing_list[mispair.first]->dim;
      int dim_2 = missing_list[mispair.second]->dim;
      mat_cov_1[i] = new double*[dim_1];
      for(int j=0;j<dim_1;j++){
        mat_cov_1[i][j] = new double[dim_2];
        memset(mat_cov_1[i][j],0,dim_2*sizeof(double));
      }
      pmi++;
    }  
  }

  if(IBD2_size>0){
    mat_cov_2 = new double **[IBD2_size];
    map<pair<int, int>, int>::iterator pmi=pair_missing_IBD2.begin();
    for(int i=0;i<IBD2_size;i++){
      pmi->second = i;
      pair<int, int> mispair = pmi->first;
      int dim_1 = missing_list[mispair.first]->dim;
      int dim_2 = missing_list[mispair.second]->dim;
      mat_cov_2[i] = new double*[dim_1];
      for(int j=0;j<dim_1;j++){
        mat_cov_2[i][j] = new double[dim_2];
        memset(mat_cov_2[i][j],0,dim_2*sizeof(double));
      }
      pmi++;
    }  
  }

  denominator = new double*[h_size-1];
  for(int i=0;i<h_size-1;i++){
    denominator[i] = new double[h_size-1];
    memset(denominator[i],0,(h_size-1)*sizeof(double));
  }  // F_p^T Omega^(-1) F_p

  numerator = new double[h_size-1];
  memset(numerator,0,(h_size-1)*sizeof(double));  // F_p^T Omega^(-1) (Z-\mu)

  tot_dim = 0;
  for(int fam=0;fam<fam_list.size();fam++){
    if(fam_list[fam]->N_typed>0){
      int cat;
      int mis_cat;
      int dim=0;
      for(int i=0;i<fam_list[fam]->N;i++){
        cat = fam_list[fam]->cat[i];
        if(geno_list[cat]->info==1){
          mis_cat = fam_list[fam]->mis_cat[i];
          dim+=missing_list[mis_cat]->dim;
        }
      }

      if(dim>tot_dim)
        tot_dim = dim;
    }
  }

  cholent = new double*[tot_dim];
  for(int i=0;i<tot_dim;i++){
    cholent[i] = new double[h_size];
    memset(cholent[i],0,h_size*sizeof(double));
  }

  chol = new double*[tot_dim];
  for(int i=0;i<tot_dim;i++){
    chol[i] = new double[tot_dim];
    memset(chol[i],0,tot_dim*sizeof(double));
  }

  cholaug = new double*[tot_dim];
  for(int i=0;i<tot_dim;i++){
    cholaug[i] = new double[h_size];
    memset(cholaug[i],0,h_size*sizeof(double));
  }

  covMatrix = new double*[tot_dim];
  for(int i=0;i<tot_dim;i++){
    covMatrix[i] = new double[tot_dim];
    memset(covMatrix[i],0,tot_dim*sizeof(double));
  }
}


void IQLAEstimator::deallocate(){

  hap_pair_map.clear();

  int h_size = hap_freq_table.size();
  int col = geno_freq_table.size();
  clear_matrix(rst,h_size-1,col);
  delete[] freq;

  int complete_geno = complete_geno_list.size();
  clear_matrix(L,complete_geno,complete_geno);
  int v_size = pair_missing_v.size();
  if(v_size>0){
    map<int, int>::iterator pi=pair_missing_v.begin();
    for(int i=0;i<v_size;i++){
      int dim_1 = missing_list[pi->first]->dim;
      for(int j=0;j<dim_1;j++)
        delete[] V[i][j];
      delete[] V[i];
      pi++;
    }
    delete[] V;
  }  

  for(int i=0;i<tot_dim;i++){
    delete[] cholent[i];
    delete[] chol[i];
    delete[] cholaug[i];
    delete[] covMatrix[i];
  }
  delete[] cholent;
  delete[] chol;
  delete[] cholaug;
  delete[] covMatrix;

  int IBD1_size = pair_missing_IBD1.size();
  int IBD2_size = pair_missing_IBD2.size();
  if(IBD1_size>0){
    map<pair<int, int>, int>::iterator pmi=pair_missing_IBD1.begin();
    for(int i=0;i<IBD1_size;i++){
      pair<int, int> mispair = pmi->first;
      int dim_1 = missing_list[mispair.first]->dim;
      for(int j=0;j<dim_1;j++)
        delete[] mat_cov_1[i][j];
      delete[] mat_cov_1[i];
      pmi++;
    }
    delete[] mat_cov_1;
  }  

  if(IBD2_size>0){
    map<pair<int, int>, int>::iterator pmi=pair_missing_IBD2.begin();
    for(int i=0;i<IBD2_size;i++){
      pair<int, int> mispair = pmi->first;
      int dim_1 = missing_list[mispair.first]->dim;
      for(int j=0;j<dim_1;j++)
        delete[] mat_cov_2[i][j];
      delete[] mat_cov_2[i];
      pmi++;
    }
    delete[] mat_cov_2;
  }

  clear_matrix(denominator,h_size-1,h_size-1);
  delete[] numerator;
}


// a single Newton-Raphson iteration
void IQLAEstimator::NR_Iteration_new(){
     
  int h_size = hap_freq_table.size();
  int col = geno_freq_table.size();

  for(int i=0;i<h_size-1;i++)
    memset(rst[i],0,col*sizeof(double));  // record derivative for each pair of haplotypes

  int loc = 0;
  map<int, double>::iterator iter=hap_freq_table.begin();
  while(iter!=hap_freq_table.end()){
    freq[loc] = iter->second;
    loc++;
    iter++;
  }

  int h_last=0;
  iter = hap_freq_table.end();
  iter--;
  h_last=iter->first;

  map<pair<int, int>, double>::iterator giter=geno_freq_table.begin();
  while(giter!=geno_freq_table.end()){
    pair<int, int> conf = giter->first;
    loc = 0;
    iter = hap_freq_table.begin();
    while(loc<h_size-1){
      int h=iter->first;

      if(conf.first == conf.second && conf.first == h)
        rst[loc][hap_pair_map[conf]] = 2*hap_freq_table[h];
      else if(conf.first == conf.second && conf.first == h_last)
        rst[loc][hap_pair_map[conf]] = -2*hap_freq_table[h_last];
      else if(conf.first < conf.second && conf.first == h && conf.second != h_last)
        rst[loc][hap_pair_map[conf]] = 2*hap_freq_table[conf.second];
      else if(conf.first < conf.second && conf.second == h)
        rst[loc][hap_pair_map[conf]] = 2*hap_freq_table[conf.first];
      else if(conf.first == h && conf.second == h_last)
        rst[loc][hap_pair_map[conf]] = 2*(hap_freq_table[h_last]-hap_freq_table[h]);
      else if(conf.first != h && conf.first != h_last && conf.second == h_last)
        rst[loc][hap_pair_map[conf]] = -2*hap_freq_table[conf.first];

      loc++;
      iter++;
    }
    giter++;
  }

  for(int i=0;i<geno_list.size();i++){
    if(geno_list[i]->get_num()>0 && geno_list[i]->info==1 && missing_list[geno_missing_map[i]]->dim>0)
      geno_list[i]->expected_hap_num(freq,geno_freq_table,hap_pair_map,count,missing_list[geno_missing_map[i]]->sites);
  }

  for(int i=0;i<missing_list.size();i++){
    if(missing_list[i]->get_num()>0 && missing_list[i]->dim>0){
      missing_list[i]->set_weight(geno_freq_table,hap_pair_map,count);
      missing_list[i]->compute_centmatrix(freq,geno_freq_table);
      missing_list[i]->compute_derivative(hap_freq_table,hap_pair_map,count,rst);
    }
  }

  // compute L matrix
  int complete_geno = complete_geno_list.size();
  for(int i=0;i<complete_geno;i++){
    complete_geno_list[i]->expectation(geno_freq_table);
    complete_geno_list[i]->expectation_IBD1(hap_freq_table);
  }

  for(int i=0;i<complete_geno;i++){
    memset(L[i],0,complete_geno*sizeof(double));
    L[i][i] = complete_geno_list[i]->prob_IBD1;
    const vector<pair<int, int> > hv_1 = complete_geno_list[i]->get_hap_vec();
    for(int j=i+1;j<complete_geno;j++){
      const vector<pair<int, int> > hv_2 = complete_geno_list[j]->get_hap_vec();
      for(int s=0;s<hv_1.size();s++)
        for(int t=0;t<hv_2.size();t++)
          L[i][j]+=expfortwo(hv_1[s],hv_2[t],hap_freq_table);
    }
  }

  // construct V
  int v_size = pair_missing_v.size();
  if(v_size>0){
    map<int, int>::iterator pi=pair_missing_v.begin();
    for(int i=0;i<v_size;i++){
      pi->second = i;
      int mis = pi->first;
      int dim_1 = missing_list[mis]->dim;
      for(int j=0;j<dim_1;j++)
        for(int k=0;k<complete_geno;k++)
          V[i][j][k] = missing_list[mis]->weight[j][missing_list[mis]->mapping_function[complete_geno_list[k]->gname]];
      pi++;
    }
  }  

  // compute VLV^T-hh^T, and VDV^T-hh^T for all pair of missing pattern
  int IBD1_size = pair_missing_IBD1.size();
  int IBD2_size = pair_missing_IBD2.size();
  if(IBD1_size>0){
    map<pair<int, int>, int>::iterator pmi=pair_missing_IBD1.begin();
    for(int i=0;i<IBD1_size;i++){
      pmi->second = i;
      pair<int, int> mispair = pmi->first;
      int dim_1 = missing_list[mispair.first]->dim;
      int dim_2 = missing_list[mispair.second]->dim;
      for(int j=0;j<dim_1;j++)
        memset(mat_cov_1[i][j],0,dim_2*sizeof(double));
      pmi++;
    }

    pmi=pair_missing_IBD1.begin();
    for(int i=0;i<IBD1_size;i++){
      pair<int, int> mispair = pmi->first;
      if(mispair.first==mispair.second){
        int dim_1 = missing_list[mispair.first]->dim;
        int it_1 = pair_missing_v[mispair.first];
        for(int j=0;j<dim_1;j++){
          for(int s=0;s<complete_geno;s++){
            mat_cov_1[i][j][j]+=V[it_1][j][s]*V[it_1][j][s]*L[s][s];
            for(int t=s+1;t<complete_geno;t++)
              mat_cov_1[i][j][j]+=2*V[it_1][j][s]*V[it_1][j][t]*L[s][t];
          }
          mat_cov_1[i][j][j]-=freq[missing_list[mispair.first]->sites[j]]*freq[missing_list[mispair.second]->sites[j]];
          for(int k=j+1;k<dim_1;k++){
            for(int s=0;s<complete_geno;s++){
              mat_cov_1[i][j][k]+=V[it_1][j][s]*V[it_1][k][s]*L[s][s];
              for(int t=s+1;t<complete_geno;t++)
                mat_cov_1[i][j][k]+=(V[it_1][j][s]*V[it_1][k][t]+V[it_1][j][t]*V[it_1][k][s])*L[s][t];
            }
            mat_cov_1[i][j][k]-=freq[missing_list[mispair.first]->sites[j]]*freq[missing_list[mispair.second]->sites[k]];
            mat_cov_1[i][k][j] = mat_cov_1[i][j][k];
          }
        }
      }
      else{
        int dim_1 = missing_list[mispair.first]->dim;
        int dim_2 = missing_list[mispair.second]->dim;
        int it_1 = pair_missing_v[mispair.first];
        int it_2 = pair_missing_v[mispair.second];
        for(int j=0;j<dim_1;j++)
          for(int k=0;k<dim_2;k++){
            for(int s=0;s<complete_geno;s++){
              mat_cov_1[i][j][k]+=V[it_1][j][s]*V[it_2][k][s]*L[s][s];
              for(int t=s+1;t<complete_geno;t++)
                mat_cov_1[i][j][k]+=(V[it_1][j][s]*V[it_2][k][t]+V[it_1][j][t]*V[it_2][k][s])*L[s][t];
            }
            mat_cov_1[i][j][k]-=freq[missing_list[mispair.first]->sites[j]]*freq[missing_list[mispair.second]->sites[k]];
          }
      }
      pmi++;
    }  
  }
  
  if(IBD2_size>0){
    map<pair<int, int>, int>::iterator pmi=pair_missing_IBD2.begin();
    for(int i=0;i<IBD2_size;i++){
      pmi->second = i;
      pair<int, int> mispair = pmi->first;
      int dim_1 = missing_list[mispair.first]->dim;
      int dim_2 = missing_list[mispair.second]->dim;
      for(int j=0;j<dim_1;j++)
        memset(mat_cov_2[i][j],0,dim_2*sizeof(double));
      pmi++;
    }

    pmi=pair_missing_IBD2.begin();
    for(int i=0;i<IBD2_size;i++){
      pair<int, int> mispair = pmi->first;
      if(mispair.first==mispair.second){
        int dim_1 = missing_list[mispair.first]->dim;
        int it_1 = pair_missing_v[mispair.first];
        for(int j=0;j<dim_1;j++){
          for(int s=0;s<complete_geno;s++)
            mat_cov_2[i][j][j]+=V[it_1][j][s]*V[it_1][j][s]*complete_geno_list[s]->prob;
          mat_cov_2[i][j][j]-=freq[missing_list[mispair.first]->sites[j]]*freq[missing_list[mispair.second]->sites[j]];
          for(int k=j+1;k<dim_1;k++){
            for(int s=0;s<complete_geno;s++)
              mat_cov_2[i][j][k]+=V[it_1][j][s]*V[it_1][k][s]*complete_geno_list[s]->prob;
            mat_cov_2[i][j][k]-=freq[missing_list[mispair.first]->sites[j]]*freq[missing_list[mispair.second]->sites[k]];
            mat_cov_2[i][k][j] = mat_cov_2[i][j][k];
          }
        }
      }
      else{
        int dim_1 = missing_list[mispair.first]->dim;
        int dim_2 = missing_list[mispair.second]->dim;
        int it_1 = pair_missing_v[mispair.first];
        int it_2 = pair_missing_v[mispair.second];
        for(int j=0;j<dim_1;j++)
          for(int k=0;k<dim_2;k++){
            for(int s=0;s<complete_geno;s++)
              mat_cov_2[i][j][k]+=V[it_1][j][s]*V[it_2][k][s]*complete_geno_list[s]->prob;
            mat_cov_2[i][j][k]-=freq[missing_list[mispair.first]->sites[j]]*freq[missing_list[mispair.second]->sites[k]];
          }
      }
      pmi++;
    }
  }

  for(int i=0;i<h_size-1;i++)
    memset(denominator[i],0,(h_size-1)*sizeof(double));  // F_p^T Omega^(-1) F_p

  memset(numerator,0,(h_size-1)*sizeof(double));  // F_p^T Omega^(-1) (Z-\mu)

  for(int fam=0;fam<fam_list.size();fam++){
    if(fam_list[fam]->N_typed>0){
      int cat;
      int mis_cat;
      int dim=0;
      for(int i=0;i<fam_list[fam]->N;i++){
        cat = fam_list[fam]->cat[i];
        if(geno_list[cat]->info==1){
          mis_cat = fam_list[fam]->mis_cat[i];
          dim+=missing_list[mis_cat]->dim;
        }
      }

      if(dim<=0)
        continue;

      for(int i=0;i<tot_dim;i++){
        memset(cholent[i],0,h_size*sizeof(double));
        memset(chol[i],0,tot_dim*sizeof(double));
        memset(cholaug[i],0,h_size*sizeof(double));
        memset(covMatrix[i],0,tot_dim*sizeof(double));
      }
      
      // populate covariance matrix
      int nb1 = 0;
      int nb2 = 0;
      int ind_1, ind_2;
      int dim_1, dim_2;
      int cat_1, cat_2;
      int mis_cat_1, mis_cat_2;
      double IBD_2, IBD_1;
      map<pair<int, int>, double>::iterator iterk;
      map<pair<int, int>, double>::iterator iter1;
      map<pair<int, int>, double>::iterator iter2;
      double value;

      for(int i=0;i<fam_list[fam]->N;i++){
        cat_1 = fam_list[fam]->cat[i];
        mis_cat_1 = fam_list[fam]->mis_cat[i];
        dim_1 = missing_list[mis_cat_1]->dim;
        if(dim_1 > 0 && geno_list[cat_1]->info==1){
          nb2 = nb1;
          ind_1 = fam_list[fam]->fam_member[i][0];
          ind_2 = ind_1;
          iterk = (fam_list[fam]->kinship_coeff).find(pair<int, int>(ind_1,ind_2));
          if(iterk == (fam_list[fam]->kinship_coeff).end()){
            printf("No IBD coefficient for individual %d from family %d. Please check...\n\n", ind_1,fam_list[fam]->fam_id);
            exit(1);
          }

          for(int j=0;j<dim_1;j++)
            for(int k=0;k<dim_1;k++)
              covMatrix[nb1+j][nb2+k] = missing_list[mis_cat_1]->centmatrix[j][k];

          nb2+=dim_1;
          
          for(int j=i+1;j<fam_list[fam]->N;j++){
            cat_2 = fam_list[fam]->cat[j];
            mis_cat_2 = fam_list[fam]->mis_cat[j];
            dim_2 = missing_list[mis_cat_2]->dim;
            if(dim_2 > 0 && geno_list[cat_2]->info==1){
              ind_2 = fam_list[fam]->fam_member[j][0];
              iterk = (fam_list[fam]->kinship_coeff).find(pair<int, int>(ind_1,ind_2));
              if(iterk == (fam_list[fam]->kinship_coeff).end()){
                printf("No IBD coefficient between individual %d and individual %d from family %d. Please check...\n\n", ind_1,ind_2,fam_list[fam]->fam_id);
                exit(1);
              }

              if(iterk->second>1e-8){ // i is related to j
                iter2 = (fam_list[fam]->IBD2_coeff).find(pair<int, int>(ind_1,ind_2));
                iter1 = (fam_list[fam]->IBD1_coeff).find(pair<int, int>(ind_1,ind_2));
                IBD_2 = iter2->second;
                IBD_1 = iter1->second;
                                
                if(mis_cat_1==mis_cat_2){
                  if(IBD_1>1e-8){
                    int tic = pair_missing_IBD1[pair<int, int>(mis_cat_1,mis_cat_2)];
                    for(int k=0;k<dim_1;k++)
                      for(int l=k;l<dim_2;l++)
                        covMatrix[nb1+k][nb2+l]+=IBD_1*mat_cov_1[tic][k][l];
                  }
                  if(IBD_2>1e-8){
                    int tic = pair_missing_IBD2[pair<int, int>(mis_cat_1,mis_cat_2)];
                    for(int k=0;k<dim_1;k++)
                      for(int l=k;l<dim_2;l++)
                        covMatrix[nb1+k][nb2+l]+=IBD_2*mat_cov_2[tic][k][l];
                  }
                  for(int k=0;k<dim_1;k++)
                    for(int l=k+1;l<dim_2;l++)
                      covMatrix[nb1+l][nb2+k] = covMatrix[nb1+k][nb2+l];
                }
                else if(mis_cat_1<mis_cat_2){
                  if(IBD_1>1e-8){
                    int tic = pair_missing_IBD1[pair<int, int>(mis_cat_1,mis_cat_2)];
                    for(int k=0;k<dim_1;k++)
                      for(int l=0;l<dim_2;l++)
                        covMatrix[nb1+k][nb2+l]+=IBD_1*mat_cov_1[tic][k][l];
                  }
                  if(IBD_2>1e-8){
                    int tic = pair_missing_IBD2[pair<int, int>(mis_cat_1,mis_cat_2)];
                    for(int k=0;k<dim_1;k++)
                      for(int l=0;l<dim_2;l++)
                        covMatrix[nb1+k][nb2+l]+=IBD_2*mat_cov_2[tic][k][l];
                  }
                }
                else if(mis_cat_1>mis_cat_2){
                  if(IBD_1>1e-8){
                    int tic = pair_missing_IBD1[pair<int, int>(mis_cat_2,mis_cat_1)];
                    for(int k=0;k<dim_1;k++)
                      for(int l=0;l<dim_2;l++)
                        covMatrix[nb1+k][nb2+l]+=IBD_1*mat_cov_1[tic][l][k];
                  }
                  if(IBD_2>1e-8){
                    int tic = pair_missing_IBD2[pair<int, int>(mis_cat_2,mis_cat_1)];
                    for(int k=0;k<dim_1;k++)
                      for(int l=0;l<dim_2;l++)
                        covMatrix[nb1+k][nb2+l]+=IBD_2*mat_cov_2[tic][l][k];
                  }
                }
                for(int k=0;k<dim_1;k++)
                  for(int l=0;l<dim_2;l++)
                    covMatrix[nb2+l][nb1+k] = covMatrix[nb1+k][nb2+l];
              }  // i, j related
              nb2+=dim_2;
            }  // j is typed
          }  // complete j
          
          const vector<double> hap_num = geno_list[cat_1]->get_hap_num();
          for(int j=0;j<dim_1;j++){
            for(int k=0;k<h_size-1;k++)
              cholent[nb1+j][k] = missing_list[mis_cat_1]->derivative[j][k];
            cholent[nb1+j][h_size-1] = hap_num[j];
          }
          nb1+=dim_1;
        }  // i is typed
      }  // complete i

      if(cholesky(covMatrix,dim,cholent,h_size,chol,cholaug,0) != 1){
        //printf("cholesky decomposition of the cov matrix failed for family %d. \n", fam_list[fam]->fam_id);
        flag_pd = 0;
        //break;
      }

      if(flag_pd==1){
      double **s = self_colmultiply(cholaug,dim,1,h_size-1,1,h_size-1);
      double **t = self_colmultiply(cholaug,dim,1,h_size-1,h_size,h_size);

      for(int i=0;i<h_size-1;i++)
        for(int j=0;j<h_size-1;j++)
          denominator[i][j]+=s[i][j];

      for(int i=0;i<h_size-1;i++)
        numerator[i]+=t[i][0];

      clear_matrix(s,h_size-1,h_size-1);
      clear_matrix(t,h_size-1,1);
      }
      else{
      double var;
      double &determ = var;
      double **v = inverse(covMatrix,dim,determ);
      double **s = quadratic_product(cholent,dim,h_size,v,dim,dim,cholent,dim,h_size);

      for(int i=0;i<h_size-1;i++)
        for(int j=0;j<h_size-1;j++)
          denominator[i][j]+=s[i][j];

      for(int i=0;i<h_size-1;i++)
        numerator[i]+=s[i][h_size-1];

      clear_matrix(s,h_size,h_size);
      clear_matrix(v,dim,dim);
      }
    }
  }

  double **cholentmain = new double*[h_size-1];
  for(int i=0;i<h_size-1;i++){
    cholentmain[i] = new double[h_size];
    memset(cholentmain[i],0,h_size*sizeof(double));
  }

  double **cholmain = new double*[h_size-1];
  for(int i=0;i<h_size-1;i++){
    cholmain[i] = new double[h_size-1];
    memset(cholmain[i],0,(h_size-1)*sizeof(double));
  }

  double **cholaugmain = new double*[h_size-1];
  for(int i=0;i<h_size-1;i++){
    cholaugmain[i] = new double[h_size];
    memset(cholaugmain[i],0,h_size*sizeof(double));
  }

  for(int i=0;i<h_size-1;i++){
    cholentmain[i][i] = 1;
    cholentmain[i][h_size-1] = numerator[i];
  }

  if(cholesky(denominator,h_size-1,cholentmain,h_size,cholmain,cholaugmain,0) != 1){
    //printf("cholesky decomposition of the denominator matrix failed.\n");
    flag_pd = 0;
    //exit(1);
  }

  if(flag_pd==1){
  double **s = self_colmultiply(cholaugmain,h_size-1,1,h_size-1,h_size,h_size);
  iter=hap_freq_table.begin();
  double n=0;
  for(int i=0;i<h_size-1;i++){
    iter->second+=s[i][0];
    n+=iter->second;
    iter++;
  }
  iter->second = 1-n;

  clear_matrix(s,h_size-1,1);
  }
  else{
  double var;
  double &determ = var;
  double **v = inverse(denominator,h_size-1,determ);
  double **w = new double*[h_size-1];
  for(int i=0;i<h_size-1;i++){
    w[i] = new double[1];
    w[i][0] = numerator[i];
  }
  double **s = multiply(v,h_size-1,h_size-1,w,h_size-1,1);
  iter=hap_freq_table.begin();
  double n=0;
  for(int i=0;i<h_size-1;i++){
    iter->second+=s[i][0];
    n+=iter->second;
    iter++;
  }
  iter->second = 1-n;

  clear_matrix(s,h_size-1,1);
  clear_matrix(v,h_size-1,h_size-1);
  clear_matrix(w,h_size-1,1);
  }
  clear_matrix(cholentmain,h_size-1,h_size);
  clear_matrix(cholmain,h_size-1,h_size-1);
  clear_matrix(cholaugmain,h_size-1,h_size);

  update_geno_freq();
}


void IQLAEstimator::Score_test_new(map<int, double> & hap_weight){
     
  int h_size = hap_freq_table.size();
  int col = geno_freq_table.size();

  for(int i=0;i<h_size-1;i++)
    memset(rst[i],0,col*sizeof(double));  // record derivative for each pair of haplotypes

  int loc = 0;
  map<int, double>::iterator iter=hap_freq_table.begin();
  while(iter!=hap_freq_table.end()){
    freq[loc] = iter->second;
    loc++;
    iter++;
  }

  vector<double> h_weight;
  iter=hap_weight.begin();
  while(iter!=hap_weight.end()){
    h_weight.push_back(iter->second);
    iter++;
  }

  int h_last=0;
  iter = hap_freq_table.end();
  iter--;
  h_last=iter->first;

  map<pair<int, int>, double>::iterator giter=geno_freq_table.begin();
  while(giter!=geno_freq_table.end()){
    pair<int, int> conf = giter->first;
    loc = 0;
    iter = hap_freq_table.begin();
    while(loc<h_size-1){
      int h=iter->first;

      if(conf.first == conf.second && conf.first == h)
        rst[loc][hap_pair_map[conf]] = 2*hap_freq_table[h];
      else if(conf.first == conf.second && conf.first == h_last)
        rst[loc][hap_pair_map[conf]] = -2*hap_freq_table[h_last];
      else if(conf.first < conf.second && conf.first == h && conf.second != h_last)
        rst[loc][hap_pair_map[conf]] = 2*hap_freq_table[conf.second];
      else if(conf.first < conf.second && conf.second == h)
        rst[loc][hap_pair_map[conf]] = 2*hap_freq_table[conf.first];
      else if(conf.first == h && conf.second == h_last)
        rst[loc][hap_pair_map[conf]] = 2*(hap_freq_table[h_last]-hap_freq_table[h]);
      else if(conf.first != h && conf.first != h_last && conf.second == h_last)
        rst[loc][hap_pair_map[conf]] = -2*hap_freq_table[conf.first];

      loc++;
      iter++;
    }
    giter++;
  }

  for(int i=0;i<geno_list.size();i++){
    if(geno_list[i]->get_num()>0 && geno_list[i]->info==1 && missing_list[geno_missing_map[i]]->dim>0)
      geno_list[i]->expected_hap_num(freq,geno_freq_table,hap_pair_map,count,missing_list[geno_missing_map[i]]->sites);
  }

  for(int i=0;i<missing_list.size();i++){
    if(missing_list[i]->get_num()>0 && missing_list[i]->dim>0){
      missing_list[i]->set_weight(geno_freq_table,hap_pair_map,count);
      missing_list[i]->compute_centmatrix(freq,geno_freq_table);
      missing_list[i]->compute_derivative(hap_freq_table,hap_pair_map,count,rst);
      missing_list[i]->compute_deriv_r(freq,geno_freq_table,hap_pair_map,hap_weight,h_weight,count);
    }
  }

  // compute L matrix
  int complete_geno = complete_geno_list.size();
  for(int i=0;i<complete_geno;i++){
    complete_geno_list[i]->expectation(geno_freq_table);
    complete_geno_list[i]->expectation_IBD1(hap_freq_table);
  }

  for(int i=0;i<complete_geno;i++){
    memset(L[i],0,complete_geno*sizeof(double));
    L[i][i] = complete_geno_list[i]->prob_IBD1;
    const vector<pair<int, int> > hv_1 = complete_geno_list[i]->get_hap_vec();
    for(int j=i+1;j<complete_geno;j++){
      const vector<pair<int, int> > hv_2 = complete_geno_list[j]->get_hap_vec();
      for(int s=0;s<hv_1.size();s++)
        for(int t=0;t<hv_2.size();t++)
          L[i][j]+=expfortwo(hv_1[s],hv_2[t],hap_freq_table);
    }
  }

  // construct V
  int v_size = pair_missing_v.size();
  if(v_size>0){
    map<int, int>::iterator pi=pair_missing_v.begin();
    for(int i=0;i<v_size;i++){
      pi->second = i;
      int mis = pi->first;
      int dim_1 = missing_list[mis]->dim;
      for(int j=0;j<dim_1;j++)
        for(int k=0;k<complete_geno;k++)
          V[i][j][k] = missing_list[mis]->weight[j][missing_list[mis]->mapping_function[complete_geno_list[k]->gname]];
      pi++;
    }
  }  

  // compute VLV^T-hh^T, and VDV^T-hh^T for all pair of missing pattern
  int IBD1_size = pair_missing_IBD1.size();
  int IBD2_size = pair_missing_IBD2.size();
  if(IBD1_size>0){
    map<pair<int, int>, int>::iterator pmi=pair_missing_IBD1.begin();
    for(int i=0;i<IBD1_size;i++){
      pmi->second = i;
      pair<int, int> mispair = pmi->first;
      int dim_1 = missing_list[mispair.first]->dim;
      int dim_2 = missing_list[mispair.second]->dim;
      for(int j=0;j<dim_1;j++)
        memset(mat_cov_1[i][j],0,dim_2*sizeof(double));
      pmi++;
    }

    pmi=pair_missing_IBD1.begin();
    for(int i=0;i<IBD1_size;i++){
      pair<int, int> mispair = pmi->first;
      if(mispair.first==mispair.second){
        int dim_1 = missing_list[mispair.first]->dim;
        int it_1 = pair_missing_v[mispair.first];
        for(int j=0;j<dim_1;j++){
          for(int s=0;s<complete_geno;s++){
            mat_cov_1[i][j][j]+=V[it_1][j][s]*V[it_1][j][s]*L[s][s];
            for(int t=s+1;t<complete_geno;t++)
              mat_cov_1[i][j][j]+=2*V[it_1][j][s]*V[it_1][j][t]*L[s][t];
          }
          mat_cov_1[i][j][j]-=freq[missing_list[mispair.first]->sites[j]]*freq[missing_list[mispair.second]->sites[j]];
          for(int k=j+1;k<dim_1;k++){
            for(int s=0;s<complete_geno;s++){
              mat_cov_1[i][j][k]+=V[it_1][j][s]*V[it_1][k][s]*L[s][s];
              for(int t=s+1;t<complete_geno;t++)
                mat_cov_1[i][j][k]+=(V[it_1][j][s]*V[it_1][k][t]+V[it_1][j][t]*V[it_1][k][s])*L[s][t];
            }
            mat_cov_1[i][j][k]-=freq[missing_list[mispair.first]->sites[j]]*freq[missing_list[mispair.second]->sites[k]];
            mat_cov_1[i][k][j] = mat_cov_1[i][j][k];
          }
        }
      }
      else{
        int dim_1 = missing_list[mispair.first]->dim;
        int dim_2 = missing_list[mispair.second]->dim;
        int it_1 = pair_missing_v[mispair.first];
        int it_2 = pair_missing_v[mispair.second];
        for(int j=0;j<dim_1;j++)
          for(int k=0;k<dim_2;k++){
            for(int s=0;s<complete_geno;s++){
              mat_cov_1[i][j][k]+=V[it_1][j][s]*V[it_2][k][s]*L[s][s];
              for(int t=s+1;t<complete_geno;t++)
                mat_cov_1[i][j][k]+=(V[it_1][j][s]*V[it_2][k][t]+V[it_1][j][t]*V[it_2][k][s])*L[s][t];
            }
            mat_cov_1[i][j][k]-=freq[missing_list[mispair.first]->sites[j]]*freq[missing_list[mispair.second]->sites[k]];
          }
      }
      pmi++;
    }  
  }
  
  if(IBD2_size>0){
    map<pair<int, int>, int>::iterator pmi=pair_missing_IBD2.begin();
    for(int i=0;i<IBD2_size;i++){
      pmi->second = i;
      pair<int, int> mispair = pmi->first;
      int dim_1 = missing_list[mispair.first]->dim;
      int dim_2 = missing_list[mispair.second]->dim;
      for(int j=0;j<dim_1;j++)
        memset(mat_cov_2[i][j],0,dim_2*sizeof(double));
      pmi++;
    }

    pmi=pair_missing_IBD2.begin();
    for(int i=0;i<IBD2_size;i++){
      pair<int, int> mispair = pmi->first;
      if(mispair.first==mispair.second){
        int dim_1 = missing_list[mispair.first]->dim;
        int it_1 = pair_missing_v[mispair.first];
        for(int j=0;j<dim_1;j++){
          for(int s=0;s<complete_geno;s++)
            mat_cov_2[i][j][j]+=V[it_1][j][s]*V[it_1][j][s]*complete_geno_list[s]->prob;
          mat_cov_2[i][j][j]-=freq[missing_list[mispair.first]->sites[j]]*freq[missing_list[mispair.second]->sites[j]];
          for(int k=j+1;k<dim_1;k++){
            for(int s=0;s<complete_geno;s++)
              mat_cov_2[i][j][k]+=V[it_1][j][s]*V[it_1][k][s]*complete_geno_list[s]->prob;
            mat_cov_2[i][j][k]-=freq[missing_list[mispair.first]->sites[j]]*freq[missing_list[mispair.second]->sites[k]];
            mat_cov_2[i][k][j] = mat_cov_2[i][j][k];
          }
        }
      }
      else{
        int dim_1 = missing_list[mispair.first]->dim;
        int dim_2 = missing_list[mispair.second]->dim;
        int it_1 = pair_missing_v[mispair.first];
        int it_2 = pair_missing_v[mispair.second];
        for(int j=0;j<dim_1;j++)
          for(int k=0;k<dim_2;k++){
            for(int s=0;s<complete_geno;s++)
              mat_cov_2[i][j][k]+=V[it_1][j][s]*V[it_2][k][s]*complete_geno_list[s]->prob;
            mat_cov_2[i][j][k]-=freq[missing_list[mispair.first]->sites[j]]*freq[missing_list[mispair.second]->sites[k]];
          }
      }
      pmi++;
    }
  }

  for(int i=0;i<h_size-1;i++)
    memset(denominator[i],0,(h_size-1)*sizeof(double));  // F_p^T Omega^(-1) F_p

  double A1 = 0;  // F_r^T Omega^(-1) F_r

  double **A2 = new double*[h_size-1];
  for(int i=0;i<h_size-1;i++){
    A2[i] = new double[1];
    memset(A2[i],0,sizeof(double));
  }  // F_p^T Omega^(-1) F_r

  double numerator_1 = 0;  // F_r^T Omega^(-1) (Z-\mu)

  for(int i=0;i<tot_dim;i++){
    delete[] cholent[i];
    cholent[i] = new double[h_size+1];
    memset(cholent[i],0,(h_size+1)*sizeof(double));
  }

  for(int i=0;i<tot_dim;i++){
    delete[] cholaug[i];
    cholaug[i] = new double[h_size+1];
    memset(cholaug[i],0,(h_size+1)*sizeof(double));
  }

  for(int fam=0;fam<fam_list.size();fam++){
    if(fam_list[fam]->N_typed>0){
      int cat;
      int mis_cat;
      int dim=0;
      for(int i=0;i<fam_list[fam]->N;i++){
        cat = fam_list[fam]->cat[i];
        if(geno_list[cat]->info==1){
          mis_cat = fam_list[fam]->mis_cat[i];
          dim+=missing_list[mis_cat]->dim;
        }
      }

      if(dim<=0)
        continue;

      for(int i=0;i<tot_dim;i++){
        memset(cholent[i],0,(h_size+1)*sizeof(double));
        memset(chol[i],0,tot_dim*sizeof(double));
        memset(cholaug[i],0,(h_size+1)*sizeof(double));
        memset(covMatrix[i],0,tot_dim*sizeof(double));
      }

      // populate covariance matrix
      int nb1 = 0;
      int nb2 = 0;
      int ind_1, ind_2;
      int dim_1, dim_2;
      int cat_1, cat_2;
      int mis_cat_1, mis_cat_2;
      double IBD_2, IBD_1;
      map<pair<int, int>, double>::iterator iterk;
      map<pair<int, int>, double>::iterator iter1;
      map<pair<int, int>, double>::iterator iter2;
      double value;
      double test_weight;

      for(int i=0;i<fam_list[fam]->N;i++){
        cat_1 = fam_list[fam]->cat[i];
        mis_cat_1 = fam_list[fam]->mis_cat[i];
        dim_1 = missing_list[mis_cat_1]->dim;
        if(dim_1 > 0 && geno_list[cat_1]->info==1){
          nb2 = nb1;
          ind_1 = fam_list[fam]->fam_member[i][0];
          ind_2 = ind_1;
          iterk = (fam_list[fam]->kinship_coeff).find(pair<int, int>(ind_1,ind_2));
          if(iterk == (fam_list[fam]->kinship_coeff).end()){
            printf("No IBD coefficient for individual %d from family %d. Please check...\n\n", ind_1,fam_list[fam]->fam_id);
            exit(1);
          }

          for(int j=0;j<dim_1;j++)
            for(int k=0;k<dim_1;k++)
              covMatrix[nb1+j][nb2+k] = missing_list[mis_cat_1]->centmatrix[j][k];

          nb2+=dim_1;

          for(int j=i+1;j<fam_list[fam]->N;j++){
            cat_2 = fam_list[fam]->cat[j];
            mis_cat_2 = fam_list[fam]->mis_cat[j];
            dim_2 = missing_list[mis_cat_2]->dim;
            if(dim_2 > 0 && geno_list[cat_2]->info==1){
              ind_2 = fam_list[fam]->fam_member[j][0];
              iterk = (fam_list[fam]->kinship_coeff).find(pair<int, int>(ind_1,ind_2));
              if(iterk == (fam_list[fam]->kinship_coeff).end()){
                printf("No IBD coefficient between individual %d and individual %d from family %d. Please check...\n\n", ind_1,ind_2,fam_list[fam]->fam_id);
                exit(1);
              }

              if(iterk->second>1e-8){ // i is related to j
                iter2 = (fam_list[fam]->IBD2_coeff).find(pair<int, int>(ind_1,ind_2));
                iter1 = (fam_list[fam]->IBD1_coeff).find(pair<int, int>(ind_1,ind_2));
                IBD_2 = iter2->second;
                IBD_1 = iter1->second;

                if(mis_cat_1==mis_cat_2){
                  if(IBD_1>1e-8){
                    int tic = pair_missing_IBD1[pair<int, int>(mis_cat_1,mis_cat_2)];
                    for(int k=0;k<dim_1;k++)
                      for(int l=k;l<dim_2;l++)
                        covMatrix[nb1+k][nb2+l]+=IBD_1*mat_cov_1[tic][k][l];
                  }
                  if(IBD_2>1e-8){
                    int tic = pair_missing_IBD2[pair<int, int>(mis_cat_1,mis_cat_2)];
                    for(int k=0;k<dim_1;k++)
                      for(int l=k;l<dim_2;l++)
                        covMatrix[nb1+k][nb2+l]+=IBD_2*mat_cov_2[tic][k][l];
                  }
                  for(int k=0;k<dim_1;k++)
                    for(int l=k+1;l<dim_2;l++)
                      covMatrix[nb1+l][nb2+k] = covMatrix[nb1+k][nb2+l];
                }
                else if(mis_cat_1<mis_cat_2){
                  if(IBD_1>1e-8){
                    int tic = pair_missing_IBD1[pair<int, int>(mis_cat_1,mis_cat_2)];
                    for(int k=0;k<dim_1;k++)
                      for(int l=0;l<dim_2;l++)
                        covMatrix[nb1+k][nb2+l]+=IBD_1*mat_cov_1[tic][k][l];
                  }
                  if(IBD_2>1e-8){
                    int tic = pair_missing_IBD2[pair<int, int>(mis_cat_1,mis_cat_2)];
                    for(int k=0;k<dim_1;k++)
                      for(int l=0;l<dim_2;l++)
                        covMatrix[nb1+k][nb2+l]+=IBD_2*mat_cov_2[tic][k][l];
                  }
                }
                else if(mis_cat_1>mis_cat_2){
                  if(IBD_1>1e-8){
                    int tic = pair_missing_IBD1[pair<int, int>(mis_cat_2,mis_cat_1)];
                    for(int k=0;k<dim_1;k++)
                      for(int l=0;l<dim_2;l++)
                        covMatrix[nb1+k][nb2+l]+=IBD_1*mat_cov_1[tic][l][k];
                  }
                  if(IBD_2>1e-8){
                    int tic = pair_missing_IBD2[pair<int, int>(mis_cat_2,mis_cat_1)];
                    for(int k=0;k<dim_1;k++)
                      for(int l=0;l<dim_2;l++)
                        covMatrix[nb1+k][nb2+l]+=IBD_2*mat_cov_2[tic][l][k];
                  }
                }
                for(int k=0;k<dim_1;k++)
                  for(int l=0;l<dim_2;l++)
                    covMatrix[nb2+l][nb1+k] = covMatrix[nb1+k][nb2+l];
              }  // i, j related
              nb2+=dim_2;
            }  // j is typed
          }  // complete j

          const vector<double> hap_num = geno_list[cat_1]->get_hap_num();
          test_weight = fam_list[fam]->test_weight[i];

          for(int j=0;j<dim_1;j++){
            for(int k=0;k<h_size-1;k++)
              cholent[nb1+j][k] = missing_list[mis_cat_1]->derivative[j][k];
            cholent[nb1+j][h_size-1] = hap_num[j];
            cholent[nb1+j][h_size] = test_weight*missing_list[mis_cat_1]->deriv_r[j];
          }
          nb1+=dim_1;
        }  // i is typed
      }  // complete i

      if(cholesky(covMatrix,dim,cholent,h_size+1,chol,cholaug,0) != 1){
        //printf("cholesky decomposition of the cov matrix failed for family %d. \n", fam_list[fam]->fam_id);
        flag_pd = 0;
        //break;
      }

      if(flag_pd==1){
      double **s = self_colmultiply(cholaug,dim,1,h_size-1,1,h_size-1);
      double **t = self_colmultiply(cholaug,dim,h_size+1,h_size+1,h_size+1,h_size+1);
      double **u = self_colmultiply(cholaug,dim,1,h_size-1,h_size+1,h_size+1);
      double **v = self_colmultiply(cholaug,dim,h_size+1,h_size+1,h_size,h_size);

      for(int i=0;i<h_size-1;i++)
        for(int j=0;j<h_size-1;j++)
          denominator[i][j]+=s[i][j];

      A1+=t[0][0];

      for(int i=0;i<h_size-1;i++)
        A2[i][0]+=u[i][0];

      numerator_1+=v[0][0];

      clear_matrix(s,h_size-1,h_size-1);
      clear_matrix(t,1,1);
      clear_matrix(u,h_size-1,1);
      clear_matrix(v,1,1);
      }
      else{
      double var;
      double &determ = var;
      double **v = inverse(covMatrix,dim,determ);
      double **s = quadratic_product(cholent,dim,h_size+1,v,dim,dim,cholent,dim,h_size+1);

      for(int i=0;i<h_size-1;i++)
        for(int j=0;j<h_size-1;j++)
          denominator[i][j]+=s[i][j];

      A1+=s[h_size][h_size];

      for(int i=0;i<h_size-1;i++)
        A2[i][0]+=s[i][h_size];

      numerator_1+=s[h_size][h_size-1];

      clear_matrix(s,h_size+1,h_size+1);
      clear_matrix(v,dim,dim);
      }
    }
  }

  h_weight.clear();

  double **cholmain = new double*[h_size-1];
  for(int i=0;i<h_size-1;i++){
    cholmain[i] = new double[h_size-1];
    memset(cholmain[i],0,(h_size-1)*sizeof(double));
  }

  double **cholaugmain = new double*[h_size-1];
  for(int i=0;i<h_size-1;i++){
    cholaugmain[i] = new double[1];
    memset(cholaugmain[i],0,sizeof(double));
  }

  if(cholesky(denominator,h_size-1,A2,1,cholmain,cholaugmain,0) != 1){
    //printf("cholesky decomposition of the denominator matrix failed.\n");
    flag_pd==0;
    //exit(1);
  }

  if(flag_pd==1){
  double **s = self_colmultiply(cholaugmain,h_size-1,1,1,1,1);

  double orig = A1-s[0][0];
  clear_matrix(s,1,1);
  
  test = numerator_1*numerator_1/orig;
  }
  else{
  double var;
  double &determ = var;
  double **v = inverse(denominator,h_size-1,determ);
  double **s = quadratic_product(A2,h_size-1,1,v,h_size-1,h_size-1,A2,h_size-1,1);

  double orig = A1-s[0][0];
  test = numerator_1*numerator_1/orig;

  clear_matrix(s,1,1);
  clear_matrix(v,h_size-1,h_size-1);
  }
  clear_matrix(cholmain,h_size-1,h_size-1);
  clear_matrix(cholaugmain,h_size-1,1);
  
  pvalue = pochisq(test,1);

  clear_matrix(A2,h_size-1,1);
}