#include <time.h>
#include <math.h>
#include <iostream.h>
#include <fstream.h>
#include "/data/xiaoxiak/thesis/header/nrutil.c"
#include "/data/xiaoxiak/thesis/header/matrix.c"
#include "/data/xiaoxiak/thesis/header/random.c"
#include "/data/xiaoxiak/thesis/header/sort.c"
#include "/data/xiaoxiak/thesis/header/math.c"
#include "/data/xiaoxiak/thesis/header/minimiz.c"
#define FREE_ARG char*
#define SHFT(a,b,c,d) (a)=(b);(b)=(c);(c)=(d);
/* function declarations */
void intrpCalc(float *xloc,float **y,float ***vals,int n,int nseq);
void intrpCalc1D(float *y,float **vals,int n);
double getCost(int i,int j, int k, float *xlocInt, float *xloc,float *seq,float *mnInt);
double getVarCost(int i, int j, int k, float *xlocInt, float *xloc, float *seq, float *mnInt, float *varInt);
void kernelSmooth(float width,float *diffxSm, float *xSm,float *ySm,int n,float *x,
   float *y,int NN);
int morf1DIntM(float *xlocInt, float *xloc,float *seq, int *peakIndex,float *mnInt, float *varInt, int n, int nInt, float lambda,
	       int *optPath, double *minCost);
int morf1DInt(float *xlocInt, float *xloc, float *seq, int *peakIndex,float *mnInt,int n,int nInt,float lambda, int *optPath);
void seqCalEM(int **initCal,float **y,float **yInt,float *xlocInt, float *xloc,float lambda,float tol,
	      int nseq,int n,float *yError,int **yyPeakIndex);
void invertMaps(int nseq,int n,int **initCal,float **calInv);
void invertMapsN(int nseq,int gg,int N,int **initCal,float **calInvN);
float getSmyInt(int i,int j,float **calInv,float **y);
int getSmyIntIndex(int i,int j,float **calInv);
float getSmyIntN(int gg,int i,int j,float **calInv,float **yy);
void getInitVal(int seqInd,float *xlocInt, float *xloc,float **y,int **initCal,int n,
		int nInt,int nseq,float lambda,int **yyPeakIndex);
int max(int a, int b);
int min(int a, int b);
/* a function for debugging */
float getSmyInt1D(int n,int i,int j,float **calInv,float **y,int nInt);

/* PENALTY controls how we penalize 1 for |f'-1|^2 2 for |log(f')|^2 and
   3 for |f(x)-x|^2*/
int ff=1,PENALTY=3;
/* maximum change;*/

int MC, MChit=1; 
float BASELINE;
int main()
{
  ifstream filer1("/data/xiaoxiak/thesis/data/yan/79ymass_15_con_norm_log.dat");
   if(!filer1){
      cerr << "Failure to open filer1\n";
      getchar();
      exit(EXIT_FAILURE);
   }
   ifstream filer2("/data/xiaoxiak/thesis/data/yan/79xmass_even.dat");
   if(!filer2){
      cerr << "Failure to open filer2\n";
      getchar();
      exit(EXIT_FAILURE);
   }
   ifstream filer3("/data/xiaoxiak/thesis/data/yan/79yy.peakindex.norm.80.window_400.dat");
   if(!filer3){
      cerr << "Failure to open filer3\n";
      getchar();
      exit(EXIT_FAILURE);
   }
   ofstream filew1("./m1.dat");
   if(!filew1){
      cerr << "Failure to open filew1\n";
      getchar();
      exit(EXIT_FAILURE);
   }
   ofstream filew2("./m2.dat");
   if(!filew2){
      cerr << "Failure to open filew2\n";
      getchar();
      exit(EXIT_FAILURE);
   }
   ofstream filew3("./m3.dat");
   if(!filew3){
      cerr << "Failure to open filew3\n";
      getchar();
      exit(EXIT_FAILURE);
   }
   ofstream filew4;
   filew4.open ("./yy.dat");

   /*
     filew4("./yy.dat");
     if(!filew4){
     cerr << "Failure to open filew4\n";
     getchar();
     exit(EXIT_FAILURE);
   } */
   ofstream costlog("./cost.log");
   if(!costlog){
      cerr << "Failure to open costlog\n";
      getchar();
      exit(EXIT_FAILURE);
   }
   ofstream filewparameter("./parameter.log");
   if(!filewparameter){
      cerr << "Failure to open filewparameter\n";
      getchar();
      exit(EXIT_FAILURE);
   }

   int NN=49999,nseq=79,n,nInt,gg=7,i,j,k,II=1,JJ=1,N=NN,**initCal;
   float width=5.0,tol=1.0e-3,lambda,*xloc,*diffloc,*xlocInt,*yLoc,*yyLoc,*xxLoc,*yError,**y,**yy,
     **xx,**yCal,**yInt,**calInv,**calInvN,***coefCal;
   int **yyPeakIndex;
   float xstep=0;
   long setSeed=-8989;
   time_t t1=time(NULL);
   yy=matrix(1,nseq,1,NN);
   xx=matrix(1,nseq,1,NN);
   yyLoc=vector(1,NN);
   xxLoc=vector(1,NN);
   yyPeakIndex=imatrix(1,nseq,1,n);

     /*this gives data values*/
   filer1.seekg(0);
   for(i=1;i<=nseq;i++){
     for(j=1;j<=NN;j++){
       filer1 >> yy[i][j];
       //if(yy[i][j]<0.0) nrerror("negative intensity");
     }
   }
   /* this gives x co-ordinate-here we assume that xx is in increasing
      order in the file */
   filer2.seekg(0);
   for(i=1;i<=nseq;i++){
     for(j=1;j<=NN;j++){
       filer2 >> xx[i][j];
     }
   }
   /* this gives the base line value*/
   filer3.seekg(0);
   for(i=1;i<=nseq;i++){
     for(j=1;j<=n;j++){
       filer3 >> yyPeakIndex[i][j];
     }
   }
   /* II =: argmax(i) xx[i][1], JJ =: argmin(j) xx[j][NN], N =: max{xx[II][N] <= xx[JJ][NN] */
   for(i=2;i<=nseq;i++){
     if(xx[i][1] > xx[II][1]) II=i;
     if(xx[i][NN] < xx[JJ][NN]) JJ=i;
   }
   while(xx[II][N]>xx[JJ][NN]){
     N--;
   }
   /*common spacing xx[II][1],xx[II][2],...xx[II][N]*/
   //   if(1.0*(N-n)/(n-1.0)!=int((N-n)/(n-1)))
   //  nrerror("nonallowed choices for N,n, and gg");
   //   gg=(N-n)/(n-1);
   n=(N-1)/(gg+1)+1;
   nInt=ff*(n-1)+n;
   N=(gg+1)*(n-1)+1;
   cout << "n = " << n << "\n";
   cout << "II = " << II << "\n";
   cout << "JJ = " << JJ << "\n";
   cout << "N = " << N  << "\n";
   filewparameter << "II = " << II << "\n";
   filewparameter << "JJ = " << JJ << "\n";
   filewparameter << "N = " << N  << "\n";

   y=matrix(1,nseq,1,n);
   yCal=matrix(1,nseq,1,n);
   yInt=matrix(1,nseq,1,nInt);
   xloc=vector(1,n);
   diffloc=vector(1,n);
   xlocInt=vector(1,nInt);
   yLoc=vector(1,n);
   yError=vector(1,nseq);
   calInv=matrix(1,nseq,1,n);
   calInvN=matrix(1,nseq,1,N);
   initCal=imatrix(1,nseq,1,n);
   coefCal=f3tensor(1,nseq,1,n-1,1,2);
   /*for(i=1;i<=n;i++) xloc[i]=xx[1]+(i-1.0)*(xx[N]-xx[1])/(n-1.0);*/
   /*gg=(N-n)/(n-1); gg+1=(N-1)/(n-1)*/
   /*modification #1 */
   for(i=1;i<=n;i++) xloc[i]=xx[II][1+(i-1)*(gg+1)];
   diffloc[1]=xx[II][2]-xx[II][1];
   diffloc[n]=xx[II][N]-xx[II][N-1];
   for(i=2;i<n;i++) {
     diffloc[i]=FMAX((xx[II][1+(i-1)*(gg+1)+1]-xloc[i]),(xloc[i]-xx[II][1+(i-1)*(gg+1)-1]));
     xstep=xstep+diffloc[i]/xloc[i];
   }
   /* specify the maximal allowed shift
   MC=125;
   filewparameter<< "MC = " << MC <<"\n";
   MC=int(MC/(gg+1))+1;
   cout<< "MC = int(MC/(gg+1))+1 = " << MC <<"\n";
   filewparameter<< "MC = int(MC/(gg+1))+1 = " << MC <<"\n";
   cout<< "nseq =  " << nseq <<"\n";
   for(i=1;i<=nseq;i++){
     for(j=1;j<=NN;j++) yyLoc[j]=yy[i][j];
     for(j=1;j<=NN;j++) xxLoc[j]=xx[i][j];
     /*modification #2 */
     kernelSmooth(width,diffloc,xloc,yLoc,n,xxLoc,yyLoc,NN);
     for(j=1;j<=n;j++) {
       y[i][j]=yLoc[j];
       filew4 << yLoc[j] << " ";
     }
   }
   filew4.close();

   /* do calculations for interpolation of the sequences-need this repeatedly
      in M-step, and needed for back interpolating at end, which may be done
      in multiple ways so should be other functions to do that */
   for(i=1;i<=nseq;i++){
     yInt[i][1]=y[i][1];
     xlocInt[1]=xloc[1];
     yInt[i][nInt]=y[i][n];
     xlocInt[nInt]=xloc[n];
   }
   /* assuming the locations are equally spaced-they aren't but okay for now */
   /* xk finish the initial value assignment */
   /*modification #3 */
   for(i=1;i<=nseq;i++){
     for(j=1;j<=n-1;j++){
       for(k=0;k<=ff;k++){
	 xlocInt[((j-1)*(ff+1)+1+k)]=xloc[j]+k*(xloc[j+1]-xloc[j]+0.0)/(ff+1.0);
	 yInt[i][((j-1)*(ff+1)+1+k)]=y[i][j];
       }
     }
   }
   /* here you must provide the ratio of the prior probability
      for the variance of f relative to the variance of the observations
      this could use improvement, but okay for now*/
   //lambda=1.0e2/pow(8,3)*50*1e-4;
   lambda=1.0e-4;
   /* get initial values-using identity like procrustes+var part
      from EM*/
   for(i=1;i<=nseq;i++){
     for(j=1;j<=n;j++){
       initCal[i][j]=1+(j-1)*(nInt-1)/(n-1);
     }
   }
   /* or else try aligning each to one of the sequences*/
   while(MChit==1){
     getInitVal(1,xlocInt,xloc,y,initCal,n,nInt,nseq,lambda,yyPeakIndex);
     /* now do alignment on low res. scale*/
     if(MChit==1){   
       lambda=lambda*1.1;
       cout<<"lambda="<<lambda<<"\n";
       continue;
     }
     seqCalEM(initCal,y,yInt,xlocInt,xloc,lambda,tol,nseq,n,yError,yyPeakIndex);
     if(MChit==1){   
       lambda=lambda*1.1;
       cout<<"lambda="<<lambda<<"\n";
     }
   }
   for(i=1;i<=nseq;i++){
     cout << "minCost["  << i << "]=" << yError[i] << endl;
     costlog << "minCost["  << i << "]=" << yError[i] << "\n";
   }
   /* write calibration curves to file */
   for(i=1;i<=nseq;i++){
     for(j=1;j<=n;j++){
     filew1 << xlocInt[initCal[i][j]] << " ";
   }
  }
   filew1 << "\n";
   /* write calibrated curves to file*/
   /* or use estimated calibration curves to calibrate full length sequences,
      use commenting to select */
   invertMapsN(nseq,gg,N,initCal,calInvN);
   //   intrpCalc(xloc,yCal,coefCal,n,nseq);
   for ( i=1; i<=nseq;i++){
     for(j=1;j<=N;j++){
       //       filew2 << coefCal[i][1+(j-1)/(gg+1)][1]+coefCal[i][1+(j-1)/(gg+1)][2]*xx[II][j]  << " ";
       filew2 << getSmyInt(j,i,calInvN,yy)  << " ";
       filew3 << getSmyIntIndex(j,i,calInvN)  << " ";
     } 
   }
   time_t t2=time(NULL);
   cout <<"Calculation minutes: " << (t2-t1)/60.0 << endl;
   costlog << "Calculation minutes: " << (t2-t1)/60.0 << endl;
   filew2  << "\n";
   filew3  << "\n";
   filewparameter<< "MC = " << MC <<"\n";
   filewparameter<< "lambda = " << lambda <<"\n";
   filewparameter << "Calculation minutes: " << (t2-t1)/60.0 << endl;
   cout << "Finished ! ";
   //   cout << "Press any key to finish";
   //   getchar();
   free_matrix(y,1,nseq,1,n);
   free_matrix(yy,1,nseq,1,NN);
   free_matrix(xx,1,nseq,1,NN);
   free_vector(yyLoc,1,NN);
   free_vector(xxLoc,1,NN);
   free_imatrix(yyPeakIndex,1,nseq,1,n);
   
   free_matrix(yCal,1,nseq,1,n);
   free_matrix(yInt,1,nseq,1,nInt);
   free_vector(xloc,1,n);
   free_vector(diffloc,1,n);
   free_vector(xlocInt,1,nInt);
   free_vector(yLoc,1,n);
   free_vector(yError,1,nseq);
   free_matrix(calInv,1,nseq,1,n);
   free_matrix(calInvN,1,nseq,1,N);
   free_imatrix(initCal,1,nseq,1,n);
   free_f3tensor(coefCal,1,nseq,1,n-1,1,2);
}

/* this function just for debugging */
float getSmyInt1D(int n,int i,int j,float **calInv,float **y,int nInt){
  int k;
  float res;
  /* this gets rid of any decimals on my compiler */
  k=int(calInv[j][i]);
  cout << "i=" << " j=" << j << " k=" << k << endl; 
  res=y[j][k];
  return res;
}

void seqCalEM(int **initCal,float **y,float **yInt,float *xlocInt, float *xloc,float lambda,float tol,
	      int nseq,int n,float *yError,int **yyPeakIndex)
{
  /* for examination of what is happening on each iter */
  ofstream filew1a("./smpls1D.dat");
  if(!filew1a){
    cerr << "Failure to open filew1a\n";
    getchar();
    exit(EXIT_FAILURE);
  }
  /* output log */
  ofstream errlog("./err.log");
  if(!errlog){
    cerr << "Failure to open errlog\n";
    getchar();
    exit(EXIT_FAILURE);
  }


  ofstream deltalog("./delta.log");
  if(!deltalog){
    cerr << "Failure to open delta.log\n";
    getchar();
    exit(EXIT_FAILURE);
  }
  ofstream deltaSlog("./deltaS.log");
  if(!deltaSlog){
    cerr << "Failure to open deltaS.log\n";
    getchar();
    exit(EXIT_FAILURE);
  }

  ofstream errylog("./erry.log");
  if(!errylog){
    cerr << "Failure to open errylog\n";
    getchar();
    exit(EXIT_FAILURE);
  }

  int maxIter=15,nInt=ff*(n-1)+n,ii,i,j,k,*optPath, **initCalPre;
  float x,avgErr, maxDif,*diff,*delta,*deltaS,*mn,*var,*mnInt,*varInt,*seq,**calInv;
  int *peakIndex;
  double  minCost,avgyError;
  optPath=ivector(1,n);
  calInv=matrix(1,nseq,1,n);
  diff=vector(1,nseq);
  delta=vector(1,nseq);
  deltaS=vector(1,nseq);
  mn=vector(1,n);
  var=vector(1,n);
  mnInt=vector(1,nInt);
  varInt=vector(1,nInt);
  peakIndex=ivector(1,n);
  seq=vector(1,n);
  initCalPre=imatrix(1,nseq,1,n);

  for(ii=1;ii<=maxIter;ii++){
    cout << "\n starting iteration " << ii << endl;
    /* first do the E-step-get average of first 2 moments,
       in bad notation call second var even though not centered */
    /* need to invert initCal[j][i] to get a matrix of these */
    invertMaps(nseq,n,initCal,calInv);
    for(i=1;i<=n;i++){
      mn[i]=var[i]=0.0;
      for(j=1;j<=nseq;j++){
	x=getSmyInt(i,j,calInv,y);
	mn[i]+=x;//yInt[j][calInv[j][i]];
	var[i]+=SQR(x);//SQR(yInt[j][calInv[j][i]]);
      }
      mn[i]/=nseq;
      var[i]/=nseq;
    }
    /* now do M-step: align each sequence to the mn
       seq, but include the var term too */
    /* need to do interpolation for the means */
    mnInt[1]=mn[1];
    mnInt[nInt]=mn[n];
    varInt[1]=var[1];
    varInt[nInt]=var[n];
    for(j=1;j<=n-1;j++){
      for(k=0;k<=ff;k++){ 
	mnInt[(j-1)*(ff+1)+1+k]=mn[j];
	varInt[(j-1)*(ff+1)+1+k]=var[j];
      }
    }
    for(i=1;i<=nseq;i++){
      delta[i]=0.0;
      deltaS[i]=0.0;
      diff[i]=0.0;
      for(j=1;j<=n;j++) {seq[j]=y[i][j];peakIndex[j]=yyPeakIndex[i][j];}
      MChit=morf1DIntM(xlocInt,xloc,seq,peakIndex,mnInt,varInt,n,nInt,lambda,optPath,&minCost);

      if(MChit==1) break;
      yError[i]=minCost;
      for(j=1;j<=n;j++) diff[i]+=SQR(initCal[i][j]-optPath[j]);
      diff[i]/=n;
      if(ii != 1){
	for(j=1;j<=n;j++){
	  delta[i]=FMAX(delta[i],fabs(initCal[i][j]-initCalPre[i][j]));
	  deltaS[i]=deltaS[i]+fabs(initCal[i][j]-initCalPre[i][j]);
	}
      }
      for(j=1;j<=n;j++) initCalPre[i][j]=initCal[i][j];
      for(j=1;j<=n;j++){
	filew1a  << optPath[j] << " ";
	initCal[i][j]=optPath[j];
      }
    }
    if(MChit==1) break;
    filew1a  << "\n";
    /* check for convergence */
    avgErr=0.0;
    avgyError=0.0;
    for(i=1;i<=nseq;i++) {
      avgErr+=diff[i];
      avgyError+=yError[i]/nseq;
    }
    avgErr/=nseq;

    cout << "avgErr=" << avgErr << endl;
    errlog << avgErr << "\n";
    cout << "avgyError=" << avgyError << endl;
    errylog << avgyError << "\n";

    if( ii != 1){
      for(i=1;i<=nseq;i++){
	cout << "delta_" << i << " = "<<delta[i] <<",  ";
	deltalog <<delta[i] <<",    ";
      }
      cout << "\n";
      for(i=1;i<=nseq;i++){
	cout << "deltaS_" << i << " = "<<deltaS[i] <<",  ";
	deltaSlog <<deltaS[i] <<",    ";
      }
      deltalog << endl;
      deltaSlog << endl;
    }
    if(avgErr<=tol) break;
    if(ii==maxIter) {
      cout << "maxIter reached before convergence";
      errlog << "maxIter reached before convergence";
      errylog << "maxIter reached before convergence";
    }
  }
  free_ivector(peakIndex,1,n);
  free_ivector(optPath,1,n);
  free_matrix(calInv,1,nseq,1,n);
  free_vector(diff,1,nseq);
  free_vector(delta,1,nseq);
  free_vector(deltaS,1,nseq);
  free_vector(mn,1,n);
  free_vector(var,1,n);
  free_vector(mnInt,1,nInt);
  free_vector(varInt,1,nInt);
  free_vector(seq,1,n);
}
/* xk finish revise getSmyInt */
float getSmyInt(int i,int j,float **calInv,float **y)
{
   int k;
   float res;
   /* this gets rid of any decimals on my compiler */
   k=int(calInv[j][i]);
   res=y[j][k];
   return res;
}
/* xk finish revise index */
int getSmyIntIndex(int i,int j,float **calInv)
{
   int k;
   float res;
   /* this gets rid of any decimals on my compiler */
   k=int(calInv[j][i]);
  return k;
}

float getSmyIntN(int gg,int i,int j,float **calInv,float **yy)
{
  int k,ii=(i-1)/(gg+1)+1;
   float res;
   //intrpCalc1D(initCal[i],coefinitCal,n);
   /* this gets rid of any decimals on my compiler */
   k=int((gg+1)*(calInv[j][ii]-1)+1);
   res=yy[j][k];
   return res;
}

/* revise the invertMaps 04/02/2008 */
void invertMaps(int nseq,int n,int **initCal,float **calInv){
  /*  does not use xloc currently-just uses xloc=1,..,n */
  int i,j,k,kk;
  for(i=1;i<=nseq;i++){
    calInv[i][1]=1;
    calInv[i][n]=n;
    for(j=2;j<=n-1;j++){
      k=1+(j-1)*(1+ff);
      kk=1;
      while(initCal[i][kk]<k) kk+=1;
      calInv[i][j]=kk-1.0+1.0*(k-initCal[i][kk-1])/(initCal[i][kk]-initCal[i][kk-1]);
    }
  }
}

void invertMapsN(int nseq,int gg,int N,int **initCal,float **calInvN){
  /* does not use xloc currently-just uses xloc=1,..,n */
  int i,j,kk;
  float k;
  for(i=1;i<=nseq;i++){
    calInvN[i][1]=1;
    calInvN[i][N]=N;
    for(j=2;j<=N-1;j++){
      k=1.0+(j-1.0)/(gg+1.0)*(1.0+ff);
      kk=1;
      while(initCal[i][kk]<k) kk+=1;
      calInvN[i][j]=(kk-1.0+1.0*(k-initCal[i][kk-1])/(initCal[i][kk]-initCal[i][kk-1])-1.0)*(gg+1.0)+1.0;
    }
  }
}

void getInitVal(int seqInd,float *xlocInt, float *xloc,float **y,int **initCal,int n,
		int nInt,int nseq,float lambda,int **yyPeakIndex)
{
  int i,j,k,*optPath,*peakIndex;
  float *seq,*mn,*mnInt;
  peakIndex=ivector(1,n);
  optPath=ivector(1,n);
  seq=vector(1,n);
  mn=vector(1,n);
  mnInt=vector(1,nInt);
  /* first the easy one */
  for(i=1;i<=n;i++) initCal[seqInd][i]=1+(i-1)*(nInt-1)/(n-1);
  for(j=1;j<=n;j++) mn[j]=y[seqInd][j];
  mnInt[nInt]=mn[n];
  for(j=1;j<=n-1;j++){
    for(k=0;k<=ff;k++){ 
      mnInt[((j-1)*(ff+1)+1+k)]=mn[j];
    }
  }
  for(i=1;i<=nseq;i++){
    if(i!=seqInd){
      for(j=1;j<=n;j++) {seq[j]=y[i][j];
	peakIndex[j]=yyPeakIndex[i][j];
      }
      MChit= morf1DInt(xlocInt, xloc,seq,peakIndex,mnInt,n,nInt,lambda,optPath);
      if(MChit==1) break;
      for(j=1;j<=n;j++) initCal[i][j]=optPath[j];
    }
  }
  free_ivector(peakIndex,1,n);
  free_ivector(optPath,1,n);
  free_vector(seq,1,n);
  free_vector(mn,1,n);
  free_vector(mnInt,1,nInt);
}
/*penalty part finished,*/
int morf1DIntM(float *xlocInt, float *xloc, float *seq, int *peakIndex,float *mnInt, float *varInt, int n, int nInt, float lambda, int *optPath, double *minCost){
  int i,j,k,ii,**pathMat,min_index;
  int sethit=0;
  /* these priors should fix endpoints */
  float lambda0=1.0e10,lambdaN=1.0e10,aa,bb,cc;
  double minCostLoc,temp,**costMat;
   pathMat=imatrix(1,n-1,1,nInt);
   costMat=dmatrix(1,n,1,nInt);
   
   for(i=1;i<=(1+(ff+1)*MC);i++){
     costMat[1][i]=SQR(seq[1]-mnInt[i])+lambda0*SQR(xloc[1]-xloc[i]);
   }
   /* do the forward computations */
   
   for(i=2;i<=n;i++){
     for(j=max(i,(1+(ff+1)*(i-MC-1)));j<=min((nInt-(n-i)),(1+(ff+1)*(i+MC-1)));j++){
       minCostLoc=1.0e10;
       costMat[i][j]=1.0e10;
       for(k=min((j-1),min((nInt-(n-(i-1))),(1+(ff+1)*(i-1+MC-1))));k>=max((i-1),(1+(ff+1)*(i-1-MC-1)));k--){
	 if(minCostLoc > costMat[i-1][k] ){
	   if(PENALTY==1)
	     temp=costMat[i-1][k]+getVarCost(i,j,k,xlocInt,xloc,seq,mnInt,varInt)+
	       lambda*SQR((j-k)/(ff+1.0)-1.0);
	   else if(PENALTY==2)
	     temp=costMat[i-1][k]+getVarCost(i,j,k,xlocInt,xloc,seq,mnInt,varInt)+
	       lambda*SQR(log((j-k)/(ff+1.0)));
	   /*PENALTY==3*/	   
	   else{

	     if((peakIndex[i-1]==1) && (peakIndex[i] == 1) &&  ((xloc[i]-xloc[i-1]) != (xlocInt[j]-xlocInt[k])))
		 temp = 1.0e10;
	     else{
	     cc=xloc[i]-xloc[i-1]+0.0;
	     bb=(xlocInt[j]-xlocInt[k])/cc+0.0;
	     aa=xlocInt[k]-xloc[i-1]*bb;
	     if(fabs(bb-1)<=1e-6)
	       temp=costMat[i-1][k]+getVarCost(i,j,k,xlocInt,xloc,seq,mnInt,varInt)+
		 lambda*SQR(aa);
	     else
	       temp=costMat[i-1][k]+getVarCost(i,j,k,xlocInt,xloc,seq,mnInt,varInt)+
		 lambda*(pow((aa+(bb-1)*xloc[i]),3)-
			 pow((aa+(bb-1)*xloc[i-1]),3))/3.0/(bb-1)/cc;
	     if(i==n) temp+=lambdaN*SQR(1.0*j-nInt);
	     }
	   }

	   if(temp < minCostLoc){
	     /* assuming no ties */
	     minCostLoc=temp;
	     costMat[i][j]=temp;
	     pathMat[i-1][j]=k;
	   }
	   if(minCostLoc < 0){
	     cout<<"******";
	     cout<< "i-1=" << i-1 <<" j=" <<j <<"minCostLoc=" << temp  <<";    ";
	   }
	 }
       }
       
     }
   }
   /* go back and find the best path */
   minCostLoc=1.0e10;
   for(i=(nInt-(ff+1)*MC);i<=nInt;i++){
     if(costMat[n][i] < minCostLoc){
       optPath[n]=i;
       minCostLoc=costMat[n][i];
     }
   }
   /* check whether or not hit the boundary*/
   if(optPath[n] == (nInt-(ff+1)*MC))
     {
       cout<<"E step hit the boundary at " << i << "th step" << " optPath="<< optPath[i]<<endl; 
       sethit=1;
     }
   *minCost=minCostLoc;
   for(i=(n-1);i>1;i--){
     optPath[i]=pathMat[i][optPath[i+1]];
     if(optPath[i] == (1+(ff+1)*(i+MC-1)) || optPath[i] == (1+(ff+1)*(i-MC-1))){
       cout<<"E step hit the boundary at " << i << "th step" << " optPath="<< optPath[i]<<endl; 
       sethit=1;
     }
     if(sethit == 1) break;
   }
   optPath[1]=1;
   free_imatrix(pathMat,1,n-1,1,nInt);
   free_dmatrix(costMat,1,n,1,nInt);
   return sethit;
}
int morf1DInt(float *xlocInt, float *xloc,float *seq, int *peakIndex,float *mnInt,int n,int nInt,float lambda, int *optPath){
  /* this does minimal chain to just align 2 seq the other func that
     is similar is for EM algorithm so costs calucalted slightly differently 
  */
  int i,j,k,ii,**pathMat,min_index,sethit=0;
  /* these priors should fix endpoints 
   */
  float lambda0=1.0e10,lambdaN=1.0e10,aa,bb,cc;
  double minCost,temp,**costMat;
  pathMat=imatrix(1,n-1,1,nInt);
  costMat=dmatrix(1,n,1,nInt);
  for(i=1;i<=(1+(ff+1)*MC);i++){
    costMat[1][i]=SQR(seq[1]-mnInt[i])+lambda0*SQR(xloc[1]-xloc[i]);
  }
  /* do the forward computations */
  for(i=2;i<=n;i++){
    for(j=max(i,(1+(ff+1)*(i-MC-1)));j<=min((nInt-(n-i)),(1+(ff+1)*(i+MC-1)));j++){
      minCost=1.0e10;
      costMat[i][j]=1.0e10;
      for(k=min((j-1),min((nInt-(n-(i-1))),(1+(ff+1)*(i-1+MC-1))));k>=max((i-1),(1+(ff+1)*(i-1-MC-1)));k--){
	if(minCost>costMat[i-1][k]){
	  if(PENALTY==1)
	    temp=costMat[i-1][k]+getCost(i,j,k,xlocInt,xloc,seq,mnInt)+
	      lambda*SQR((j-k)/(ff+1.0)-1.0);
	  else if(PENALTY==2)
	    temp=costMat[i-1][k]+getCost(i,j,k,xlocInt,xloc,seq,mnInt)+
	      lambda*SQR(log((j-k)/(ff+1.0)));
	  /* PENALTY==3 */

	  else{
	    if((peakIndex[i-1]==1) && (peakIndex[i] == 1) &&  ((xloc[i]-xloc[i-1]) != (xlocInt[j]-xlocInt[k])))
	      temp = 1.0e10;
	    else{
	      temp=costMat[i-1][k]+getCost(i,j,k,xlocInt,xloc,seq,mnInt)+
	      lambda*SQR(xlocInt[j]-xloc[i-1]);
	    // temp=costMat[i-1][k]+getCost(i,j,k,xlocInt,xloc,seq,mnInt)+
	    //  lambda*SQR(j-((ff+1.0)*(j-1.0)+1.0));
	    }
	    if(i==n) temp+=lambdaN*SQR(1.0*j-nInt);
	  }
	  if(temp < minCost){
            /* assuming no ties */
	    minCost=temp;
	    costMat[i][j]=temp;
	    pathMat[i-1][j]=k;
	  }
	}
      }
    }
  }
  /* go back and find the best path */
  minCost=1.0e10;
  for(i=(nInt-(ff+1)*MC);i<=nInt;i++){
    if(costMat[n][i] < minCost){
      optPath[n]=i;
      minCost=costMat[n][i];
    }
  }
  if(optPath[n] == (nInt-(ff+1)*MC)){
    cout<<"hit the boundary at " << n << " step" << endl; 
    sethit=1;
    }
  
  for(i=(n-1);i>1;i--) {
    optPath[i]=pathMat[i][optPath[i+1]];
    if(optPath[i] ==(1+(ff+1)*(i+MC-1)) || optPath[i] == (1+(ff+1)*(i-MC-1))){
      cout<<"hit the boundary at " << i << " step" << endl; 
      sethit=1;
    }
    if(sethit==1) break;
  }
  optPath[1]=1;
  free_imatrix(pathMat,1,n-1,1,nInt);
  free_dmatrix(costMat,1,n,1,nInt);
  return sethit;
}


void kernelSmooth(float width,float *diffxSm,float *xSm,float *ySm,int n,float *x,
		  float *y,int NN)
{
  /* gets f(x)=ySm of length n for a set of x's from data vector y (which
     are both of length N-get average over region of total width
     2*width-rectangular kernel here*/
  /* cntNew is the guess for the index of the first x that corresponds
     to the first xSm and wndwMx controls size of window on index scale
     to search for relevant x */
  int i,j,k,kOld;
  float m,width_x;
  k=kOld=1;
  for(i=1;i<=n;i++){
    /* cnt is the guess of the index for the x val corresponding
       to xSm-set up a window in the index of width called wndwMx
       through which look for relevant y points */
      ySm[i]=0.0;
      m=0.0;
      width_x=width*diffxSm[i];
      for(j=k;j<=NN;j++){
	//	cout << "i=" << i << " j=" << j << " xSm[i]= " << xSm[i] << " x[j]= " << x[j] << " dif=" << fabs(xSm[i]-x[j]) << endl;
	if(fabs(xSm[i]-x[j]) <= 0.5*width_x){
	  //cout << "get here" << endl;
	  ySm[i]+=y[j];
	  m+=1.0;
	}
	if(x[j]-0.5*width_x > xSm[i]){
	  if(i<n){
	    if(xSm[i+1]-xSm[i]>width_x){
	      k=j;
               kOld=j;
	    }
	    if(xSm[i+1]-xSm[i]<width_x) k=kOld;
	    break;
	  }
	  else break;
         }
      }
      //cout << "m=" << m << " y=" << ySm[i] << endl;
      if(m==0) nrerror("too fine of grid");
      ySm[i]/=m;
  }
}


double getCost(int i,int j, int k, float *xlocInt,float *xloc,float *seq,float *mnInt)
{
  /* this for getting costs with first order splines and integrals-
     this version for just aligning 2 sequence, there is a different
     one for the M-step in EM that includes the variance effect-called
     getVarCost */
  double res=0.0;
  int ll;
  for(ll=k;ll<=j-1;ll++){
    res=res+SQR(seq[i-1]-mnInt[ll])*(xlocInt[ll+1]-xlocInt[ll]);
  }
  res=res/(xlocInt[j]-xlocInt[k]);
  return res;
}

double getVarCost(int i,int j, int k, float *xlocInt,float *xloc,float *seq, float *mnInt, float *varInt){
  /* this for getting the "cost" or the M-step in EM */
  double res=0.0,temp=0.0;
  int ll;
  for(ll=k;ll<=j-1;ll++){
    temp=temp+(2*seq[i-1]*mnInt[ll]-varInt[ll])*(xlocInt[ll+1]-xlocInt[ll]);
  }
  res=res+SQR(seq[i-1])-temp/(xlocInt[j]-xlocInt[k]);
  res=res;
  return res;
}

void intrpCalc(float *xloc, float **y,float ***vals,int n,int nseq)
{
  int i,j;
  for(i=1;i<=nseq;i++){
    for(j=1;j<=n-1;j++){
      //      vals[i][j][1]=y[i][j]-j*(y[i][j+1]-y[i][j]);
      //      vals[i][j][2]=y[i][j+1]-y[i][j];
      /* more generally, if data isn't at just 1,2,..,n */
      vals[i][j][2]=(y[i][j+1]-y[i][j])/(xloc[j+1]-xloc[j]);
      vals[i][j][1]=y[i][j]-vals[i][j][2]*xloc[j];
    }
  }
}

void intrpCalc1D(float *y,float **vals,int n)
{
  int j;
  for(j=1;j<=n-1;j++){
    vals[j][1]=y[j]-j*(y[j+1]-y[j]);
    vals[j][2]=y[j+1]-y[j];
   }
}

int max(int a, int b){
  return (b<a)?a:b;
}
int min(int a, int b){
  return (a<b)?a:b;
}