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

#include <fstream.h>
extern ofstream errorfs;



#include "declare.h"

/*
  This file contains source code for functions that make objects needed
  by the estimation functions, from the data which has been inputted. 
  That data may be shuffled, loci may be out of order, etc. These functions
  put it into a form that the estimation functions require.
 */

void makedatafrompeds(igrid &data, const pedvector &peds, 
		      const int &num_markers, const ivector &markers,
		      const int &num_indiv, const ivector &indiv_list,
		      const int &debug)
{
  //this function makes the array of affected haplotypes/genotypes
  //from the peds object read from pedfile

  int h,t,i,j; 

  for (t=0;t<num_markers;t++) {
    for (h=0;h<num_indiv;h++) {
      for (i=0;i<2;i++) {
	data(2*h+i,t)=peds[ indiv_list[h] ].loci[ markers[t] ][i]; 
      }
    }
  } 

  if (debug==1) {
    cout << "data (affected haps./genotypes)" << endl;
    for (i=0;i<data.dimx();i++) {
      for (j=0;j<data.dimy();j++) {
	cout << data(i,j) << " "; }
      cout << endl; }
  }
	  
}//makedatafrompeds

void makemap(dvector &y, const int &num_markers, const ivector &markers,
	     const int &map_type, const int &map_size,
	     const dvector &map,
	     const int &debug)
{
  //this function assembles the intermarker distances, y, from the map
  //given in datafile, and puts these distances in M, rather than cM

  int t;

  if (map_type==1) {
    for (t=0;t<num_markers-1;t++) {
      y[t]=map[t]; 
    }
  }
  else {
    if (map_type==2) {
      for (t=0;t<num_markers-1;t++) y[t]=map[t+1]-map[t]; }
    else {//map_type==3
      for (t=0;t<num_markers-1;t++) 
	y[t]= map[ markers[t+1] ] - map[ markers[t] ]; } }

  //rescale to M from cM
  for (t=0;t<y.dim();t++) y[t]*=0.01;

  if (debug==1) { 
    cout << "y" << endl;
    for (t=0;t<y.dim();t++) cout << y[t] << " "; cout << endl; }

}//makemap

void makeallelecountsff(ivector &allelecountsff, 
			const int &num_markers, const ivector &markers,
			const ivector &num_alleles,
			const int &hap_or_freq,
			const int &debug)
{
  //when allele frequencies for controls are read, this records numbers
  //of alleles at each locus in vector allelecountsff

  int t;

  if (hap_or_freq==1 || hap_or_freq==2) {
    errorfs << "ERROR: makeallelecountsff() called for hap_or_freq=1 or =2\n";
    exit(1);
  }

  if (hap_or_freq==3)
    for (t=0;t<num_markers;t++) {
      allelecountsff(t)= num_alleles[ markers[t] ]; 
    }
  else for (t=0;t<num_markers;t++) {
    allelecountsff(t)= num_alleles[ t ]; 
  }
  if (debug==1) {
    cout << "allelecountsff" << endl;
    for (t=0;t<num_markers;t++) cout << allelecountsff[t] << " "; 
    cout << endl; }

}//makeallelecountsff


void makeindfreq(igrid &freqallele, dgrid &freqfreq,
		 const int &num_markers, const ivector &markers,
		 const ivector &num_alleles,
		 const ivectors &allele_list, const dvectors &allele_freq,
		 const int &hap_or_freq,
		 const int &debug)
{
  //when allele frequencies are read, this forms single-marker
  //frequencies objects freqallele and freqfreq from this info

  int t,rw,rwtemp,i,j;
  
  if (hap_or_freq==1 || hap_or_freq==2) {
    errorfs << "ERROR: makeallelecountsff() called for hap_or_freq=1 or =2\n";
    exit(1);
  }

  if (hap_or_freq==3) {
    //find number of rows in freqallele,freqfreq: max_l (num_alleles[l])
    rw=num_alleles[ markers[0] ];
    for (t=1;t<num_markers;t++) {
      rwtemp=num_alleles[ markers[t] ];
      if (rwtemp>rw) rw=rwtemp; } 
    
    //initialize freqallele,freqfreq;
    if (rw>20) {
      errorfs << "Program cannot currently accept more than 20 alleles \n"
	   << "per locus \n";
      exit(1);
    }
    freqallele.init(rw,num_markers);
    freqfreq.init(rw,num_markers);
  
    //write to grids
    for (t=0;t<num_markers;t++) {
      for (i=0;i<num_alleles[ markers[t] ];i++) {
	freqallele(i,t)=allele_list[markers(t)][i];
	freqfreq(i,t)  =allele_freq[markers(t)][i];
      }
    }
  }
  else {//hap_or_freq=4
    //find number of rows in freqallele,freqfreq: max_l (num_alleles[l])
    rw=num_alleles[ 0 ];
    for (t=1;t<num_markers;t++) {
      rwtemp=num_alleles[ t ];
      if (rwtemp>rw) rw=rwtemp; } 
    
    //initialize freqallele,freqfreq;
    if (rw>20) {
      errorfs << "Program cannot currently accept more than 20 alleles \n"
	   << "per locus \n";
      exit(1);
    }
    freqallele.init(rw,num_markers);
    freqfreq.init(rw,num_markers);
  
    //write to grids
    for (t=0;t<num_markers;t++) {
      for (i=0;i<num_alleles[ t ];i++) {
	freqallele(i,t)=allele_list[t][i];
	freqfreq(i,t)  =allele_freq[t][i];
      }
    }
  }
  if (debug==1) {
    cout << "freqallele" << endl;
    for (i=0;i<freqallele.dimx();i++) {
      for (j=0;j<freqallele.dimy();j++) {
	cout << freqallele(i,j) << " "; }
      cout << endl; 
    }
    cout << "freqfreq" << endl;
    for (i=0;i<freqfreq.dimx();i++) {
      for (j=0;j<freqfreq.dimy();j++) {
	cout << freqfreq(i,j) << " "; }
      cout << endl; 
    }

  }
}//makeindfreq

void makeindjointfreq2(dgrid &jointfreq2, const ivector &allelecountsff,
		       const dgrid &freqfreq, const int &num_markers,
		       const int &debug)
{
  //when only allele frequencies are available, this creates array
  //of 2-marker haplotype frequencies ASSUMING LINKAGE EQUILIBRIUM

  int t,rw,rwtemp,i,j;

  //find number of rows needed for jointfreq2
  rw=allelecountsff(0)*allelecountsff(1);
  for (t=1;t<num_markers-1;t++) {
    rwtemp=allelecountsff(t)*allelecountsff(t+1);
    if (rwtemp>rw) rw=rwtemp; 
  }

  //initialize jointfreq2;
  jointfreq2.init(rw,num_markers-1);

  //fill in jointfreq2
  for (t=0;t<num_markers-1;t++) {
    for (i=0;i<allelecountsff(t);i++) {
      for (j=0;j<allelecountsff(t+1);j++) {
	jointfreq2( allelecountsff(t+1)*i+j,t )=
	  freqfreq(i,t)*freqfreq(j,t+1); } } }

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

}//makeindjointfreq2

void makejointfreq2C(dgrid &jointfreq2C, const ivector &allelecountsff,
		     const dgrid &freqfreq, const int &C, 
		     const int &debug)
{
  //when allele frequencies are inputted, this function makes 2-marker
  //haplotype frequencies between each marker and the center, ASSUMING
  //LINKAGE EQUILIBRIUM

  int i,j,t;
  const int numloc=allelecountsff.dim();

  //initialize jointfreq2C
  jointfreq2C.init(freqfreq.dimx()*allelecountsff(C),freqfreq.dimy());
  jointfreq2C.clear();
  for (i=0;i<allelecountsff(C);i++) {
    jointfreq2C(i*freqfreq.dimx()+i,C)=freqfreq(i,C);
    for (t=0;t<numloc;t++) {
      if (t!=C) {
	for (j=0;j<allelecountsff(t);j++) {
	  jointfreq2C(i*freqfreq.dimx()+j,t)=
	    freqfreq(j,t)*freqfreq(i,C); } } } }

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

}

void makejointfreq3(dvector &jointfreq3, const dgrid &freqfreq, 
		    const ivector &allelecountsff,
		    const int &C, const int &debug)
{
  //when only allele frequencies are available, this function makes 
  //3-marker hap. freqs for center and flanking loci, ASSUMING LINKAGE
  //EQUILIBRIUM

  int i,j,k;

  //initialize jointfreq3
  jointfreq3.init( allelecountsff(C-1)*
		   allelecountsff(C  )*
		   allelecountsff(C+1) );
  
  for (i=0;i<allelecountsff(C-1);i++) {
    for (j=0;j<allelecountsff(C);j++) {
      for (k=0;k<allelecountsff(C+1);k++) {
	
	jointfreq3(i*allelecountsff(C)*allelecountsff(C+1)+
		   j*allelecountsff(C+1)+
		   k)=
	  freqfreq(i,C-1)*freqfreq(j,C)*freqfreq(k,C+1); } } }
  
  if (debug==1) {
    cout << "jointfreq3" << endl;
    for (i=0;i<jointfreq3.dim();i++) cout << jointfreq3(i) << " ";
    cout << endl;
  }

}

void makemut(dvector &mut_markers, const dvector &mut_rates,
	     const ivector &markers, const int &debug)
{
  //this function assembles vector of mutation rates in map order

  int j;
  for (j=0;j<markers.dim();j++) {
    mut_markers(j)=mut_rates( markers(j) ); }

  if (debug==1) {
    cout << "mut_markers" << endl;
    for (j=0;j<mut_markers.dim();j++) cout << mut_markers(j) << " ";
    cout << endl;
  }

}//makemut
  
void rescalemut(dvector &mut_markers_rescale, const dvector &mut_markers,
		const ivector &allelecountsff, const int &C, 
		const int &debug)
{
  //this function takes the inputted mutation rate, interpreted as the
  //rate of mutation into another allele per meisis per locus and rescales
  //it-- in the model the mutation rate also includes a possibility of
  //mutating into itself

  int j;
  for (j=0;j<mut_markers.dim();j++) {
    if (allelecountsff( j )==1) mut_markers_rescale(j)=0;
    else {
      if ( mut_markers( j )==0) mut_markers_rescale(j)=0; 
      else {
	mut_markers_rescale(j) = 
	  mut_markers( j ) *
	  allelecountsff( j )/
	  ( allelecountsff( j ) - 1 ); 
      }
    }
  }
   
  if (debug==1) {
    for (j=0;j<mut_markers_rescale.dim();j++) 
      cout << mut_markers_rescale(j) << " ";
    cout << endl;
  }
  
}//rescalemut

//freqfunct      
void jointfreq3reset(dvector &jointfreq3, const igrid &freqallele,
		     const dgrid &freqfreq, const ivector &allelecounts,
		     const int &callele, const int &loc,
		     const int &debug)
{
  //this function takes the three-marker haplotype frequencies around the
  //center and conditions on matching at the center-- used for haplotype
  //data and for mapping. In the first case, we sort haplotypes by center
  //allele; when mapping all alleles match at the center by assumption.


  int i,j,k, pos=-1;
  const int maxnum=freqallele.dimx();

  for (i=0;i<maxnum;i++) {
    if (freqallele(i,loc)==callele) {
      pos=i;
      break; } }
  if (pos==-1) errorfs << "WARNING: allele " << callele << " not found at locus "
		    << loc << "+1 in jointfreq3reset()";
  
  //now condition on matching at center
  for (i=0;i<allelecounts(loc-1);i++) {
    for (j=0;j<allelecounts(loc);j++) {
      for (k=0;k<allelecounts(loc+1);k++) {
	if (j==pos) {
	  jointfreq3(i*allelecounts(loc)*allelecounts(loc+1)+
		     j*allelecounts(loc+1)+
		     k) /= freqfreq(pos,loc); }
	else {
	  jointfreq3(i*allelecounts(loc)*allelecounts(loc+1)+
		     j*allelecounts(loc+1)+
		     k) = 0; } } } }

  if (debug==1) {
    cout << "jointfreq3 (reset)" << endl;
    for (i=0;i<jointfreq3.dim();i++) cout << jointfreq3(i) << " ";
    cout << endl;
  }


}
	
void jointfreq2reset(dgrid &jointfreq2, const igrid &freqallele,
		     const dgrid &freqfreq, const ivector &allelecounts,
		     const int &callele, const int &loc,
		     const int &debug)
{
  //this function takes the two-marker haplotype frequencies
  //and conditions on matching at the center-- used for haplotype
  //data and for mapping. In the first case, we sort haplotypes by center
  //allele; when mapping all alleles match at the center by assumption.

  int i,j,k, pos=-1;
  const int maxnum=freqallele.dimx();
  const int numloc=allelecounts.dim();

  for (i=0;i<maxnum;i++) {
    if (freqallele(i,loc)==callele) {
      pos=i;
      break; } }
  if (pos==-1) errorfs << "WARNING: allele " << callele << " not found at locus "
		    << loc << "+1 in jointfreq2reset()";

  //now condition on matching at center
  if (loc==0) {
    for (i=0;i<allelecounts(0);i++) {
      for (j=0;j<allelecounts(1);j++) {
	if (i==pos) {
	  jointfreq2(i*allelecounts(1)+j,0) /= freqfreq(pos,loc); }
	else {
	  jointfreq2(i*allelecounts(1)+j,0) = 0; } } } }
  else {
    if (loc==numloc-1) {
      for (i=0;i<allelecounts(numloc-2);i++) {
	for (j=0;j<allelecounts(numloc-1);j++) {
	  if (j==pos) {
	    jointfreq2(i*allelecounts(numloc-1)+j,numloc-2) 
	      /=freqfreq(pos,loc); }
	  else {
	    jointfreq2(i*allelecounts(numloc-1)+j,numloc-2)
	      = 0; } } } } 
    else {
      for (i=0;i<allelecounts(loc-1);i++) {
	for (j=0;j<allelecounts(loc);j++) {
	  
	  if(j==pos) {
	    jointfreq2(i*allelecounts(loc)+j,loc-1) 
	      /=freqfreq(pos,loc); }
	  else {
	    jointfreq2(i*allelecounts(loc)+j,loc-1) 
	      = 0; } } }
      for (j=0;j<allelecounts(loc);j++) {
	for (k=0;k<allelecounts(loc+1);k++) {
	  if(j==pos) {
	    jointfreq2(j*allelecounts(loc+1)+k,loc) 
	      /=freqfreq(pos,loc); }
	  else { 
	    jointfreq2(j*allelecounts(loc+1)+k,loc) 
	      = 0; } } }
    }
  }

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

}

void jointfreq2Creset(dgrid &jointfreq2C, const igrid &freqallele,
		      const dgrid &freqfreq, const ivector &allelecounts,
		      const int &callele, const int &loc,
		      const int &debug)
{
  //this function takes the two-marker haplotype frequencies around the
  //center and conditions on matching at the center-- used for haplotype
  //data and for mapping. In the first case, we sort haplotypes by center
  //allele; when mapping all alleles match at the center by assumption.

  int i,j, pos=-1;
  const int maxnum=freqallele.dimx();
  const int numloc=freqallele.dimy();

  //find position in freqallele and set freq to 1.0
  for (i=0;i<maxnum;i++) {
    if (freqallele(i,loc)==callele) {
      pos=i;
      break; } }
  if (pos==-1) errorfs << "WARNING: allele " << callele << " not found at locus "
		    << loc << "+1 in jointfreq2reset()";

  //now condition on matching at center
  for (j=0;j<numloc;j++) {
    for (i=0;i<pos*maxnum;i++) {
      jointfreq2C(i,j) = 0; } 
    for (i=pos*maxnum;i<(pos+1)*maxnum;i++) { 
      jointfreq2C(i,j) /=freqfreq(pos,loc); } 
    for (i=(pos+1)*maxnum;i<jointfreq2C.dimx();i++) {
      jointfreq2C(i,j) = 0; } }

  if (debug==1) {
    cout << "jointfreq2C (reset)" << endl;
    for (i=0;i<jointfreq2C.dimx();i++) {
      for (j=0;j<jointfreq2C.dimy();j++) {
	cout << jointfreq2C(i,j) << " "; }
      cout << endl;
    }
  }


}

void freqfreqreset(dgrid &freqfreq, const igrid &freqallele, 
		   const int &callele, const int &loc,
		   const int &debug)
{
  //this function adjusts the allele frequencies to condition for matching
  //at the center locus

  int i,j,pos=-1;
  const int maxnum=freqallele.dimx();
  for (i=0;i<maxnum;i++) freqfreq(i,loc)=0;

  //find position in freqallele and set freq to 1.0
  for (i=0;i<maxnum;i++) {
    if (freqallele(i,loc)==callele) {
      pos=i;
      break; } }
  if (pos==-1) errorfs << "WARNING: allele " << callele << " not found at locus "
		   << loc << "+1 in freqfreqreset()";
  freqfreq(pos,loc)=1.0;  

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


}
    
void makecondfreq2l(dgrid &condfreq2l, const igrid &freqallele,
		    const dgrid &freqfreq, const dgrid &jointfreq2,
		    const ivector &allelecounts, const int &C,
		    const int &debug)
{
  //this function takes 2-marker hap. frequencies and marginal
  //allele frequencies and forms conditional allele frequencies
  //where the allele on which we condition is on the left
  condfreq2l.clear();

  int i,j,t;
  for (t=0;t<C;t++) {
    for (i=0;i<allelecounts(t);i++) {
      for (j=0;j<allelecounts(t+1);j++) {
	condfreq2l(i*allelecounts(t+1)+j,t)
	  =jointfreq2(i*allelecounts(t+1)+j,t)/freqfreq(i,t); } } }

  if (debug==1) {
    cout << "condfreq2l" << endl;
    for (i=0;i<condfreq2l.dimx();i++) {
      for (j=0;j<condfreq2l.dimy();j++) {
        cout << condfreq2l(i,j) << " "; }
      cout << endl; }
  }
  //assert that all entries are between 0 and 1
  for (i=0;i<condfreq2l.dimx();i++) {
    for (j=0;j<condfreq2l.dimy();j++) {
      assert(-0.00001<condfreq2l(i,j) && condfreq2l(i,j)<1.00001);} }

}

void makecondfreq2r(dgrid &condfreq2r, const igrid &freqallele,
		    const dgrid &freqfreq, const dgrid &jointfreq2,
		    const ivector &allelecounts, const int &C,
		    const int &debug)
{
  //this function takes 2-marker hap. frequencies and marginal
  //allele frequencies and forms conditional allele frequencies
  //where the allele on which we condition is on the right

  int i,j,t;
  condfreq2r.clear(0);
  const int numloc=freqallele.dimy();
  for (t=C;t<numloc-1;t++) {
    for (i=0;i<allelecounts(t);i++) {
      for (j=0;j<allelecounts(t+1);j++) {
	if (freqfreq(j,t+1)<=0.0) {
	  errorfs << "ERROR [makecondfreq2r]: allele " << freqallele(j,t+1) 
		  << " [" << j << "]  at locus " << t+1 
		  << " [+1] has frequency " << freqfreq(j,t+1)
		  << "; cannot divide by 0" << endl;
	  exit(1); }
	else 
	  condfreq2r(i*allelecounts(t+1)+j,t-C)
	    =jointfreq2(i*allelecounts(t+1)+j,t)/freqfreq(j,t+1); } } }
  

  //assert that all entries are between 0 and 1
  for (i=0;i<condfreq2r.dimx();i++) {
    for (j=0;j<condfreq2r.dimy();j++) {
      if ( !(-0.00001<condfreq2r(i,j) && condfreq2r(i,j)<1.00001) ) {
	errorfs << endl << "makecondfreq2r: " << " "
	     << condfreq2r.dimx() << " " << condfreq2r.dimy() << " " 
	     << i << " " << j << " " << j+C << " "
	     << condfreq2r(i,j) << endl;
	exit(1); }
      assert(-0.00001<condfreq2r(i,j) && condfreq2r(i,j)<1.00001);} }

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

}

void makecondfreq3l(dvector &condfreq3l, const dvector &jointfreq3,
		    const dgrid &freqfreq,
		    const ivector &allelecounts, const int &C,
		    const int &debug)
{
  //this function takes 3-marker hap. frequencies and marginal
  //allele frequencies and forms conditional hap frequencies
  //where the allele on which we condition is on the left

  int i,j,k;
  for (i=0;i<allelecounts(C-1);i++) {
    for (j=0;j<allelecounts(C);j++) {
      for (k=0;k<allelecounts(C+1);k++) {
	if (freqfreq(i,C-1) !=0)
	  condfreq3l(i*allelecounts(C)*allelecounts(C+1)+
		     j*allelecounts(C+1)+
		     k)
	    =jointfreq3(i*allelecounts(C)*allelecounts(C+1)+
			j*allelecounts(C+1)+
			k)/freqfreq(i,C-1); 
	else condfreq3l(i*allelecounts(C)*allelecounts(C+1)+
			j*allelecounts(C+1)+
			k)
	       = 0.0;
      } } }

  if (debug==1) {
    cout << "condfreq3l" << endl;
    for (i=0;i<condfreq3l.dim();i++) cout << condfreq3l(i) << " ";
    cout << endl; }

  //assert that all entries are between 0 and 1
  for (i=0;i<condfreq3l.dim();i++) {
    assert(-0.00001<condfreq3l(i) && condfreq3l(i)<1.00001);}

}
		   
void makecondfreq3r(dvector &condfreq3r, const dvector &jointfreq3,
		    const dgrid &freqfreq,
		    const ivector &allelecounts, const int &C,
		    const int &debug)
{
  //this function takes 3-marker hap. frequencies and marginal
  //allele frequencies and forms conditional hap frequencies
  //where the allele on which we condition is on the right

  int i,j,k;
  for (i=0;i<allelecounts(C-1);i++) {
    for (j=0;j<allelecounts(C);j++) {
      for (k=0;k<allelecounts(C+1);k++) {
	condfreq3r(i*allelecounts(C)*allelecounts(C+1)+
		   j*allelecounts(C+1)+
		   k)
	  =jointfreq3(i*allelecounts(C)*allelecounts(C+1)+
		      j*allelecounts(C+1)+
		      k)/freqfreq(k,C+1);
 
      } } }


  //assert that all entries are between 0 and 1
  for (i=0;i<condfreq3r.dim();i++) {
    assert(-0.00001<condfreq3r(i) && condfreq3r(i)<1.00001);}

  if (debug==1) {
    cout << "condfreq3r" << endl;
    for (i=0;i<condfreq3r.dim();i++) cout << condfreq3r(i) << " ";
    cout << endl; }

}

void makecondfreq3F(dgrid &condfreq3F, const dvector &jointfreq3, 
		    const dgrid &freqfreq,
		    const ivector &allelecounts, const int &C, 
		    const int &debug)
{
  //this function takes 3-marker hap. frequencies and marginal
  //allele frequencies and forms conditional hap frequencies
  //where the alleles on which we condition are the flankers

  int i,j,k;

  //set entries to 0;
  for (i=0;i<condfreq3F.dimx();i++) condfreq3F(i,0)=0; 

  //make joint frequencies
  for (i=0;i<allelecounts(C-1);i++) {
    for (k=0;k<allelecounts(C+1);k++) {

      for (j=0;j<allelecounts(C);j++) {
	condfreq3F(i*allelecounts(C+1)+k,0)+=
	  jointfreq3(i*allelecounts(C)*allelecounts(C+1)+
		     j*allelecounts(C+1)+
		     k); }
    }
  }

  //copy jointfreq's into 2nd column
  for (i=0;i<condfreq3F.dimx();i++) {
    condfreq3F(i,1)=condfreq3F(i,0); }
  
  //divide by appropriate freq to condition
  for (i=0;i<allelecounts(C-1);i++) {
    for (k=0;k<allelecounts(C+1);k++) {
      
      condfreq3F(i*allelecounts(C+1)+k,0)*=
	1.0/freqfreq(i,C-1);
      condfreq3F(i*allelecounts(C+1)+k,1)*=
	1.0/freqfreq(k,C+1);
    } 
  }

  //assert that all entries are between 0 and 1
  for (i=0;i<condfreq3F.dimx();i++) {
    for (j=0;j<condfreq3F.dimy();j++) {
      if ( !(-0.00001<condfreq3F(i,j) && condfreq3F(i,j)<1.00001) )
	errorfs << "ERROR: problem in condfreq3F " 
		<< i << " " << j << " " << condfreq3F(i,j) << endl;
      assert(-0.00001<condfreq3F(i,j) && condfreq3F(i,j)<1.00001);
    }
  }
 
  if (debug==1) {
    cout << "condfreq3F" << endl;
    for (i=0;i<condfreq3F.dimx();i++) {
      for (j=0;j<condfreq3F.dimy();j++) {
	cout << condfreq3F(i,j) << " "; }
      cout << endl;
    }
  }

}

void makecondfreq2C(dgrid &condfreq2C, const dgrid &jointfreq2C, 
		    const igrid &freqallele,
		    const dgrid &freqfreq, const ivector &allelecounts,
		    const int &loc,
		    const int &debug)
{
  //this function takes 2-marker hap. frequencies (between center
  //and other markers) and marginal
  //allele frequencies and forms conditional hap frequencies
  //where the alleles on which we condition is the non-center allele

  int i,j,t;
  const int maxnum=freqallele.dimx();
  const int numloc=freqallele.dimy();
  for (i=0;i<maxnum;i++) condfreq2C(i,loc)=0;

  for (i=0;i<allelecounts(loc);i++) {
    for (t=0;t<numloc;t++) {
      for (j=0;j<allelecounts(t);j++) {
	if (freqfreq(j,t)==0) {
	  if (jointfreq2C(i*maxnum+j,t)!=0) {
	    errorfs << "Error: frequency for allele " << j << " at locus "
		 << t << " (+1) given allele " << i << " at " << loc 
		 << " is !0/0. \n";
	    exit(1);
	  }
	  else condfreq2C(i*maxnum+j,t)=0;
	}
	else condfreq2C(i*maxnum+j,t)=
	       jointfreq2C(i*maxnum+j,t)/freqfreq(j,t); 
	
      } } }

  //assert all values between 0 and 1
  for (i=0;i<condfreq2C.dimx();i++) {
    for (j=0;j<condfreq2C.dimy();j++) {
      assert(-0.00001<condfreq2C(i,j) && condfreq2C(i,j)<1.00001);} }

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


} 

void makex(const dvector &y, dvector &x, const int &C, const int &debug)
{
  //this function forms a vector of cumulative (unsigned) distance from 
  //the center, namely x, in which x[C]=0

  int i;
  x[C]=0;
  for (i=C-1;i>=0;i--) x[i]=x[i+1]+y[i];
  for (i=C+1;i<x.dim();i++) x[i]=x[i-1]+y[i-1];

  if (debug==1) {
    cout << "x" << endl;
    for (i=0;i<x.dim();i++) cout << x(i) << " "; cout << endl;
  }

}//makex

void makecontrolsfrompeds(igrid &controls, const pedvector &peds, 
			  const int &num_markers, const ivector &markers,
			  const int &num_control, const ivector &control_list,
			  const int &debug)
{
  //when control haps or genotypes are available, this function forms
  //an array of said items, for use later in estimating the haplotype freqs

  int h,t,i,j; 

  for (t=0;t<num_markers;t++) {
    for (h=0;h<num_control;h++) {
      for (i=0;i<2;i++) {
	//cout << indiv_list[h] << " " << markers[t] << " " << i << endl;
	controls(2*h+i,t)=peds[ control_list[h] ].loci[ markers[t] ][i]; 
      }
    }
  } 

  if (debug==1) {
    cout << "controls (control haps./genotypes)" << endl;
    for (i=0;i<controls.dimx();i++) {
      for (j=0;j<controls.dimy();j++) {
	cout << controls(i,j) << " "; }
      cout << endl; }
  }
	  
}//makecontrolsfrompeds

void make_augy(dvector &augy, const dvector &y, const int &C,
	       const int &sub, const int &num_sub, const int &debug)
{
  //this function takes the intermarker distances and adjusts them,
  //allowing for an extra locus, namely C. Only one distances is changed,
  //but others are displaced by 1 entry in the vector

  //note: this is awkward, because C counts from 1 and sub from 0

  int j;
  
  augy.clear(0);
  for (j = 0; j < C; j++)
    augy(j) = y(j);
  for (j = C; j < y.dim(); j++)
    augy(j + 1) = y(j);
  augy(C - 1) = y(C - 1)*(sub+1.0)/(num_sub+1.0);
  augy(C) = y(C - 1)-augy(C - 1);
  
  if (debug==1) {
    cout << "augy" << endl;
    for (j=0;j<augy.dim();j++) cout << augy(j) << " ";
    cout << endl; }
}

void make_mody(dvector &mody, const dvector &y, const int &C,
	       const int &sub, const int &num_sub, 
	       const int &res, const int &map_res,
	       const int &debug)
{
  //this function takes the intermarker distances and adjusts them,
  //allowing for an extra locus, namely C. Only one distances is changed,
  //but others are displaced by 1 entry in the vector

  //note: this is awkward, because C counts from 1 and sub from 0

  int j;
  
  mody.clear(0);
  for (j = 0; j < C; j++)
    mody(j) = y(j);
  for (j = C; j < y.dim(); j++)
    mody(j + 1) = y(j);
  mody(C - 1) =  y(C - 1)*
    ( (map_res+1.0)*sub+(res+1.0) )/
    ( num_sub*(map_res+1.0) );
  mody(C) = y(C - 1)-mody(C - 1);

  if (debug==1) {
    cout << "mody" << endl;
    for (j=0;j<mody.dim();j++) cout << mody(j) << " ";
    cout << endl; }
}

void makejointfreq3mkv(dvector &jointfreq3, const dgrid &jointfreq2,
		       const dgrid &freqfreq,
		       const ivector &allelecounts, const int &C,
		       const int &debug)
{
  //this function estimates the frequencies for three-locus haploytypes
  //in which the variant is an allele at the middle locus 

  jointfreq3.init(allelecounts(C-1)*allelecounts(C  )*allelecounts(C+1));
  
  int i,j,k;

  for (i=0;i<allelecounts(C-1);i++) {
    for (j=0;j<allelecounts(C  );j++) {
      for (k=0;k<allelecounts(C+1);k++) {

	if (freqfreq(j,C)==0) 
	  jointfreq3(i*allelecounts(C  )*allelecounts(C+1)+
		     j*allelecounts(C+1)+
		     k)=0;
	else 
	  jointfreq3(i*allelecounts(C  )*allelecounts(C+1)+
		     j*allelecounts(C+1)+
		     k)=
	    jointfreq2(i*allelecounts(C  )+j,C-1)*
	    jointfreq2(j*allelecounts(C+1)+k,C  )/
	    freqfreq(j,C);
      }
    }
  }
  
  if (debug) {
    cout << "jointfreq3 (mkv)" << endl;
    for (i=0;i<jointfreq3.dim();i++) cout << jointfreq3(i) << " ";
    cout << endl; }
}

void jointfreq2resetmkv(dgrid &jointfreq2, dvectors &h_freq_lists,
			const ivectors &a_lists,
			const int &callele, const int &loc,
			const int &debug)
{
  //currently assumes LE<C<RE; otherwise I would have to reset
  //jointfreq2C

  int l,i,j;
  int numloc=a_lists.dim();
  double sum=0.0;
  double zero=1e-3;

  //make vector of allele frequencies at initial allele
  //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); }

  //make matrix of conditional allele frequencies
  //fill in objects
  make_a_freq_lists_Gfr(a_freq_lists, h_freq_lists, 1);
  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, 0);
  //exit(1);

  //make changes to condition on ancestral allele
  //first find position of ancestral allele
  int pos=-1;
  for (i=0;i<a_lists[loc].dim();i++) {
    if (a_lists[loc][i] == callele) { pos=i; break; } }
  if (pos==-1) {
    errorfs << "center allele not found in list of alleles "
	    << "(jointfreq2resetmkv)" << endl;
    exit(1); }

  if (loc==0) {
    errorfs << "ERROR: dhsmap cannot currently run with no markers \n"
	    << "to one side of variant (jointfreq2resetmkv)\n";
    exit(1); }

  //MEAT OF THE FUNCTION
  for (j=0;j<trans[loc-1].dim();j++) trans[loc-1][j]=0.0;
  for (i=0;i<a_lists[loc-1].dim();i++) {
    trans[loc-1][i*a_lists[loc].dim()+pos] = 1.0; }

  //now adjust transitions from zero so that there are no haplotypes
  //with zero probability;
  sum=0; int num_j=0;
  for (j=0;j<a_lists[loc+1].dim();j++) 
    if (trans[loc][pos*a_lists[loc+1].dim()+j]<=zero) 
      { num_j++; trans[loc][pos*a_lists[loc+1].dim()+j]+=zero; }
  for (j=0;j<a_lists[loc+1].dim();j++) 
    trans[loc][pos*a_lists[loc+1].dim()+j]/=(1+num_j*zero);

  //make jointfreq2 from initial and conditional allele frequencies
  make_a_freq_lists_fr_trans(a_freq_lists, init,trans, numloc-1, 0);

  for (l=0;l<h_freq_lists.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[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.dim();l++) {
    sum=0;
    for (i=0;i<h_freq_lists[l].dim();i++) sum+=h_freq_lists[l][i];
    if ( !(0.999<sum && sum<1.001) ) {
      errorfs << "ERROR: col. " << l << " in h_freq_list [jointfreq2resetmkv]"
	   << "\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);

  makejf2(jointfreq2,h_freq_lists, 0);

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

void makecondfreq2Cdt1(dgrid &condfreq2C, 
		       const dgrid &freqfreq, const ivector &allelecounts,
		       const int &loc,
		       const int &debug)
{

  int i,j,t;
  const int maxnum=freqfreq.dimx();
  const int numloc=freqfreq.dimy();
  for (i=0;i<maxnum;i++) condfreq2C(i,loc)=0;

  for (i=0;i<allelecounts(loc);i++) {
    for (t=0;t<numloc;t++) {
      for (j=0;j<allelecounts(t);j++) {
	if (freqfreq(j,t)==0) condfreq2C(i*maxnum+j,t)=0;
	else { 
	  condfreq2C(i*maxnum+j,t)= 1.0; }
      } } }

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