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

#include <fstream.h>
extern ofstream errorfs;

//declarations
#include "declare.h"


void onehap(dvector &resultsone, const int &C, int &LE, int &RE,
    const igrid &data,
    const ivector &anchap, const dvector &x, const dvector &y,
    const igrid &freqallele, const dgrid &freqfreq, 
    const dgrid &jointfreq2,
    const dgrid &condfreq2l, const dgrid &condfreq2r, 
    const dgrid &condfreq2C,
    const dvector &condfreq3l, const dvector &condfreq3r,
    const ivector &allelecounts, 
    dgrid &obsprobs, dgrid &obsprobsCnum,
    dgrid &obsprobsCden,
    const dvector &mut, dvector &mutmatch, dvector &mutnomatch, 
    dgrid &initstore, dgrid &transstore, dgrid &transstoreC, 
    dgrid &alpha, dgrid &beta, dgrid &gamma,
    dvector &cstar, dvector &cstarmut, dvector &loglikvec,
    const int &estp, const double &pinit, const double &m)
{
  //this function maximizes the likelihood over 1/tau and p, given the
  //location of the gene and the ancestral haplotype, for haplotype
  //data

  int i,j,k; i=j=k=0;
  const int numhap=data.dimx();

  //reset alpha,beta,gamma
  alpha.clear(0); beta.clear(0); gamma.clear(0);

  /*make parts of observation probabilities that do not depend on parameters*/

  //store null sharing probabilities
  makenullshare(obsprobs, data, freqallele, freqfreq, condfreq2l, condfreq2r,
condfreq2C, allelecounts, LE, RE, C);
  makenullshareCnum(obsprobsCnum, data, freqallele, freqfreq, allelecounts,
    jointfreq2, 
    condfreq3l,condfreq3r, C, LE,RE,m, anchap); 
  makenullshareCden(obsprobsCden, data, freqallele,
    freqfreq, C, LE, RE); 

  //give initial values for parameters
  double tau=5.0; double tau1;
  double p=pinit; double p1;

  //compute likelihood at initial values of the parameters
  double modellik=0; double modellikgs= 0;
  makemutmatch(mutmatch, mutnomatch, mut, allelecounts, tau, C);
  makemodlshare(obsprobs,data,mutmatch,mutnomatch,anchap,C, LE, RE);
  makemodlshareCnum(obsprobsCnum, data, obsprobs, C, LE, RE); 
  makemodlshareCden(obsprobsCden, data, obsprobs, C, LE, RE); 
  makeinit(initstore,x,tau,p,C,LE,RE);
  maketrans(transstore, x, tau, p, C, LE, RE);
  maketransC(transstoreC, x, tau, p, C, LE, RE);
  makealpha(alpha, initstore, transstore, transstoreC,
    obsprobs, obsprobsCnum, obsprobsCden, 
    C, LE, RE);
  makeloglik(loglikvec,alpha, initstore, C, RE);
  modellik=0; for (i=0;i<numhap;i++) modellik+=loglikvec[i];

  //perform grid search on parameter space to see if this is best starting pt.
  double taugs=0; double pgs=0;
  for (i=0;i<4;i++) 
    {
      if (i==0) taugs=1.0;
      else taugs=1.0/(0.002*i*i*i);

      if (m>0) {
makemutmatch(mutmatch, mutnomatch, mut, allelecounts, taugs, C);
makemodlshare(obsprobs,data,mutmatch,mutnomatch,anchap,C, LE, RE);
makemodlshareCnum(obsprobsCnum, data, obsprobs, C, LE, RE); 
makemodlshareCden(obsprobsCden, data, obsprobs, C, LE, RE); 
      }
      if (estp==0) {
makeinit(initstore,x,taugs,p,C,LE,RE);
maketrans(transstore, x, taugs, p, C, LE, RE);
maketransC(transstoreC, x, taugs, p, C, LE, RE);
makealpha(alpha, initstore, transstore, transstoreC,
  obsprobs, obsprobsCnum, obsprobsCden, 
  C, LE, RE);
makeloglik(loglikvec,alpha, initstore, C, RE);
modellikgs=0; for (k=0;k<numhap;k++) modellikgs+=loglikvec[k];
if (modellikgs>modellik) { 
  modellik=modellikgs; tau=taugs; } }
      else {
for (j=0;j<3;j++) {
  pgs=0.05+0.45*j;
  makeinit(initstore,x,taugs,pgs,C,LE,RE);
  maketrans(transstore, x, taugs, pgs, C, LE, RE);
  maketransC(transstoreC, x, taugs, pgs, C, LE, RE);
  makealpha(alpha, initstore, transstore, transstoreC,
    obsprobs, obsprobsCnum, obsprobsCden, 
    C, LE, RE);
  makeloglik(loglikvec,alpha, initstore, C, RE);
  modellikgs=0; for (k=0;k<numhap;k++) modellikgs+=loglikvec[k]; 
  if (modellikgs>modellik) { 
    modellik=modellikgs; tau=taugs; p=pgs; } } } }
  
  //run EM algorithm to maximize likelihood over parameters
  int iter;
  int maxiter=200;

  for (iter=0;iter<maxiter;iter++) {

    if (m>0 && iter>0) {
      //make mutation and no-mutation rates for each locus
      makemutmatch(mutmatch, mutnomatch, mut, allelecounts, tau, C);
      //fill in observation probabilities for ancestral state
      makemodlshare(obsprobs,data,mutmatch,mutnomatch,anchap,C, LE, RE);
      makemodlshareCnum(obsprobsCnum, data, obsprobs, C, LE, RE); 
      makemodlshareCden(obsprobsCden, data, obsprobs, C, LE, RE); }


    makeinit(initstore,x,tau,p,C,LE,RE);
    //initstore.dump();
    //for (i=0;i<initstore.dimx();i++) cout << initstore(i,0) << " "; 
    //cout << endl;

    maketrans(transstore, x, tau, p, C, LE, RE);
    //transstore.dump();
    //    for (i=0;i<transstore.dimx();i++) {
    //      for (j=0;j<transstore.dimy();j++) {
    // cout << transstore(i,j) << " "; }
    //      cout << endl; }
    maketransC(transstoreC, x, tau, p, C, LE, RE);
    //transstoreC.dump();
    //    for (i=0;i<transstoreC.dimx();i++) {
    //      for (j=0;j<transstoreC.dimy();j++) {
    //      cout << transstoreC(i,j) << " "; }
    //      cout << endl; }

    makealpha(alpha, initstore, transstore, transstoreC,
      obsprobs, obsprobsCnum, obsprobsCden, 
      C, LE, RE);
    //    alpha.dump(1);
    //    cout << "alpha:" << endl;
    //for (i=0*2;i<(0+1)*2;i++) {
    //for (j=0;j<alpha.dimy();j++) {
    //cout << alpha(i,j) << " "; }
    //cout << endl; }

    makebeta(beta, initstore, transstore, transstoreC,
     obsprobs, obsprobsCnum, obsprobsCden, 
     C, LE, RE);
    //beta.dump();
    //cout << "beta:" << endl;
    //for (i=0*2;i<(0+1)*2;i++) {
    //for (j=0;j<beta.dimy();j++) {
    //cout << beta(i,j) << "\t"; }
    //cout << endl; }
    //cout << " beta(0,0) " << beta(0,0) << endl;

    makegamma(gamma, alpha, beta, C, LE, RE);
    //gamma.dump();
    //    cout << "gamma:" << endl;
    //for (i=0*2 ;i<(0+1)*2;i++) {
    // for (j=C-1;j<=C+1;j++) {
    //cout << gamma(i,j) << "\t"; }
    //cout << endl; }

    makecstar(cstar, gamma, C, LE, RE);
    //for (i=0;i<cstar.dim();i++) cout << cstar[i] << " "; cout << endl;
    if (m>0) makecstarmut(cstarmut, cstar,gamma,data,anchap,mutmatch,C,LE,RE);
    //for (i=0;i<cstar.dim();i++) cout << cstarmut[i] << " "; cout << endl;

    tau1=tau; p1=p;

    tau=bisecmle(cstar, cstarmut, y, x, C, LE, RE, mut, allelecounts,m);
    if (estp==1 && (0.000001<p && p<0.999999) ) 
      p=makep(cstar, data.dimx(), C);

    if (estp==0) 
      {if ( ((-0.00001<(tau1-tau)/tau &&
      (tau1-tau)/tau <0.00001) 
     || (-0.00001<tau && tau<0.00001)) ) break;}
    else
      {if ( ((-0.00001<(tau1-tau)/tau &&
      (tau1-tau)/tau <0.00001) 
     || (-0.00001<tau && tau<0.00001)) && 
    ( (-0.00001<(p1-p)/p && (p1-p)/p <0.00001)
        || (-0.00001<(p1-p)&& (p1-p)   <0.00001) ) 
    ) break;}

  }

  /*  
  if (iter>=maxiter) { 
    errorfs << "Warning: Algorithm did not converge for anc. hap" 
<< endl;
    for (i=LE;i<=RE;i++) cout << anchap[i] << " "; cout << endl; }
  */

  //make model loglikelihood
  makemutmatch(mutmatch, mutnomatch, mut, allelecounts, tau, C);
  makemodlshare(obsprobs,data,mutmatch,mutnomatch,anchap,C, LE, RE);
  makemodlshareCnum(obsprobsCnum, data, obsprobs, C, LE, RE); 
  makemodlshareCden(obsprobsCden, data, obsprobs, C, LE, RE); 
  makeinit(initstore,x,tau,p,C,LE,RE);
  maketrans(transstore, x, tau, p, C, LE, RE);
  maketransC(transstoreC, x, tau, p, C, LE, RE);
  makealpha(alpha, initstore, transstore, transstoreC,
    obsprobs, obsprobsCnum, obsprobsCden, 
    C, LE, RE);
  makeloglik(loglikvec,alpha, initstore, C, RE);
  modellik=0; for (i=0;i<numhap;i++) modellik+=loglikvec[i];

  //make null loglikelihood
  makeinit(initstore,x,tau,1,C,LE,RE);
  maketrans(transstore, x, tau, 1, C, LE, RE);
  maketransC(transstoreC, x, tau, 1, C, LE, RE);
  makealpha(alpha, initstore, transstore, transstoreC,
    obsprobs, obsprobsCnum, obsprobsCden, 
    C, LE, RE);
  makeloglik(loglikvec,alpha, initstore, C, RE);
  double nulllik=0; for (i=0;i<numhap;i++) nulllik+=loglikvec[i];

  //Fill in output into resultsone
  resultsone[0]=1/tau * 100;    //1/tau (cM)
  resultsone[1]=0;              //se for 1/tau 
  resultsone[2]=p;              //p
  resultsone[3]=0;              //se for p
  resultsone[4]=modellik;       //maximized model likelihood
  resultsone[5]=nulllik;        //null likelihood (p=1)
  resultsone[6]=cstar[LE];      //E(Sharing at LE|data,model)
  resultsone[7]=cstar[RE];      //E(Sharing at RE|data,model)
  resultsone[8]=cstar[C];       //E(Sharing at C|data,model)
  resultsone[9]=iter;           //iterations in EM/HMM

  //for (i=0;i<=9;i++) cout << resultsone[i] << " "; cout << endl; exit(1);
}//onehap

void onegen(dvector &resultsone, const int &C, int &LE, int &RE,
    const igrid &data,
    const ivector &anchap, const dvector &x, const dvector &y,
    const igrid &freqallele, const dgrid &freqfreq, 
    const dgrid &jointfreq2,
    const dgrid &condfreq2l, const dgrid &condfreq2r, 
    const dgrid &condfreq2C,
    const dvector &condfreq3l, const dvector &condfreq3r,
    const ivector &allelecounts, 
    dgrid &obsprobs, dgrid &obsprobsCnum,
    dgrid &obsprobsCden,
    const dvector &mut, dvector &mutmatch, dvector &mutnomatch, 
    dgrid &initstore, dgrid &transstore, dgrid &transstoreC, 
    dgrid &alpha, dgrid &beta, dgrid &gamma,
    dvector &cstar, dvector &cstarmut, dvector &loglikvec,
    const int &estp, const double &pinit, const double &m,
    //extra objects for genotype code
    dvector &scratchvec, const dgrid &jointfreq2C,
    const dgrid &condfreq3F, 
    dvector &obsprobsCltRE, dgrid &nullprobs, 
    dgrid &nullprobsCnum, dgrid &nullprobsCden, 
    dvector &nullprobsCltRE)
{
  //this function maximizes the likelihood over 1/tau and p, given the
  //location of the gene and the ancestral haplotype, for genotype
  //data

  int i,j,g; i=j=0;
  const int numgen=data.dimx()/2;

  //reset alpha,beta,gamma
  alpha.clear(0); beta.clear(0); gamma.clear(0);

  /*make parts of observation probabilities that do not depend on parameters*/

  //store null sharing probabilities
  makenullsharegen(nullprobs, data, freqallele, freqfreq,
   condfreq2l, condfreq2r, condfreq2C,
   allelecounts, LE, RE, C);
  makenullshareCnumgen(nullprobsCnum, data,
       freqallele, freqfreq,
       allelecounts, jointfreq2, condfreq2C,
       condfreq3l, condfreq3r, condfreq3F,
       C, LE, RE);
  makenullshareCdengen(nullprobsCden, data, freqallele,
       freqfreq, C, LE, RE); 
  makenullshareCltREgen(nullprobsCltRE,data,freqallele,
freqfreq, allelecounts, jointfreq2C,C, RE);

  //need frequency of center allele for use in MC probs.
  const double freqC=getfreq(freqallele, freqfreq, anchap(C), C);

  //give initial values for parameters 
  double tau=0.5; double tau1;
  double p=pinit; double p1;
  
  //compute likelihood at initial values of the parameters
  double modellik=0; double modellikgs= 0;
  makemutmatch(mutmatch, mutnomatch, mut, allelecounts, tau, C);
  makemodlsharegen(obsprobs,nullprobs,
   data,mutmatch,mutnomatch,anchap,C,LE,RE);
  makemodlshareCnumgen(obsprobsCnum, nullprobsCnum,
       data, anchap, mutmatch, mutnomatch, C, LE, RE);
  makemodlshareCdengen(obsprobsCden, nullprobsCden,
       data, anchap, mutmatch, mutnomatch, C, LE, RE);
  makemodlshareCltREgen(obsprobsCltRE, nullprobsCltRE, data,anchap,
mutmatch, mutnomatch, C, RE);
  makeinitgen(initstore,scratchvec,x,tau,p,freqC,C,LE,RE);
  maketransgen(transstore, scratchvec,x, tau, p, freqC, C, LE, RE);
  maketransCgen(transstoreC, scratchvec,x, tau, p, C, LE, RE);
  makealphagen(alpha, initstore, transstore, transstoreC,
       obsprobs, obsprobsCnum, obsprobsCden, 
       C, LE, RE);
  makeloglikgen(loglikvec,alpha, initstore, C, RE);
  modellik=0; for (g=0;g<numgen;g++) modellik+=loglikvec[g];

  //perform grid search on parameter space to see if this is best starting pt.
  double taugs=0; double pgs=0;
  for (i=0;i<4;i++) {
    if (i==0) taugs=1.0;
    else taugs=1.0/(0.002*i*i*i);
    if (m>0) {
      makemutmatch(mutmatch, mutnomatch, mut, allelecounts, taugs, C);
      makemodlsharegen(obsprobs,nullprobs,
       data,mutmatch,mutnomatch,anchap,C,LE,RE);
      makemodlshareCnumgen(obsprobsCnum, nullprobsCnum,
   data, anchap, mutmatch, mutnomatch, C, LE, RE);
      makemodlshareCdengen(obsprobsCden, nullprobsCden,
   data, anchap, mutmatch, mutnomatch, C, LE, RE); 
      makemodlshareCltREgen(obsprobsCltRE, nullprobsCltRE, data,anchap,
    mutmatch, mutnomatch, C, RE); }
    if (estp==0) {
      makeinitgen(initstore,scratchvec,x,taugs,p,freqC,C,LE,RE);
      maketransgen(transstore, scratchvec,x, taugs, p, freqC, C, LE, RE);
      maketransCgen(transstoreC, scratchvec,x, taugs, p, C, LE, RE);
      makealphagen(alpha, initstore, transstore, transstoreC,
   obsprobs, obsprobsCnum, obsprobsCden, 
   C, LE, RE);
      makeloglikgen(loglikvec,alpha, initstore, C, RE);
      modellikgs=0; for (g=0;g<numgen;g++) modellikgs+=loglikvec[g];
      if (modellikgs>modellik) { 
modellik=modellikgs; tau=taugs; } }
    else {
      for (j=0;j<3;j++) {
pgs=0.05+0.45*j;
makeinitgen(initstore,scratchvec,x,taugs,pgs,freqC,C,LE,RE);
maketransgen(transstore, scratchvec,x, taugs, pgs, freqC, C, LE, RE);
maketransCgen(transstoreC, scratchvec,x, taugs, pgs, C, LE, RE);
makealphagen(alpha, initstore, transstore, transstoreC,
     obsprobs, obsprobsCnum, obsprobsCden, 
     C, LE, RE);
makeloglikgen(loglikvec,alpha, initstore, C, RE);
modellikgs=0; for (g=0;g<numgen;g++) modellikgs+=loglikvec[g];
if (modellikgs>modellik) { 
  modellik=modellikgs; tau=taugs; p=pgs; } } } } 
  //cout << tau << " " << p << " " << modellik << endl;
  
  //run EM algorithm to maximize likelihood over parameters
  int iter;
  int maxiter=100;

  for (iter=0;iter<maxiter;iter++) {

    if (m>0 && iter>0) {
      //make mutation and no-mutation rates for each locus
      makemutmatch(mutmatch, mutnomatch, mut, allelecounts, tau, C);
      //fill in observation probabilities for ancestral state
      makemodlsharegen(obsprobs,nullprobs,
       data,mutmatch,mutnomatch,anchap,C,LE,RE);
      makemodlshareCnumgen(obsprobsCnum, nullprobsCnum,
   data, anchap, mutmatch, mutnomatch, C, LE, RE);
      makemodlshareCdengen(obsprobsCden, nullprobsCden,
   data, anchap, mutmatch, mutnomatch, C, LE, RE); 
      makemodlshareCltREgen(obsprobsCltRE, nullprobsCltRE, data,anchap,
    mutmatch, mutnomatch, C, RE); }

    makeinitgen(initstore,scratchvec,x,tau,p,freqC,C,LE,RE);
    //initstore.dump();
    //    for (i=0;i<initstore.dimx();i++) {
    // for (j=0;j<initstore.dimy();j++) {
    // cout << initstore(i,j) << " "; }
    // cout << endl; }

    maketransgen(transstore,scratchvec,x, tau, p, freqC, C, LE, RE);
    //transstore.dump();
    //    for (i=0;i<transstore.dimx();i++) {
    //      for (j=0;j<transstore.dimy();j++) {
    // cout << transstore(i,j) << " "; }
    //      cout << endl; }

    maketransCgen(transstoreC, scratchvec, x, tau, p, C, LE, RE);
    //transstoreC.dump();
    //     for (i=0;i<transstoreC.dimx();i++) {
    //  for (j=0;j<transstoreC.dimy();j++) {
    //  cout << transstoreC(i,j) << " "; }
    //  cout << endl; }
    //    exit(1);

    makealphagen(alpha, initstore, transstore, transstoreC,
obsprobs, obsprobsCnum, obsprobsCden, 
C, LE, RE);
    //alpha.dump();
    /*
    cout << "alpha: " << C << endl;
    for (i=16*13;i<17*13;i++) {
      if (C<LE) cout << alpha(i,C) << "   ";
      for (j=LE;j<=RE;j++) {
cout << alpha(i,j) << " "; }
      if (RE<C) cout << "    " << alpha(i,C);
      cout << endl; }
    */

    makebetagen(beta, initstore, transstore, transstoreC,
obsprobs, obsprobsCnum, obsprobsCden, obsprobsCltRE, 
C, LE, RE);
    //beta.dump();
    /*
    cout << "beta:" << endl;
    for (i=16*13;i<(16+1)*13;i++) {
      if (C<LE) cout << beta(i,C) << "   ";
      for (j=LE;j<=RE;j++) {
cout << beta(i,j) << " "; }
      if (RE<C) cout << "    " << beta(i,C);
      cout << endl; }
    */

    makegammagen(gamma, alpha, beta, C, LE, RE);
    //gamma.dump();
    /*
    cout << "gamma:" << endl;
    for (i=4*13;i<(4+1)*13;i++) {
      if (C<LE) cout << gamma(i,C) << "   ";
      for (j=LE;j<=RE;j++) {
       cout << gamma(i,j) << " "; }
      if (RE<C) cout << "    " << gamma(i,C);
      cout << endl; }
    */

    makecstargen(cstar, gamma, C, LE, RE);
    //for (i=0;i<cstar.dim();i++) cout << cstar[i] << " "; cout << endl;
    if (m>0) makecstarmutgen(cstarmut, cstar,gamma,data,anchap,
     mutmatch,C,LE,RE);
    //for (i=0;i<cstar.dim();i++) cout << cstarmut[i] << " "; cout << endl;

    tau1=tau; p1=p;

    tau=bisecmle(cstar, cstarmut, y, x, C, LE, RE, mut, allelecounts,m);
    if (estp==1 && (0.000001<p && p<0.999999) ) 
      p=makep(cstar, data.dimx(), C);

    if (estp==0) 
      {if ( ((-0.00001<(tau1-tau)/tau &&
      (tau1-tau)/tau <0.00001) 
     || (-0.00001<tau && tau<0.00001)) ) break;}
    else
      {if ( ((-0.00001<(tau1-tau)/tau &&
      (tau1-tau)/tau <0.00001) 
     || (-0.00001<tau && tau<0.00001)) && 
    ( (-0.00001<(p1-p)/p && (p1-p)/p <0.00001)
        || (-0.00001<(p1-p)&& (p1-p)   <0.00001)) ) break;}

  }

  /*  
  if (iter>=maxiter) { 
    errorfs << "Warning: Algorithm did not converge for anc. hap" 
<< endl;
    for (i=LE;i<=RE;i++) cout << anchap[i] << " "; cout << endl; }
  */

  //make model loglikelihood
  makemutmatch(mutmatch, mutnomatch, mut, allelecounts, tau, C);
  makemodlsharegen(obsprobs,nullprobs,
   data,mutmatch,mutnomatch,anchap,C,LE,RE);
  makemodlshareCnumgen(obsprobsCnum, nullprobsCnum,
       data, anchap, mutmatch, mutnomatch, C, LE, RE);
  makemodlshareCdengen(obsprobsCden, nullprobsCden,
       data, anchap, mutmatch, mutnomatch, C, LE, RE); 
  makemodlshareCltREgen(obsprobsCltRE, nullprobsCltRE, data,anchap,
mutmatch, mutnomatch, C, RE);  
  makeinitgen(initstore,scratchvec,x,tau,p,freqC,C,LE,RE);
  maketransgen(transstore, scratchvec,x, tau, p, freqC, C, LE, RE);
  maketransCgen(transstoreC, scratchvec,x, tau, p, C, LE, RE);
  makealphagen(alpha, initstore, transstore, transstoreC,
       obsprobs, obsprobsCnum, obsprobsCden, 
       C, LE, RE);
  makeloglikgen(loglikvec,alpha, initstore, C, RE);
  modellik=0; for (g=0;g<numgen;g++) modellik+=loglikvec[g];

  //make null loglikelihood
  makeinitgen(initstore,scratchvec,x,tau,1,freqC,C,LE,RE);
  maketransgen(transstore, scratchvec,x, tau, 1, freqC, C, LE, RE);
  maketransCgen(transstoreC, scratchvec,x, tau, 1, C, LE, RE);
  makealphagen(alpha, initstore, transstore, transstoreC,
       obsprobs, obsprobsCnum, obsprobsCden, 
       C, LE, RE);
  makeloglikgen(loglikvec,alpha, initstore, C, RE);
  double nulllik=0; for (g=0;g<numgen;g++) nulllik+=loglikvec[g];


  //Fill in output into resultsone
  resultsone[0]=1/tau * 100;    //1/tau (cM)
  resultsone[1]=0;              //se for 1/tau 
  resultsone[2]=p;              //p
  resultsone[3]=0;              //se for p
  resultsone[4]=modellik;       //maximized model likelihood
  resultsone[5]=nulllik;        //null likelihood (p=1)
  resultsone[6]=cstar[LE];      //E(Sharing at LE|data,model)
  resultsone[7]=cstar[RE];      //E(Sharing at RE|data,model)
  resultsone[8]=cstar[C];       //E(Sharing at C|data,model)
  resultsone[9]=iter;           //iterations in EM/HMM

  //for (i=0;i<=9;i++) cout << resultsone[i] << " "; cout << endl; //exit(1);

}//onegen



void max(dvector &resultsmax, const int &ctrall,
	 const igrid &allelelist,
	 igrid &candanc, dgrid &candres, 
	 const double &threshfix, const double &threshvar, 
	 const int &mapest, const int &E, const int &data_type,
	 //onehapobjects
	 dvector &resultsone, const int &C, int &LE, int &RE,
	 const igrid &data,
	 ivector &anchap, const dvector &x, const dvector &y,
	 const igrid &freqallele, const dgrid &freqfreq, 
	 const dgrid &jointfreq2,
	 const dgrid &condfreq2l, const dgrid &condfreq2r, 
	 const dgrid &condfreq2C,
	 const dvector &condfreq3l, const dvector &condfreq3r,
	 const ivector &allelecounts, 
	 dgrid &obsprobs, dgrid &obsprobsCnum,
	 dgrid &obsprobsCden,
	 const dvector &mut, dvector &mutmatch, dvector &mutnomatch, 
	 dgrid &initstore, dgrid &transstore, dgrid &transstoreC, 
	 dgrid &alpha, dgrid &beta, dgrid &gamma,
	 dvector &cstar, dvector &cstarmut, dvector &loglikvec,
	 const int &estp, const double &pinit, const double &m,
	 //extra objects for genotype code
	 dvector &scratchvec, const dgrid &jointfreq2C,
	 const dgrid &condfreq3F, 
	 dvector &obsprobsCltRE, dgrid &nullprobs, 
	 dgrid &nullprobsCnum, dgrid &nullprobsCden, 
	 dvector &nullprobsCltRE)
{

  //this function estimates the ancestral haplotype, by applying onehap/onegen
  //for all three locus anc. hap. candidates, sorting by likelihood. Then for
  //those kept, it tries all alleles at the next locus and maximizes the lik
  //for the 4-locus candidates. It continues in this fashion until the
  //ancestral hap. candidates include all markers in the dataset

  int i,j,k;

  //reset candanc, anchap
  candanc.clear(0);
  anchap.clear(0);

  /*we take a jump to the right if the current x-distance on the right
    is smaller than that to the left of the ctr; action is an array
    with 1's and -1's where a 1 [-1] at the hth place means there is a
    jump to the right [left] at the hth jump*/

  int jump=0; //index for stepping over loci in reconstruction;
  int numjump=data.dimy()-3;
  int dirjump=0;

  int numcand=0;//number of candidates at step
  int numkeep=0;//number of candidates kept for next step

  //E_copy will be interval around which we estimate ancestral haplotype
  int E_copy;
  if (E==0) E_copy=C; else E_copy=E;

  //list markers flanking E_copy
  if (E_copy==C) { LE= C-1; RE= C+1; }
  else {
    if (E_copy<C) { LE=E_copy-1; RE=E_copy; }
    else { LE = E_copy; RE=E_copy+1; } }

  //prepare cumulative distance variables to tell direction of next jump;
  //d2LE is "distance to left end"
  double d2LE,d2RE;
  if (E_copy==C) { d2LE=y[C-1]; d2RE=y[C]; }
  else {
    if (E_copy<C) { d2LE = d2RE = y[E_copy-1]/2.0; }
    else { d2LE = d2RE = y[E_copy]/2.0; }
  }
  //  cout << C << " " << E_copy << " " << LE << " " << RE << " " 
  //   << d2LE << " " << d2RE << " (initial values)" << endl;

  for (jump=0;jump<= numjump;jump++)
    {
      
      if (jump==0) { makeinitcandanc(candanc, C, LE, RE, ctrall, 
     allelelist, numcand);       }

      else { makecandanc(candanc, allelelist, dirjump, numcand, numkeep, 
C, LE, RE, ctrall); }
      
      //update LE/RE/d2LE/d2RE
      if (dirjump == -1) 
{ 
  if (LE-1 == C) { d2LE += y[LE-2] + y[LE-1]; LE+=-2; }
  else { d2LE += y[LE-1]; LE+=-1; } 
}
      if (dirjump == 1) 
{ 
  if (RE+1 == C) { d2RE += y[RE] + y[RE+1]; RE+=2; }
  else { d2RE += y[RE]; RE+=1; } 
}
      //if (jump>0) 
      //cout << C << " " << E_copy << " " << LE << " " << RE << " " 
      //   << d2LE << " " << d2RE << " (updated values)" << endl;

    
      /*we now estimate parameters for each candidate ancestral haplotype*/
      for (k=0; k<numcand; k++)      
{

  for (i=LE;i<=RE;i++){anchap[i]=candanc(k,i);}
  anchap[C]=candanc(k,C);

  if (data_type==1) {
    onehap(resultsone, C, LE, RE,
   data,
   anchap, x, y,
   freqallele, freqfreq, 
   jointfreq2,
   condfreq2l, condfreq2r, 
   condfreq2C,
   condfreq3l, condfreq3r,
   allelecounts, 
   obsprobs, obsprobsCnum,
   obsprobsCden,
   mut, mutmatch, mutnomatch, 
   initstore, transstore, transstoreC, 
   alpha, beta, gamma,
   cstar, cstarmut, loglikvec,
   estp, pinit, m); 
  }
  else {//data_type==2
    onegen(resultsone, C, LE, RE,
   data,
   anchap, x, y,
   freqallele, freqfreq, 
   jointfreq2,
   condfreq2l, condfreq2r, 
   condfreq2C,
   condfreq3l, condfreq3r,
   allelecounts, 
   obsprobs, obsprobsCnum,
   obsprobsCden,
   mut, mutmatch, mutnomatch, 
   initstore, transstore, transstoreC, 
   alpha, beta, gamma,
   cstar, cstarmut, loglikvec,
   estp, pinit, m,
   scratchvec,jointfreq2C, condfreq3F, 
   obsprobsCltRE,
   nullprobs, nullprobsCnum, nullprobsCden,
   nullprobsCltRE); 
  }
  //for (i=0;i<candres.dimy();i++) cout << resultsone[i] << " ";
  //cout << endl;
  for (i=0;i<candres.dimy();i++) candres(k,i)=resultsone[i];
}
      
      //employ correction (adding log(2)) to equalize null likelihoods
      correctlik(candres, numcand, 4);
      
      /*sort by loglikelihood*/ 
      sortcandancbylik(candanc, candres, numcand, 4);
      
      /* 
      //print candanc to screen 
      for (i=0;i<numcand;i++) {
for (j=0;j<data.dimy();j++) { cout << candanc(i,j) << " "; }
cout << endl; }
      */

      /*
      //print candres to screen
      for (i=0;i<numcand;i++) {
for (j=0;j<candres.dimy();j++) { cout << candres(i,j) << " "; }
cout << endl; } 
      */
      
      /*figure out direction of next intended jump*/
      if (LE==0) dirjump = 1; 
      else {
if (RE==data.dimy()-1) dirjump=-1;
else {
  if (C==LE-1) {
    if (d2LE + y[LE-2] + y[LE-1] < d2RE + y[RE]) dirjump=-1;
    else dirjump=1; }
  else {
    if (C==RE+1) {
      if (d2LE + y[LE-1] < d2RE + y[RE] + y[RE+1]) dirjump=-1;
      else dirjump=1; }
    else {
      if (d2LE + y[LE-1]<d2RE + y[RE]) dirjump=-1;
      else dirjump=1; } } } } 

      /*find number of haplotypes you want to keep*/
      numkeep=findnumkeep(candres, dirjump, threshfix, threshvar,
     numcand); 

      //if we want to force extention to go to end of map
      if (numkeep>mapest && mapest>0) numkeep=mapest;
      else if (mapest>0 && numkeep==0) {
if (numcand<=mapest) numkeep=numcand;
else numkeep=mapest; }

      //when not mapping, the ancestral haplotype may not be completely
      //reconstructed
      if (numkeep==0)
{
  //cout << "extension?" << endl;
  numkeep=findnumkeep(candres, -dirjump, threshfix,
threshvar, numcand); 

  
  if (numkeep==0 || LE-dirjump<0 || RE-dirjump>=data.dimy()) 
    {
      //cout << "No" << endl; 
      break;
    }
  else 
    {
      //cout << "Yes" << endl;
      dirjump=-dirjump;
    }
}      
      cleantemps(candanc, candres, numcand, numkeep, LE, RE);
    }

  for (i=LE;i<=RE;i++) anchap[i]=candanc(0,i);  
  for (j=0;j<10;j++) resultsmax[j]=candres(0,j);
  //  resultsmax[10]=0;
  //  resultsmax[11]=0;

}//max

void onehapmkv2(dvector &resultsone, const int &C, int &LE, int &RE,
		const igrid &data,
		const ivector &anchap, const dvector &x, const dvector &y,
		const igrid &freqallele, const dgrid &freqfreq, 
		const dgrid &jointfreq2, const dgrid &jointfreq3mkv2,
		const dgrid &jointfreq3fmkv2,
		const dgrid &jointfreq4mkv2,
		const dgrid &jointfreq4fmkv2,
		const dgrid &condfreq2l, const dgrid &condfreq2r,  
		const dgrid &condfreq3fmkv2, const dgrid &condfreq3lmkv2, 
		const dgrid &condfreq3rmkv2,
		const ivector &allelecounts, 
		dgrid &obsprobs, dvector &obsprobsAstore,
		const dvector &mut, dvector &mutmatch, dvector &mutnomatch, 
		dgrid &initstore, dgrid &transstore, dgrid &transstoreC, 
		dgrid &alpha, dgrid &beta, dgrid &gamma,
		dvector &cstar, dvector &cstarmut, dvector &loglikvec,
		const int &estp, const double &pinit, const double &m)
{
  //this function maximizes the likelihood over 1/tau and p, given the
  //location of the gene and the ancestral haplotype, for haplotype
  //data

  int i,j,k; i=j=k=0;
  const int numhap=data.dimx();

  //reset alpha,beta,gamma
  alpha.clear(0); beta.clear(0); gamma.clear(0);

  /*make parts of observation probabilities that do not depend on parameters*/

  //  for (i=0;i<data.dimy();i++) cout << data(11,i) << " "; cout << endl;

  //store null sharing probabilities
  makenullsharemkv2(obsprobs, data, freqallele, freqfreq, condfreq2l, 
    condfreq2r, condfreq3fmkv2, condfreq3lmkv2,
    condfreq3rmkv2, allelecounts, LE, RE, C,0);
  makeobsprobsAstore(obsprobsAstore, data, allelecounts,
     freqallele, freqfreq,jointfreq2,
     jointfreq3mkv2,jointfreq3fmkv2,
     jointfreq4mkv2,jointfreq4fmkv2,
     LE, RE, C,0);
  /*
  //delete me
  cout << freqallele(0,17) << " "<<freqallele(0,18)<<" "<<freqallele(0,19) << endl;
  cout << freqallele(1,17) << " "<<freqallele(1,18)<<" "<<freqallele(1,19) << endl;
  cout << freqfreq(0,17) << " " << freqfreq(0,18) << " "<< freqfreq(0,19) << endl;
  cout << freqfreq(1,17) << " " << freqfreq(1,18) << " "<< freqfreq(1,19) << endl;
  cout << endl;
  cout << jointfreq2(0,17) << " " << jointfreq2(1,17) << " " << jointfreq2(2,17) << " "
       << jointfreq2(3,17) << endl;
  cout << jointfreq2(0,18) << " " << jointfreq2(1,18) << " " << jointfreq2(2,18) << " "
       << jointfreq2(3,18) << endl;
  cout << endl;
  for (i=0;i<8;i++) cout << jointfreq3mkv2(i,17) << " "; cout << endl;

  cout << getcondfreq3fmkv2(freqallele,allelecounts,jointfreq3fmkv2,
    17,data(11,17),data(11,19)) << endl;
  cout << getfreq(freqallele,freqfreq,data(11,19),19) << endl;
  cout << getfreq(freqallele,freqfreq,data(11,17),17) << endl;
  cout << obsprobs(2*11+1,17) << " " << obsprobs(2*11+1,18) << endl;
  exit(1);
  */

  //give initial values for parameters
  double tau=1e-8; double tau1;//delete me
  double p=pinit; double p1;

  //compute likelihood at initial values of the parameters
  double modellik=0; double modellikgs= 0;
  makemutmatch(mutmatch, mutnomatch, mut, allelecounts, tau, C);
  makemodlshare(obsprobs,data,mutmatch,mutnomatch,anchap,C, LE, RE);
  //  makemodlshareCnum(obsprobsCnum, data, obsprobs, C, LE, RE); 
  //makemodlshareCden(obsprobsCden, data, obsprobs, C, LE, RE); 
  makeinit(initstore,x,tau,p,C,LE,RE);
  maketrans(transstore, x, tau, p, C, LE, RE);
  maketransC(transstoreC, x, tau, p, C, LE, RE);
  makealphamkv2(alpha, initstore, transstore, 
      obsprobs, obsprobsAstore, C, LE, RE);
  //delete me
  //for (i=0;i<alpha.dimy();i++) cout << i << " " << alpha(1,i) << endl;
  //exit(1);



  makeloglik(loglikvec,alpha, initstore, C, RE);
  modellik=0; for (i=0;i<numhap;i++) modellik+=loglikvec[i];
  
  //  for (i=0;i<numhap;i++) cout << loglikvec[i] << endl;

  //perform grid search on parameter space to see if this is best starting pt.
  double taugs=0; double pgs=0;
  for (i=0;i<4;i++) 
    {
      if (i==0) taugs=1.0;
      else taugs=1.0/(0.002*i*i*i);

      if (m>0) {
makemutmatch(mutmatch, mutnomatch, mut, allelecounts, taugs, C);
makemodlshare(obsprobs,data,mutmatch,mutnomatch,anchap,C, LE, RE);
      }
      if (estp==0) {
makeinit(initstore,x,taugs,p,C,LE,RE);
maketrans(transstore, x, taugs, p, C, LE, RE);
maketransC(transstoreC, x, taugs, p, C, LE, RE);
makeloglik(loglikvec,alpha, initstore, C, RE);
modellikgs=0; for (k=0;k<numhap;k++) modellikgs+=loglikvec[k];
if (modellikgs>modellik) { 
  modellik=modellikgs; tau=taugs; } }
      else {
for (j=0;j<3;j++) {
  pgs=0.05+0.45*j;
  makeinit(initstore,x,taugs,pgs,C,LE,RE);
  maketrans(transstore, x, taugs, pgs, C, LE, RE);
  maketransC(transstoreC, x, taugs, pgs, C, LE, RE);
  //  makealpha(alpha, initstore, transstore, transstoreC,
  //    obsprobs, obsprobsCnum, obsprobsCden, 
  //    C, LE, RE);
  makeloglik(loglikvec,alpha, initstore, C, RE);
  modellikgs=0; for (k=0;k<numhap;k++) modellikgs+=loglikvec[k]; 
  if (modellikgs>modellik) { 
    modellik=modellikgs; tau=taugs; p=pgs; } } } }
  
  //run EM algorithm to maximize likelihood over parameters
  int iter;
  int maxiter=2000;//was 200

  for (iter=0;iter<maxiter;iter++) {

    //    cout << tau << " " << p << endl;

    if (m>0 && iter>0) {
      //make mutation and no-mutation rates for each locus
      makemutmatch(mutmatch, mutnomatch, mut, allelecounts, tau, C);
      //fill in observation probabilities for ancestral state
      makemodlshare(obsprobs,data,mutmatch,mutnomatch,anchap,C, LE, RE);
      //makemodlshareCnum(obsprobsCnum, data, obsprobs, C, LE, RE); 
      //makemodlshareCden(obsprobsCden, data, obsprobs, C, LE, RE); 
    }


    makeinit(initstore,x,tau,p,C,LE,RE);
    //initstore.dump();
    //for (i=0;i<initstore.dimx();i++) cout << initstore(i,0) << " "; 
    //cout << endl;

    maketrans(transstore, x, tau, p, C, LE, RE);
    //transstore.dump();
    //    for (i=0;i<transstore.dimx();i++) {
    //      for (j=0;j<transstore.dimy();j++) {
    // cout << transstore(i,j) << " "; }
    //      cout << endl; }
    maketransC(transstoreC, x, tau, p, C, LE, RE);
    //transstoreC.dump();
    //    for (i=0;i<transstoreC.dimx();i++) {
    //      for (j=0;j<transstoreC.dimy();j++) {
    //      cout << transstoreC(i,j) << " "; }
    //      cout << endl; }

    makealphamkv2(alpha, initstore, transstore, 
          obsprobs, obsprobsAstore, C, LE, RE);
    //    alpha.dump(1);
    //    cout << "alpha:" << endl;
    //for (i=0*2;i<(0+1)*2;i++) {
    //for (j=0;j<alpha.dimy();j++) {
    //cout << alpha(i,j) << " "; }
    //cout << endl; }

    makebetamkv2(beta, initstore, transstore, transstoreC,
         obsprobs, obsprobsAstore, C, LE, RE);
    //delete me
    //for (i=0;i<alpha.dimy();i++) cout << i << " " << beta(0,i) << endl;
    //exit(1);
    //beta.dump();
    //cout << "beta:" << endl;
    //for (i=0*2;i<(0+1)*2;i++) {
    //for (j=0;j<beta.dimy();j++) {
    //cout << beta(i,j) << "\t"; }
    //cout << endl; }
    //cout << " beta(0,0) " << beta(0,0) << endl;

    makegamma(gamma, alpha, beta, C, LE, RE);
    //gamma.dump();
    //    cout << "gamma:" << endl;
    //for (i=0*2 ;i<(0+1)*2;i++) {
    // for (j=C-1;j<=C+1;j++) {
    //cout << gamma(i,j) << "\t"; }
    //cout << endl; }

    makecstar(cstar, gamma, C, LE, RE);
    //for (i=0;i<cstar.dim();i++) cout << cstar[i] << " "; cout << endl;
    if (m>0) makecstarmut(cstarmut, cstar,gamma,data,anchap,mutmatch,C,LE,RE);
    //for (i=0;i<cstar.dim();i++) cout << cstarmut[i] << " "; cout << endl;
    
    tau1=tau; p1=p;
    
    tau=bisecmle(cstar, cstarmut, y, x, C, LE, RE, mut, allelecounts,m);
    if (estp==1 && (0.000001<p && p<0.999999) ) 
      p=makep(cstar, data.dimx(), C);

    if (estp==0) 
      {if ( ((-0.00001<(tau1-tau)/tau &&
      (tau1-tau)/tau <0.00001) 
     || (-0.00001<tau && tau<0.00001)) ) break;}
    else
      {if ( ((-0.00001<(tau1-tau)/tau &&
      (tau1-tau)/tau <0.00001) 
     || (-0.00001<tau && tau<0.00001)) && 
    ( (-0.00001<(p1-p)/p && (p1-p)/p <0.00001)
        || (-0.00001<(p1-p)&& (p1-p)   <0.00001) ) 
    ) break;}

    }

  /*  
  if (iter>=maxiter) { 
    errorfs << "Warning: Algorithm did not converge for anc. hap" 
<< endl;
    for (i=LE;i<=RE;i++) cout << anchap[i] << " "; cout << endl; }
  */

  //make model loglikelihood
  makemutmatch(mutmatch, mutnomatch, mut, allelecounts, tau, C);
  makemodlshare(obsprobs,data,mutmatch,mutnomatch,anchap,C, LE, RE);
  //makemodlshareCnum(obsprobsCnum, data, obsprobs, C, LE, RE); 
  //makemodlshareCden(obsprobsCden, data, obsprobs, C, LE, RE); 
  makeinit(initstore,x,tau,p,C,LE,RE);
  maketrans(transstore, x, tau, p, C, LE, RE);
  maketransC(transstoreC, x, tau, p, C, LE, RE);
  makealphamkv2(alpha, initstore, transstore, 
obsprobs, obsprobsAstore,
      C, LE, RE);
  makeloglik(loglikvec,alpha, initstore, C, RE);
  modellik=0; for (i=0;i<numhap;i++) modellik+=loglikvec[i];

  //make null loglikelihood
  makeinit(initstore,x,tau,1,C,LE,RE);
  maketrans(transstore, x, tau, 1, C, LE, RE);
  maketransC(transstoreC, x, tau, 1, C, LE, RE);
  makealphamkv2(alpha, initstore, transstore, 
      obsprobs, obsprobsAstore, 
      C, LE, RE);
  makeloglik(loglikvec,alpha, initstore, C, RE);
  double nulllik=0; for (i=0;i<numhap;i++) nulllik+=loglikvec[i];

  makebetamkv2(beta,initstore,transstore,transstoreC,obsprobs,obsprobsAstore,C,LE,RE);
  //for (i=0;i<alpha.dimy();i++) 
  //cout << i << " " << C << " " << beta(22,i) << "  " << beta(23,i) << endl;

  /*
  //cout << "C " << C << endl; 
  //for (i=0;i<numhap;i++) cout << i << " " << loglikvec[i] << endl;
  cout << endl << endl;
  if (C==16) cout << C << " loc19 " 
  << getcondfreq3fmkv2(freqallele,allelecounts,jointfreq3fmkv2,17,
       data(11,17),data(11,19))/
       getfreq(freqallele,freqfreq,data(11,17),17)
  << endl;
  if (C==17) cout << C << " loc19 "
  << getcondfreq4lSMART(freqallele,freqfreq,allelecounts,
jointfreq2, jointfreq3mkv2,jointfreq4mkv2,
jointfreq4fmkv2,16,data(11,16),data(11,17),
data(11,18),data(11,19),17) << " "
  << getjointfreq4fmkv2(freqallele,allelecounts,jointfreq4fmkv2,16,
data(11,16),data(11,19),17)/
       getfreq(freqallele,freqfreq,data(11,16),16)
  << endl;
  */
  /*  
//write routine to calculate likelihood for family 11
  double lik11=1.0;
  lik11*=getfreq(freqallele,freqfreq,data(11,0),0);
  lik11*=getcondfreq2l(freqallele,allelecounts,condfreq2l,0,data(11,0),data(11,1));
  for (i=2;i<C;i++) {
    lik11*=getcondfreq3mkv2(freqallele,allelecounts,condfreq3lmkv2,
i-2,data(11,i-2),data(11,i-1),data(11,i));
    cout << i << " " << lik11 << endl; }
  for (i=C+1;i<=C+2;i++) {
    lik11*=getcondfreq4lSMART(freqallele,freqfreq,allelecounts,
     jointfreq2,jointfreq3mkv2,jointfreq4mkv2,
     jointfreq4fmkv2,i-3,
     data(11,i-3),data(11,i-2),
     data(11,i-1),data(11,i),C);
    cout << i << " " << lik11 << endl; }
  for (i=C+3;i<data.dimy();i++) {
    if (data(11,i)!=0) {
      if (data(11,i-1) !=0 && data(11,i-2)!=0)
lik11*=getcondfreq3mkv2(freqallele,allelecounts,jointfreq3mkv2,i-2,
data(11,i-2),data(11,i-1),data(11,i))/
  getcondfreq2l(freqallele,allelecounts,jointfreq2,i-2,data(11,i-2),data(11,i-1));
      else {
if (data(11,i-1)==0) {
  lik11*=getcondfreq3fmkv2(freqallele,allelecounts,jointfreq3fmkv2,i-2,
   data(11,i-2),data(11,i))/
    getfreq(freqallele,freqfreq,data(11,i-2),i-2); }
else {
  lik11*=getcondfreq2l(freqallele,allelecounts,jointfreq2,i-1,
       data(11,i-1),data(11,i))/
    getfreq(freqallele,freqfreq,data(11,i-1),i-1); }
      }
    }
    cout << i << " " << lik11 << endl;
  }
  */

  //Fill in output into resultsone
  resultsone[0]=1/tau * 100;    //1/tau (cM)
  resultsone[1]=0;              //se for 1/tau 
  resultsone[2]=p;              //p
  resultsone[3]=0;              //se for p
  resultsone[4]=modellik;       //maximized model likelihood
  resultsone[5]=nulllik;        //null likelihood (p=1)
  resultsone[6]=cstar[LE];      //E(Sharing at LE|data,model)
  resultsone[7]=cstar[RE];      //E(Sharing at RE|data,model)
  resultsone[8]=cstar[C];       //E(Sharing at C|data,model)
  resultsone[9]=iter;           //iterations in EM/HMM

  //cout << x[0]*100 << "   ";
  //for (i=0;i<=9;i++) cout << resultsone[i] << " "; cout << endl; //exit(1);
}//onehapmkv2

void onegenmkv2(dvector &resultsone, const int &C, int &LE, int &RE,
		const igrid &data,
		const ivector &anchap, const dvector &x, const dvector &y,
		const igrid &freqallele, const dgrid &freqfreq, 
		const dgrid &jointfreq2, const dgrid &jointfreq3, 
		const dgrid &jointfreq3f,
		const dgrid &jointfreq4, const dgrid &jointfreq4f,
		const ivector &allelecounts, 
		dgrid &obsprobs, dvectors &obsCnum, dvectors &obsCden,
		const dvector &mut, dvector &mutmatch, dvector &mutnomatch, 
		dgrid &initstore, dgrid &transstore, dgrid &transstoreC, 
		dgrid &alpha, dgrid &beta, dgrid &gamma,
		dvector &cstar, dvector &cstarmut, dvector &loglikvec,
		const int &estp, const double &pinit, const double &m,
		//extra objects for genotype code
		dvector &scratchvec, dgrid &nullprobs,
		dvectors &nullCnum, dvectors &nullCden, dvectors &gammaC)
{
  //this function maximizes the likelihood over 1/tau and p, given the
  //location of the gene and the ancestral haplotype, for genotype
  //data

  int i,j,g; 
  const int numgen=data.dimx()/2;

  //reset alpha,beta,gamma
  alpha.clear(0); beta.clear(0); gamma.clear(0);
  
  /*make parts of observation probabilities that do not depend on parameters*/
  
  //store null sharing probabilities
  makenullsharegenmkv2(nullprobs, data, freqallele, freqfreq,jointfreq2,
		       jointfreq3, jointfreq3f, allelecounts, LE, RE, C,0);
  makenullCnumgenmkv2(nullCnum, data,
		      freqallele, freqfreq, jointfreq2, 
		      jointfreq3, jointfreq3f,jointfreq4,
		      jointfreq4f, allelecounts,
		      LE, RE, C, 0);
  makenullCdengenmkv2(nullCden, data, freqallele, freqfreq, jointfreq2,
		      allelecounts, LE, RE, C, 0); 

  //give initial values for parameters 
  double tau=1e-8; double tau1;
  double p=pinit; double p1;
  
  //compute likelihood at initial values of the parameters
  double modellik=0; double modellikgs= 0;
  makemutmatch(mutmatch, mutnomatch, mut, allelecounts, tau, C);
  makeobsgenmkv2(obsprobs,nullprobs,
		 data,mutmatch,mutnomatch,anchap,C,LE,RE,0);

  makeobsCnumgenmkv2(obsCnum, nullCnum,
		     data, mutmatch, mutnomatch, anchap, C, LE, RE,0);
  makeobsCdengenmkv2(obsCden, nullCden,
		     data, mutmatch, mutnomatch, anchap, C, LE, RE,0);

  makeinitgenmkv2(initstore,scratchvec,x,tau,p,C,LE,RE,0);
  maketransgenmkv2(transstore, scratchvec,x, tau, p, C, LE, RE,0);
  maketransCgenmkv2(transstoreC, scratchvec,y, tau, p, C, LE, RE,0);
  makealphagenmkv2(alpha, initstore, transstore, transstoreC,
		   obsprobs, obsCnum, obsCden, 
		   C, LE, RE);

  //delete me
  //cout << "alpha (g=0) " << endl;
  //for (j=0;j<alpha.dimy();j++) {
  //for (i=0*7;i<(0+1)*7; i++) {
  //  cout << alpha(i,j) << " "; }
  //cout << endl; }

  makeloglikgenmkv2(loglikvec,alpha, initstore, obsprobs, C, RE);
  //exit(1);
  modellik=0; for (g=0;g<numgen;g++) modellik+=loglikvec[g];

  //perform grid search on parameter space to see if this is best starting pt.
  double taugs=0; double pgs=0;
  for (i=0;i<4;i++) {
    if (i==0) taugs=1.0;
    else taugs=1.0/(0.002*i*i*i);
    if (m>0) {
      makemutmatch(mutmatch, mutnomatch, mut, allelecounts, taugs, C);
      makeobsgenmkv2(obsprobs,nullprobs,
		     data,mutmatch,mutnomatch,anchap,C,LE,RE,0);
      makeobsCnumgenmkv2(obsCnum, nullCnum,
			 data, mutmatch, mutnomatch, anchap, C, LE, RE,0);
      makeobsCdengenmkv2(obsCden, nullCden,
			 data, mutmatch, mutnomatch, anchap, C, LE, RE,0);
    }
    if (estp==0) {
      makeinitgenmkv2(initstore,scratchvec,x,tau,p,C,LE,RE,0);
      maketransgenmkv2(transstore, scratchvec,x, tau, p, C, LE, RE,0);
      maketransCgenmkv2(transstoreC, scratchvec,y, tau, p, C, LE, RE,0);
      makealphagenmkv2(alpha, initstore, transstore, transstoreC,
		       obsprobs, obsCnum, obsCden, 
		       C, LE, RE);
      makeloglikgenmkv2(loglikvec,alpha, initstore, obsprobs, C, RE);
      modellikgs=0; for (g=0;g<numgen;g++) modellikgs+=loglikvec[g];
      if (modellikgs>modellik) { 
	modellik=modellikgs; tau=taugs; } }
    else {
      for (j=0;j<3;j++) {
	pgs=0.05+0.45*j;
	makeinitgenmkv2(initstore,scratchvec,x,tau,pgs,C,LE,RE,0);
	maketransgenmkv2(transstore, scratchvec,x, tau, pgs, C, LE, RE,0);
	maketransCgenmkv2(transstoreC, scratchvec,y, tau, pgs, C, LE, RE,0);
	makealphagenmkv2(alpha, initstore, transstore, transstoreC,
			 obsprobs, obsCnum, obsCden, 
			 C, LE, RE);
	makeloglikgenmkv2(loglikvec,alpha, initstore, obsprobs, C, RE);
	modellikgs=0; for (g=0;g<numgen;g++) modellikgs+=loglikvec[g];
	if (modellikgs>modellik) { 
	  modellik=modellikgs; tau=taugs; p=pgs; } } } } 
  //  cout << "gs " <<tau << " " << p << " " << modellik << endl;
  
  //run EM algorithm to maximize likelihood over parameters
  int iter;
  int maxiter=200;

  for (iter=0;iter<maxiter;iter++) {

    //delete me
    //tau=62.5; p=1.0;
    //tau=62.5; p=0.95;//tau=0.239657; p=0.859961;
    //cout << "tau " << tau << " p " << p << endl;

    if (m>0 && iter>0) {
      //make mutation and no-mutation rates for each locus
      makemutmatch(mutmatch, mutnomatch, mut, allelecounts, tau, C);
      //fill in observation probabilities for ancestral state
      makeobsgenmkv2(obsprobs,nullprobs,
		     data,mutmatch,mutnomatch,anchap,C,LE,RE,0);
      makeobsCnumgenmkv2(obsCnum, nullCnum,
			 data, mutmatch, mutnomatch, anchap, C, LE, RE,0);
      makeobsCdengenmkv2(obsCden, nullCden,
			 data, mutmatch, mutnomatch, anchap, C, LE, RE,0);
    }

    makeinitgenmkv2(initstore,scratchvec,x,tau,p,C,LE,RE,0);
    //initstore.dump();
    //    for (i=0;i<initstore.dimx();i++) {
    // for (j=0;j<initstore.dimy();j++) {
    // cout << initstore(i,j) << " "; }
    // cout << endl; }
    
    maketransgenmkv2(transstore, scratchvec,x, tau, p, C, LE, RE,0);
    //transstore.dump();
    //    for (i=0;i<transstore.dimx();i++) {
    //      for (j=0;j<transstore.dimy();j++) {
    // cout << transstore(i,j) << " "; }
    //      cout << endl; }


    maketransCgenmkv2(transstoreC, scratchvec,y, tau, p, C, LE, RE,0);
    //transstoreC.dump();
    //     for (i=0;i<transstoreC.dimx();i++) {
    //  for (j=0;j<transstoreC.dimy();j++) {
    //  cout << transstoreC(i,j) << " "; }
    //  cout << endl; }
    //    exit(1);


    makealphagenmkv2(alpha, initstore, transstore, transstoreC,
		     obsprobs, obsCnum, obsCden, 
		     C, LE, RE);
    //alpha.dump();
    //cout << "alpha:" << endl;
    //for (j=LE;j<=RE;j++) {
    //for (i=7*7;i<7*(7+1);i++)
    //cout << alpha(i,j) << " ";
    //cout << endl; }
    
    //delete me
    //cout << "alph " << alpha(1,2) << endl;

    makebetagenmkv2(beta, initstore, transstore, transstoreC,
		    obsprobs, obsCnum, obsCden, scratchvec,
		    C, LE, RE);
    //beta.dump();
    //cout << "beta:" << endl;
    //for (j=LE;j<=RE;j++) {
    //for (i=7*7;i<7*(7+1);i++)
    //cout << beta(i,j) << " ";
    //cout << endl; }
    //cout << "enable beta asserts" << endl;
    //exit(1);

    makegammaC(gammaC, alpha, beta, obsprobs, obsCnum,
	       obsCden, initstore,transstore, transstoreC, C, LE, RE);

    makegammagenmkv2(gamma, alpha, beta, C, LE, RE,gammaC);
    //gamma.dump();
    //cout << "gamma:" << endl;
    //for (j=8;j<=18;j++) {
    //for (i=8*7;i<(8+1)*7;i++) {
    //cout << gamma(i,j) << " "; } 
    //cout << endl;
    //}
    
    /*
    //delete me
    for (g=0;g<numgen;g++)
      if (g==8)
	cout << g << " "
	     << 
	  2*gamma(7*g,8)+
	  gamma(7*g+1,8)+gamma(7*g+2,8)+
	  gamma(7*g+4,8)+gamma(7*g+5,8) 
	     << " "
	     <<
	  2*gamma(7*g,C)+
	  gamma(7*g+1,C)+gamma(7*g+2,C)+
	  gamma(7*g+4,C)+gamma(7*g+5,C) 
	     << endl;
    */

    makecstargenmkv2(cstar, gamma, C, LE, RE);
    //for (i=0;i<cstar.dim();i++) cout << cstar[i] << " "; cout << endl;
    if (m>0) makecstarmutgenmkv2(cstarmut, cstar,gamma,data,anchap,
				 mutmatch,C,LE,RE);
    //for (i=0;i<cstar.dim();i++) cout << cstarmut[i] << " "; cout << endl;

    //delete me
    //exit(1);
    
    tau1=tau; p1=p;
    
    tau=bisecmle(cstar, cstarmut, y, x, C, LE, RE, mut, allelecounts,m);
    if (estp==1 && (0.000001<p && p<0.999999) ) 
      p=makep(cstar, data.dimx(), C);

    if (estp==0) 
      {if ( ((-0.00001<(tau1-tau)/tau &&
	      (tau1-tau)/tau <0.00001) 
	     || (-0.00001<tau && tau<0.00001)) ) break;}
    else
      {if ( ((-0.00001<(tau1-tau)/tau &&
	      (tau1-tau)/tau <0.00001) 
	     || (-0.00001<tau && tau<0.00001)) && 
	    ( (-0.00001<(p1-p)/p && (p1-p)/p <0.00001)
	      || (-0.00001<(p1-p)&& (p1-p)   <0.00001)) ) break;}
    
  }

    
  //if (iter>=maxiter) { 
  //errorfs << "Warning: Algorithm did not converge for anc. hap" 
  //<< endl;
  //for (i=LE;i<=RE;i++) cout << anchap[i] << " "; cout << endl; }

  //make model loglikelihood
  makemutmatch(mutmatch, mutnomatch, mut, allelecounts, tau, C);
  makeobsgenmkv2(obsprobs,nullprobs,
		 data,mutmatch,mutnomatch,anchap,C,LE,RE,0);
  makeobsCnumgenmkv2(obsCnum, nullCnum,
		     data, mutmatch, mutnomatch, anchap, C, LE, RE,0);
  makeobsCdengenmkv2(obsCden, nullCden,
		     data, mutmatch, mutnomatch, anchap, C, LE, RE,0);
  makeinitgenmkv2(initstore,scratchvec,x,tau,p,C,LE,RE,0);
  maketransgenmkv2(transstore, scratchvec,x, tau, p, C, LE, RE,0);
  maketransCgenmkv2(transstoreC, scratchvec,y, tau, p, C, LE, RE,0);
  makealphagenmkv2(alpha, initstore, transstore, transstoreC,
		   obsprobs, obsCnum, obsCden, 
		   C, LE, RE);
  makeloglikgenmkv2(loglikvec,alpha, initstore, obsprobs, C, RE);
  modellik=0; for (g=0;g<numgen;g++) modellik+=loglikvec[g];

  //make null loglikelihood
  makeinitgenmkv2(initstore,scratchvec,x,tau,1,C,LE,RE,0);
  maketransgenmkv2(transstore, scratchvec,x, tau, 1, C, LE, RE,0);
  maketransCgenmkv2(transstoreC, scratchvec,y, tau, 1, C, LE, RE,0);
  makealphagenmkv2(alpha, initstore, transstore, transstoreC,
		   obsprobs, obsCnum, obsCden, 
		   C, LE, RE);
  //delete me
  //cout << "alpha " << C << endl;
  //for (j=LE;j<=RE;j++) {
  //for (i=5*7;i<(5+1)*7;i++) {
  //cout << alpha(i,j) << " "; }
  //cout << endl; }

  makeloglikgenmkv2(loglikvec,alpha, initstore, obsprobs, C, RE);
  double nulllik=0; for (g=0;g<numgen;g++) nulllik+=loglikvec[g];

  //delete me
  //for (i=0;i<numgen;i++) { cout << C << " " << i << " " << loglikvec[i] << endl;}

  //Fill in output into resultsone
  resultsone[0]=1/tau * 100;    //1/tau (cM)
  resultsone[1]=0;              //se for 1/tau 
  resultsone[2]=p;              //p
  resultsone[3]=0;              //se for p
  resultsone[4]=modellik;       //maximized model likelihood
  resultsone[5]=nulllik;        //null likelihood (p=1)
  resultsone[6]=cstar[LE];      //E(Sharing at LE|data,model)
  resultsone[7]=cstar[RE];      //E(Sharing at RE|data,model)
  resultsone[8]=cstar[C];       //E(Sharing at C|data,model)
  resultsone[9]=iter;           //iterations in EM/HMM

  //for (i=0;i<=9;i++) cout << resultsone[i] << " "; cout << endl; //exit(1);

}//onegenmkv2

void maxmkv2(dvector &resultsmax, const int &ctrall,
	     const igrid &allelelist,
	     igrid &candanc, dgrid &candres, 
	     const double &threshfix, const double &threshvar, 
	     const int &mapest, const int &E, const int &data_type,
	     //onehapobjects
	     dvector &resultsone, const int &C, int &LE, int &RE,
	     const igrid &data,
	     ivector &anchap, const dvector &x, const dvector &y,
	     const igrid &freqallele, const dgrid &freqfreq, 
	     const dgrid &jointfreq2,
	     const dgrid &condfreq2l, const dgrid &condfreq2r, 
	     const dgrid &condfreq2C,
	     const dvector &condfreq3l, const dvector &condfreq3r,
	     const ivector &allelecounts, 
	     dgrid &obsprobs, dgrid &obsprobsCnum,
	     dgrid &obsprobsCden,
	     const dvector &mut, dvector &mutmatch, dvector &mutnomatch, 
	     dgrid &initstore, dgrid &transstore, dgrid &transstoreC, 
	     dgrid &alpha, dgrid &beta, dgrid &gamma,
	     dvector &cstar, dvector &cstarmut, dvector &loglikvec,
	     const int &estp, const double &pinit, const double &m,
	     //extra objects for genotype code
	     dvector &scratchvec, const dgrid &jointfreq2C,
	     const dgrid &condfreq3F, 
	     dvector &obsprobsCltRE, dgrid &nullprobs, 
	     dgrid &nullprobsCnum, dgrid &nullprobsCden, 
	     dvector &nullprobsCltRE, 
	     //for onehapmkv2 
	     const dgrid &jointfreq3mkv2, const dgrid &jointfreq3fmkv2,
	     const dgrid &jointfreq4mkv2, const dgrid &jointfreq4fmkv2,
	     const dgrid &condfreq3fmkv2, const dgrid &condfreq3lmkv2,
	     const dgrid &condfreq3rmkv2, dvector &obsprobsAstore,
	     //for onegenmkv2
	     dvectors &obsCnum, dvectors &obsCden,
	     dvectors &nullCnum, dvectors &nullCden, dvectors &gammaC)
{

  //this function estimates the ancestral haplotype, by applying onehap/onegen
  //for all three locus anc. hap. candidates, sorting by likelihood. Then for
  //those kept, it tries all alleles at the next locus and maximizes the lik
  //for the 4-locus candidates. It continues in this fashion until the
  //ancestral hap. candidates include all markers in the dataset

  int i,j,k;

  //reset candanc, anchap
  candanc.clear(0);
  anchap.clear(0);

  /*we take a jump to the right if the current x-distance on the right
    is smaller than that to the left of the ctr; action is an array
    with 1's and -1's where a 1 [-1] at the hth place means there is a
    jump to the right [left] at the hth jump*/

  int jump=0; //index for stepping over loci in reconstruction;
  int numjump=data.dimy()-3;
  int dirjump=0;
  
  int numcand=0;//number of candidates at step
  int numkeep=0;//number of candidates kept for next step
  
  //E_copy will be interval around which we estimate ancestral haplotype
  int E_copy;
  if (E==0) E_copy=C; else E_copy=E;
  
  //list markers flanking E_copy
  if (E_copy==C) { LE= C-1; RE= C+1; }
  else {
    if (E_copy<C) { LE=E_copy-1; RE=E_copy; }
    else { LE = E_copy; RE=E_copy+1; } }
  
  //prepare cumulative distance variables to tell direction of next jump;
  //d2LE is "distance to left end"
  double d2LE,d2RE;
  if (E_copy==C) { d2LE=y[C-1]; d2RE=y[C]; }
  else {
    if (E_copy<C) { d2LE = d2RE = y[E_copy-1]/2.0; }
    else { d2LE = d2RE = y[E_copy]/2.0; }
  }
  //  cout << C << " " << E_copy << " " << LE << " " << RE << " " 
  //   << d2LE << " " << d2RE << " (initial values)" << endl;
  
  for (jump=0;jump<= numjump;jump++)
    {
      
      if (jump==0) { makeinitcandanc(candanc, C, LE, RE, ctrall, 
				     allelelist, numcand);       }
      
      else { makecandanc(candanc, allelelist, dirjump, numcand, numkeep, 
			 C, LE, RE, ctrall); }
      
      //update LE/RE/d2LE/d2RE
      if (dirjump == -1) 
	{ 
	  if (LE-1 == C) { d2LE += y[LE-2] + y[LE-1]; LE+=-2; }
	  else { d2LE += y[LE-1]; LE+=-1; } 
	}
      if (dirjump == 1) 
	{ 
	  if (RE+1 == C) { d2RE += y[RE] + y[RE+1]; RE+=2; }
	  else { d2RE += y[RE]; RE+=1; } 
	}
      //if (jump>0) 
      //cout << C << " " << E_copy << " " << LE << " " << RE << " " 
      //   << d2LE << " " << d2RE << " (updated values)" << endl;
      
      
      /*we now estimate parameters for each candidate ancestral haplotype*/
      for (k=0; k<numcand; k++)      
	{
	  
	  for (i=LE;i<=RE;i++){anchap[i]=candanc(k,i);}
	  anchap[C]=candanc(k,C);

	  //delete me
	  //for (i=0;i<anchap.dim();i++) cout << anchap[i] << " ";
	  //cout << endl;
	  
	  if (data_type==1) {
	    onehapmkv2(resultsone, C, LE, RE,
		       data,
		       anchap, x, y,
		       freqallele, freqfreq, 
		       jointfreq2, jointfreq3mkv2, jointfreq3fmkv2,
		       jointfreq4mkv2, jointfreq4fmkv2,
		       condfreq2l, condfreq2r, 
		       condfreq3fmkv2, condfreq3lmkv2, condfreq3rmkv2,
		       allelecounts, 
		       obsprobs, obsprobsAstore,
		       mut, mutmatch, mutnomatch, 
		       initstore, transstore, transstoreC, 
		       alpha, beta, gamma,
		       cstar, cstarmut, loglikvec,
		       estp, pinit, m); 
	  }
	  else {//data_type==2
	    onegenmkv2(resultsone, C, LE, RE,
		       data,
		       anchap, x, y,
		       freqallele, freqfreq, 
		       jointfreq2, jointfreq3mkv2, jointfreq3fmkv2, 
		       jointfreq4mkv2, jointfreq4fmkv2,
		       allelecounts, 
		       obsprobs, obsCnum, obsCden,
		       mut, mutmatch, mutnomatch, 
		       initstore, transstore, transstoreC, 
		       alpha, beta, gamma,
		       cstar, cstarmut, loglikvec,
		       estp, pinit, m,
		       scratchvec, nullprobs, nullCnum, nullCden, gammaC);
	  }
	  //for (i=0;i<candres.dimy();i++) cout << resultsone[i] << " ";
	  //cout << endl;
	  for (i=0;i<candres.dimy();i++) candres(k,i)=resultsone[i];
	}
      
      //employ correction (adding log(2)) to equalize null likelihoods
      correctlik(candres, numcand, 4);
      
      /*sort by loglikelihood*/ 
      sortcandancbylik(candanc, candres, numcand, 4);
      
      /*
      //print candanc to screen 
      for (i=0;i<numcand;i++) {
	for (j=0;j<data.dimy();j++) { cout << candanc(i,j) << " "; }
	cout << endl; }
      
      //print candres to screen
      for (i=0;i<numcand;i++) {
	for (j=0;j<candres.dimy();j++) { cout << candres(i,j) << " "; }
	cout << endl; } 
      */

      /*figure out direction of next intended jump*/
      if (LE==0) dirjump = 1; 
      else {
	if (RE==data.dimy()-1) dirjump=-1;
	else {
	  if (C==LE-1) {
	    if (d2LE + y[LE-2] + y[LE-1] < d2RE + y[RE]) dirjump=-1;
	    else dirjump=1; }
	  else {
	    if (C==RE+1) {
	      if (d2LE + y[LE-1] < d2RE + y[RE] + y[RE+1]) dirjump=-1;
	      else dirjump=1; }
	    else {
	      if (d2LE + y[LE-1]<d2RE + y[RE]) dirjump=-1;
	      else dirjump=1; } } } } 
      
      /*find number of haplotypes you want to keep*/
      numkeep=findnumkeep(candres, dirjump, threshfix, threshvar,
			  numcand); 
      
      //if we want to force extention to go to end of map
      if (numkeep>mapest && mapest>0) numkeep=mapest;
      else if (mapest>0 && numkeep==0) {
	if (numcand<=mapest) numkeep=numcand;
	else numkeep=mapest; }
      
      //when not mapping, the ancestral haplotype may not be completely
      //reconstructed
      if (numkeep==0)
	{
	  //cout << "extension?" << endl;
	  numkeep=findnumkeep(candres, -dirjump, threshfix,
			      threshvar, numcand); 
	  
	  
	  if (numkeep==0 || LE-dirjump<0 || RE-dirjump>=data.dimy()) 
	    {
	      //cout << "No" << endl; 
	      break;
	    }
	  else 
	    {
	      //cout << "Yes" << endl;
	      dirjump=-dirjump;
	    }
	}      
      cleantemps(candanc, candres, numcand, numkeep, LE, RE);
    }
  
  for (i=LE;i<=RE;i++) anchap[i]=candanc(0,i);  
  for (j=0;j<10;j++) resultsmax[j]=candres(0,j);
  //  resultsmax[10]=0;
  //  resultsmax[11]=0;
  
}//maxmkv2