#include "includes.h"
#include "grid.h"
#include "vector.h"

#include "time.h"

#include <fstream.h>
extern ofstream errorfs;

#include "declare.h"


/* This file contains source code for estimating two-locus haplotype
   frequencies jointly using an HMM-- this complication is necessary
   to get "consistent" estimates across overlapping pairs of loci
   when there are missing alleles, ie., alleles not typed */

/* Following this, there is source code for adjustments to these
   frequencies for the following cases:
   1. Allele appears in affected genotypes/haps. but not in controls 
   (assigning freq. of 0 seems to harsh and will lead to loglik of -inf)
   2. Haplotype appears in affecteds (or is compatible with genotypes)
   but either does not appear in controls or the estimate of freq.
   is vanishingly small (once again, seems too harsh a penalty)

   This is not trivial, because changing one joint frequency could
   change freq's at neighboring marker-pairs, etc., if not done correctly.
*/

void makehap2fr_hmm_Gcon(dvectors &h_freq_lists, 
			 const ivectors &a_lists, 
			 const igrid &controls, 
			 const int &debug)
{
  //this routine estimates two-marker haplotype frequencies, for genotype
  //controls, using a HMM

  int l,g,i,iter,conv,loop;
  double diff, loglik=0, loglik_test=0;
  const int numloc=controls.dimy();
  const int numgen=controls.dimx()/2;

  //delete me
  //for (l=0;l<numloc;l++) {
  //cout << l << " " << a_lists[l].dim() << " ";
  //if (l<numloc-1) cout << h_freq_lists[l].dim() << " ";
  //cout << endl; }


  //declare and intialize and clear lists of allele freqs 
  //corresponding to a_lists
  dvectors a_freq_lists; 
  a_freq_lists.init(numloc);
  for (l=0;l<numloc;l++) 
    { a_freq_lists[l].init(a_lists[l].dim()); a_freq_lists[l].clear(0); }
  
  //declare and initialize and clear copy to be used to determine convergence
  dvectors h_freq_lists_copy(h_freq_lists.dim());
  dvectors h_freq_lists_test(h_freq_lists.dim());
  for (l=0;l<numloc-1;l++) 
    { 
      h_freq_lists_copy[l].init(h_freq_lists[l].dim()); 
      h_freq_lists_test[l].init(h_freq_lists[l].dim()); 
      h_freq_lists_copy[l].clear(0); 
      h_freq_lists_test[l].clear(0); 
    }
  
  //declare and initialize and clear hmm objects
  dvectorss b(numgen);
  dvectorss alpha(numgen),beta(numgen),gamma(numgen),xi(numgen);
  for (g=0;g<numgen;g++) {
    b[g].init(numloc);
    alpha[g].init(numloc);
    beta[g].init(numloc);
    gamma[g].init(numloc);
    xi[g].init(numloc-1);
    for (l=0;l<numloc;l++) {
      b[g][l].init(a_lists[l].dim()*a_lists[l].dim()); b[g][l].clear(0);
      alpha[g][l].init(a_lists[l].dim()*a_lists[l].dim()); alpha[g][l].clear(0);
      beta[g][l].init(a_lists[l].dim()*a_lists[l].dim()); beta[g][l].clear(0);
      gamma[g][l].init(a_lists[l].dim()*a_lists[l].dim()); gamma[g][l].clear(0);
      if (l!=numloc-1) {
	xi[g][l].init(a_lists[l].dim()*a_lists[l].dim()*
		      a_lists[l+1].dim()*a_lists[l+1].dim());
	xi[g][l].clear(0); }
    }
  }
  
  dvectors trans(numloc-1);
  for (l=0;l<numloc-1;l++) {
    trans[l].init(a_lists[l].dim()*a_lists[l].dim()*
		  a_lists[l+1].dim()*a_lists[l+1].dim());
    trans[l].clear(0); }
  
  //make observation probabilities [b]
  makeb_Gfr(b, a_lists, controls, debug);

  //assign initial values to parameters
  long seed;
  seed=(-1)*(long)time(NULL);
  double sum;

  //loop over multiple initial starting points
  for (loop=0;loop<10;loop++) {//
    //cout <<"WARNING: Only est. hap. freq. with one set of starting values"
    //<< endl;
    /*
    //assign FIXED initial haplotype frequencies
    cout << "Note: Fixed initial freq's for hap-freq-est. hmm" << endl;
    for (l=0;l<numloc-1;l++) {
      for (i=0;i<h_freq_lists[l].dim();i++) 
	h_freq_lists[l][i]=1.0/h_freq_lists[l].dim(); }
    */
    
    //assign random initial haplotype frequencies
    for (l=0;l<numloc-1;l++) {
      for (i=0;i<h_freq_lists[l].dim();i++) h_freq_lists[l][i]=
					      1.0 * rand()/RAND_MAX;
      sum=0; for (i=0;i<h_freq_lists[l].dim();i++) sum+=h_freq_lists[l][i];
      for (i=0;i<h_freq_lists[l].dim();i++) h_freq_lists[l][i]/=sum; }
    

    //make initial allele freq's
    make_a_freq_lists_Gfr(a_freq_lists, h_freq_lists, debug);
    
    //iterate hmm
    int maxiter=500;
    for (iter=1;iter<=maxiter;iter++) {
      //cout << "iter " << iter << endl;
      
      //copy hapl. freq. to use later in checking convergence
      for (l=0;l<h_freq_lists.dim();l++) h_freq_lists_copy[l]=h_freq_lists[l];
      
      //make trans
      maketrans_Gfr(trans, a_freq_lists, h_freq_lists, debug);
      
      //make alpha
      makealpha_Gfr(alpha, a_freq_lists, trans, b, debug);
      
      //make beta
      makebeta_Gfr(beta, trans, b, debug);
      
      //make xi
      makexi_Gfr(xi, alpha, beta, trans, b, debug);
      
      //update h_freq_lists
      make_h_freq_lists_Gfr(h_freq_lists, a_freq_lists, xi, debug);
      
      //update allele freq's
      make_a_freq_lists_Gfr(a_freq_lists, h_freq_lists, debug);
      
      //check convergence
      conv=0;
      for (l=0;l<numloc-1;l++) {
	for (i=0;i<h_freq_lists[l].dim();i++) {
	  diff=h_freq_lists[l][i]-h_freq_lists_copy[l][i];
	  if ( diff<-1e-10 || 1e-10<diff ) { conv=1; break; } }
	if (conv==1) break; }
      if (conv==0) break;

    }//iter

    //    cout << iter << endl;
    if (iter==maxiter) errorfs << "WARNING: makehap2fr_hmm_Gcon did not converge"
			    << endl;

    //check that two ways of getting a_freq_lists are identical
    check_a_freq_lists(a_freq_lists, h_freq_lists);

    //compute likelihood
    maketrans_Gfr(trans, a_freq_lists, h_freq_lists, debug);
    makealpha_Gfr(alpha, a_freq_lists, trans, b, debug);
    loglik=makelik_Gfr(alpha);
    //cout << "loglik " << loglik << " iter " << iter << endl;

    if (loop==0) {
      loglik_test=loglik;
      for (l=0;l<h_freq_lists.dim();l++) h_freq_lists_test[l]=h_freq_lists[l];
    }
    else {
      if (loglik>loglik_test) {
	loglik_test=loglik;
	for (l=0;l<h_freq_lists.dim();l++) 
	  h_freq_lists_test[l]=h_freq_lists[l];
      }
    }

  }//loop

  //copy h_freq_lists_test back into h_freq_lists
  for (l=0;l<h_freq_lists.dim();l++) h_freq_lists[l]=h_freq_lists_test[l];
  //cout << "highest lik. " << loglik_test << endl;

}

void makeb_Gfr(dvectorss &b, const ivectors &a_lists,
	       const igrid &controls, const int &debug)
{
  //this function makes the observation probabilities-- in this
  //case observed haplotype frequencies-- given the states of the chain

  int i,j,g,l;

  const int numgen=controls.dimx()/2;
  const int numloc=controls.dimy();

  for (g=0;g<numgen;g++) {
    for (l=0;l<numloc;l++) {
      
      if ( controls(2*g  ,l)!=0) {
	if (controls(2*g+1,l)!=0 ) {

	  b[g][l][ 
		  getposinlist( controls(2*g  ,l), a_lists[l] )*
		  a_lists[l].dim()+
		  getposinlist( controls(2*g+1,l), a_lists[l] )]=1.0;
	  
	  b[g][l][ 
		  getposinlist( controls(2*g+1,l), a_lists[l] )*
		  a_lists[l].dim()+
		  getposinlist( controls(2*g  ,l), a_lists[l] )]=1.0;
	  
	}
	else {//controls(2*g+1,l)==0
	  
	  for (j=0;j<a_lists[l].dim();j++) {
	    b[g][l][ 
		    getposinlist( controls(2*g  ,l), a_lists[l] )*
		    a_lists[l].dim()+
		    j]=1.0;
	    
	    b[g][l][ 
		    j*
		    a_lists[l].dim()+
		    getposinlist( controls(2*g  ,l), a_lists[l] )]=1.0;
	  }
	}
      }
      else {//controls(2*g  ,l)==0
	if (controls(2*g+1,l)!=0 ) {

	  for (i=0;i<a_lists[l].dim();i++) {
	    
	    b[g][l][ 
		    i*
		    a_lists[l].dim()+
		    getposinlist( controls(2*g+1,l), a_lists[l] )]=1.0;
	    b[g][l][ 
		    getposinlist( controls(2*g+1,l), a_lists[l] )*
		    a_lists[l].dim()+
		    i]=1.0;
	  }
	}
	else {//controls(2*g+1,l)==0

	  for (i=0;i<a_lists[l].dim();i++) {
	    for (j=0;j<a_lists[l].dim();j++) {

	      b[g][l][ 
		    i*
		    a_lists[l].dim()+
		    j]=1.0;
	    }
	  }
	}
      }
    }
  }

  if (debug) {
    cout << "b [obs. probs.] " << endl;
    for (g=0;g<numgen;g++) {
      for (l=0;l<numloc;l++) {
	for (i=0;i<b[g][l].dim();i++) {
	  
	  cout << b[g][l][i] << " "; }
	cout << endl;
      }
    }
  }
  
}

void make_a_freq_lists_Gfr(dvectors &a_freq_lists, 
			   const dvectors &h_freq_lists,
			   const int &debug)
{
  //this function generates the list of allele frequencies at the loci
  //by summing over the joint frequencies

  int i,j,l;
  const int numloc=a_freq_lists.dim();
  //double diff;

  //check that get same estimates by summing first and second elements of pair
  //dvectors scratch; scratch.init(a_freq_lists.dim());
  //for (l=0;l<numloc;l++) {
  //scratch[l].init(a_freq_lists[l].dim());
  // scratch[l].clear(0); }

  //zero-out lists
  for (l=0;l<numloc;l++) {
    a_freq_lists[l].clear(0); 
  }

  for (l=0;l<numloc-1;l++) {
    for (i=0;i<a_freq_lists[l].dim();i++) {
      for (j=0;j<a_freq_lists[l+1].dim();j++) {

	a_freq_lists[l][i]+=h_freq_lists[l][i*a_freq_lists[l+1].dim()+j];
	if(l==numloc-2) 
	  a_freq_lists[l+1][j]+=h_freq_lists[l][i*a_freq_lists[l+1].dim()+j];

	//scratch[l+1][j]+=h_freq_lists[l][i*a_freq_lists[l+1].dim()+j];
      }
    }
  }

  //check that columns sum to 1.0
  double value;
  for (l=0;l<numloc;l++) {
    value=0.0;
    for (i=0;i<a_freq_lists[l].dim();i++) {
      value+=a_freq_lists[l][i];
    }
    if ( !(0.9999<value && value <1.0001) ) {
      errorfs << "ERROR: all. f.'s of makehap2fr_hmm_Gcon do not sum to 1.0 at locus "
	      << l << "(+1) [" << value << "] " 
	   << endl;
      assert( 0.9999<value && value <1.0001 );
      exit(1); }
  }


  //check that scratch==a_freq_lists
  //for (l=1;l<numloc;l++) {
  //for (i=0;i<a_freq_lists[l].dim();i++) {
  //  diff=scratch[l][i]-a_freq_lists[l][i];
  //  if ( !(-0.0001<diff && diff<0.0001) ) {
  //errorfs << "ERROR in make_a_freq_lists_Gfr; " << endl
  //     << "two estimates for a_freq_lists_Gfr[" << l 
  //     << "][" << i << "] are not equal" << endl;
  //exit(1);
  //  }
  //}
  //}

  if (debug) {
    cout << "a_freq_lists" << endl;
    for (l=0;l<numloc;l++) {
      for (i=0;i<a_freq_lists[l].dim();i++) cout << a_freq_lists[l][i] << " "; 
      cout << endl; 
    }
  }
  
}

void maketrans_Gfr(dvectors &trans, const dvectors &a_freq_lists,
		   const dvectors &h_freq_lists, const int &debug)
{
  //makes transition probabilities for HMM, assuming genotype data

  int g,h,i,j,l;
  const int numloc=a_freq_lists.dim();
  double value;

  //clear trans
  for (l=0;l<numloc-1;l++) {
    trans[l].clear(0); }
  
  for (l=0;l<numloc-1;l++) {

    for (g=0;g<a_freq_lists[l].dim();g++) {
      for (h=0;h<a_freq_lists[l+1].dim();h++) {
	for (i=0;i<a_freq_lists[l].dim();i++) {
	  for (j=0;j<a_freq_lists[l+1].dim();j++) {
	
	    if (a_freq_lists[l][g]!=0.0 && a_freq_lists[l][i]!=0.0)
	      trans[l][
		       (g*a_freq_lists[l  ].dim()+i)*
		       (a_freq_lists[l+1].dim()*a_freq_lists[l+1].dim())+
		       (h*a_freq_lists[l+1].dim()+j)
	      ]=
		(h_freq_lists[l][g*a_freq_lists[l+1].dim()+h]*
		 h_freq_lists[l][i*a_freq_lists[l+1].dim()+j])
		/
		(a_freq_lists[l][g]*a_freq_lists[l][i]);
	  }
	}
      }
    }

  }

  //check that P(omega)=1
  for (l=0;l<numloc-1;l++) {

    for (i=0;i<a_freq_lists[l].dim()*a_freq_lists[l].dim();i++) {
      value=0.0;
      for (j=0;j<a_freq_lists[l+1].dim()*a_freq_lists[l+1].dim();j++) 
	value+=trans[l][i*a_freq_lists[l+1].dim()*a_freq_lists[l+1].dim()+j];
      if ( !( (0.9999<value && value<1.0001) || value==0.0) ) {
	errorfs << "ERROR: in maketrans_Gfr, probs do not sum to 1 ["
	     << value << "] " << endl << "for i-allele "
	     << i << " (+1- denotes position) at column "
	     << l << " (+1) " << endl;
	exit(1); } } }

  if (debug) {
    cout << "trans [in maketrans_Gfr] " << endl;
    for (l=0;l<numloc-1;l++) {
      for (i=0;i<trans[l].dim();i++) {	
	cout << trans[l][i] << " "; }
      cout << endl;
    }
  }

}

	
void makealpha_Gfr(dvectorss &alpha, const dvectors &a_freq_lists,
		   const dvectors &trans, const dvectorss &b,
		   const int &debug)
{
  //makes alpha object, assuming genotype data

  int g,i,j,l;
  const int numgen=alpha.dim();
  const int numloc=alpha[0].dim();

  //clear alpha
  for (g=0;g<numgen;g++) {
    for (l=0;l<numloc;l++) {
      alpha[g][l].clear(0); } }

  for (g=0;g<numgen;g++) {

    //make first column
    for (i=0;i<a_freq_lists[0].dim();i++) {
      for (j=0;j<a_freq_lists[0].dim();j++) {

      if (b[g][0][i*a_freq_lists[0].dim()+j] != 0) 
	alpha[g][0][i*a_freq_lists[0].dim()+j]=
	  a_freq_lists[0][i]*a_freq_lists[0][j]*
	  b[g][0][i*a_freq_lists[0].dim()+j];

      }
    }
    
    //make rest of alpha
    for (l=0;l<numloc-1;l++) {
      
      for (j=0;j<alpha[g][l+1].dim();j++) {
	
	if (b[g][l+1][j] != 0) { 
	  for (i=0;i<alpha[g][l].dim();i++) {
	    
	    if (alpha[g][l][i] != 0) {

	      alpha[g][l+1][j]+=
		alpha[g][l][i]*
		trans[l][i*alpha[g][l+1].dim()+j]*
		b[g][l+1][j];

	    }
	  }
	}
      }

    }
  }

  //check that all entries are between 0 and 1
  for (g=0;g<numgen;g++) {
    for (l=0;l<numloc-1;l++) {
      for (i=0;i<alpha[g][l].dim();i++) {
	if ( !(0.0<=alpha[g][l][i] && alpha[g][l][i]<1.0001) ) {
	  errorfs << "ERROR: alpha[" << g << "][" << l << "][" << i
	       << "] is not between 0 and 1 ["
	       << alpha[g][l][i] << "]" << endl;
	  exit(1); } } } }
  
  if (debug) {
    cout << "alpha [in maketrans_Gfr] " << endl;
    for (g=0;g<numgen;g++) {
      for (l=0;l<numloc;l++) {
	for (i=0;i<alpha[g][l].dim();i++) {	
	  cout << alpha[g][l][i] << " "; }
	cout << endl;
      }
    }
  }
  
}

void makebeta_Gfr(dvectorss &beta, const dvectors &trans, const dvectorss &b,
		  const int &debug)
{
  //makes beta object, assuming genotype controls

  int g,i,j,l;
  const int numgen=beta.dim();
  const int numloc=beta[0].dim();

  //clear beta
  for (g=0;g<numgen;g++) {
    for (l=0;l<numloc;l++) {
      beta[g][l].clear(0); } }

  for (g=0;g<numgen;g++) {

    //make last column
    for (i=0;i<beta[g][numloc-1].dim();i++) {
      
      beta[g][numloc-1][i]=1.0;
      
    }

    //make rest of beta
    for (l=numloc-2;l>=0;l--) {
      
      for (i=0;i<beta[g][l].dim();i++) {

	for (j=0;j<beta[g][l+1].dim();j++) {
	  if (b[g][l+1][j] != 0) { 

	    beta[g][l][i]+=
	      beta[g][l+1][j]*
	      trans[l][i*beta[g][l+1].dim()+j]*
	      b[g][l+1][j];
	  }
	}
      }

    }
  }

  //check that all entries are between 0 and 1
  for (g=0;g<numgen;g++) {
    for (l=0;l<numloc-1;l++) {
      for (i=0;i<beta[g][l].dim();i++) {
	if ( !(0.0<=beta[g][l][i] && beta[g][l][i]<1.0001) ) {
	  errorfs << "ERROR: beta[" << g << "][" << l << "][" << i
	       << "] is not between 0 and 1 ["
	       << beta[g][l][i] << "]" << endl;
	  exit(1); } } } }
  
  if (debug) {
    cout << "beta [in makebeta_Gfr] " << endl;
    for (g=0;g<numgen;g++) {
      for (l=0;l<numloc;l++) {
	for (i=0;i<beta[g][l].dim();i++) {	
	  cout << beta[g][l][i] << " "; }
	cout << endl;
      }
    }
  }
  
}

void makexi_Gfr(dvectorss &xi, const dvectorss &alpha, const dvectorss &beta, 
		const dvectors &trans, const dvectorss &b, 
		const int &debug)
{
  //makes xi object, assuming genotype controls

  int g,i,j,l;
  double denom;
  const int numgen=alpha.dim();
  const int numloc=alpha[0].dim();

  //clear xi
  for (g=0;g<numgen;g++) {
    for (l=0;l<numloc-1;l++) {
      xi[g][l].clear(0); } }

  //make xi
  for (g=0;g<numgen;g++) {

    for (l=0;l<numloc-1;l++) {  
      for (i=0;i<alpha[g][l].dim();i++) {

	for (j=0;j<alpha[g][l+1].dim();j++) {
	
	  if (b[g][l+1][j] != 0)
	    xi[g][l][i*alpha[g][l+1].dim()+j]=
	      alpha[g][l][i]*
	      trans[l][i*alpha[g][l+1].dim()+j]*
	      b[g][l+1][j]*
	      beta[g][l+1][j];

	}
      }
    }
    //make denominator
    denom=0.0;
    for (i=0;i<xi[g][0].dim();i++) denom+=xi[g][0][i];
    if (denom<=0) {
      errorfs << "ERROR: in makexi_Gfr, denominator for xi at genotype " 
	   << g << "(+1) <=0 [" << denom << "] " << endl;
      exit(1); }
    //divide all entries by this number
    for (l=0;l<numloc-1;l++) {  
      for (i=0;i<xi[g][l].dim();i++) { 
	xi[g][l][i]/=denom;
      }
    }

  }

  //check that all entries sum to 1 at each locus for each genotype
  for (g=0;g<numgen;g++) {
    for (l=0;l<numloc-1;l++) {  
      denom=0.0;
      for (i=0;i<xi[g][l].dim();i++) denom+=xi[g][l][i];
      if ( !(0.9999<denom && denom<1.0001) ) {
	errorfs << "ERROR: xi[" << g << "][" << l << "][] does not sum to 1.0 ["
	     << denom << "]" << endl;
      }
    }
  }

  if (debug) {
    cout << "xi [in makexi_Gfr] " << endl;
    for (g=0;g<numgen;g++) {
      for (l=0;l<numloc-1;l++) {
	for (i=0;i<xi[g][l].dim();i++) {	
	  cout << xi[g][l][i] << " "; }
	cout << endl;
      }
    }
  }
  
}

void make_h_freq_lists_Gfr(dvectors &h_freq_lists, dvectors &a_freq_lists,
			   const dvectorss &xi, const int &debug)
{
  //makes haplotype frequencies from xi in HMM, assuming genotype controls

  int gen,g,h,i,j,l;
  double value;

  const int numgen=xi.dim();
  const int numloc=h_freq_lists.dim()+1;

  //clear h_freq_lists
  for (l=0;l<numloc-1;l++) {
    h_freq_lists[l].clear(0); } 

  for (gen=0;gen<numgen;gen++) {
    for (l=0;l<numloc-1;l++) {

      for (g=0;g<a_freq_lists[l].dim();g++) {
	for (h=0;h<a_freq_lists[l+1].dim();h++) {
	  for (i=0;i<a_freq_lists[l].dim();i++) {
	    for (j=0;j<a_freq_lists[l+1].dim();j++) {

	      value=
		xi[gen][l][(g*a_freq_lists[l].dim()+i)*
			  (a_freq_lists[l+1].dim()*a_freq_lists[l+1].dim())+
			  (h*a_freq_lists[l+1].dim()+j) ];

	      h_freq_lists[l][g*a_freq_lists[l+1].dim()+h]+=value;
	      h_freq_lists[l][i*a_freq_lists[l+1].dim()+j]+=value;
	      
	    }
	  }
	}
      }
    }
  }

  //divide by numgen
  for (l=0;l<numloc-1;l++) {
    for (i=0;i<h_freq_lists[l].dim();i++) {
      h_freq_lists[l][i]*=0.5/numgen; } }
  
  //check that frequencies sum to 1 for each pair
  for (l=0;l<numloc-1;l++) {
    value=0.0;
    for (i=0;i<h_freq_lists[l].dim();i++) value+=h_freq_lists[l][i];
    if ( !(0.9999<value && value<1.0001) ) {
      errorfs << "ERROR: h_freq_lists[" << l << "] does not sum to 1 ["
	   << value << "]" << endl;
      exit(1); } }

  if (debug) {
    cout << "h_freq_lists [in makexi_Gfr] " << endl;
    for (l=0;l<numloc-1;l++) {
      for (i=0;i<h_freq_lists[l].dim();i++) {	
	cout << h_freq_lists[l][i] << " "; }
      cout << endl;
    }
  }
  
}

//haplotype controls

void makehap2fr_hmm_Hcon(dvectors &h_freq_lists, 
			 const ivectors &a_lists, 
			 const igrid &controls, 
			 const int &debug)
{

  //estimates two-marker haplotype frequencies, using HMM, assuming
  //haplotype controls

  int l,h,i,iter,conv;
  double diff;
  const int numloc=controls.dimy();
  const int numhap=controls.dimx();

  //make vector indicating haplotypes with missing values
  int is_missing=0;
  ivector missing; missing.init(numhap);
  make_missing_Hfr(missing, is_missing, controls, 0);

  //declare and intialize and clear lists of allele freqs 
  //corresponding to a_lists
  dvectors a_freq_lists; 
  a_freq_lists.init(numloc);
  for (l=0;l<numloc;l++) 
    { a_freq_lists[l].init(a_lists[l].dim()); a_freq_lists[l].clear(0); }
  
  //declare and initialize and clear copy to be used to determine convergence
  dvectors h_freq_lists_copy(h_freq_lists.dim());
  for (l=0;l<numloc-1;l++) 
    { 
      h_freq_lists_copy[l].init(h_freq_lists[l].dim()); 
      h_freq_lists_copy[l].clear(0); 
    }
  
  //declare and initialize and clear hmm objects
  dvectorss b(numhap);
  dvectorss alpha(numhap),beta(numhap),gamma(numhap),xi(numhap);
  for (h=0;h<numhap;h++) {
    if (missing[h]==0) {
      b[h].init(numloc);
      alpha[h].init(numloc);
      beta[h].init(numloc);
      gamma[h].init(numloc);
      for (l=0;l<numloc;l++) {
	b[h][l].init(a_lists[l].dim()); b[h][l].clear(0);
	alpha[h][l].init(a_lists[l].dim()); alpha[h][l].clear(0);
	beta[h][l].init(a_lists[l].dim()); beta[h][l].clear(0);
	gamma[h][l].init(a_lists[l].dim()); gamma[h][l].clear(0);
      }
    }
  }
  //initialize xi vectors for all haps
  for (h=0;h<numhap;h++) {
    xi[h].init(numloc-1);
    for (l=0;l<numloc-1;l++) {
      xi[h][l].init(a_lists[l].dim()*a_lists[l+1].dim());
      xi[h][l].clear(0); }
  }
  
  dvectors trans(numloc-1);
  for (l=0;l<numloc-1;l++) {
    trans[l].init(a_lists[l].dim()*a_lists[l+1].dim());
    trans[l].clear(0); }
  
  //fill in xi for haplotypes with no missing alleles
  makexi_nomissing_Hfr(xi, missing, a_lists, controls, 0);

  //make observation probabilities [b]
  makeb_Hfr(b, missing, a_lists, controls, 0);

  //assign initial values to parameters
  for (l=0;l<numloc-1;l++) {
    for (i=0;i<h_freq_lists[l].dim();i++) 
      h_freq_lists[l][i]=1.0/h_freq_lists[l].dim(); }
  
  //make initial allele freq's
  make_a_freq_lists_Gfr(a_freq_lists, h_freq_lists, 0);

  //iterate hmm
  int maxiter=200;
  for (iter=1;iter<=maxiter;iter++) {

    //copy hapl. freq. to use later in checking convergence
    for (l=0;l<h_freq_lists.dim();l++) h_freq_lists_copy[l]=h_freq_lists[l];

    //make trans
    maketrans_Hfr(trans, a_freq_lists, h_freq_lists, 0);

    //make alpha
    makealpha_Hfr(alpha, missing, a_freq_lists, trans, b, 0);

    //make beta
    makebeta_Hfr(beta, missing, trans, b, 0);

    //make xi
    makexi_Hfr(xi, missing, alpha, beta, trans, b, 0);//delete me

    //update h_freq_lists
    make_h_freq_lists_Hfr(h_freq_lists, xi, 0);
    
    //make allele freq's
    make_a_freq_lists_Gfr(a_freq_lists, h_freq_lists, 0);

    //check convergence
    conv=0;
    for (l=0;l<numloc-1;l++) {
      for (i=0;i<h_freq_lists[l].dim();i++) {
	diff=h_freq_lists[l][i]-h_freq_lists_copy[l][i];
	if ( diff<-1e-10 || 1e-10<diff ) { conv=1; break; } }
      if (conv==1 || is_missing==1) break; }
    if (conv==0) break;

  }//iter

  //check that two ways of getting a_freq_lists are identical
  check_a_freq_lists(a_freq_lists, h_freq_lists);

  if (iter==maxiter) errorfs << "WARNING: makehap2fr_hmm_Gcon did not converge"
			  << endl;

  if (debug) {
    cout << "h_freq_lists [" << iter << " iterations]" << endl;
    for (l=0;l<numloc-1;l++) {
      for (i=0;i<h_freq_lists[l].dim();i++) {	
	cout << h_freq_lists[l][i] << " "; }
      cout << endl;
    }
  }

}

void make_missing_Hfr(ivector &missing, int &is_missing, 
		      const igrid &controls, 
		      const int &debug)
{
  //make vector of indicators, telling if there is at least one missing
  //allele in each control haplotype

  int h,l;
  const int numhap=controls.dimx();
  const int numloc=controls.dimy();

  //set all entries to 1;
  for (h=0;h<numhap;h++) { missing[h]=1; }

  for (h=0;h<numhap;h++) { 
    for (l=0;l<numloc;l++) 
      if ( controls(h, l)==0 ) { missing[h]=0; break; } }

  //find if there is a missing value
  for (h=0;h<missing.dim();h++) { if (missing(h)==0) { is_missing=1; break; }}
  
  if (debug) {
    cout << "missing" << endl;
    for (h=0;h<numhap;h++) cout << missing[h] << " "; 
    cout << endl;
    cout << "is_missing " << is_missing << endl;
  }
  
}

void makexi_nomissing_Hfr(dvectorss &xi, const ivector &missing, 
			  const ivectors &a_lists, 
			  const igrid &controls, const int &debug)
{
  //makes xi, for control haplotypes without a missing allele

  int i,l,h;
  const int numhap=controls.dimx();
  const int numloc=controls.dimy();

  for (h=0;h<numhap;h++) {

    if (missing[h]==1) {
      for (l=0;l<numloc-1;l++) {

	xi[h][l][ 
		 getposinlist( controls(h,l), a_lists[l] )*
		 a_lists[l+1].dim()+
		 getposinlist( controls(h,l+1), a_lists[l+1] )
	]=1.0;
      }
    }
  }

  if (debug) {
    cout << "xi [in makexi_nomissing_Hfr] " << endl;
    for (h=0;h<numhap;h++) {
      if (missing[h]==1) {
	cout << "hap " << h << endl;
	for (l=0;l<numloc-1;l++) {
	  for (i=0;i<xi[h][l].dim();i++) {	
	    cout << xi[h][l][i] << " "; }
	  cout << endl;
	}
      }
    }
  }

}

void makeb_Hfr(dvectorss &b, const ivector &missing, const ivectors &a_lists,
	       const igrid &controls, const int &debug)
{
  //makes observation probabilities, assuming haplotype data, and alleles
  //not missing

  int i,j,h,l;

  const int numhap=controls.dimx();
  const int numloc=controls.dimy();

  for (h=0;h<numhap;h++) {
    if (missing[h]==0) {
      
      for (l=0;l<numloc;l++) {
	
	if ( controls(h,l)!=0) {
	  b[h][l][ getposinlist( controls(h,l), a_lists[l] ) ]=1.0;
	}
	else {//controls(h,l)==0

	  for (j=0;j<a_lists[l].dim();j++) {
	    b[h][l][j]=1.0;
	  }

	}
      }
    }
  }

  if (debug) {
    cout << "b [obs. probs.] " << endl;
    for (h=0;h<numhap;h++) {
      for (l=0;l<numloc;l++) {
	for (i=0;i<b[h][l].dim();i++) {
	  cout << b[h][l][i] << " "; }
	cout << endl;
      }
    }
  }
  
}

void maketrans_Hfr(dvectors &trans, const dvectors &a_freq_lists,
		   const dvectors &h_freq_lists, const int &debug)
{
  //makes transition probabilities for haplotype HMM

  int i,j,l;
  const int numloc=a_freq_lists.dim();
  double value;

  //clear trans
  for (l=0;l<numloc-1;l++) {
    trans[l].clear(0); }
  
  for (l=0;l<numloc-1;l++) {

    for (i=0;i<a_freq_lists[l].dim();i++) {
      for (j=0;j<a_freq_lists[l+1].dim();j++) {
	
	if (a_freq_lists[l][i]!=0.0)
	  trans[l][ i*a_freq_lists[l+1].dim()+j ]=
	    h_freq_lists[l][i*a_freq_lists[l+1].dim()+j]/a_freq_lists[l][i];
      }
    }
  }

  //check that P(omega)=1
  for (l=0;l<numloc-1;l++) {

    for (i=0;i<a_freq_lists[l].dim();i++) {
      value=0.0;
      for (j=0;j<a_freq_lists[l+1].dim();j++) 
	value+=trans[l][i*a_freq_lists[l+1].dim()+j];
      if ( !( (0.9999<value && value<1.0001) || value==0.0) ) {
	errorfs << "ERROR: in maketrans_Hfr, probs do not sum to 1 ["
	     << value << "] " << endl << "for i-allele "
	     << i << " (+1- denotes position) at column "
	     << l << " (+1) " << endl;
	exit(1); } } }

  if (debug) {
    cout << "trans [in maketrans_Hfr] " << endl;
    for (l=0;l<numloc-1;l++) {
      for (i=0;i<trans[l].dim();i++) {	
	cout << trans[l][i] << " "; }
      cout << endl;
    }
  }

}

void makealpha_Hfr(dvectorss &alpha, const ivector &missing,
		   const dvectors &a_freq_lists,
		   const dvectors &trans, const dvectorss &b,
		   const int &debug)
{
  //makes alpha object, assuming haplotype controls

  int h,i,j,l;
  const int numhap=alpha.dim();
  const int numloc=a_freq_lists.dim();

  //clear alpha
  for (h=0;h<numhap;h++) {
    if (missing[h]==0) {
      for (l=0;l<numloc;l++) {
	alpha[h][l].clear(0); } } }

  for (h=0;h<numhap;h++) {

    if (missing[h]==0) {
      //make first column
      for (i=0;i<a_freq_lists[0].dim();i++) {
	
	if (b[h][0][i] != 0) 
	  alpha[h][0][i]=
	    a_freq_lists[0][i]*b[h][0][i];
	
      }
    
    
      //make rest of alpha
      for (l=0;l<numloc-1;l++) {
      
	for (j=0;j<alpha[h][l+1].dim();j++) {
	
	  if (b[h][l+1][j] != 0) { 
	    for (i=0;i<alpha[h][l].dim();i++) {
	    
	      if (alpha[h][l][i] != 0) {
		alpha[h][l+1][j]+=
		alpha[h][l][i]*
		  trans[l][i*alpha[h][l+1].dim()+j]*
		  b[h][l+1][j];

	      }
	    }
	  }
	}
	
      }
    }
  }

  //check that all entries are between 0 and 1
  for (h=0;h<numhap;h++) {
    if (missing[h]==0) 
      for (l=0;l<numloc-1;l++) {
	for (i=0;i<alpha[h][l].dim();i++) {
	  if ( !(0.0<=alpha[h][l][i] && alpha[h][l][i]<1.0001) ) {
	    errorfs << "ERROR: alpha[" << h << "][" << l << "][" << i
		 << "] is not between 0 and 1 ["
		 << alpha[h][l][i] << "]" << endl;
	    exit(1); } } } }
  
  if (debug) {
    cout << "alpha [in maketrans_Hfr] " << endl;
    for (h=0;h<numhap;h++) {
      if (missing[h]==0) {
	for (l=0;l<numloc;l++) {
	  for (i=0;i<alpha[h][l].dim();i++) {	
	    cout << alpha[h][l][i] << " "; }
	  cout << endl;
	}
      }
    }
  }

}

void makebeta_Hfr(dvectorss &beta, const ivector &missing, 
		  const dvectors &trans, const dvectorss &b,
		  const int &debug)
{
  //makes beta object, assuming haplotype controls

  int h,i,j,l;
  const int numhap=beta.dim();
  const int numloc=trans.dim()+1;

  //clear beta
  for (h=0;h<numhap;h++) {
    if (missing[h]==0) {
      for (l=0;l<numloc;l++) {
	beta[h][l].clear(0); } } }

  for (h=0;h<numhap;h++) {

    if (missing[h]==0) {
      //make last column
      for (i=0;i<beta[h][numloc-1].dim();i++) {
	
	beta[h][numloc-1][i]=1.0;
	
      }
      
      //make rest of beta
      for (l=numloc-2;l>=0;l--) {
	
	for (i=0;i<beta[h][l].dim();i++) {
	  
	  for (j=0;j<beta[h][l+1].dim();j++) {
	    if (b[h][l+1][j] != 0) { 
	      
	      beta[h][l][i]+=
		beta[h][l+1][j]*
		trans[l][i*beta[h][l+1].dim()+j]*
		b[h][l+1][j];
	    }
	  }
	}
	
      }
    }
  }

  //check that all entries are between 0 and 1
  for (h=0;h<numhap;h++) {
    if (missing[h]==0)
      for (l=0;l<numloc-1;l++) {
	for (i=0;i<beta[h][l].dim();i++) {
	  if ( !(0.0<=beta[h][l][i] && beta[h][l][i]<1.0001) ) {
	    errorfs << "ERROR: beta[" << h << "][" << l << "][" << i
		 << "] is not between 0 and 1 ["
		 << beta[h][l][i] << "]" << endl;
	    exit(1); } } } }
  
  if (debug) {
    cout << "beta [in makebeta_Hfr] " << endl;
    for (h=0;h<numhap;h++) {
      if (missing[h]==0) {
	for (l=0;l<numloc;l++) {
	  for (i=0;i<beta[h][l].dim();i++) {	
	    cout << beta[h][l][i] << " "; }
	  cout << endl;
	}
      }
    }
  }

}

void makexi_Hfr(dvectorss &xi, const ivector &missing, 
		const dvectorss &alpha, const dvectorss &beta, 
		const dvectors &trans, const dvectorss &b, 
		const int &debug)
{
  //makes xi object assuming haplotype controls

  int h,i,j,l;
  double denom;
  const int numhap=alpha.dim();
  const int numloc=trans.dim()+1;

  //clear xi
  for (h=0;h<numhap;h++) {
    if (missing[h]==0) {
      for (l=0;l<numloc-1;l++) {
	xi[h][l].clear(0); } } }

  //make xi
  for (h=0;h<numhap;h++) {

    if (missing[h]==0) {
      for (l=0;l<numloc-1;l++) {  
	for (i=0;i<alpha[h][l].dim();i++) {

	  for (j=0;j<alpha[h][l+1].dim();j++) {
	
	    if (b[h][l+1][j] != 0)
	      xi[h][l][i*alpha[h][l+1].dim()+j]=
		alpha[h][l][i]*
		trans[l][i*alpha[h][l+1].dim()+j]*
		b[h][l+1][j]*
		beta[h][l+1][j];
	    
	    //delete me
	    //if (h==0 && l==0)
	    //cout << i << " " << j << " "
	    //   << xi[h][l][i*alpha[h][l+1].dim()+j] << " "
	    //   << alpha[h][l][i] << " " 
	    //   << trans[l][i*alpha[h][l+1].dim()+j] << " "
	    //   << b[h][l+1][j] << " "
	    //   << beta[h][l+1][j] << " "
	    //   << endl;

	  }
	}
      }
      //make denominator
      denom=0.0;
      for (i=0;i<xi[h][0].dim();i++) denom+=xi[h][0][i];
      if (denom<=0) {
	errorfs << "ERROR: in makexi_Hfr, denominator for xi at haplotype " 
	     << h << "(+1) <=0 [" << denom << "] " << endl;
	exit(1); }
      //divide all entries by this number
      for (l=0;l<numloc-1;l++) {  
	for (i=0;i<xi[h][l].dim();i++) { 
	  xi[h][l][i]/=denom;
	}
      }

    }
  }

  //check that all entries sum to 1 at each locus for each haplotype
  for (h=0;h<numhap;h++) {
    for (l=0;l<numloc-1;l++) {  
      denom=0.0;
      for (i=0;i<xi[h][l].dim();i++) denom+=xi[h][l][i];
      if ( !(0.9999<denom && denom<1.0001) ) {
	errorfs << "ERROR: xi[" << h << "][" << l << "][] does not sum to 1.0 ["
	     << denom << "]" << endl;
      }
    }
  }

  if (debug) {
    cout << "xi [in makexi_Hfr] " << endl;
    for (h=0;h<numhap;h++) {
      for (l=0;l<numloc-1;l++) {
	for (i=0;i<xi[h][l].dim();i++) {	
	  cout << xi[h][l][i] << " "; }
	cout << endl;
      }
    }
  }
  
}

void make_h_freq_lists_Hfr(dvectors &h_freq_lists, 
			   const dvectorss &xi, const int &debug)
{
  //makes haplotype frequencies from xi, assuming haplotype controls

  int h,i,l;
  double value;

  const int numhap=xi.dim();
  const int numloc=h_freq_lists.dim()+1;

  //clear h_freq_lists
  for (l=0;l<numloc-1;l++) {
    h_freq_lists[l].clear(0); } 

  for (h=0;h<numhap;h++) {
    for (l=0;l<numloc-1;l++) {
      
      for (i=0;i<h_freq_lists[l].dim();i++) {

	  h_freq_lists[l][i]+=xi[h][l][i];
	      
      }
    }
  }

  //divide by numhap
  for (l=0;l<numloc-1;l++) {
    for (i=0;i<h_freq_lists[l].dim();i++) {
      h_freq_lists[l][i]/=numhap; } }
  
  //check that frequencies sum to 1 for each pair
  for (l=0;l<numloc-1;l++) {
    value=0.0;
    for (i=0;i<h_freq_lists[l].dim();i++) value+=h_freq_lists[l][i];
    if ( !(0.9999<value && value<1.0001) ) {
      errorfs << "ERROR: h_freq_lists[" << l << "] does not sum to 1 ["
	   << value << "]" << endl;
      exit(1); } }

  if (debug) {
    cout << "h_freq_lists [in makexi_Hfr] " << endl;
    for (l=0;l<numloc-1;l++) {
      for (i=0;i<h_freq_lists[l].dim();i++) {	
	cout << h_freq_lists[l][i] << " "; }
      cout << endl;
    }
  }
  
}

//adjustment functions
void make_adj_to_hfreqlists(dvectors &h_freq_lists_adj, 
			    const dvectors &h_freq_lists, 
			    ivectors &a_lists, const double &e,
			    const ivector &additionalalleles,
			    const int &hap_or_freq,
			    const igrid &controls,
			    const int &debug)
{

  //this function adjust for joint frequencies with estimated freq. of 0

  int i,j,l;
  const int numloc=controls.dimy();
  double sum;

  //initialize h_freq_lists_adj
  for (l=0;l<h_freq_lists_adj.dim();l++) {
    h_freq_lists_adj[l].init(a_lists[l].dim()*a_lists[l+1].dim());
    h_freq_lists_adj[l].clear(0); }

  //delete me
  //for (l=0;l<numloc;l++) {
  //cout << l << " " << a_lists[l].dim() << " ";
  //if (l<numloc-1) cout << h_freq_lists[l].dim() << " "
  //		 << h_freq_lists_adj[l].dim() << " ";
  //cout << endl; }



  //copy h_freq_lists into h_freq_lists_adj
  for (l=0;l<h_freq_lists_adj.dim();l++) {
    h_freq_lists_adj[l]=h_freq_lists[l]; }

  //for (i=0;i<a_lists[l].dim()-additionalalleles[l];i++) {
  //  for (j=0;j<a_lists[l+1].dim()-additionalalleles[l+1];j++) {
  //h_freq_lists_adj[l][i*a_lists[l+1].dim()+j]=
  //  h_freq_lists[l][i*(a_lists[l+1].dim()-
  //		     additionalalleles[l+1])+j]; } } }

  //translate into hmm objects

  //declare and initialize objects
  dvector init(a_lists[0].dim()); init.clear(0);
  dvectors trans(numloc-1);
  for (l=0;l<numloc-1;l++) {
    trans[l].init(a_lists[l].dim()*a_lists[l+1].dim());
    trans[l].clear(0); }

  //declare and intialize and clear lists of allele freqs 
  //corresponding to a_lists
  dvectors a_freq_lists; 
  a_freq_lists.init(numloc);
  for (l=0;l<numloc;l++) 
    { a_freq_lists[l].init(a_lists[l].dim()); a_freq_lists[l].clear(0); }

  //fill in objects
  make_a_freq_lists_Gfr(a_freq_lists, h_freq_lists_adj, debug);
  for (i=0;i<a_freq_lists[0].dim();i++) init(i)=a_freq_lists[0][i];
  //cout << a_freq_lists[0].dim() << " " << a_freq_lists[1].dim() << endl;
  maketrans_Hfr(trans, a_freq_lists, h_freq_lists_adj, debug);

  //make adjustments
  
  //first adjust init (for alleles that do not appear in controls)
  adjust_init(init,e,additionalalleles, controls,0);
  //cout << "init ";  for (i=0;i<init.dim();i++) cout << init(i) << " "; cout << endl;

  //now make adjustments to trans, marker by marker
  //for (i=0;i<trans[0].dim();i++) cout << trans[0][i] << " "; cout << endl;
  double zero=1e-4;
  adjust_trans(trans, init, controls,e, zero, a_lists, a_freq_lists,
	       additionalalleles,hap_or_freq,debug);
  //for (i=0;i<trans[numloc-2].dim();i++) 
  //cout << trans[numloc-2][i] << " "; cout << endl;

  //make back into h_freq_lists

  //do final update of a_freq_lists
  make_a_freq_lists_fr_trans(a_freq_lists, init,trans, numloc-1, debug);

  for (l=0;l<h_freq_lists_adj.dim();l++) {
    for (j=0;j<a_freq_lists[l+1].dim();j++) {
      for (i=0;i<a_freq_lists[l].dim();i++)
	h_freq_lists_adj[l][i*a_freq_lists[l+1].dim()+j]=
	  trans[l][i*a_freq_lists[l+1].dim()+j]*a_freq_lists[l][i]; } }

  //check that freqs sum to 1.0 for each vector
  for (l=0;l<h_freq_lists_adj.dim();l++) {
    sum=0;
    for (i=0;i<h_freq_lists_adj[l].dim();i++) sum+=h_freq_lists_adj[l][i];
    if ( !(0.999<sum && sum<1.001) ) {
      errorfs << "ERROR: col. " << l << " in h_freq_list [make_adj_to_hfreqlists]"
	   << "\n does not sum to 1.0 [" << sum << "] " << endl;
      exit(1); } }

  //check that two ways of getting a_freq_lists are identical
  check_a_freq_lists(a_freq_lists, h_freq_lists_adj);

  if (debug) {
    cout << "h_freq_list [make_adj_to_hfreqlists]" << endl;
    for (l=0;l<h_freq_lists_adj.dim();l++) {
      for (i=0;i<h_freq_lists_adj[l].dim();i++) 
	cout << h_freq_lists_adj[l][i] << " ";
      cout << endl; } }
    
}

void adjust_init(dvector &init, const double &e, 
		 const ivector &additionalalleles,
		 const igrid &controls,
		 const int &debug)
{
  //makes object with allele frequencies at first marker, in adjustment
  //of frequencies estimates

  int i;
  double sum;

  //count non-missing alleles at first locus
  int non0=0;
  for (i=0;i<controls.dimx();i++) if (controls(i,0)!=0) non0+=1;
  if (non0==0) non0=1;

  //adjust entries
  double entry;
  if (additionalalleles[0]>0) {
    entry=e/(additionalalleles[0]*non0);
    for (i=0;i<init.dim();i++) {
      if (init(i)==0.0) init(i)=entry;
      else init(i)*=(1-additionalalleles[0]*entry); }
  }

  //check sum to 1
  sum=0;
  for (i=0;i<init.dim();i++) sum+=init(i);
  if ( !(0.999<sum && sum <1.001) ) {
    errorfs << "ERROR: init() in adjust_init() does not sum to 1 [" << sum << "]"
	 << endl;
    exit(1);
  }

  if (debug) {
    cout << "init [in adjust_init()]" << endl;
    for (i=0;i<init.dim();i++) cout << init(i) << " "; cout << endl;
  }

}
    
void adjust_trans(dvectors &trans, const dvector &init, const igrid &controls,
		  const double &e, const double &zero,
		  const ivectors &a_lists, dvectors &a_freq_lists,
		  const ivector &additionalalleles,
		  const int &hap_or_freq,
		  const int &debug)
{
  //adjust conditional allele frequencies in case some estimates are 0

  int h,i,j,l;
  const int numloc=controls.dimy();
  const int numhap=controls.dimx();
  const int numgen=controls.dimx()/2;
  int k_i_lp1;
  double num_i,value,sum,denom;

  for (l=0;l<numloc-1;l++) {

    //allele i at l that appeared in controls
    for (i=0;i<a_lists[l].dim()-additionalalleles[l];i++) {
      
      //count j at l+1 for which P(j|i)=0
      k_i_lp1=0;
      for (j=0;j<a_lists[l+1].dim();j++) {
	if (trans[l][i*a_lists[l+1].dim()+j]<=zero) k_i_lp1+=1; }

      //count number of times i appeared with non-missing allele
      num_i=0;
      if (hap_or_freq==1) {
	for (h=0;h<numhap;h++) {
	  if (controls(h,l)==a_lists[l][i] && controls(h,l+1)!=0) num_i+=1.0; 
	} }
      else {
	for (h=0;h<numgen;h++) {
	  if( controls(2*h  ,l)==a_lists[l][i] && 
	      controls(2*h+1,l)==a_lists[l][i] ) {
	    
	    if(controls(2*h  ,l+1) != 0 && controls(2*h+1,l+1) !=0)
	      num_i+=2.0;
	    else {
	      if (controls(2*h  ,l+1) != 0 || controls(2*h+1,l+1) !=0)
		num_i+=1.0; 
	      //note: used to be +=2.0. Jian noticed bug 10/10/00
	    } }

	  else {
	    if (controls(2*h  ,l)==a_lists[l][i] || 
		controls(2*h+1,l)==a_lists[l][i] ) {
	      
	      if(controls(2*h  ,l+1) != 0 && controls(2*h+1,l+1) !=0)
		num_i+=1.0;
	      else {
		if (controls(2*h  ,l+1) != 0 || controls(2*h+1,l+1) !=0)
		  num_i+=0.5; } } }
	}
      }

      //if (num_i==0) { 
      //cout << i << " " << a_lists[l][i] << endl;
      //for (j=0;j<a_lists[l+1].dim();j++) 
      //  cout << trans[l][i*a_lists[l+1].dim()+j] << " ";
      //cout << endl; }

      if (num_i==0) num_i=1;//don't divide by 0

      if (k_i_lp1>0) {    
	//cout << l << " " << num_i << " " << e/(k_i_lp1*num_i) << " ";
	for (j=0;j<a_lists[l+1].dim();j++) {
	  //cout << trans[l][i*a_lists[l+1].dim()+j] << " ";
	  if (trans[l][i*a_lists[l+1].dim()+j]<=zero) 
	    trans[l][i*a_lists[l+1].dim()+j]=e/(k_i_lp1*num_i);
	  else {
	    trans[l][i*a_lists[l+1].dim()+j]*=(1-e/num_i); }
	  //cout << trans[l][i*a_lists[l+1].dim()+j] << " "; 
	}
	//cout << endl << endl;
      }
    }
  
    //update marginal frequencies of alleles up to locus 
    make_a_freq_lists_fr_trans(a_freq_lists, init,trans, l, 0);
    //if (l==2) exit(1);
    
    //allele i at l that did NOT appear in controls
    denom=0.0;
    for (i=0;i<a_lists[l].dim()-additionalalleles[l];i++) 
      denom+=a_freq_lists[l][i];
    
    for (j=0;j<a_lists[l+1].dim();j++) {
      sum=0;
      for (i=0;i<a_lists[l].dim()-additionalalleles[l];i++) 
	sum+=a_freq_lists[l][i]*trans[l][i*a_lists[l+1].dim()+j];
      
      for (i=a_lists[l].dim()-additionalalleles[l];i<a_lists[l].dim();i++) {
	trans[l][i*a_lists[l+1].dim()+j]=sum/denom; }
    }
  }

  //check that all entries are between 0 and 1
  for (l=0;l<numloc-1;l++) {
    for (i=0;i<trans[l].dim();i++) {
      if ( !(0.0<=trans[l][i] && trans[l][i]<1.001) ) {
	errorfs << "ERROR: entry not between 0 and 1 [" << trans[l][i] 
	     << "] in adjust_trans()" << endl;
	exit(1); } } }

  //check that sum to 1 for each i
  for (l=0;l<numloc-1;l++) {

    for (i=0;i<a_freq_lists[l].dim();i++) {
      value=0.0;
      for (j=0;j<a_freq_lists[l+1].dim();j++) 
	value+=trans[l][i*a_freq_lists[l+1].dim()+j];
      //cout << l << " " << i << " " << value << endl;
      if ( !( (0.999<value && value<1.001) || value==0.0) ) {
	errorfs << "ERROR: in adjust_trans, probs do not sum to 1 ["
	     << value << "] " << endl << "for i-allele "
	     << i << " (+1- denotes position) at column "
	     << l << " (+1) " << endl;
	exit(1); } } }

  if (debug) {
    cout << "trans [in make_adj_to_hfreqlists()]" << endl;
    for (l=0;l<numloc-1;l++) {
      for (i=0;i<trans[l].dim();i++) cout << trans[l][i] << " ";
      cout << endl; }
  }

}
    
void make_a_freq_lists_fr_trans(dvectors &a_freq_lists, const dvector &init,
				const dvectors &trans, const int &lastloc,
				const int &debug)
{
  //makes allele frequencies up to some locus, from conditional allele
  //frequencies-- needed to update frequencies objects after adjustments
  //made to each locus

  int i,j,l;
  const int numloc=a_freq_lists.dim();
  double sum;

  //clear a_freq_lists
  for (l=0;l<numloc;l++) a_freq_lists[l].clear(0);

  //fill in first column
  for (i=0;i<a_freq_lists[0].dim();i++) a_freq_lists[0][i]=init(i);
  //fill in rest

  for (l=0;l<lastloc;l++) {

    for (j=0;j<a_freq_lists[l+1].dim();j++) {

      for (i=0;i<a_freq_lists[l].dim();i++) {
	a_freq_lists[l+1][j]+=
	  a_freq_lists[l][i]*
	  trans[l][i*a_freq_lists[l+1].dim()+j];
      }
    }
  }

  if (debug) {
    cout << "a_freq_lists [in make_adj_to_hfreqlists()]" << endl;
    for (l=0;l<numloc;l++) {
      for (i=0;i<a_freq_lists[l].dim();i++) cout << a_freq_lists[l][i] << " ";
      cout << endl; }
  }

  //check that each vector sums to 1.0
  for (l=0;l<lastloc+1;l++) {
    sum=0;
    for (i=0;i<a_freq_lists[l].dim();i++) sum+=a_freq_lists[l][i];
    assert(0.999<sum && sum<1.001);
    if ( !(0.999<sum && sum<1.001) ) {
      errorfs << "ERROR: col. " << l << " in make_a_freq_lists_fr_trans \n" 
	   << "does not sum to 1.0 [" << sum << "] " << endl;
      exit(1);
    }
  }


}
	
double makelik_Gfr(const dvectorss &alpha)
{
  //computes likelihood of set of controls, given genotype data

  int i, g;
  double logsum;
  const int numgen=alpha.dim();
  const int numloc=alpha[0].dim();
  
  dvector lik(numgen); lik.clear(0);
  for (g=0;g<numgen;g++) {
    for (i=0;i<alpha[g][numloc-1].dim();i++) lik[g]+=alpha[g][numloc-1][i];
    lik[g]=log(lik[g]); }

  logsum=0; for (g=0;g<numgen;g++) logsum+=lik[g];

  if ( !(logsum<0) ) {
    errorfs << "ERROR: logsum in makelik_Gfr() is !<0 [" << logsum 
	 << "]" <<endl;
    exit(1);
  }
  return logsum;
}

double make_null_lik_Gfr(igrid &controls, const ivectors &a_lists,
			 const dvectors &h_freq_lists)
{
  //another way to compute likelihood of controls assuming Markov(1)
  //model

  int g,l;
  const int numgen=controls.dimx()/2;
  const int numloc=controls.dimy();
  const int debug=0;

  //generate likelihood
  dvectors a_freq_lists; 
  a_freq_lists.init(numloc);
  for (l=0;l<numloc;l++) 
    { a_freq_lists[l].init(a_lists[l].dim()); a_freq_lists[l].clear(0); }
  make_a_freq_lists_Gfr(a_freq_lists, h_freq_lists, debug);
  dvectors trans(numloc-1);
  for (l=0;l<numloc-1;l++) {
    trans[l].init(a_lists[l].dim()*a_lists[l].dim()*
		  a_lists[l+1].dim()*a_lists[l+1].dim());
    trans[l].clear(0); }
  maketrans_Gfr(trans, a_freq_lists, h_freq_lists, debug);
  dvectorss b(numgen);
  dvectorss alpha(numgen);
  for (g=0;g<numgen;g++) {
    b[g].init(numloc);
    alpha[g].init(numloc);
    for (l=0;l<numloc;l++) {
      b[g][l].init(a_lists[l].dim()*a_lists[l].dim()); b[g][l].clear(0);
      alpha[g][l].init(a_lists[l].dim()*a_lists[l].dim()); 
      alpha[g][l].clear(0); } }
  makeb_Gfr(b, a_lists, controls, debug);

  makealpha_Gfr(alpha, a_freq_lists, trans, b, debug);

  int i;
  double sum;
  for (l=0;l<numloc;l++) {
    sum=0;
    for (i=0;i<alpha[1][l].dim();i++) sum+=alpha[1][l][i];
    //cout << "r " << l << " " << log(sum) << endl;
  }
  
  double loglik=makelik_Gfr(alpha);

  return loglik;

}

double make_lik_Gfr_DIRECT(const igrid &controls, const ivectors &a_lists,
			   const dvectors &h_freq_lists)
{
  //third way to generate likelihood of genotype controls; this one
  //does it directly with allele and haplotype frequencies

  //assumes both are missing if at all
  int g,l;
  double res1,res2;

  const int numgen=controls.dimx()/2;
  const int numloc=controls.dimy();
  const int debug=0;

  //make allele freq's
  dvectors a_freq_lists; 
  a_freq_lists.init(numloc);
  for (l=0;l<numloc;l++) 
    { a_freq_lists[l].init(a_lists[l].dim()); a_freq_lists[l].clear(0); }
  make_a_freq_lists_Gfr(a_freq_lists, h_freq_lists, debug);

  //compute lik
  dvector lik(numgen); lik.clear(0);
  for (g=0;g<numgen;g++) {
    
    //fill in for first locus
    if (controls(2*g+0,0) != 0) 
      lik[g]=
	log(
	a_freq_lists[0][ getposinlist( controls(2*g+0,0),
				       a_lists[0] ) ]*
	a_freq_lists[0][ getposinlist( controls(2*g+1,0),
				       a_lists[0] ) ]
	);

    else lik[g]=0.0;
  
    //loop over next loci
    for (l=0;l<numloc-1;l++) {
      if (controls(2*g,l+1) != 0) {
	if (controls(2*g,l) != 0) {
	  
	  res1=
	    h_freq_lists[l][ 
			    getposinlist( controls(2*g+0,l  ),a_lists[l  ] )*
			    a_lists[l+1].dim()+
			    getposinlist( controls(2*g+0,l+1),a_lists[l+1] ) ]*
	    h_freq_lists[l][ 
			    getposinlist( controls(2*g+1,l  ),a_lists[l  ] )*
			    a_lists[l+1].dim()+
			    getposinlist( controls(2*g+1,l+1),a_lists[l+1] ) ];
	  res2=
	    h_freq_lists[l][ 
			    getposinlist( controls(2*g+0,l  ),a_lists[l  ] )*
			    a_lists[l+1].dim()+
			    getposinlist( controls(2*g+1,l+1),a_lists[l+1] ) ]*
	    h_freq_lists[l][ 
			    getposinlist( controls(2*g+1,l  ),a_lists[l  ] )*
			    a_lists[l+1].dim()+
			    getposinlist( controls(2*g+0,l+1),a_lists[l+1] ) ];


	  lik[g]+=
	    log(
		(res1 + res2) /
		(a_freq_lists[l][getposinlist( controls(2*g+0,l  ),
					       a_lists[l  ] ) ]*
		 a_freq_lists[l][getposinlist( controls(2*g+1,l  ),
					       a_lists[l  ] ) ] )
		);

	}
	else {
	  lik[g]+=
	    log(
	    a_freq_lists[l+1][getposinlist( controls(2*g+0,l+1),
					    a_lists[l+1] ) ]*
	    a_freq_lists[l+1][getposinlist( controls(2*g+1,l+1),
					    a_lists[l+1] ) ]
	    );
	}
      }

    }//l
  }//g
 
  double nulllik=0;
  for (g=0;g<numgen;g++) nulllik+=lik[g];


  return nulllik;
}
 
int check_a_freq_lists(const dvectors &a_freq_lists, 
		       const dvectors &h_freq_lists)
{
  //this function checks that estimates of joint frequencies are
  //consistent, in the sense that, given three markers, one should
  //get the same estimates for the allele frequencies at the second
  //locus by summing over the first locus in the haploytpe freqs.
  //for the first and second locus, AND by summing over the third
  //locus in hap. frequencies for second and third markers

  const int numloc=a_freq_lists.dim();
  int l, i,j;
  
  //check that get same estimates by summing first and second 
  //elements of pair
  double diff=0;
  dvectors scratch; scratch.init(a_freq_lists.dim());
  for (l=0;l<numloc;l++) {
    scratch[l].init(a_freq_lists[l].dim());
    scratch[l].clear(0); }
  
  for (l=0;l<numloc-1;l++) {
    for (i=0;i<a_freq_lists[l].dim();i++) {
      for (j=0;j<a_freq_lists[l+1].dim();j++) {
	
	scratch[l+1][j]+=h_freq_lists[l][i*a_freq_lists[l+1].dim()+j];
      }
    }
  }
  
  //check that scratch==a_freq_lists
  for (l=1;l<numloc;l++) {
    for (i=0;i<a_freq_lists[l].dim();i++) {
      diff=scratch[l][i]-a_freq_lists[l][i];
      if ( !(-0.0001<diff && diff<0.0001) ) {
	errorfs << "ERROR in make_a_freq_lists_Gfr; " << endl
	     << "two estimates for a_freq_lists_Gfr[" << l 
	     << "][" << i << "] are not equal" << endl;
	exit(1);
      }
    }
  }

  return 0;
}



//The following functions are not used currently-- I programmed these
//methods before realizing that they do not apply to our situation.
//They estimate two locus haplotype frequencies for each pair of loci
//independently-- I won't delete them because this may be useful later on.

void m2lochfr_Hcon(dvector &h_freq_list, const int &loci, const int &locj, 
		   const ivector &c_allele_lists_i, 
		   const ivector &c_allele_lists_j, 
		   const igrid &controls, 
		   const int &debug)
{
  int i,conv,iter;
  double diff;
  const int numhap=controls.dimx();

  dvectors a_freq_lists; a_freq_lists.init(2);
  a_freq_lists[0].init(c_allele_lists_i.dim());
  a_freq_lists[1].init(c_allele_lists_j.dim());
  dvector h_freq_list_copy(h_freq_list.dim());

  //declare objects for use in resolving genotypes
  dgrid haplotype_exp;
  ivector scratch;
  ivector missing;

  //initialize objects
  haplotype_exp.init(numhap,h_freq_list.dim());
  missing.init(numhap); 
  scratch.init(2); 

  //make missing (1=no; 0=yes)
  int is_missing=0;
  make_missing_Hcon(missing, is_missing, controls, loci, locj, debug);

  //fill in haplotype_res
  make_haplotype_exp_Hcon(haplotype_exp, controls,
			  c_allele_lists_i, c_allele_lists_j, loci, locj,
			  scratch, missing, debug);
  

  //make initial values for haplotype frequencies
  make_init_h_freq(h_freq_list, debug);
  //reduce to allele frequencies
  make_a_freq_lists(a_freq_lists, h_freq_list, debug);
  
  //begin iteration
  int maxiter=200;
  for (iter=1;iter<=maxiter;iter++) {

    //copy hapl. freq. to use later in checking convergence
    //for (i=0;i<h_freq_list.dim();i++) {
    h_freq_list_copy=h_freq_list;
    
    //expectation step
    if (is_missing==1) 
      mh_expectation_step_hap(haplotype_exp, h_freq_list, 
			      controls, c_allele_lists_i, c_allele_lists_j, 
			      loci, locj, 
			      a_freq_lists, scratch, missing, debug);
    
    //maximization step
    maximization_step_hap(h_freq_list, haplotype_exp, debug);
    make_a_freq_lists(a_freq_lists, h_freq_list, debug);
    
    //check convergence
    conv=0;
    for (i=0;i<h_freq_list.dim();i++) {
      diff=h_freq_list[i]-h_freq_list_copy[i];
      if ( diff<-0.00001 || 0.00001<diff ) { conv=1; break; } }   
    if (conv==0 || is_missing==0) break;
    
    if (iter==maxiter) errorfs << "WARNING: m2loc... did not converge for loci "
			    << loci << " and " << locj << endl;
  }//iter


}

void m2lochfr_Gcon(dvector &h_freq_list, const int &loci, const int &locj, 
		   const ivector &c_allele_lists_i, 
		   const ivector &c_allele_lists_j, 
		   const igrid &controls, 
		   const int &debug)
{
  int i,g,conv,iter;
  double diff;
  const int numgen=controls.dimx()/2;

  dvectors a_freq_lists; a_freq_lists.init(2);
  a_freq_lists[0].init(c_allele_lists_i.dim());
  a_freq_lists[1].init(c_allele_lists_j.dim());
  dvector h_freq_list_copy(h_freq_list.dim());

  //declare grids for use in resolving genotypes
  igrid haplotype_res;
  dgrid genotype_res;
  ivector scratch;
  ivector missing;

  //initialize objects
  haplotype_res.init(numgen,4);
  genotype_res.init(numgen,2);
  missing.init(numgen); 
  scratch.init(4);

  //make missing (1=no; 0=yes)
  int is_missing=0;
  make_missing_Gcon(missing, is_missing, controls, loci, locj, debug);

  //initialize object for use with missing values
  dvectors mg_genoteype_res; 
  if (is_missing>0) {
    mg_genoteype_res.init(numgen);
    for (g=0;g<numgen;g++) {
      if (missing(g)==0) {
	mg_genoteype_res[g].init(h_freq_list.dim()*h_freq_list.dim());
      } }
  }

  //fill in haplotype_res
  make_haplotype_res_Gcon(haplotype_res, controls,
			  c_allele_lists_i,c_allele_lists_j, loci, locj,
			  scratch, missing, debug);
  

  //make initial values for haplotype frequencies
  make_init_h_freq(h_freq_list, debug);
  //reduce to allele frequencies
  make_a_freq_lists(a_freq_lists, h_freq_list, debug);
  
  //begin iteration
  int maxiter=200;
  for (iter=1;iter<=maxiter;iter++) {

    //copy hapl. freq. to use later in checking convergence
    //for (i=0;i<h_freq_list.dim();i++) {
    h_freq_list_copy=h_freq_list;
    
    //expecation step (use hapl. freq. to calc. prob. of resolving
    //each phenotype into different possible genotypes)
    do_expectation_step_gen(genotype_res, h_freq_list, haplotype_res, 
			    missing,debug);
    mg_expectation_step_gen(mg_genoteype_res, h_freq_list, a_freq_lists,
			    c_allele_lists_i, c_allele_lists_j, 
			    controls, missing,
			    loci, locj, debug);
      
    //maximization step
    do_maximization_step_gen(h_freq_list, genotype_res, haplotype_res,
			     missing, debug);
    mg_maximization_step_gen(h_freq_list, mg_genoteype_res,
			     missing, debug);
  
    make_a_freq_lists(a_freq_lists, h_freq_list, debug);
    
    //check convergence
    conv=0;
    for (i=0;i<h_freq_list.dim();i++) {
      diff=h_freq_list[i]-h_freq_list_copy[i];
      if ( diff<-0.00001 || 0.00001<diff ) { conv=1; break; } }   
    if (conv==0) break;
    
    if (iter==maxiter) errorfs << "WARNING: m2loc... did not converge for loci "
			    << loci << " and " << locj << endl;

  }//iter


}

void count_alleles(ivector &allelecounts, const igrid &controls, 
		   const int &debug)
{
  int i,j,k,flag;
  const int numconthap=controls.dimx();
  const int numloci=controls.dimy();

  //zero-out allelecounts
  for (j=0;j<numloci; j++) allelecounts[j]=0;
  
  for (j=0;j<numloci;j++) {
    for (i=0; i<numconthap; i++) {
      if (controls(i,j)>0) {
	flag = 1;
	for (k=0; k<i; k++) {
	  if (controls(i,j) == controls(k,j)) { flag = 0; break; }
	}
	if (flag == 1) allelecounts[j]+=1;
      }
    }
  }
  
  //check that counts are positive
  for (j=0;j<numloci;j++) assert(allelecounts[j]>0);
  
  //print to screen
  if (debug) {
    cout << "Allelecounts: " << endl;
    for (j=0;j<numloci;j++) cout << allelecounts[j] << " "; cout << endl;
  } 
  
}

void make_c_allele_lists(ivectors &c_allele_lists, const igrid &controls, 
			 const int &debug)
{
  int i,j,k,flag,place;
  const int numconthap=controls.dimx();
  const int numloci=controls.dimy();

  //zero-out lists
  for (j=0;j<numloci;j++) {
    for (i=0;i<c_allele_lists[j].dim();i++) c_allele_lists[j][i]=0; 
  
    place=0;
    for (i=0; i<numconthap; i++) {
      if (controls(i,j)>0) {
	flag = 1;
	for (k=0; k<i; k++) {
	  if (controls(i,j) == controls(k,j)) { flag = 0; break; }
	}
	if (flag == 1) { c_allele_lists[j][place]=controls(i,j); place+=1; }
      }
    }
  }

  //print to screen
  if (debug) {
    cout << "c_allele_lists" << endl;
    
    for (j=0;j<numloci;j++) {
      cout << j << " ";
      for (i=0;i<c_allele_lists[j].dim();i++) 
	cout << c_allele_lists[j][i] << " "; 
      cout << endl;
    }
  }
}

void make_haplotype_exp_Hcon(dgrid &haplotype_exp, 
			     const igrid &controls,
			     const ivector &c_allele_lists_i, 
			     const ivector &c_allele_lists_j, 
			     const int &i, const int &j,
			     ivector &scratch, const ivector &missing,
			     const int &debug)
{

  int h,k,l;
  const int numhap=controls.dimx();

  //reset haplotype_exp
  for (h=0;h<numhap;h++) for (k=0;k<haplotype_exp.dimy();k++)
    haplotype_exp(h,k)=0.0;

  for (h=0;h<numhap;h++) {

    if (missing(h)!=0) {

      scratch(0)=scratch(1)=-1;

      //find positions for iallele	  
      for (k=0;k<c_allele_lists_i.dim();k++) {
	if ( controls(h,i)==c_allele_lists_i[k] ) { scratch(0)=k; break; } }
      if (scratch(0)==-1) {
	errorfs << "Error: allele " << controls(h,i) 
	     << " not found in c_allele_list[0] " << i << endl; 
	exit(1);
      }
      
      //find positions for jallele
      for (k=0;k<c_allele_lists_j.dim();k++) {
	if ( controls(h,j)==c_allele_lists_j[k] ) { scratch(1)=k; break; } }
      if (scratch(1)==-1) {
	errorfs << "Error: allele " << controls(h,j) 
	     << " not found in c_allele_lists_j " << j << endl; 
	exit(1);
      }
      
      //list haplotype
      haplotype_exp(h, scratch(0)*c_allele_lists_j.dim()+scratch(1) )=1.0;
    }
  }

  if (debug) {
    cout << "haplotype_res" << endl;
    for (h=0;h<haplotype_exp.dimx();h++) {
      for (l=0;l<haplotype_exp.dimy();l++) {
	cout << haplotype_exp(h,l) << " "; }
      cout << endl; }
  }
  
}

void make_haplotype_res_Gcon(igrid &haplotype_res, 
			     const igrid &controls,
			     const ivector &c_allele_lists_i, 
			     const ivector &c_allele_lists_j, 
			     const int &i, const int &j,
			     ivector &scratch, const ivector &missing,
			     const int &debug)
{

  int g,k,l;
  const int numgen=controls.dimx()/2;

  for (g=0;g<numgen;g++) {
      
    if (missing(g) != 0) {
      for (l=0;l<scratch.dim();l++) scratch(l)=-1;
      
      //find positions for ialleles
      for (l=0;l<2;l++) {
	
	for (k=0;k<c_allele_lists_i.dim();k++) {
	  if ( controls(2*g+l,i)==c_allele_lists_i[k] ) {
	    scratch(l)=k; break; }
	}
	if (scratch(l)==-1) {
	  errorfs << "Error: allele " << controls(2*g+l,i) 
	       << " not found in c_allele_lists_i " << i << endl; 
	  exit(1);
	}
	
      }
      
      //find positions for jalleles
      for (l=0;l<2;l++) {
	
	for (k=0;k<c_allele_lists_j.dim();k++) {
	  if ( controls(2*g+l,j)==c_allele_lists_j[k] ) {
	    scratch(2+l)=k; break; }
	}
	if (scratch(2+l)==-1) {
	  errorfs << "Error: allele " << controls(2*g+l,j) 
	       << " not found in c_allele_lists_j " << j << endl; 
	  exit(1);
	}
	
      }
      
      //list haplotypes
      haplotype_res(g,0)=scratch(0)*c_allele_lists_j.dim()+scratch(2);
      haplotype_res(g,1)=scratch(1)*c_allele_lists_j.dim()+scratch(3);
      haplotype_res(g,2)=scratch(0)*c_allele_lists_j.dim()+scratch(3);
      haplotype_res(g,3)=scratch(1)*c_allele_lists_j.dim()+scratch(2);
      
    }
  }

  if (debug) {
    cout << "haplotype_res" << endl;
    for (g=0;g<haplotype_res.dimx();g++) {
      for (l=0;l<haplotype_res.dimy();l++) {
	cout << haplotype_res(g,l) << " "; }
      cout << endl; }
  }
  
}

void do_expectation_step_gen(dgrid &genotype_res, const dvector &h_freq_list, 
			     igrid &haplotype_res, const ivector &missing,
			     const int &debug)
{
  int g,i; double sum;
  const int numgen=haplotype_res.dimx();
  
  for (g=0;g<numgen;g++) {
    
    if (missing[g]!=0) {
      genotype_res(g,0)=
	h_freq_list[ haplotype_res(g,0) ]*
	h_freq_list[ haplotype_res(g,1) ];
      
      genotype_res(g,1)=
	h_freq_list[ haplotype_res(g,2) ]*
	h_freq_list[ haplotype_res(g,3) ];
      
      sum=genotype_res(g,0)+genotype_res(g,1);
      genotype_res(g,0)/=sum;
      genotype_res(g,1)/=sum;
      
    }
  }

  for (g=0;g<genotype_res.dimx(); g++) {
    for (i=0;i<genotype_res.dimy();i++) {
      assert( genotype_res(g,i)>=0.0 ); } }

  if (debug) {
    cout << "genotype_res [E-step]" << endl;
    for (g=0; g<genotype_res.dimx(); g++) {
      for (i=0;i<genotype_res.dimy();i++) 
	cout << genotype_res(g,i) << " "; 
      cout << endl;
    }
  }
  
}

void do_maximization_step_gen(dvector &h_freq_list, const dgrid &genotype_res,
			      const igrid &haplotype_res, 
			      const ivector &missing,
			      const int &debug)
{
  int g,i;
  const int numgen=genotype_res.dimx();

  for (i=0;i<h_freq_list.dim();i++) h_freq_list[i]=0.0;

  for (g=0;g<numgen;g++) {
    
    if (missing[g] != 0) {
      h_freq_list( haplotype_res(g,0) )+= genotype_res(g,0);
      h_freq_list( haplotype_res(g,1) )+= genotype_res(g,0);
      h_freq_list( haplotype_res(g,2) )+= genotype_res(g,1);
      h_freq_list( haplotype_res(g,3) )+= genotype_res(g,1);
    }
  }

  for (i=0;i<h_freq_list.dim();i++) h_freq_list[i]/=(2*numgen);

  if (debug) {
    cout << "h_freq_list [M-step]" << endl;
    for (i=0;i<h_freq_list.dim();i++) cout << h_freq_list[i] << " ";
    cout << endl;
  }
}

void make_missing_Hcon(ivector &missing, int &is_missing, 
		       const igrid &controls, 
		       const int &loci, const int &locj, 
		       const int &debug)
{
  int h;
  const int numhap=controls.dimx();
  
  //set all entries to 1;
  for (h=0;h<numhap;h++) { missing[h]=1; }

  for (h=0;h<numhap;h++) { 
    if ( controls(h, loci)==0 || controls(h, locj)==0 ) missing[h]=0; }

  //find if there is a missing value
  for (h=0;h<missing.dim();h++) { if (missing(h)==0) { is_missing=1; break; }}
  
  if (debug) {
    cout << "missing" << endl;
    for (h=0;h<numhap;h++) cout << missing[h] << " "; 
    cout << endl;
    cout << "is_missing " << is_missing << endl;
  }
  
}

void make_missing_Gcon(ivector &missing, int &is_missing, 
		       const igrid &controls, 
		       const int &loci, const int &locj, 
		       const int &debug)
{
  int g;
  const int numgen=controls.dimx()/2;
  
  //set all entries to 1;
  for (g=0;g<numgen;g++) { missing[g]=1; } 

  for (g=0;g<numgen;g++) {
    if ( controls(2*g  ,loci)==0 || controls(2*g+1,loci)==0 ||
	 controls(2*g  ,locj)==0 || controls(2*g+1,locj)==0 ) missing[g]=0;
  } 

  //find if there is a missing value
  for (g=0;g<missing.dim();g++) { if (missing(g)==0) { is_missing=1; break; }}
  
  if (debug) {
    cout << "missing" << endl;
    for (g=0;g<numgen;g++) cout << missing[g] << " "; 
    cout << endl;
    cout << "is_missing " << is_missing << endl;
  }
  
}

void make_a_freq_lists(dvectors &a_freq_lists, const dvector &h_freq_list,
		       const int &debug)
{
  int i,j;
  
  //zero-out both lists
  for (i=0;i<a_freq_lists[0].dim();i++) a_freq_lists[0][i]=0.0;
  for (j=0;j<a_freq_lists[1].dim();j++) a_freq_lists[1][j]=0.0;

  for (i=0;i<a_freq_lists[0].dim();i++) {
    for (j=0;j<a_freq_lists[1].dim();j++) {
      a_freq_lists[0][i]+=h_freq_list[i*a_freq_lists[1].dim()+j];
      a_freq_lists[1][j]+=h_freq_list[i*a_freq_lists[1].dim()+j];
    }
  }
  
  if (debug) {
    cout << "a_freq_lists" << endl;
    for (i=0;i<a_freq_lists[0].dim();i++) cout << a_freq_lists[0][i] << " ";
    cout << endl;
    for (j=0;j<a_freq_lists[1].dim();j++) cout << a_freq_lists[0][j] << " ";
    cout << endl;
  }

}

void mg_expectation_step_gen(dvectors &mg_genoteype_res, 
			     const dvector &h_freq_list, 
			     const dvectors &a_freq_lists,
			     const ivector &c_allele_lists_i, 
			     const ivector &c_allele_lists_j, 
			     const igrid &controls,
			     const ivector &missing,
			     const int &loci, const int &locj, 
			     const int &debug)
{
  int g;
  const int numgen=controls.dimx()/2;
  const int nai=a_freq_lists[0].dim();//(n)umber of (a)lleles at loc (i)
  const int naj=a_freq_lists[1].dim();
  const int nah=h_freq_list.dim();
  int i0,j0,i1,j1=0,h0,h1;
  double denom,sum;

  for (g=0;g<numgen;g++) {
    
    if (missing(g)==0) {

      //zero-out vector
      for (i0=0;i0<mg_genoteype_res[g].dim();i0++)
	mg_genoteype_res[g][i0]=0.0;

      //fill in haplotype probabilities
      if (controls(2*g  ,loci) != 0 && controls(2*g  ,locj) != 0 && 
	  controls(2*g+1,loci) != 0 && controls(2*g+1,locj) != 0) {
	errorfs << "ERROR: missing value at genotype \n" << g 
	     << " and loci " << loci << " and " << locj << "not found \n";
	exit(1); }
      else {
	if (controls(2*g  ,loci) == 0) {
	  if (controls(2*g  ,locj) == 0) {
	    if (controls(2*g+1,loci) == 0) {
	      if (controls(2*g+1,locj) == 0) {

		//00/00
		for (h0=0;h0<nah;h0++) { for (h1=0;h1<nah;h1++) { 
		  mg_genoteype_res[g][h0*nah+h1]+=
		    h_freq_list(h0)*h_freq_list(h1); } }

	      }
	      else {

		//00/0-
		j1=getposinlist( controls(2*g+1,locj), c_allele_lists_j );
		denom=a_freq_lists[1][j1];
		for (h0=0;h0<nah;h0++) { for (i1=0;i1<nai;i1++) {
		  mg_genoteype_res[g][h0*nah+ i1*naj+j1]+=
		    h_freq_list(h0)*h_freq_list(i1*naj+j1)
		    /denom; } }
		
	      }
	    }
	    else {
	      if (controls(2*g+1,locj) == 0) {

		//00/-0
		i1=getposinlist( controls(2*g+1,loci), c_allele_lists_i );
		denom=a_freq_lists[0][i1];
		for (h0=0;h0<nah;h0++) { for (j1=0;j1<naj;j1++) {
		  mg_genoteype_res[g][h0*nah+ i1*naj+j1]+=
		    h_freq_list(h0)*h_freq_list(i1*naj+j1)
		    /denom; } }
	      		
	      }
	      else {

		//00/--
		i1=getposinlist( controls(2*g+1,loci), c_allele_lists_i );
		j1=getposinlist( controls(2*g+1,locj), c_allele_lists_j );
		denom=(h_freq_list(i1*naj+j1)+
			 a_freq_lists[0][i1]*a_freq_lists[1][j1]);
		for (h0=0;h0<nah;h0++) {
		  mg_genoteype_res[g][h0*nah+ i1*naj+j1]+=
		    h_freq_list(h0)*h_freq_list(i1*naj+j1)
		    /denom; }
		for (i0=0;i0<nai;i0++) { for (j0=0;j0<naj;j0++) {
		  mg_genoteype_res[g][(i0*naj+j0)*nah+ i1*naj+j1]+=
		    h_freq_list(i0*naj+j1)*h_freq_list(i1*naj+j0)
		    /denom; } }

	      }
	    }
	  }
	  else {
	    if (controls(2*g+1,loci) == 0) {
	      if (controls(2*g+1,locj) == 0) {

		//0-/00
		j0=getposinlist( controls(2*g  ,locj), c_allele_lists_j );
		denom=a_freq_lists[1][j0];
		for (h0=0;h0<nah;h0++) { for (i0=0;i0<naj;i0++) {
		  mg_genoteype_res[g][h0*nah+ i0*naj+j0]+=
		    h_freq_list(h0)*h_freq_list(i0*naj+j0)
		    /denom; } }
		
	      }
	      else {

		//0-/0-
		j0=getposinlist( controls(2*g  ,locj), c_allele_lists_j );
		j1=getposinlist( controls(2*g+1,locj), c_allele_lists_j );
		denom=a_freq_lists[1][j0]*a_freq_lists[1][j1];
		for (i0=0;i0<nai;i0++) { for (i1=0;i1<naj;i1++) {
		  mg_genoteype_res[g][(i0*naj+j0)*nah+i1*naj+j1]+=
		    h_freq_list(i0*naj+j0)*h_freq_list(i1*naj+j1)
		    /denom; } }

	      }
	    }
	    else {
	      if (controls(2*g+1,locj) == 0) {
		
		//0-/-0
		j0=getposinlist( controls(2*g  ,locj), c_allele_lists_j );
		i1=getposinlist( controls(2*g+1,loci), c_allele_lists_i );
		denom=(a_freq_lists[1][j0]*a_freq_lists[0][i1]+
			 h_freq_list(i1*naj+j0));
		for (i0=0;i0<nai;i0++) { for (j1=0;j1<naj;j1++) {
		  mg_genoteype_res[g][(i0*naj+j0)*nah+ i1*naj+j1]+=
		    h_freq_list(i0*naj+j0)*h_freq_list(i1*naj+j1)
		    /denom; } }
		for (h0=0;h0<nah;h0++) {
		  mg_genoteype_res[g][h0*nah+ i1*naj+j1]+=
		    h_freq_list(h0)*h_freq_list(i1*naj+j0)
		    /denom; }
		
	      }
	      else {

		//0-/--
		j0=getposinlist( controls(2*g  ,locj), c_allele_lists_j );
		i1=getposinlist( controls(2*g+1,loci), c_allele_lists_i );
		j1=getposinlist( controls(2*g+1,locj), c_allele_lists_j );
		denom=
		  (a_freq_lists[1][j0]*h_freq_list(i1*naj+j1)+
		   a_freq_lists[1][j1]*h_freq_list(i1*naj+j0));
		for (i0=0;i0<nai;i0++) { 
		  mg_genoteype_res[g][(i0*naj+j0)*nah+(i1*naj+j1) ]+=
		    h_freq_list(i0*naj+j0)*h_freq_list(i1*naj+j1)
		    /denom; }
		for (i0=0;i0<nai;i0++) { 
		  mg_genoteype_res[g][(i0*naj+j0)*nah+(i1*naj+j1) ]+=
		    h_freq_list(i0*naj+j1)*h_freq_list(i1*naj+j0)
		    /denom; }
	      }
	    }
	  }
	}
	else {
	  if (controls(2*g  ,locj) == 0) {
	    if (controls(2*g+1,loci) == 0) {
	      if (controls(2*g+1,locj) == 0) {

		//-0/00
		i0=getposinlist( controls(2*g  ,loci), c_allele_lists_i );
		denom=a_freq_lists[0][i0];
		for (j0=0;j0<naj;j0++) { for (h1=0;h1<nah;h1++) { 
		  mg_genoteype_res[g][(i0*naj+j0)*nah+ (h1)]+=
		    h_freq_list(i0*naj+j0)*h_freq_list(h1)
		    /denom; } }
	      }
	      else {
		
		//-0/0-
		i0=getposinlist( controls(2*g  ,loci), c_allele_lists_i );
		j1=getposinlist( controls(2*g+1,locj), c_allele_lists_j );
		denom=(a_freq_lists[0][i0]*a_freq_lists[1][j1]+
			 h_freq_list(i0*naj+j1));
		for (j0=0;j0<naj;j0++) { for (i1=0;i1<naj;i1++) {
		  mg_genoteype_res[g][(i0*naj+j0)*nah+(i1*naj+j1)]+=
		    h_freq_list(i0*naj+j0)*h_freq_list(i1*naj+j1)
		    /denom; } }
		for (j0=0;j0<naj;j0++) { for (i1=0;i1<naj;i1++) {
		  mg_genoteype_res[g][(i0*naj+j1)*nah+(i1*naj+j0)]+=
		    h_freq_list(i0*naj+j1)*h_freq_list(i1*naj+j0)
		    /denom; } }
		
	      }
	    }
	    else {
	      if (controls(2*g+1,locj) == 0) {

		//-0/-0
		i0=getposinlist( controls(2*g  ,loci), c_allele_lists_i );
		i1=getposinlist( controls(2*g+1,loci), c_allele_lists_i );
		denom=a_freq_lists[0][i0]*a_freq_lists[0][i1];
		for (j0=0;j0<naj;j0++) { for (j1=0;j1<naj;j1++) {
		  mg_genoteype_res[g][(i0*naj+j0)*nah+i1*naj+j1]+=
		    h_freq_list(i0*naj+j0)*h_freq_list(i1*naj+j1)
		    /denom; } }

	      }
	      else {

		//-0/--
		i0=getposinlist( controls(2*g  ,loci), c_allele_lists_i );
		i1=getposinlist( controls(2*g+1,loci), c_allele_lists_i );
		j1=getposinlist( controls(2*g+1,locj), c_allele_lists_j );
		denom=
		  (a_freq_lists[0][i0]*h_freq_list(i1*naj+j1)+
		   a_freq_lists[0][i1]*h_freq_list(i0*naj+j1));
		for (j0=0;j0<naj;j0++) { 
		  mg_genoteype_res[g][(i0*naj+j0)*nah+i1*naj+j1]+=
		    h_freq_list(i0*naj+j0)*h_freq_list(i1*naj+j1)
		    /denom; }
		for (j0=0;j0<naj;j0++) { 
		  mg_genoteype_res[g][(i0*naj+j1)*nah+(i1*naj+j0)]+=
		    h_freq_list(i0*naj+j1)*h_freq_list(i1*naj+j0)
		    /denom; }
	      
	      }
	    }
	  }
	  else {
	    if (controls(2*g+1,loci) == 0) {
	      if (controls(2*g+1,locj) == 0) {

		//--/00
		i0=getposinlist( controls(2*g  ,loci), c_allele_lists_i );
		j0=getposinlist( controls(2*g  ,locj), c_allele_lists_j );
		denom=
		  (h_freq_list(i0*naj+j0)+
		   a_freq_lists[0][i0]*a_freq_lists[1][j0]);
		for (i1=0;i1<naj;i1++) { for (j1=0;j1<naj;j1++) {
		  mg_genoteype_res[g][(i0*naj+j0)*nah+(i1*naj+j1)]+=
		    h_freq_list(i0*naj+j0)*h_freq_list(i1*naj+j1)
		    /denom; } }
		for (i1=0;i1<naj;i1++) { for (j1=0;j1<naj;j1++) {
		  mg_genoteype_res[g][(i0*naj+j1)*nah+(i1*naj+j0)]+=
		    h_freq_list(i0*naj+j1)*h_freq_list(i1*naj+j0)
		    /denom; } }
		
	      }
	      else {

		//--/0-
		i0=getposinlist( controls(2*g  ,loci), c_allele_lists_i );
		j0=getposinlist( controls(2*g  ,locj), c_allele_lists_j );
		j1=getposinlist( controls(2*g+1,locj), c_allele_lists_j );
		denom=
		  (a_freq_lists[1][j1]*h_freq_list(i0*naj+j0)+
		   a_freq_lists[1][j0]*h_freq_list(i0*naj+j1));
		for (i1=0;i1<naj;i1++) { 
		  mg_genoteype_res[g][(i0*naj+j0)*nah+(i1*naj+j1)]+=
		    h_freq_list(i0*naj+j0)*h_freq_list(i1*naj+j1)
		    /denom; }
		for (i1=0;i1<naj;i1++) { 
		  mg_genoteype_res[g][(i0*naj+j1)*nah+(i1*naj+j0)]+=
		    h_freq_list(i0*naj+j1)*h_freq_list(i1*naj+j0)
		    /denom; }
	      }
	    }
	    else {
	      if (controls(2*g+1,locj) == 0) {

		//--/-0
		i0=getposinlist( controls(2*g  ,loci), c_allele_lists_i );
		j0=getposinlist( controls(2*g  ,locj), c_allele_lists_j );
		i1=getposinlist( controls(2*g+1,loci), c_allele_lists_i );
		denom=
		  (a_freq_lists[0][i1]*h_freq_list(i0*naj+j0)+
		   a_freq_lists[0][i0]*h_freq_list(i1*naj+j0));
		for (j1=0;j1<naj;j1++) { 
		  mg_genoteype_res[g][(i0*naj+j0)*nah+(i1*naj+j1)]+=
		    h_freq_list(i0*naj+j0)*h_freq_list(i1*naj+j1)
		    /denom; }
		for (j1=0;j1<naj;j1++) { 
		  mg_genoteype_res[g][(i0*naj+j1)*nah+(i1*naj+j0)]+=
		    h_freq_list(i0*naj+j1)*h_freq_list(i1*naj+j0)
		    /denom; }

	      }
	      else {
		//--/--
		errorfs << "ERROR " << endl; exit(1);
	      }
	    }
	  }
	}
      }  
    
      sum=0.0;
      for (i0=0;i0<mg_genoteype_res[g].dim();i0++) 
	sum+=mg_genoteype_res[g][i0];
      assert(0.999<sum && sum<1.001);
    }
  }
  
  if (debug) {
    cout << "mg_expectation_step" << endl;
    for (g=0;g<numgen;g++) {
      if (missing(g)==0) {
	cout << "[" << g << "] ";
	for (i0=0;i0<mg_genoteype_res[g].dim();i0++)
	  cout << mg_genoteype_res[g][i0] << " "; 
	cout << endl;
      } 
    } 
  }
  
}

int getposinlist(const int &allele, const ivector &list)
{
  int i,pos=-1;

  for (i=0;i<list.dim();i++) {
    if ( allele==list(i) ) { pos=i; break; }
  }

  if (pos==-1) {
    errorfs << "Error: allele " << allele << " not found in list \n";
    exit(1); }

  return pos;

}

void mg_maximization_step_gen(dvector &h_freq_list, 
			      const dvectors &mg_genoteype_res,
			      const ivector &missing,
			      const int &debug)
{

  int g,h,i;
  const int numgen=missing.dim();
  const int nh=h_freq_list.dim();
  double value;

  for (g=0;g<numgen;g++) {
    
    if (missing(g)==0) {

      for (h=0;h<nh;h++) {

	value=0.0;
	for (i=0;i<nh;i++) {
	  //first hap in genotype
	  value+=mg_genoteype_res[g][h*nh+i];
	  //second hap in genotype
	  value+=mg_genoteype_res[g][i*nh+h]; }

	h_freq_list[h]+=value/(2*numgen);
	
      }

    }
  }

  //check that the haplotype frequencies sum to 1.0
  value=0.0;
  for (h=0;h<nh;h++) {
    value+=h_freq_list[h]; }
  assert(0.999<value && value<1.001);

  if (debug) {
    cout << "h_freq_list AFTER MG [M-step]" << endl;
    for (i=0;i<h_freq_list.dim();i++) cout << h_freq_list[i] << " ";
    cout << endl;
  }

}

void mh_expectation_step_hap(dgrid &haplotype_exp, 
			     const dvector &h_freq_list, 
			     const igrid &controls,
			     const ivector &c_allele_lists_i, 
			     const ivector &c_allele_lists_j, 
			     const int &i, const int &j,
			     const dvectors &a_freq_lists,
			     ivector &scratch, const ivector &missing,
			     const int &debug)
{
  int h,k;
  double sum;
  const int numhap=controls.dimx();

  for (h=0;h<numhap;h++) {

    if (missing(h)==0) {

      if ( controls(h,i)==0 && controls(h,j)==0 ) {
	for (k=0;k<h_freq_list.dim();k++) 
	  haplotype_exp(h,k)=h_freq_list(k);
      }
      else {
	if ( controls(h,i)!=0 ) {

	  //find position of allele at loc i
	  for (k=0;k<c_allele_lists_i.dim();k++) {
	    if ( controls(h,i)==c_allele_lists_i[k] ){scratch(0)=k; break; } }
	  if (scratch(0)==-1) {
	    errorfs << "Error: allele " << controls(h,i) 
		 << " not found in c_allele_lists_i " << i << endl; 
	    exit(1);
	  }
	  
	  for (k=0;k<c_allele_lists_j.dim();k++) {
	    haplotype_exp(h,scratch(0)*c_allele_lists_j.dim()+k)=
	      h_freq_list(scratch(0)*c_allele_lists_j.dim()+k)
	      /a_freq_lists[0][scratch(0)]; }

	}
	else {//controls(h,j)!=0

	  //find position of allele at loc j
	  for (k=0;k<c_allele_lists_j.dim();k++) {
	    if ( controls(h,j)==c_allele_lists_j(k) ){scratch(1)=k; break; } }
	  if (scratch(1)==-1) {
	    errorfs << "Error: allele " << controls(h,j) 
		 << " not found in c_allele_lists_j " << j << endl; 
	    exit(1);
	  }
	  
	  for (k=0;k<c_allele_lists_i.dim();k++) {
	    haplotype_exp(h,k*c_allele_lists_j.dim()+scratch(1))=
	      h_freq_list(k*c_allele_lists_j.dim()+scratch(1))
	      /a_freq_lists[1][scratch(1)]; }

	}
      }

      //check that row sums to 1.0
      sum=0.0;
      for (k=0;k<haplotype_exp.dimy();k++) sum+=haplotype_exp(h,k);
      if ( !(0.999<sum && sum<1.001) ) {
	errorfs << "ERROR: row " << h << "[+1] for haplotype_exp does not " 
	     << endl << "sum to 1.0 [" << sum << "]" << endl;
	for (k=0;k<haplotype_exp.dimy();k++) errorfs << haplotype_exp(h,k) << " ";
	errorfs << endl;
	exit(1); }
    }

  }

  if (debug) {
    cout << "haplotype_exp" << endl;
    for (h=0;h<numhap;h++) {
      for (k=0;k<haplotype_exp.dimy();k++)
	cout << haplotype_exp(h,k) << " ";
      cout << endl; }
  }

}

void maximization_step_hap(dvector &h_freq_list, 
			   const dgrid &haplotype_exp, const int &debug)
{
  int h,i;
  double sum;
  const int numhap=haplotype_exp.dimx();
  
  //zero-out freq vector
  for (i=0;i<h_freq_list.dim();i++) h_freq_list(i)=0.0;

  for (i=0;i<h_freq_list.dim();i++) {
    for (h=0;h<numhap;h++) {
      h_freq_list(i)+=haplotype_exp(h,i); } }

  //normalize to sum to 1;
  for (i=0;i<h_freq_list.dim();i++) h_freq_list(i)/=numhap;
  
  //check that sum to 1;
  sum=0.0; for (i=0;i<h_freq_list.dim();i++) sum+=h_freq_list(i);
  if ( !(0.999<sum && sum<1.001) ) {
    errorfs << "ERROR: haplotype freqs do not sum to 1 [" << sum << "]" << endl;
    exit(1); }
  
}

void make_init_h_freq(dvector &h_freq_list, const int &debug)
{
  int i;

  for (i=0;i<h_freq_list.dim();i++) 
    h_freq_list[i]=1.0/h_freq_list.dim();

  if (debug) {
    for (i=0;i<h_freq_list.dim();i++) cout << h_freq_list[i] << " ";
    cout << endl;
  }
}

void countadditionalalleles(ivector &additionalalleles, 
			    const ivectors &c_allele_lists,
			    const igrid &data, const int &debug)
{
  int i,k,loc,flag;
  const int numhap=data.dimx();
  const int numloci=data.dimy();
  
  //zero-out 
  for (loc=0;loc<numloci;loc++) additionalalleles[loc]=0;

  for (loc=0;loc<numloci;loc++) {
    
    for (i=0; i<numhap; i++) {
      if (data(i,loc)>0) {
	flag = 1;
	//check that it is not in c_allele_lists
	for (k=0; k<c_allele_lists[loc].dim(); k++) {
	  if (data(i,loc) == c_allele_lists[loc][k]) { flag = 0; break; }
	}
	//check that it has not appeared earlier in data
	if (flag == 1) {
	  for (k=0; k<i; k++) {
	    if (data(i,loc) == data(k,loc) ) { flag = 0; break; }
	  }
	}
	if (flag == 1) additionalalleles[loc]+=1;
      }
    }
  }
  
  if (debug) {
    cout << "additionalalleles" << endl;
    for (loc=0;loc<numloci;loc++) cout << additionalalleles[loc] << " ";
    cout << endl;
  }
}

void finish_allele_list(ivectors &allele_list, const ivectors &c_allele_lists,
			const igrid &data,
			const int &debug)
{

  int i,k,h,loc,flag,place;
  const int numhap=data.dimx();
  const int numloci=data.dimy();
  
  //zero-out 
  for (loc=0;loc<numloci;loc++) {
    for (h=0;h<allele_list[loc].dim();h++) {
      allele_list[loc][h]=0; } }

  //copy c_allele_list into allele_list
  for (loc=0;loc<numloci;loc++) {
    for (h=0;h<c_allele_lists[loc].dim();h++) {
      allele_list[loc][h]=c_allele_lists[loc][h]; } }
  
  //add alleles that did not appear in controls
  for (loc=0;loc<numloci;loc++) {
    if (allele_list[loc].dim()>c_allele_lists[loc].dim()) {
      
      place=c_allele_lists[loc].dim();
      for (i=0; i<numhap; i++) {
	if (data(i,loc)>0) {
	  flag = 1;
	  //check that it is not in c_allele_lists
	  for (k=0; k<c_allele_lists[loc].dim(); k++) {
	    if (data(i,loc) == c_allele_lists[loc][k]) { flag = 0; break; }
	  }
	  //check that it has not appeared earlier in data
	  if (flag == 1) {
	    for (k=0; k<i; k++) {
	      if (data(i,loc) == data(k,loc) ) { flag = 0; break; }
	    }
	  }
	  if (flag == 1) { allele_list[loc][place]=data(i,loc); place++; }
	}
      }
    }
  }

  if (debug) {
    cout << "allele_lists" << endl; 
    for (loc=0;loc<numloci;loc++) {
      cout << loc << " ";
      for (i=0;i<allele_list[loc].dim();i++) 
	cout << allele_list[loc][i] << " "; 
      cout << endl;
    }
  }

}

void makejf2(dgrid &jointfreq2, const dvectors &hap_freq, const int &debug)
{

  int l,i,max;

  //find max number of entries in hap_freq
  max=hap_freq[0].dim();
  for (l=1;l<hap_freq.dim();l++) 
    if (hap_freq[l].dim()>max) max=hap_freq[l].dim();

  //initialize jointfreq2
  jointfreq2.init(max,hap_freq.dim());
  jointfreq2.clear(0);

  for (l=0;l<jointfreq2.dimy();l++) {
    for (i=0;i<hap_freq[l].dim();i++) {
      jointfreq2(i,l)=hap_freq[l][i]; }
  }

  for (i=0;i<jointfreq2.dimx();i++) {
    for (l=0;l<jointfreq2.dimy();l++) {
      assert(jointfreq2(i,l)>=0.0 && jointfreq2(i,l)<=1.0 ); } }

  if (debug) {
    cout << "jointfreq2" << endl;
    for (i=0;i<jointfreq2.dimx();i++) {
      for (l=0;l<jointfreq2.dimy();l++) {
	cout << jointfreq2(i,l) << " "; }
      cout << endl; }
  }

}

void makefa(igrid &freqallele, const ivectors &allele_list, const int &debug)
{
  int l,i,max;

  //find max number of entries in allele_list
  max=allele_list[0].dim();
  for (l=1;l<allele_list.dim();l++) 
    if (allele_list[l].dim()>max) max=allele_list[l].dim();
  
  //initialize freqallele
  freqallele.init(max,allele_list.dim());
  freqallele.clear(0);

  for (l=0;l<freqallele.dimy();l++) {
    for (i=0;i<allele_list[l].dim();i++) {
      freqallele(i,l)=allele_list[l][i]; }
  }

  if (debug) {
    cout << "freqallele" << endl;
    for (i=0;i<freqallele.dimx();i++) {
      for (l=0;l<freqallele.dimy();l++) {
	cout << freqallele(i,l) << " "; }
      cout << endl; }
  }

}

void makeff(dgrid &freqfreq, const igrid &freqallele,
	    const ivectors &allele_list, 
	    const dgrid &jointfreq2, const int &debug)
{
  int l,i,j;

  //initialize freqfreq
  freqfreq.init(freqallele.dimx(),freqallele.dimy());
  freqfreq.clear(0);

  for (l=0;l<freqfreq.dimy()-1;l++) {
    
    //make columns 
    for (i=0;i<allele_list[l].dim();i++) {
      for (j=0;j<allele_list[l+1].dim();j++) {
	
	//if (l==34 && i==0) cout << freqfreq(i,l) << " "
	//		<< jointfreq2(i*allele_list[l+1].dim()+j,l)
	//		<< " ";

	freqfreq(i,l)+=jointfreq2(i*allele_list[l+1].dim()+j,l); 
	if (l==freqfreq.dimy()-2)
	  freqfreq(j,l+1)+=jointfreq2(i*allele_list[l+1].dim()+j,l); 
      } }
    
  }

  //  cout << jointfreq2(0,34) << endl;
  //  exit(1);

  for (i=0;i<freqfreq.dimx();i++) {
    for (l=0;l<freqfreq.dimy();l++) {
      assert(freqfreq(i,l)>=0.0 && freqfreq(i,l)<=1.0 ); } }

  if (debug) {
    cout << "freqfreq" << endl;
    for (i=0;i<freqfreq.dimx();i++) {
      for (l=0;l<freqfreq.dimy();l++) {
	cout << freqfreq(i,l) << " "; }
      cout << endl; }
  }

}

//Bayesian version of mkv1 hap.freq. estimation software
void makehap2fr_hmm_GconBAYES(dvectors &h_freq_lists, 
			      const ivectors &a_lists, 
			      const igrid &controls, const int &bayes,
			      const int &debug)
{
  //this routine estimates two-marker haplotype frequencies, for genotype
  //controls, using a HMM

  //note that bayes=0 makes this the EM
  int l,g,i,j,iter,conv;
  double diff, sum=0;
  const int numloc=controls.dimy();
  const int numgen=controls.dimx()/2;

  //delete me
  //cout << controls(0,0) << " " << controls(0,1) << " " << controls(0,2) 
  //   << endl 
  //   << controls(1,0) << " " << controls(1,1) << " " << controls(1,2)
  //   << endl;
  //cout << endl
  //   << a_lists[0][0] << " " << a_lists[1][0] << " " << a_lists[2][0]
  //   << endl;

  //delete me
  //for (l=0;l<numloc;l++) {
  //cout << l << " " << a_lists[l].dim() << " ";
  //if (l<numloc-1) cout << h_freq_lists[l].dim() << " ";
  //cout << endl; }


  //declare and intialize and clear lists of allele freqs 
  //corresponding to a_lists
  dvectors a_freq_lists; 
  a_freq_lists.init(numloc);
  for (l=0;l<numloc;l++) 
    { a_freq_lists[l].init(a_lists[l].dim()); a_freq_lists[l].clear(0); }
  
  //declare and initialize and clear copy to be used to determine convergence
  //dvectors h_freq_lists_copy(h_freq_lists.dim());
  //dvectors h_freq_lists_test(h_freq_lists.dim());
  //for (l=0;l<numloc-1;l++) 
  //{ 
  //  h_freq_lists_copy[l].init(h_freq_lists[l].dim()); 
  //  h_freq_lists_test[l].init(h_freq_lists[l].dim()); 
  //  h_freq_lists_copy[l].clear(0); 
  //  h_freq_lists_test[l].clear(0); 
  //}
  
  //declare and initialize and clear hmm objects
  dvectorss b(numgen);
  dvectorss alpha(numgen),beta(numgen),gamma(numgen),xi(numgen);
  for (g=0;g<numgen;g++) {
    b[g].init(numloc);
    alpha[g].init(numloc);
    beta[g].init(numloc);
    gamma[g].init(numloc);
    xi[g].init(numloc-1);
    for (l=0;l<numloc;l++) {
      b[g][l].init(a_lists[l].dim()*a_lists[l].dim()); b[g][l].clear(0);
      alpha[g][l].init(a_lists[l].dim()*a_lists[l].dim()); alpha[g][l].clear(0);
      beta[g][l].init(a_lists[l].dim()*a_lists[l].dim()); beta[g][l].clear(0);
      gamma[g][l].init(a_lists[l].dim()*a_lists[l].dim()); gamma[g][l].clear(0);
      if (l!=numloc-1) {
	xi[g][l].init(a_lists[l].dim()*a_lists[l].dim()*
		      a_lists[l+1].dim()*a_lists[l+1].dim());
	xi[g][l].clear(0); }
    }
  }
  
  dvector init;
  init.init( a_lists[0].dim() );
  init.clear(0.0);

  //make copy to determine convergence
  dvector initC; initC.init( init.dim() ); initC.clear(0.0);
  init.clear(0.0);

  dvectors trans(numloc-1);
  for (l=0;l<numloc-1;l++) {
    trans[l].init(a_lists[l].dim()*a_lists[l+1].dim());
    trans[l].clear(0); }
  
  //make copy of trans to determine convergence
  dvectors transC(numloc-1);
  for (l=0;l<numloc-1;l++) {
    transC[l].init(trans[l].dim()); transC[l].clear(0); }
 
  //make observation probabilities [b]
  makeb_Gfr(b, a_lists, controls, debug);
  //delete me
  //cout << "b " << b[0][2][0] << " " << controls(0,2) << " " 
  //   << controls(1,2) << endl;

  //assign initial values to parameters
  for (i=0;i<init.dim();i++) init(i)=1.0/init.dim();
  for (l=0;l<numloc-1;l++) {
    for (i=0;i<a_lists[l].dim();i++) {
      for (j=0;j<a_lists[l+1].dim();j++) {
	trans[l][i*a_lists[l+1].dim()+j]=
	  1.0/( a_lists[l+1].dim() ); } } }

  //assign initial values to parameters
  //  long seed;
  //seed=(-1)*(long)time(NULL);
  //double sum;
  //
  //loop over multiple initial starting points
  //for (loop=0;loop<10;loop++) {//
  //cout <<"WARNING: Only est. hap. freq. with one set of starting values"
  //<< endl;
  /*
    //assign FIXED initial haplotype frequencies
    cout << "Note: Fixed initial freq's for hap-freq-est. hmm" << endl;
    for (l=0;l<numloc-1;l++) {
    for (i=0;i<h_freq_lists[l].dim();i++) 
    h_freq_lists[l][i]=1.0/h_freq_lists[l].dim(); }
  */
  
  //assign random initial haplotype frequencies
  //for (l=0;l<numloc-1;l++) {
  //for (i=0;i<h_freq_lists[l].dim();i++) h_freq_lists[l][i]=
  //				      1.0 * rand()/RAND_MAX;
  //  sum=0; for (i=0;i<h_freq_lists[l].dim();i++) sum+=h_freq_lists[l][i];
  //  for (i=0;i<h_freq_lists[l].dim();i++) h_freq_lists[l][i]/=sum; }
  

  //make initial allele freq's
  //make_a_freq_lists_Gfr(a_freq_lists, h_freq_lists, debug);
    
  //iterate hmm
  int maxiter=500;
  for (iter=1;iter<=maxiter;iter++) {
    //   cout << "iter " << iter << endl;//delete me
      
    //copy init to use later in checking convergence
    initC=init;

    //copy hapl. freq. to use later in checking convergence
    for (l=0;l<transC.dim();l++) 
      transC[l]=trans[l];

    //copy hapl. freq. to use later in checking convergence
    //for (l=0;l<h_freq_lists.dim();l++) h_freq_lists_copy[l]=h_freq_lists[l];
      
    //make trans
    //maketrans_Gfr(trans, a_freq_lists, h_freq_lists, debug);
      
    //make alpha
    makealpha_GfrBAYES(alpha, a_freq_lists, init, trans, b, debug);
      
    //make beta
    makebeta_GfrBAYES(beta, a_freq_lists, trans, b, debug);
      
    //make xi
    makexi_GfrBAYES(xi, alpha, beta, trans, b, a_freq_lists, debug);
      
    //update h_freq_lists
    //make_h_freq_lists_Gfr(h_freq_lists, a_freq_lists, xi, debug);
      
    //update allele freq's
    //make_a_freq_lists_Gfr(a_freq_lists, h_freq_lists, debug);
      
    //give expected counts of 2-locus haplotypes
    makeh2countsGfrxi(h_freq_lists,xi,a_freq_lists,0);
    make_a_freq_lists_HfrBAYES(a_freq_lists, 2*numgen, h_freq_lists, 0);

    //Bayesian adjustment/update to init
    init=a_freq_lists[0];
    for (i=0;i<init.dim();i++) { 
      init(i)+=1.0*bayes; 
      init(i)/=2.0*numgen+1.0*init.dim()*bayes; }

    //check that init sums to 1
    sum=0;
    for (i=0;i<init.dim();i++) sum+=init(i);
    if ( !(0.9999<sum && sum<1.0001) ) {
      errorfs << "ERROR: Bayesian adjustment to init in hap. freq. est.; "
	      << "sums to " << sum << endl;
      exit(1); }

    //Bayesian adjustment/update to trans
    for (l=0;l<numloc-1;l++) {
      for (i=0;i<a_lists[l  ].dim();i++) {
	for (j=0;j<a_lists[l+1].dim();j++) {
	  if (h_freq_lists[l][i*a_lists[l+1].dim()+j]+1.0*bayes <1e-100
	      && a_freq_lists[l][i]+ 1.0*a_lists[l+1].dim()*bayes<1e-100)
	    trans[l][i*a_lists[l+1].dim()+j]=0.0;
	  else
	    trans[l][i*a_lists[l+1].dim()+j]=
	      ( h_freq_lists[l][i*a_lists[l+1].dim()+j]+1.0*bayes )/
	      ( a_freq_lists[l][i]+ 1.0*a_lists[l+1].dim()*bayes);
	}
	//if they're all 0, set first to 1
	conv=0;
	for (j=0;j<a_lists[l+1].dim();j++) 
	  if (trans[l][i*a_lists[l+1].dim()+j]>0) { conv=1; break; }
	if (conv==0) trans[l][i*a_lists[l+1].dim()  ]=1.0;
      }
    }//l

    //check trans
    for (l=0;l<numloc-1;l++) {
      for (i=0;i<a_lists[l  ].dim();i++) {
	sum=0;
	for (j=0;j<a_lists[l+1].dim();j++) {
	  sum+=trans[l][i*a_lists[l+1].dim()+j]; }
	if ( !(0.9999<sum && sum<1.0001) ) {
	  errorfs << "ERROR in Bayes adj. of trans. Doesn't sum to 1 for "
		  << "i " << i << " j " << j 
		  << " at locus " << l << " ( " << sum << " ) " << endl;
	  exit(1); }} }


    //check convergence
    conv=0;
    for (i=0;i<init.dim();i++) {
      diff=init(i)-initC(i);
      if ( diff<-1e-8 || 1e-8<diff ) { conv=1; break; } }
    if (conv==0) {
      for (l=0;l<numloc-1;l++) {
	for (i=0;i<trans[l].dim();i++) {
	  diff=trans[l][i]-transC[l][i];
	  if ( diff<-1e-8 || 1e-8<diff ) { conv=1; break; } }
	if (conv==1) break; } }
    if (conv==0) break; 

  }//iter

  //check that two ways of getting a_freq_lists are identical
  //check_a_freq_lists(a_freq_lists, h_freq_lists);

  //make h_freq_lists from trans, init
  makeh2freqlistsH(h_freq_lists,a_freq_lists,init,trans,a_lists,0);

  if (debug) {
    cout << "iter " << iter << endl;
    cout << "init " << endl;
    for (i=0;i<init.dim();i++) cout << init(i) << " "; cout << endl;
    cout << "trans " << endl;
    for (l=0;l<numloc-1;l++) {
      for (i=0;i<trans[l].dim();i++) {
	cout << trans[l][i] << " "; }
      cout << endl; }

    cout << "h2_freq_lists" << endl;
    for (l=0;l<numloc-1;l++) {
      for (i=0;i<h_freq_lists[l].dim();i++) {	
	cout << h_freq_lists[l][i] << " "; }
      cout << endl;
    }

    //compute likelihood
    //make alpha
    makealpha_GfrBAYES(alpha, a_freq_lists, init, trans, b, debug);
    double lik=0,loglik=0;
    for (g=0;g<numgen;g++) {
      lik=0;
      for (i=0;i<alpha[g][numloc-1].dim();i++) {
	lik+=alpha[g][numloc-1][i]; }
      if ( !(0<=lik && lik<=1.0) ) {
	errorfs << "ERROR: lik. for gen " << g << "+1 is " << lik << endl;
	exit(1); }
      loglik+=log(lik); }
    cout << "lik " << loglik << endl; 

  }
}

void makealpha_GfrBAYES(dvectorss &alpha, const dvectors &a_freq_lists,
			const dvector &init,
			const dvectors &trans, const dvectorss &b,
			const int &debug)
{
  //makes alpha object, assuming genotype data

  int g,i,j,k,l,m;
  const int numgen=alpha.dim();
  const int numloc=alpha[0].dim();

  //clear alpha
  for (g=0;g<numgen;g++) {
    for (l=0;l<numloc;l++) {
      alpha[g][l].clear(0); } }

  for (g=0;g<numgen;g++) {

    //make first column
    for (i=0;i<a_freq_lists[0].dim();i++) {
      for (j=0;j<a_freq_lists[0].dim();j++) {

      if (b[g][0][i*a_freq_lists[0].dim()+j] != 0) 
	alpha[g][0][i*a_freq_lists[0].dim()+j]=
	  init[i]*init[j]*
	  b[g][0][i*a_freq_lists[0].dim()+j];

      }
    }
    
    //make rest of alpha
    for (l=0;l<numloc-1;l++) {
      
      for (i=0;i<a_freq_lists[l].dim();i++)
	for (j=0;j<a_freq_lists[l].dim();j++)
	  for (k=0;k<a_freq_lists[l+1].dim();k++)
	    for (m=0;m<a_freq_lists[l+1].dim();m++)
	      
	      
	      alpha[g][l+1][k*a_freq_lists[l+1].dim()+m]+=
		alpha[g][l][i*a_freq_lists[l].dim()+j]*
		trans[l][i*a_freq_lists[l+1].dim()+k]*
		trans[l][j*a_freq_lists[l+1].dim()+m]*
		b[g][l+1][k*a_freq_lists[l+1].dim()+m];

    }
  }

  //check that all entries are between 0 and 1
  for (g=0;g<numgen;g++) {
    for (l=0;l<numloc-1;l++) {
      for (i=0;i<alpha[g][l].dim();i++) {
	if ( !(0.0<=alpha[g][l][i] && alpha[g][l][i]<1.0001) ) {
	  errorfs << "ERROR: alpha[" << g << "][" << l << "][" << i
		  << "] is not between 0 and 1 ["
		  << alpha[g][l][i] << "]" << endl;
	  exit(1); } } } }
  
  if (debug) {
    cout << "alpha [in maketrans_Gfr] " << endl;
    for (g=0;g<numgen;g++) {
      for (l=0;l<numloc;l++) {
	for (i=0;i<alpha[g][l].dim();i++) {	
	  cout << alpha[g][l][i] << " "; }
	cout << endl;
      }
    }
  }
  
}

void makebeta_GfrBAYES(dvectorss &beta, 
		       const dvectors &a_freq_lists,
		       const dvectors &trans, 
		       const dvectorss &b,
		       const int &debug)
{
  //makes beta object, assuming genotype controls

  int g,i,j,k,l,m;
  const int numgen=beta.dim();
  const int numloc=beta[0].dim();

  //clear beta
  for (g=0;g<numgen;g++) {
    for (l=0;l<numloc;l++) {
      beta[g][l].clear(0); } }

  for (g=0;g<numgen;g++) {

    //make last column
    for (i=0;i<beta[g][numloc-1].dim();i++) {
      
      beta[g][numloc-1][i]=1.0;
      
    }

    //make rest of beta
    for (l=numloc-2;l>=0;l--) {
      
      for (i=0;i<a_freq_lists[l].dim();i++) {
	for (j=0;j<a_freq_lists[l].dim();j++) {
	  for (k=0;k<a_freq_lists[l+1].dim();k++) {
	    for (m=0;m<a_freq_lists[l+1].dim();m++) {

	      beta[g][l][i*a_freq_lists[l].dim()+j]+=
		beta[g][l+1][k*a_freq_lists[l+1].dim()+m]*
		trans[l][i*a_freq_lists[l+1].dim()+k]*
		trans[l][j*a_freq_lists[l+1].dim()+m]*
		b[g][l+1][k*a_freq_lists[l+1].dim()+m];
	    }
	  }
	}

      }
    }
  }

  //check that all entries are between 0 and 1
  for (g=0;g<numgen;g++) {
    for (l=0;l<numloc-1;l++) {
      for (i=0;i<beta[g][l].dim();i++) {
	if ( !(0.0<=beta[g][l][i] && beta[g][l][i]<1.0001) ) {
	  errorfs << "ERROR: beta[" << g << "][" << l << "][" << i
	       << "] is not between 0 and 1 ["
	       << beta[g][l][i] << "]" << endl;
	  exit(1); } } } }
  
  if (debug) {
    cout << "beta [in makebeta_Gfr] " << endl;
    for (g=0;g<numgen;g++) {
      for (l=0;l<numloc;l++) {
	for (i=0;i<beta[g][l].dim();i++) {	
	  cout << beta[g][l][i] << " "; }
	cout << endl;
      }
    }
  }
  
}

void makexi_GfrBAYES(dvectorss &xi, const dvectorss &alpha, 
		     const dvectorss &beta, 
		     const dvectors &trans, const dvectorss &b, 
		     const dvectors &a_freq_lists,
		     const int &debug)
{
  //makes xi object, assuming genotype controls

  int g,i,j,k,m,l;
  double denom;
  const int numgen=alpha.dim();
  const int numloc=alpha[0].dim();

  //clear xi
  for (g=0;g<numgen;g++) {
    for (l=0;l<numloc-1;l++) {
      xi[g][l].clear(0); } }

  //make xi
  for (g=0;g<numgen;g++) {

    for (l=0;l<numloc-1;l++) {  

      for (i=0;i<a_freq_lists[l].dim();i++) {
	for (j=0;j<a_freq_lists[l].dim();j++) {
	  for (k=0;k<a_freq_lists[l+1].dim();k++) {
	    for (m=0;m<a_freq_lists[l+1].dim();m++) {
	      
	      xi[g][l][
		       i*a_freq_lists[l].dim()*a_freq_lists[l+1].dim()*
		       a_freq_lists[l+1].dim()+
		       j*a_freq_lists[l+1].dim()*a_freq_lists[l+1].dim()+
		       k*a_freq_lists[l+1].dim()+m
	      ]=
		alpha[g][l][i*a_freq_lists[l].dim()+j]*
		trans[l][i*a_freq_lists[l+1].dim()+k]*
		trans[l][j*a_freq_lists[l+1].dim()+m]*
		b[g][l+1][k*a_freq_lists[l+1].dim()+m]*
		beta[g][l+1][k*a_freq_lists[l+1].dim()+m];

	      //delete me
	      //if (g==0 && l==1 && i==0 && j==1 && k==0 && m==0) 
	      //cout << "moo " << xi[g][l][i*8+j*4+k*2+m*1] << " " 
	      //     << alpha[g][1][i*2+j] << " "
	      //     << b[g][l+1][k*2+m] << " "
	      //     << endl;

	    }
	  }
	}
      }
    }

    //delete me
    //if (g==0) {
    //cout << endl;
    //for (i=0;i<xi[g][1].dim();i++) cout << xi[g][1][i] << " ";
    //cout << endl; }

    //make denominator
    denom=0.0;
    for (i=0;i<xi[g][0].dim();i++) denom+=xi[g][0][i];
    //if (g==0) cout << denom << endl; //delete me
    if (denom<=0) {
      errorfs << "ERROR: in makexi_Gfr, denominator for xi at genotype " 
	      << g << "(+1) <=0 [" << denom << "] " << endl;
      exit(1); }
    //divide all entries by this number
    for (l=0;l<numloc-1;l++) {  
      for (i=0;i<xi[g][l].dim();i++) { 
	xi[g][l][i]/=denom;
      }
    }
    
  }
  
  //check that all entries sum to 1 at each locus for each genotype
  for (g=0;g<numgen;g++) {
    for (l=0;l<numloc-1;l++) {  
      denom=0.0;
      for (i=0;i<xi[g][l].dim();i++) denom+=xi[g][l][i];
      if ( !(0.9999<denom && denom<1.0001) ) {
	errorfs << "ERROR: xi[" << g << "][" << l << "][] does not sum to 1.0 ["
	     << denom << "]" << endl;
	exit(1);
      }
    }
  }

  if (debug) {
    cout << "xi [in makexi_Gfr] " << endl;
    for (g=0;g<numgen;g++) {
      for (l=0;l<numloc-1;l++) {
	for (i=0;i<xi[g][l].dim();i++) {	
	  cout << xi[g][l][i] << " "; }
	cout << endl;
      }
    }
  }
  
}

void makeh2countsGfrxi(dvectors &h_freq_lists, 
		       const dvectorss &xi, 
		       const dvectors &a_freq_lists, const int &debug)
{
  //makes haplotype frequencies from xi, assuming haplotype controls

  int g,i,j,k,m,l;
  double value;

  const int numgen=xi.dim();
  const int numhap=2*numgen;
  const int numloc=h_freq_lists.dim()+1;

  //clear h2_freq_lists
  for (l=0;l<numloc-1;l++) {
    h_freq_lists[l].clear(0); } 

  for (g=0;g<numgen;g++) {
    for (l=0;l<numloc-1;l++) {
      
      for (i=0;i<a_freq_lists[l  ].dim();i++) {
	for (j=0;j<a_freq_lists[l  ].dim();j++) {
	  for (k=0;k<a_freq_lists[l+1].dim();k++) {
	    for (m=0;m<a_freq_lists[l+1].dim();m++) {

	      value=xi[g][l][
			     i*a_freq_lists[l].dim()*a_freq_lists[l+1].dim()*
			     a_freq_lists[l+1].dim()+
			     j*a_freq_lists[l+1].dim()*a_freq_lists[l+1].dim()+
			     k*a_freq_lists[l+1].dim()+
			     m];

	      h_freq_lists[l][i*a_freq_lists[l+1].dim()+k]+=value;
	      h_freq_lists[l][j*a_freq_lists[l+1].dim()+m]+=value;
	      
	    }
	  }
	}
      }
    }
  }

  //divide by numhap
  //for (l=0;l<numloc-2;l++) {
  //for (i=0;i<h3_freq_lists[l].dim();i++) {
  //h3_freq_lists[l][i]/=numhap; } }
  
  if (debug) {
    cout << "makeh2countsGfrxi" << endl;
    for (l=0;l<numloc-1;l++) {
      for (i=0;i<h_freq_lists[l].dim();i++) {	
	cout << h_freq_lists[l][i] << " "; }
      cout << endl;
    }
  }

  //check that frequencies sum to numhap for each pair
  for (l=0;l<numloc-1;l++) {
    value=0.0;
    for (i=0;i<h_freq_lists[l].dim();i++) value+=h_freq_lists[l][i];
    if ( !(numhap-0.0001<value && value<numhap+0.0001) ) {
      errorfs << "ERROR in makeh2countsGfrxi: h2_freq_lists[" 
	      << l << "] does not sum to numhap ["
	      << value << "]" << endl;
      exit(1); } }

  
}