/* This file contains the highest level source code for DHS mapping
   software */

#include <iostream.h>
#include <iomanip.h>
#include <fstream.h>

#include <stdio.h>
#include <stdlib.h>
#include <ctype.h>

#include "includes.h"
#include "unistd.h"
#include "grid.h"
#include "vector.h"
#include "declare.h"
#include "time.h"

ofstream errorfs;

int main(int argc, char** argv)
{
  if (argc != 3) {
    cerr << "Usage: readinput datafile pedfile\n";
    exit(1);
  }
  
  char* datafile = argv[1];
  char* pedfile = argv[2];
  int debug = 0;

  errorfs.open("dhsmap_errors");
  if (!errorfs.is_open()) {
    cout << "ERROR: Unable to open dhsmap_errors for reading\n"; 
    exit(1);
  }

  int i,j;
  
  //declarations needed for readinput
  int program_code, data_type, num_loci, numblanks, num_markers;
  ivector markers; 
  int map_type, map_size;
  dvector map; 
  int num_indiv, num_total_affecteds;
  ivector indiv_list; 
  int hap_or_freq, mkvorder, bayes, num_total_control, num_control;
  ivector control_list, num_alleles;
  ivectors allele_list; 
  dvectors allele_freq;
  dvector mut_rates;
  int est_p;
  double p, max_res;
  int map_res, max_cand, anc_hap_known, num_anc_hap_known, 
    C_loc, C_all, E_int, L_End, R_End;
  ivectors anc_hap; 
  double fixed_thresh, var_thresh;
  pedvector peds;
  //char ancout[100]; char maxout[100]; char oneout[100];
  simpstrg resout, ancout, maxout, oneout;

  //read input
  readinput(datafile, pedfile, 0,
	    program_code, data_type, num_loci,
	    numblanks, num_markers, markers, 
	    map_type, map_size, map, 
	    num_total_affecteds,num_indiv, 
	    indiv_list, hap_or_freq, mkvorder, bayes,
	    num_total_control, num_control,
	    control_list, num_alleles,
	    allele_list, allele_freq,
	    mut_rates, est_p, p, max_res,
	    map_res, max_cand, anc_hap_known,
	    num_anc_hap_known, anc_hap, C_loc, C_all, E_int,
	    resout,ancout,maxout,oneout,
	    L_End, R_End, fixed_thresh, 
	    var_thresh, peds);

  //check that inputted C_all matches anc_hap for pc==3
  if (program_code==3) {
    if (C_all != anc_hap[0](C_loc) ) {
      errorfs << "ERROR: Entered allele at center (" << C_all 
	      << ") \n does not match allele in ancestral haplotype ("
	      << anc_hap[0](C_loc) << ") \n";
      exit(1); }
  }

  //initialize anc_hap
  if (program_code==2) { anc_hap.init(1); anc_hap[0].init(num_markers); }
  
  //markers, indiv_list, control_list given as counting from 1; 
  //change to counting from 0
  for (i=0;i<markers.dim();i++) markers(i)+=-1;
  for (i=0;i<indiv_list.dim();i++) indiv_list(i)+=-1;
  if (hap_or_freq<=2)
    for (i=0;i<control_list.dim();i++) control_list(i)+=-1;
  //Center_loc, Left_End, Right_End given as counting from 1; 
  //change to counting from 0
  if (program_code==2 || program_code==3) {
    C_loc+=-1;
    if (program_code==3) {
      L_End+=-1;
      R_End+=-1; }
  }
  
  //make data object
  igrid data(2*num_indiv,num_markers);
  makedatafrompeds(data, peds, num_markers, markers,
		   num_indiv, indiv_list, 0);

  //make map object (AND RESCALE TO M from cM)
  dvector y(num_markers - 1);
  makemap(y, num_markers, markers, map_type, map_size, map, debug);
  
  //rescale max_res from cM to M
  max_res/=100;
  
  //make frequencies objects
  ivector allelecountsff(num_markers);
  ivector additionalalleles(num_markers);
  ivectors c_allele_lists; c_allele_lists.init(num_markers);
  if (hap_or_freq !=3 && hap_or_freq !=4) {
    allele_list.init(num_markers);
    allele_freq.init(num_markers); }
  //dvectors hap_freq; hap_freq.init(num_markers-1);//OLD
  dvectors h2_freq_lists; h2_freq_lists.init(num_markers-1);//NEW
  dvectors h2_freq_lists_adj; h2_freq_lists_adj.init(num_markers-1);//NEW
  dvectors h3_freq_lists; h3_freq_lists.init(num_markers-2);
  dvectors h3_freq_lists_adj;

  igrid freqallele;
  dgrid freqfreq, jointfreq2,jointfreq2C;
  dvector jointfreq3;
  dgrid jointfreq3mkv2;
  dgrid jointfreq3fmkv2;
  
  igrid controls; 
  
  //set seed for random generation of EM starting points for 
  //estimating haplotype frequencies for genotype data
  srand((long)time(NULL));
  
  //igrid controls1;//test: double rows;
  
  if (hap_or_freq==1 || hap_or_freq==2) {//controls available
    
    //make controls grid
    controls.init(2*num_control, num_markers);
    makecontrolsfrompeds(controls, peds, num_markers, 
			 markers, num_control, control_list,0);

    
    //make lists of alleles in controls
    count_alleles(allelecountsff, controls, 0);
    for (j=0;j<num_markers;j++) 
      { c_allele_lists[j].init(allelecountsff(j)); }
    make_c_allele_lists(c_allele_lists, controls, 0);//debug

    //count numbers of alleles at each locus that appear in data
    //but not in controls
    countadditionalalleles(additionalalleles, c_allele_lists,data,0);
    for (j=0;j<allelecountsff.dim();j++) 
      allelecountsff[j]+=additionalalleles[j];

    //make lists of alleles in data AND controls
    for (j=0;j<num_markers;j++) {
      allele_list[j].init( allelecountsff(j) ); }
    finish_allele_list(allele_list,c_allele_lists,data,0);

    //initialize lists of hap/allele frequencies
    for (j=0;j<num_markers-1;j++) { 
      h2_freq_lists[j].init(allelecountsff(j)*allelecountsff(j+1)); }
    for (j=0;j<num_markers-2;j++) { 
      h3_freq_lists[j].init(allelecountsff(j)*allelecountsff(j+1)*
			    allelecountsff(j+2)); 
    }

      
      if (mkvorder==1) {
	//estimate two-marker haplotype frequencies    
	if (hap_or_freq==1) { //control haplotypes
	  //makehap2fr_hmm_Hcon(h2_freq_lists, 
	  //		   allele_list, controls, 1);
	  makehap2fr_hmm_HconBAYES(h2_freq_lists, bayes,
				   allele_list, controls, 0); 
	}

	else {//(hap_or_freq==2); control genotypes
	  //makehap2fr_hmm_Gcon(h2_freq_lists, //bayes, 
	  //	      allele_list, controls, 0); 

	  //delete me
	  //for (j=0;j<h2_freq_lists.dim();j++) {
	  //for (i=0;i<h2_freq_lists[j].dim();i++)
	  //cout << h2_freq_lists[j][i] << " ";
	  //cout << endl; }


	  makehap2fr_hmm_GconBAYES(h2_freq_lists, 
				   allele_list, controls, bayes, 0);

	  //cout << "BAYES" << endl;
	  //for (j=0;j<h2_freq_lists.dim();j++) {
	  //for (i=0;i<h2_freq_lists[j].dim();i++)
	  //cout << h2_freq_lists[j][i] << " ";
	  //cout << endl; }
	 
	  //exit(1);
 
	}
	
	//exit(1);

	  //for (i=0;i<h2_freq_lists.dim();i++) {
	  //for (j=0;j<h2_freq_lists[i].dim();j++) {
	  //cout << h2_freq_lists[i][j] << " "; }
	  //cout << endl; }
	  //exit(1);
	  
	  //cout << "cin >> h2_freq_lists " << endl;
	  //for (i=0;i<h2_freq_lists.dim();i++) 
	  //for (j=0;j<h2_freq_lists[i].dim();j++)
	  //  cin >> h2_freq_lists[i][j];
	
	
	
	//adjust in case alleles/haplotypes do not appear in controls
	//const double e=0.01; //how small we allow minimum freq. to be 
	//make_adj_to_hfreqlists(h2_freq_lists_adj, h2_freq_lists, allele_list, 
	//	       e, additionalalleles,hap_or_freq,controls,debug);
	
	//make other freq. objects for use in onehap,onegen
	makejf2(jointfreq2,h2_freq_lists, 0); 
	makefa(freqallele,allele_list, 0);
	makeff(freqfreq, freqallele, allele_list, jointfreq2, 0);
	
	//delete me
	//double loglik=0;
	//for (i=0;i<controls.dimx();i++)
	//for (j=0;j<controls.dimy();j++) 
	//loglik+=getfreqSMART(freqallele, freqfreq,controls(i,j),j);
	//cout << loglik << endl;
	//exit(1);

	//make objects for mkv2 lag
	makejf3(jointfreq3mkv2, jointfreq2, freqfreq, allele_list,0);
	makejf3f(jointfreq3fmkv2, jointfreq3mkv2, allele_list,0);
      }
      else {
	if (hap_or_freq==1) {
	  //mkv2 model for LD in controls
	  makehap3fr_hmm_HconBAYES(h3_freq_lists, allele_list, controls, 
				   bayes, 0);
	  //makeh2freqlistsmkv2(h2_freq_lists,h3_freq_lists,allele_list,0);
	  
	  //const double e=0.1;
	  //adjustmkv2(h3_freq_lists_adj, h3_freq_lists,h2_freq_lists,e,
	  //     allele_list,0);
	  
	//copy h3_freq_lists_adj into h3_freq_lists
	//for (i=0;i<h3_freq_lists_adj.dim();i++) 
	//h3_freq_lists[i]=h3_freq_lists_adj[i];
	  
	  makejf3fromh3fl(jointfreq3mkv2,h3_freq_lists,0);
	  makejf3f(jointfreq3fmkv2, jointfreq3mkv2, allele_list,0);
	  
	  makejf2fromjf3(jointfreq2,jointfreq3mkv2,allele_list,0);
	  makefa(freqallele,allele_list, debug);
	  makeff(freqfreq, freqallele, allele_list, jointfreq2, 0);
	}
	else {//hap_or_freq==2

	  makehap3fr_hmm_GconBAYES(h3_freq_lists,allele_list,controls,
				   bayes,0);

	  //delete me
	  //for (i=0;i<h3_freq_lists.dim();i++) {
	  //for (j=0;j<h3_freq_lists[i].dim();j++) {
	  //cout << h3_freq_lists[i][j] << " "; }
	  //cout << endl; }
	  //exit(1);
	  
	  //cout << "cin >> h3_freq_lists " << endl;
	  //for (i=0;i<h3_freq_lists.dim();i++) 
	  //for (j=0;j<h3_freq_lists[i].dim();j++)
	  //cin >> h3_freq_lists[i][j];

	  makejf3fromh3fl(jointfreq3mkv2,h3_freq_lists,0);
	  makejf3f(jointfreq3fmkv2, jointfreq3mkv2, allele_list,0);
	  
	  makejf2fromjf3(jointfreq2,jointfreq3mkv2,allele_list,0);
	  makefa(freqallele,allele_list, 0);
	  makeff(freqfreq, freqallele, allele_list, jointfreq2, 0);

	}
      }
  }
  else {//(hap_or_freq==3); allele frequencies reported
    //give counts of alleles in order
    makeallelecountsff(allelecountsff, num_markers, markers, 
		       num_alleles, hap_or_freq, debug);
    
    //check if there are alleles in data that do not appear in list
    //of control freqs
    countadditionalalleles(additionalalleles, allele_list,data,debug);
    for (j=0;j<allelecountsff.dim();j++) 
      if (additionalalleles[j]>0) {
	errorfs << "ERROR: allele found in data not found in reported list"
		<< endl;
	exit(1);
      }

    makeindfreq(freqallele, freqfreq, num_markers, markers,
		num_alleles, allele_list, allele_freq, hap_or_freq,debug);
    makeindjointfreq2(jointfreq2, allelecountsff, freqfreq, 
		      num_markers, 0);
    makeindjointfreq3(jointfreq3mkv2, allelecountsff, freqfreq, 
		      num_markers, 0);
    makejf3f(jointfreq3fmkv2, jointfreq3mkv2, allele_list,0);

    
    //if estimating tau at marker, make jointfreq2C,jointfreq3
    //if (program_code==2 || program_code==3) {
    //makejointfreq2C(jointfreq2C, allelecountsff, freqfreq, C_loc, debug);
      
    //if (0<C_loc && C_loc<num_markers-1)
    //makejointfreq3(jointfreq3, freqfreq, allelecountsff, C_loc, debug);
  }


  //make vector of mutation rates for used markers
  dvector mut_markers(num_markers);
  makemut(mut_markers, mut_rates, markers, debug);
  //make average mutation rate for output
  double m=0; for (i=0;i<mut_markers.dim();i++) m+=mut_markers(i); 
  m/=mut_markers.dim();

  //make declarations
  igrid augdata;
  igrid augfreqallele;
  dgrid augfreqfreq;
  dgrid augjointfreq2;
  dgrid augjointfreq3mkv2;
  dgrid augjointfreq3fmkv2;
  dgrid augjointfreq4mkv2;
  dgrid augjointfreq4fmkv2;
  dvector augy;
  dvector mody;
  dvector x;
  dvector modx;
  dvector augmut_markers;
  ivector augallelecountsff;
  ivector auganc_hap;

  //declare vector to store locations of center where we'll estimate
  //ancestral haplotype
  dvector Cloc_list;

  //declare vectors to store log-likelihoods and assumed locations of genes
  //in mapping context
  int max_counter=0;
  int best=0;
  int ancdone=0;
  dvector d;
  dvectors ll; ll.init(2);
  ivector maxlikCint;
  dvector maxlikCloc;
  ivectors anchapstore;
  dvectors resonetempstore;
  ivector edge;
  int numedge;
  int npts;
  int counter=0;
  int anchapcounter=0;
  double smp=0;

  //make initializations
  if (program_code==1) {
    augdata.init(data.dimx(),data.dimy()+1);
    augfreqallele.init(freqallele.dimx(),freqallele.dimy()+1);
    augfreqfreq.init(freqfreq.dimx(),freqfreq.dimy()+1);
    augjointfreq2.init(jointfreq2.dimx(),jointfreq2.dimy()+1);
    jointfreq3.init(jointfreq2.dimx());
    augjointfreq3mkv2.init(jointfreq3mkv2.dimx(),jointfreq3mkv2.dimy()+1);
    augjointfreq3fmkv2.init(jointfreq3fmkv2.dimx(),jointfreq3fmkv2.dimy()+1);
    augjointfreq4mkv2.init(jointfreq3mkv2.dimx(),2);
    augjointfreq4fmkv2.init(jointfreq3fmkv2.dimx(),2);
    augy.init(y.dim()+1);
    mody.init(y.dim()+1);
    x.init(num_markers+1);
    modx.init(num_markers+1);
    augmut_markers.init(num_markers+1);
    augallelecountsff.init(allelecountsff.dim()+1);
    auganc_hap.init(anc_hap[0].dim()+1);
    C_loc=1; C_all=9;
    fixed_thresh=0, var_thresh=0;
  }
  else {
    x.init(num_markers);
  }

  //delete haplotypes that do not have center alleles, and
  //genotypes that do not possess at least one copy of this allele
  if (program_code==2 || program_code==3) {
    makesmallerdataset(augdata, data, C_loc, C_all, data_type, debug); }

  //declarations
  int augnum_markers= num_markers;
  if (program_code==1) augnum_markers+=1;

  dvector mut_markers_rescale(augnum_markers);
  dgrid condfreq3lmkv2(augjointfreq3mkv2.dimx(),augjointfreq3mkv2.dimy());
  dgrid condfreq3rmkv2(augjointfreq3mkv2.dimx(),augjointfreq3mkv2.dimy());
  dgrid condfreq3fmkv2(augjointfreq3fmkv2.dimx(),augjointfreq3fmkv2.dimy());
  dgrid condfreq2l(jointfreq2.dimx(),augnum_markers-1);//C columns
  dgrid condfreq2r(jointfreq2.dimx(),augnum_markers-1);//a.n.l.-C-1 col.
  dvector condfreq3l(jointfreq3.dim());
  dvector condfreq3r(jointfreq3.dim());
  dgrid condfreq3F(jointfreq3.dim(),2); //need for genotype data
  dgrid condfreq2C(freqfreq.dimx()*allele_list[C_loc].dim(),augnum_markers); 
  dvector mutmatch(augnum_markers);
  dvector mutnomatch(augnum_markers);
  dvector cstar(augnum_markers);
  dvector cstarmut(augnum_markers);
  dvector resultsone(10);
  dvector resultsmax(12);

  igrid candanc; dgrid candres;
  if (program_code==1) {
    candanc.init(2000,augnum_markers);
    candres.init(2000,10);
  }
  else {
    if (program_code==2) {
      candanc.init(10000,augnum_markers);
      candres.init(10000,10);
    }
  }


  //dimensions of some objects depend on whether we have g. or h. data
  //make declarations
  int numhap,numgen;
  dgrid obsprobs,nullprobs,obsprobsCnum,nullprobsCnum,
    obsprobsCden,nullprobsCden;
  dvectors nullCnum, nullCden, obsCnum,obsCden;
  dvector obsprobsCltRE,nullprobsCltRE;
  dgrid initstore, transstore, transstoreC;  
  dvector scratchvec;
  dgrid alpha, beta, gamma;
  dvectors gammaC;
  dvector loglikvec;
  dvector obsprobsAstore;

  if (data_type==1) {//(haplotype data)
    numhap=augdata.dimx();
    obsprobs.init(2*numhap,augnum_markers);
    obsprobsAstore.init(2*numhap);
    initstore.init(2,4);
    transstore.init(4,augnum_markers-1);  
    transstoreC.init(4,2);  
    alpha.init(2*numhap,augnum_markers);
    beta.init(2*numhap,augnum_markers);
    gamma.init(2*numhap,augnum_markers);
    loglikvec.init(numhap);
  }
  else {//data_type==2 (genotype data)
    numgen=augdata.dimx()/2;
    obsprobs.init(13*numgen,augnum_markers); 
    nullprobs.init(13*numgen,augnum_markers);
    obsprobsCnum.init(40*numgen,2); nullprobsCnum.init(40*numgen,2);
    obsprobsCden.init(7*numgen,2); nullprobsCden.init(7*numgen,2);
    obsprobsCltRE.init(14*numgen); nullprobsCltRE.init(14*numgen);
    obsCnum.init(6); nullCnum.init(6); obsCden.init(4); nullCden.init(4);
    obsCnum[0].init(20*numgen); nullCnum[0].init(20*numgen);
    obsCnum[1].init(28*numgen); nullCnum[1].init(28*numgen);
    obsCnum[2].init(23*numgen); nullCnum[2].init(23*numgen);
    obsCnum[3].init(38*numgen); nullCnum[3].init(38*numgen);
    obsCnum[4].init(23*numgen); nullCnum[4].init(23*numgen);
    obsCnum[5].init(38*numgen); nullCnum[5].init(38*numgen);
    obsCden[0].init( 3*numgen); nullCden[0].init( 3*numgen);
    obsCden[1].init(10*numgen); nullCden[1].init(10*numgen);
    obsCden[2].init( 3*numgen); nullCden[2].init( 3*numgen);
    obsCden[3].init(10*numgen); nullCden[3].init(10*numgen);

    gammaC.init(2);
    gammaC[0].init(49*numgen); gammaC[1].init(49*numgen); 

    initstore.init(4,5);
    transstore.init(28,augnum_markers-1);  
    transstoreC.init(28,4);  
    scratchvec.init(4);
    alpha.init(7*numgen,augnum_markers);
    beta.init(7*numgen,augnum_markers);
    gamma.init(7*numgen,augnum_markers);
    loglikvec.init(numgen);
  }
 
  //run routines
  if (program_code==1) {

    //count number of calls to max()
    int count=0;
    for (C_loc=1;C_loc<=y.dim();C_loc++) 
      count+= int(ceil(y[C_loc-1]/max_res));

    if (anc_hap_known==2) {
      if ( count != num_anc_hap_known) {
errorfs << "ERROR: Number of ancestral haplotypes given ["
     << num_anc_hap_known << "]\n does not match number expected ["
     << count << "]" << endl;
exit(1); } }

    //initialize vectors to store log-likelihoods and assumed locations 
    //of genes in mapping context
    npts=map_res*count;
    d.init( npts ); d.clear(0);
    ll[0].init( npts ); ll[0].clear(0);
    ll[1].init( npts ); ll[1].clear(0);

    //initialize ivectors to store ancestral haplotypes
    anchapstore.init(2*count);
    for (i=0;i<anchapstore.dim();i++) {
      anchapstore[i].init(y.dim()+1); anchapstore[i].clear(0); }

    //initialize vector to store corresponding likelihood
    maxlikCint.init(count); maxlikCint.clear(0);
    maxlikCloc.init(count); maxlikCloc.clear(0);


    //open ancout,maxout,oneout
    FILE *res, *anc, *mhf, *ohf;
    if ((res = fopen(resout,"w")) == 0) {
      errorfs << "Unable to open " << resout << " for writing.\n";
      exit(1);
    }
    if ((anc = fopen(ancout,"w")) == 0) {
      errorfs << "Unable to open " << ancout << " for writing.\n";
      exit(1);
    }
    if ((mhf = fopen(maxout,"w")) == 0) {
      errorfs << "Unable to open " << maxout << " for writing.\n";
      exit(1);
    }
    if ((ohf = fopen(oneout,"w")) == 0) {
      errorfs << "Unable to open " << oneout << " for writing.\n";
      exit(1);
    }

    fprintf(mhf, "%5s%3s%7s%3s%9s%9s%6s%6s%6s%6s\n", 
    "ind", "C", "1/Tau", "p", 
    "s-m.lik", "s-n.lik", //"c-m.lik", "c-n.lik", 
    "lloc", "rloc", "cloc", "iter");
    fprintf(ohf, "%5s %3s %7s %3s %9s %9s %9s %9s %6s %6s %6s %6s %6s\n", 
    "ind", "C", "1/Tau", "p", 
    "s-m.lik", "s-n.lik", "c-m.lik", "c-n.lik", 
    "lloc", "rloc", "cloc", "iter", "anchap");
    fflush(mhf);
    fflush(ohf);

    //compute divisor for conditional coalescent correction to lik.
    smp=smapprox(augdata.dimx()); //cout << smp << endl;
    smp=1+(augdata.dimx()-1)*smp; //cout << smp << endl;

    //Est. anc. haps around assumed center and location of max. lik.
    if (anc_hap_known==0) {
    
      ancdone=0;
      while (ancdone == 0) {

	//print out interval around which we reconstruct ancestral haps
	if (E_int==0) {
	  fprintf(mhf, "%16s%3d\n", "[Stage 1] E_int=", E_int);
	  fprintf(anc, "%16s%3d\n", "[Stage 1] E_int=", E_int); }
	else { 
	  fprintf(mhf, "%16s%3d\n", "[Stage 2] E_int=", E_int);
	  fprintf(anc, "%16s%3d\n", "[Stage 2] E_int=", E_int); }
	

	//keep track of calls to max()
	max_counter=0;
	
	//Loop over inter-marker intervals
	for (C_loc = 1; C_loc<=y.dim(); C_loc++) {
	  
	  //if (C_loc==77) exit(1);

	  //make objects read into other programs as files 
	  //(except jointfreq2C which is not needed)
	  augmentdata(data, augdata, freqallele, augfreqallele,
		      freqfreq, augfreqfreq, 
		      jointfreq2, augjointfreq2,
		      //y, augy, 
		      allelecountsff, augallelecountsff,
		      mut_markers, augmut_markers,
		      C_loc, C_all, debug);
	  augmentdatamkv2(augjointfreq3mkv2, jointfreq3mkv2,
			  augjointfreq3fmkv2, jointfreq3fmkv2,
			  jointfreq2, freqfreq,
			  allelecountsff, C_loc, 0);
	  makeaugjointfreq4mkv2(augjointfreq4mkv2, jointfreq3mkv2, C_loc);
	  makeaugjointfreq4fmkv2(augjointfreq4fmkv2, jointfreq3fmkv2, C_loc);
	  
	  //make conditional frequencies objects
	  makecondfreq3lmkv2(condfreq3lmkv2, augfreqallele, augjointfreq2,
			     augjointfreq3mkv2, augallelecountsff, C_loc,0);
	  makecondfreq3rmkv2(condfreq3rmkv2, augfreqallele, augjointfreq2,
			     augjointfreq3mkv2, augallelecountsff, C_loc,0);
	  makecondfreq3fmkv2(condfreq3fmkv2, augjointfreq3fmkv2,
			     augfreqfreq, augallelecountsff,
			     C_loc, 0);
	  makecondfreq2l(condfreq2l, augfreqallele, augfreqfreq, augjointfreq2,
			 augallelecountsff, C_loc,debug);
	  makecondfreq2r(condfreq2r, augfreqallele, augfreqfreq, augjointfreq2,
			 augallelecountsff, C_loc,debug);
	  
	  //not sure yet if I'll need any of these
	  //makecondfreq3l(condfreq3l, jointfreq3, augfreqfreq, 
	  //	 augallelecountsff, 
	  //	 C_loc,debug);
	  //makecondfreq3r(condfreq3r, jointfreq3, augfreqfreq, 
	  //     augallelecountsff, 
	  //	 C_loc,debug); 
	  //if (data_type==2)
	  //makecondfreq3F(condfreq3F, jointfreq3, augfreqfreq, 
	  //	   augallelecountsff, 
	  //	   C_loc,debug); 
	  //makecondfreq2C(condfreq2C, augfreqfreq, augfreqallele, 
	  //	 augfreqfreq, augallelecountsff, C_loc,debug);
	  
	  //rescale mutation rates
	  rescalemut(mut_markers_rescale, augmut_markers, augallelecountsff, 
		     C_loc, debug);

  //loop over max within interval
  for (int sub=0; sub < int(ceil(y[C_loc-1]/max_res)); sub++) {

    //make y
    make_augy(augy,y,C_loc,sub, int(ceil(y[C_loc-1]/max_res)),0);
  
    //make x object
    makex(augy,x,C_loc,debug);

    //estimate ancestral haplotype
    maxmkv2(resultsmax, C_all,augfreqallele,candanc,candres, 
	    fixed_thresh,var_thresh,max_cand,E_int,data_type,
	    //onehapobjects
	    resultsone, C_loc, L_End, R_End,
	    augdata,
	    auganc_hap, x, augy,
	    augfreqallele, augfreqfreq, 
	    augjointfreq2,
	    condfreq2l, condfreq2r, 
	    condfreq2C,
	    condfreq3l, condfreq3r,
	    augallelecountsff, 
	    obsprobs, obsprobsCnum,
	    obsprobsCden,
	    mut_markers_rescale, mutmatch, mutnomatch, 
	    initstore, transstore, transstoreC, 
	    alpha, beta, gamma,
	    cstar, cstarmut, loglikvec,
	    est_p, p, m,
	    scratchvec,augfreqfreq, condfreq3F, 
	    obsprobsCltRE,
	    nullprobs, nullprobsCnum, nullprobsCden,
	    nullprobsCltRE,
	    augjointfreq3mkv2, augjointfreq3fmkv2,
	    augjointfreq4mkv2,augjointfreq4fmkv2,
	    condfreq3fmkv2, condfreq3lmkv2, condfreq3rmkv2,
	    obsprobsAstore,
	    obsCnum, obsCden, nullCnum, nullCden, gammaC); 
  

    fprintf(anc, "%1s%4d%1s ", "(",C_loc,")");
    for (j=0; j<augdata.dimy(); j++)
      fprintf(anc, "%4d ", candanc(0,j));
    fprintf(anc, "\n");
    fflush(anc);
    
    fprintf(mhf,"%3d %7.7f %7.5f %7.5f %7.5f %7.5f %7.3f %7.3f %7.3f %3.0f\n",
    C_loc, 100*x[0], resultsmax[0], resultsmax[2],
    resultsmax[4], resultsmax[5],
    //resultsmax[4]/smp, resultsmax[5]/smp,
    resultsmax[6], resultsmax[7], resultsmax[8],
    resultsmax[9]);
    fflush(mhf);
    
    maxlikCint[max_counter]=C_loc; 
    maxlikCloc[max_counter]=resultsmax[4]; 
    
    //has this ancestral haplotype been seen before?
    updateanchapstore(anchapstore, anchapcounter,
      C_loc, candanc, 0);

    //increment max_counter
    max_counter++;
  }//sub
}//C loop
      
if (E_int != 0) ancdone=1;
else {//find E_int 
  E_int=assignE(maxlikCint,maxlikCloc); }

      }//ancdone
      
    }//anchapknown
    else {
      if (anc_hap_known==1) anchapcounter=num_anc_hap_known; 
      else anchapcounter=1; }

    //write candidates to anchapstore if anc_hap_known==1
    if (anc_hap_known==1) {
      for (i=0;i<anchapcounter;i++) anchapstore[i]=anc_hap[i]; }

    //write list of candidate ancestral haplotypes to file
    if (anc_hap_known==0 || anc_hap_known==1) {
      fprintf(anc, "%39s\n", "[Stage 3] Set S of ancestral haplotypes");
      for (i=0;i<anchapcounter;i++) {
	fprintf(anc, "%1s%2d%1s ", "(",i,")");
	for (j=0; j<augdata.dimy()-1; j++)
	  fprintf(anc, "%4d ", anchapstore[i][j]);
	fprintf(anc, "\n");
	fflush(anc); } }
    
    //initialize dvectors in which to store resultsone for each cand. anc. hap
    resonetempstore.init(anchapcounter);
    for (i=0;i<resonetempstore.dim();i++) {
      resonetempstore[i].init(11); resonetempstore[i].clear(0); }

    //now maximize lik on fine grid
    //Loop over inter-marker intervals
    max_counter=0;
    for (C_loc = 1; C_loc <=y.dim(); C_loc++) {
      
      //make objects read into other programs as files 
      //(except jointfreq2C which is not needed)
      augmentdata(data, augdata, freqallele, augfreqallele,
		  freqfreq, augfreqfreq, 
		  jointfreq2, augjointfreq2,
		  //y, augy, 
		  allelecountsff, augallelecountsff,
		  mut_markers, augmut_markers,
		  C_loc, C_all, debug);
      augmentdatamkv2(augjointfreq3mkv2, jointfreq3mkv2,
		      augjointfreq3fmkv2, jointfreq3fmkv2,
		      jointfreq2, freqfreq,
		      allelecountsff, C_loc, 0);
      makeaugjointfreq4mkv2(augjointfreq4mkv2, jointfreq3mkv2, C_loc);
      makeaugjointfreq4fmkv2(augjointfreq4fmkv2, jointfreq3fmkv2, C_loc);

      //make conditional frequencies objects
      makecondfreq3lmkv2(condfreq3lmkv2, augfreqallele, augjointfreq2,
			 augjointfreq3mkv2, augallelecountsff, C_loc,0);
      makecondfreq3rmkv2(condfreq3rmkv2, augfreqallele, augjointfreq2,
			 augjointfreq3mkv2, augallelecountsff, C_loc,0);
      makecondfreq3fmkv2(condfreq3fmkv2, augjointfreq3fmkv2,
			 augfreqfreq, augallelecountsff,
			 C_loc, 0);
      makecondfreq2l(condfreq2l, augfreqallele, augfreqfreq, augjointfreq2,
		     augallelecountsff, C_loc,debug);
      makecondfreq2r(condfreq2r, augfreqallele, augfreqfreq, augjointfreq2,
		     augallelecountsff, C_loc,debug);

      //not sure yet if I need any of these
      //makecondfreq3l(condfreq3l, jointfreq3, augfreqfreq, augallelecountsff, 
      //	     C_loc,debug);
      //makecondfreq3r(condfreq3r, jointfreq3, augfreqfreq, augallelecountsff, 
      //	     C_loc,debug); 
      //if (data_type==2)
      //makecondfreq3F(condfreq3F, jointfreq3, augfreqfreq, augallelecountsff, 
      //	       C_loc,debug); 
      //makecondfreq2C(condfreq2C, augfreqfreq, augfreqallele, 
      //	     augfreqfreq, augallelecountsff, C_loc,debug);
      
      //rescale mutation rates
      rescalemut(mut_markers_rescale, augmut_markers, augallelecountsff, 
		 C_loc, debug);
      
      //loop over max within interval
      for (int sub=0; sub < int(ceil(y[C_loc-1]/max_res)); sub++) {

	//update ancestral haplotype
	if (anc_hap_known==2)
	  make_auganc_hap(C_loc, C_all, anc_hap_known,
			  anc_hap, auganc_hap,
			  max_counter, debug);
	
	//make y
	make_augy(augy,y,C_loc,sub, int(ceil(y[C_loc-1]/max_res)),0);
	
	//make x object
	makex(augy,x,C_loc,debug);
	
	//maximize likelihood for fine grid within interval
	for (int loc = 0; loc < map_res; ++loc) {
	  
	  make_mody(mody,y,C_loc,sub, int(ceil(y[C_loc-1]/max_res)),
		    loc,map_res,0);
	  
	  makex(mody, modx,C_loc,0);
	  
	  L_End=0;
	  R_End=augnum_markers-1;
	  
	  //delete me
	  //L_End=2;
	  //R_End=6;

	  for (i=0;i<anchapcounter;i++) {
	    
	    if (anc_hap_known==0 || anc_hap_known==1)
	      make_auganc_hap(C_loc, 9, 2, anchapstore, auganc_hap, i, 0);

	    //maximize lik. over 1/tau and p given ancestral hap., gene loc.
	    if (data_type==1) {//haplotype data
	      onehapmkv2(resultsone, C_loc, L_End, R_End,
			 augdata,
			 auganc_hap, modx, mody,
			 augfreqallele, augfreqfreq, 
			 augjointfreq2, augjointfreq3mkv2, augjointfreq3fmkv2,
			 augjointfreq4mkv2,augjointfreq4fmkv2,
			 condfreq2l, condfreq2r, 
			 condfreq3fmkv2, condfreq3lmkv2, condfreq3rmkv2,
			 augallelecountsff, 
			 obsprobs, obsprobsAstore,
			 mut_markers_rescale, mutmatch, mutnomatch, 
			 initstore, transstore, transstoreC, 
			 alpha, beta, gamma,
			 cstar, cstarmut, loglikvec,
			 est_p, p, m); 
	    }
	    else {//data_type==2; genotype data
	      onegenmkv2(resultsone, C_loc, L_End, R_End,
			 augdata,
			 auganc_hap, modx, mody,
			 augfreqallele, augfreqfreq, 
			 augjointfreq2, augjointfreq3mkv2,
			 augjointfreq3fmkv2,
			 augjointfreq4mkv2,augjointfreq4fmkv2,
			 augallelecountsff, 
			 obsprobs, obsCnum,
			 obsCden,
			 mut_markers_rescale, mutmatch, mutnomatch, 
			 initstore, transstore, transstoreC, 
			 alpha, beta, gamma,
			 cstar, cstarmut, loglikvec,
			 est_p, p, m,
			 scratchvec,
			 nullprobs, nullCnum, nullCden, gammaC);
    }

    for (j=0;j<resultsone.dim();j++) {
      resonetempstore[i][j]=resultsone[j];
      resonetempstore[i][10]=i; }
  }

  //pick maximum lik anc. hap. candidate
  best=pickmaxlikanc(resonetempstore);

  fprintf(ohf,"%3d %7.7f %7.5f %7.5f %7.5f %7.5f %7.5f %7.5f %7.3f %7.3f %7.3f %3.0f %3.0f\n",
  C_loc, 100*modx[0], resonetempstore[best][0], 
  resonetempstore[best][2],
  resonetempstore[best][4], resonetempstore[best][5],
  resonetempstore[best][4]/smp, resonetempstore[best][5]/smp,
  resonetempstore[best][6], resonetempstore[best][7], 
  resonetempstore[best][8],
  resonetempstore[best][9], resonetempstore[best][10]);
  fflush(ohf);
  
  //save loglik and position of center for later
  ll[0][counter]=resonetempstore[best][4]; 
  ll[1][counter]=resonetempstore[best][4]/smp;
  d[counter]=100*modx[0];
  counter++;

}//loc

max_counter++;
      }//sub

    } // C loop


    //Format final mapping output

    //first present point estimate for location of gene
    //find maximum likelihood
    int maxlik=0;
    for (j=0;j<ll[0].dim();j++) if (ll[0][j]>ll[0][maxlik]) maxlik=j;
    //find how many locations share maximum likelihood 
    int num_maxlik=0;
    for (j=0;j<ll[0].dim();j++) if (ll[0][j]==ll[0][maxlik]) num_maxlik+=1;
    //list locations
    ivector list_maxlik(num_maxlik); int counter1=0;
    for (j=0;j<ll[0].dim();j++) if (ll[0][j]==ll[0][maxlik]) {
      list_maxlik[counter1]=j; counter1++; }

    fprintf(res, "%4s", 
    "Point Estimate for location of trait-associated variant: ");
    for (j=0;j<list_maxlik.dim();j++) {
      fprintf(res, " %7.5f\n", d[ list_maxlik[j] ]); }
    //    fprintf(res, "\nLog-likelihood: %7.5f\n", ll[0][list_maxlik[0]]);

    //make 95\% confidence region, assuming star-shaped genealogy
    cr(npts, d, ll[0], 0.95, edge, numedge);
    fprintf(res, "%4s", "95% CI [Star-shaped genealogy]: ");
    for (j = 0; j < edge.dim()/2; ++j) {
      fprintf(res, "(%7.5f, %7.5f)", d[edge[2*j]], d[edge[2*j+1]]);
    }
    fprintf(res, "\n");
    fflush(res);

    //now make coalescent CR, assuming coalescent-shaped genology
    cr(npts, d, ll[1], 0.95, edge, numedge);
    fprintf(res, "%4s", "95% CI [Cond. Coalescent]: ");
    for (j = 0; j < edge.dim()/2; ++j) {
      fprintf(res, "(%7.5f, %7.5f)", d[edge[2*j]], d[edge[2*j+1]]);
    }
    fprintf(res, "\n");
    fflush(res);

    //close files
    fclose(res);
    fclose(anc);
    fclose(mhf);
    fclose(ohf);

  }
  else { //if (program_code==2 || program_code==3) 

    if (data_type==2) {
      errorfs << "WARNING: if variant " << C_all << " is not present \n"
   << "with 2 copies in every genotype, the estimates are not \n"
   << "currently correct." << endl;
      exit(1);
    }

    //if haplotype data, check that all haplotypes contain ancestral allele;
    //then condition on this fact
    if (data_type==1) {
      //reset joint frequencies to condition on appearance of center 
      //allele
      jointfreq2resetmkv(jointfreq2, h2_freq_lists_adj,
allele_list,
C_all, C_loc, 0);
      //for data_type=2, you'll need to program a function to get
      //joint frequencies for alleles at every locus and center allele;
      //for data_type=1, we just need condfreq2C and this is just a
      //matrix of 1's and 0's

      makeff(freqfreq, freqallele, allele_list, jointfreq2, 0);

      //make jointfreq3 (NOTE: I currently estimate these using Markov
      //model, i.e., P(a_(-1),a_0)P(a_1|a_0) and jointfreq2C
      makejointfreq3mkv(jointfreq3, jointfreq2,
freqfreq, allelecountsff, C_loc, 0);
    }

    //make conditional frequencies matrices
    makecondfreq2l(condfreq2l, freqallele, freqfreq, jointfreq2,
   allelecountsff, C_loc,0);
    makecondfreq2r(condfreq2r, freqallele, freqfreq, jointfreq2,
   allelecountsff, C_loc,debug);
    condfreq3l.init(jointfreq3.dim()); condfreq3l.clear(0.0);
    condfreq3r.init(jointfreq3.dim()); condfreq3r.clear(0.0);
    makecondfreq3l(condfreq3l, jointfreq3, freqfreq, allelecountsff, 
   C_loc,0);
    makecondfreq3r(condfreq3r, jointfreq3, freqfreq, allelecountsff, 
   C_loc,debug); 
    if (data_type==2)
      makecondfreq3F(condfreq3F, jointfreq3, freqfreq, allelecountsff, 
     C_loc,debug);   
    //makecondfreq2C(condfreq2C, jointfreq2C, freqallele,
    //    freqfreq, allelecountsff, C_loc,debug);
    if (data_type==1) 
      makecondfreq2Cdt1(condfreq2C, freqfreq, allelecountsff, C_loc,debug);
    else { 
      errorfs << "ERROR: need function to estimate jointfreq2C for dt=2\n";
      exit(1); }


    //rescale mutation rates
    rescalemut(mut_markers_rescale, mut_markers, allelecountsff, 
       C_loc, debug);

    //make x object
    makex(y,x,C_loc,debug);
      
    if (program_code==2) {
      max(resultsmax, C_all,freqallele,candanc,candres, 
  fixed_thresh,var_thresh,max_cand,E_int,data_type,
  //onehapobjects
  resultsone, C_loc, L_End, R_End,
  augdata,
  anc_hap[0], x, y,
  freqallele, freqfreq, 
  jointfreq2,
  condfreq2l, condfreq2r, 
  condfreq2C,
  condfreq3l, condfreq3r,
  allelecountsff, 
  obsprobs, obsprobsCnum,
  obsprobsCden,
  mut_markers_rescale, mutmatch, mutnomatch, 
  initstore, transstore, transstoreC, 
  alpha, beta, gamma,
  cstar, cstarmut, loglikvec,
  est_p, p, m,
  scratchvec,
  freqfreq,//subst ff for jointfreq2C 1/18/01 
  condfreq3F, 
  obsprobsCltRE,
  nullprobs, nullprobsCnum, nullprobsCden,
  nullprobsCltRE); 

      printf("\nEstimates:\n");
      printf("  1/tau [cM]: \t %7.5f (%7.5f) \t 95p CI: (%7.5f,%7.5f)\n",
     resultsmax[0], 
     resultsmax[1], 
     resultsmax[0]-1.96*resultsmax[1], 
     resultsmax[0]+1.96*resultsmax[1]);
      printf("  tau [gen]: \t %7.5f (%7.5f) \t 95p CI: (%7.5f,%7.5f)\n", 
     1/(resultsmax[0]/100),
     (resultsmax[1]*1/(resultsmax[0]*resultsmax[0]/(100*100))),
     1/((resultsmax[0]+1.96*resultsmax[1])/100),
     1/((resultsmax[0]-1.96*resultsmax[1])/100));
      if (est_p==1) printf("  p: \t %7.5f (%7.5f) \n",resultsmax[2],
  resultsmax[3]);
      else printf("  p: \t %7.5f (N/A) [as specified] \n",resultsmax[2]);
      printf("  Ave. mut. rate (per locus per meiosis): \t %7.3f (*10^-6)\n",
     m*1e6);
      
      printf("  Ancestral Haplotype:\n \t");
      for (i=0;i<data.dimy();i++) printf("%4d ", candanc(0,i));
      printf("\n");
      
      printf("  E. # IBD at Center (%2.0i):\t %7.5f \n", 
     C_loc+1, resultsmax[8]);
      printf("  E. # IBD at locus %4d: \t %7.5f \n", L_End+1, resultsmax[6]);
      printf("  E. # IBD at locus %4d: \t %7.5f \n", R_End+1, resultsmax[7]);
      printf("Diagnostics: \n");
      printf("  Iterations: \t \t \t %4.0f \n",resultsmax[9]);
      printf("  Model Log-Lik.: \t \t %7.5f \n  Null Log-Lik.:  \t \t %7.5f \n\n",
     resultsmax[4], resultsmax[5]);
      
      
      
      
      
    }
    else {// if(program_code==3) 
      
      if (data_type==1) {
onehap(resultsone, C_loc, L_End, R_End,
       augdata,
       anc_hap[0], x, y,
       freqallele, freqfreq, 
       jointfreq2,
       condfreq2l, condfreq2r, 
       condfreq2C,
       condfreq3l, condfreq3r,
       allelecountsff, 
       obsprobs, obsprobsCnum,
       obsprobsCden,
       mut_markers_rescale, mutmatch, mutnomatch, 
       initstore, transstore, transstoreC, 
       alpha, beta, gamma,
       cstar, cstarmut, loglikvec,
       est_p, p, m); 
      }
      else {//data_type==2
onegen(resultsone, C_loc, L_End, R_End,
       augdata,
       anc_hap[0], x, y,
       freqallele, freqfreq, 
       jointfreq2,
       condfreq2l, condfreq2r, 
       condfreq2C,
       condfreq3l, condfreq3r,
       allelecountsff, 
       obsprobs, obsprobsCnum,
       obsprobsCden,
       mut_markers_rescale, mutmatch, mutnomatch, 
       initstore, transstore, transstoreC, 
       alpha, beta, gamma,
       cstar, cstarmut, loglikvec,
       est_p, p, m,
       scratchvec,jointfreq2C, condfreq3F,
       obsprobsCltRE,
       nullprobs, nullprobsCnum, nullprobsCden,
       nullprobsCltRE); 
      }
      
      printf("\nEstimates:\n");
      printf("  1/tau [cM]: \t %7.5f (%7.5f) \t 95p CI: (%7.5f,%7.5f)\n",
     resultsone[0], 
     resultsone[1], 
     resultsone[0]-1.96*resultsone[1], 
     resultsone[0]+1.96*resultsone[1]);
      printf("  tau [gen]: \t %7.5f (%7.5f) \t 95p CI: (%7.5f,%7.5f)\n", 
     1/(resultsone[0]/100),
     (resultsone[1]*1/(resultsone[0]*resultsone[0]/(100*100))),
     1/((resultsone[0]+1.96*resultsone[1])/100),
     1/((resultsone[0]-1.96*resultsone[1])/100));
      if (est_p==1) printf("  p: \t %7.5f (%7.5f) \n",resultsone[2],
  resultsone[3]);
      else printf("  p: \t %7.5f (N/A) [as specified] \n",resultsone[2]);
      printf("  Ave. mut. rate (per locus per meiosis): \t %7.3f (*10^-6)\n",
     m*1e6);
      
      printf("  Ancestral Haplotype:\n \t");
      for (i=0;i<data.dimy();i++) printf("%4d ", anc_hap[0](i));
      printf("\n");
      
      printf("  E. # IBD at Center (%2.0i):\t %7.5f \n", 
     C_loc+1, resultsone[8]);
      printf("  E. # IBD at locus %4d: \t %7.5f \n", L_End+1, resultsone[6]);
      printf("  E. # IBD at locus %4d: \t %7.5f \n", R_End+1, resultsone[7]);
      printf("Diagnostics: \n");
      printf("  Iterations: \t \t \t %4.0f \n",resultsone[9]);
      printf("  Model Log-Lik.: \t \t %7.5f \n  Null Log-Lik.:  \t \t %7.5f \n\n",
     resultsone[4], resultsone[5]);
      
      
    }

     
  }

  
  return 1;

} // main