/* Curve Registration using dynamic programming


   Cavan Reilly, 2000

   This program solves curve registration problems using dyamic programing-
   here solves for the distortion function as a measure of fit using
   the integrated costs we obtain by linear interpolating
   -in this version we allow interpolation over the range of the
   discrete network
     We can either find the statistic for one value, and write the
   deformation to morfOut2.dat and the deformed predicitons to
   morfOut2.dat, or we can evaluate for a range of lambdas and write
   lambda and the predictive SD using the optimal deformation for that
   value of lambda-the program prompts for choices */

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

static double sqrarg;
#define SQR(a) ((sqrarg=(a)) == 0.0 ? 0.0 : sqrarg*sqrarg)

#define FREE_ARG char*

/* function declarations */
void intrpCalc(double *y,double **vals,int n);
double getCost(int i,int j,int k);
double *vector(long nl,long nh);
void free_vector(double *v);
int *ivector(long nl,long nh);
void free_ivector(int *v);
double **matrix(long nrl,long nrh,long ncl,long nch);
void free_matrix(double **m);
int **imatrix(long nrl,long nrh,long ncl,long nch);
void free_imatrix(int **m);
void nrerror(char error_text[]);

/* Global Variables */
// PENALTY controls how we penalize 1 for |f'-1|^2 2 for |log(f')|^2 and
// 3 for |f(x)-x|^2
int ff=3,PENALTY=3;
double *yGlb,*yHatGlb,**coefGlb,**coefHatGlb,**coefHatGlbInt;

int main()
{
/* this file holds observed values */
   ifstream filer1("c:\\Spluswin\\home\\smpld1D.dat");
   if(!filer1){
      cerr << "Failure to open filer1\n";
      getch();
      exit(EXIT_FAILURE);
   }
/* this file holds the predictions-one for each observation */
   ifstream filer2("c:\\Spluswin\\home\\pred1D.dat");
   if(!filer2){
      cerr << "Failure to open filer2\n";
      getch();
      exit(EXIT_FAILURE);
   }
/* this file hold co-ordinates of sites */
   ifstream filer3("c:\\Spluswin\\home\\xpos1D.dat");
   if(!filer3){
      cerr << "Failure to open filer3\n";
      getch();
      exit(EXIT_FAILURE);
   }
/* output file for distortion function */
   ofstream filew1("c:\\Spluswin\\home\\morfOut1.dat");
   if(!filew1){
      cerr << "Failure to open filew1\n";
      getch();
      exit(EXIT_FAILURE);
   }
/* output file for the distorted predictions */
   ofstream filew2("c:\\Spluswin\\home\\morfOut2.dat");
   if(!filew2){
      cerr << "Failure to open filew2\n";
      getch();
      exit(EXIT_FAILURE);
   }
   int n=34,nInt=ff*(n-1)+n,numEval=1,i,j,k;
   int *optPath;
   int **pathMat;
   double lambda=1.0e20,lambda0=1.0e30,lambdaN=1.0e30,b4MSS=0.0,temp,
      minCost,lamInc,lamhi,alfa,beta;
   double *xloc,*xlocInt,*yInt,*yHatInt;
   double **costMat;
   yGlb=vector(1,n);
   yHatGlb=vector(1,n);
   xloc=vector(1,n);
   yInt=vector(1,nInt);
   yHatInt=vector(1,nInt);
   xlocInt=vector(1,nInt);
   optPath=ivector(1,n);
   costMat=matrix(1,n,1,nInt);
   coefGlb=matrix(1,n-1,1,2);
   coefHatGlb=matrix(1,n-1,1,2);
   coefHatGlbInt=matrix(1,nInt-1,1,2);
   pathMat=imatrix(1,n-1,1,nInt);
/*this gives data values*/
   filer1.seekg(0);
   for(i=1;i<=n;i++){
      filer1 >> yGlb[i];
   }
/* this gets predictions */
   filer2.seekg(0);
   for(i=1;i<=n;i++){
      filer2 >> yHatGlb[i];
   }
/* this gives x co-ordinate-here we assume that xloc is in increasing
   order in the file */
   filer3.seekg(0);
   for(i=1;i<=n;i++){
      filer3 >> xloc[i];
   }
   intrpCalc(yGlb,coefGlb,n);
   intrpCalc(yHatGlb,coefHatGlb,n);
   yInt[1]=yGlb[1];  yHatInt[1]=yHatGlb[1]; xlocInt[1]=xloc[1];
   yInt[nInt]=yGlb[n];  yHatInt[nInt]=yHatGlb[n]; xlocInt[nInt]=xloc[n];
   /* assuming the locations are equally spaced */
   for(i=2;i<=nInt-1;i++){
      xlocInt[i]=xloc[1]+(i-1.0)*(xloc[n]-xloc[1])/(nInt-1.0);
      yInt[i]=coefGlb[1+(i-1)/(ff+1)][1]+coefGlb[1+(i-1)/(ff+1)][2]*
         (1.0+1.0*(i-1.0)/(ff+1.0));
      yHatInt[i]=coefHatGlb[1+(i-1)/(ff+1)][1]+
         coefHatGlb[1+(i-1)/(ff+1)][2]*(1.0+1.0*(i-1.0)/(ff+1.0));
   }
   intrpCalc(yHatInt,coefHatGlbInt,nInt);
   cout << "Give a value for lowest log lambda (the trade-off parameter):";
   cin >> lambda;
   cout << "how many evaluations:";
   cin >> numEval;
   if(numEval > 1){
      cout << "what is high value of log lambda:";
      cin >> lamhi;
      lamInc=(lamhi-lambda)/(numEval-1.0);
   }
   for(i=1;i<=nInt;i++){
      costMat[1][i]=SQR(yGlb[1]-yHatInt[i])+lambda0*SQR(1.0-i);
   }
   for(int ii=1;ii<=numEval;ii++){
      cout << "starting evaluation number " << ii << endl;
      /* do the forward computations */
      for(i=2;i<=n;i++){
         for(j=1;j<=nInt;j++){
            minCost=1.0e300;
            costMat[i][j]=1.0e300;
            for(k=1;k<j;k++){
               if(PENALTY==1)
                  temp=costMat[i-1][k]+getCost(i,j,k)+
                     exp(lambda)*SQR((j-k)/(ff+1.0)-1.0);
               else if(PENALTY==2)
                  temp=costMat[i-1][k]+getCost(i,j,k)+
                     exp(lambda)*SQR(log((j-k)/(ff+1.0)));
               else
                  temp=costMat[i-1][k]+getCost(i,j,k)+
                     exp(lambda)*SQR(j-((ff+1.0)*(i-1)+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.0e300;
      for(i=1;i<=nInt;i++){
         if(costMat[n][i] < minCost){
            optPath[n]=i;
            minCost=costMat[n][i];
         }
      }
      for(i=n-1;i>=1;i--) optPath[i]=pathMat[i][optPath[i+1]];
      /* find two components of error and write them to file with lambda */
         temp=0.0;
         if(PENALTY==1){
            for(i=2;i<=n;i++)
               temp+=SQR((xlocInt[optPath[i]]-xlocInt[optPath[i-1]])/
                  (xloc[i]-xloc[i-1])-1.0)*(xloc[i]-xloc[i-1]);
         } else if(PENALTY==2){
            for(i=2;i<=n;i++)
               temp+=SQR(log((xlocInt[optPath[i]]-xlocInt[optPath[i-1]])/
                  (xloc[i]-xloc[i-1])))*(xloc[i]-xloc[i-1]);
         } else{
            for(i=2;i<=n;i++){
               alfa=xlocInt[optPath[i-1]]-xloc[i-1]*(xlocInt[optPath[i]]-
                  xlocInt[optPath[i-1]])/(xloc[i]-xloc[i-1]);
               beta=(xlocInt[optPath[i]]-xlocInt[optPath[i-1]])/
                  (xloc[i]-xloc[i-1])-1.0;
               if(beta==0)
                  temp+=SQR(alfa)*(xloc[i]-xloc[i-1]);
               else
                  temp+=(pow(alfa+xloc[i]*beta,3)-pow(alfa+xloc[i-1]*beta,3))/beta;
            }
            temp/=3.0;
         }
      if(numEval > 1){
         filew1 << exp(lambda) << " " << sqrt(temp/(n-1.0)) << " " <<
            sqrt((minCost-costMat[1][optPath[1]]-exp(lambda)*temp)/(n-1.0)) << " ";
         lambda+=lamInc;
      }
   }
   if(numEval==1){
      for(i=2;i<=n;i++) b4MSS += ((pow(coefGlb[i-1][1]-coefHatGlb[i-1][1]+
         i*(coefGlb[i-1][2]-coefHatGlb[i-1][2]),3)-pow(coefGlb[i-1][1]-
         coefHatGlb[i-1][1]+(i-1.0)*(coefGlb[i-1][2]-
         coefHatGlb[i-1][2]),3))/(coefGlb[i-1][2]-coefHatGlb[i-1][2]));
      b4MSS /= (3.0*(n-1.0));
      cout << "predictive s.d. before transformation:" << sqrt(b4MSS)
         << endl;
      cout << "predictive s.d. after transformation:" <<
         sqrt((minCost-costMat[1][optPath[1]]-exp(lambda)*temp)/(n-1.0)) << endl;
      cout << "standardized deformation penalty:" << sqrt(temp/(n-1.0)) << endl;
      for(i=1;i<=n;i++) filew1 << xloc[i] << " ";
      for(i=1;i<=n;i++) filew1 << xlocInt[optPath[i]] << " ";
      for(i=1;i<=n;i++) filew2 << yHatInt[optPath[i]] << " ";
   }
   cout << "Press any key to finish";
   getch();
   free_vector(yGlb);
   free_vector(yHatGlb);
   free_vector(xloc);
   free_vector(yInt);
   free_vector(yHatInt);
   free_vector(xlocInt);
   free_ivector(optPath);
   free_matrix(costMat);
   free_matrix(coefGlb);
   free_matrix(coefHatGlb);
   free_matrix(coefHatGlbInt);
   free_imatrix(pathMat);
}

double getCost(int i,int j,int k)
{
   int l;
   double res=0.0;
   for(l=k;l<=j-1;l++){
      res+=((pow(coefGlb[i-1][1]-coefHatGlbInt[l][1]-
         coefHatGlbInt[l][2]*l+coefGlb[i-1][2]*
         (i-1.0+1.0*(l-k)/(j-k))+(coefGlb[i-1][2]/(j-k)-
         coefHatGlbInt[l][2]),3)-pow(coefGlb[i-1][1]-
         coefHatGlbInt[l][1]-coefHatGlbInt[l][2]*l+coefGlb[i-1][2]*
         (i-1.0+1.0*(l-k)/(j-k)),3))/(coefGlb[i-1][2]-
         (j-k)*coefHatGlbInt[l][2]));
   }
   return res/3.0;
}

/*double getCost(int i,int j,int k)
{
// this routine usees notation from paper but other things must change too-
// in other routine j and k go up to nInt-this is different here
   int l,l1,l2,m=ff+1;
   double res=0.0;
   j=j/ff;
   k=k/ff;
   l1=(m+k)/m;
   l2=j/m+1;
   for(l=l1;l<=l2;l++){
      res+=((pow(coefGlb[i-1][1]-coefHatGlbInt[l][1]-
         coefHatGlbInt[l][2]*(l+1.0)+coefGlb[i-1][2]*
         (i-1.0+1.0*(l*m-k)/(j-k)),3)-pow(coefGlb[i-1][1]-
         coefHatGlbInt[l][1]-coefHatGlbInt[l][2]*l+coefGlb[i-1][2]*
         (i-1.0+1.0*(m*(l-1.0)-k)/(j-k)),3))/(coefGlb[i-1][2]-
         (j-k)*coefHatGlbInt[l][2]/m));
   }
   return res/3.0;
}
*/
void intrpCalc(double *y,double **vals,int n)
{
   int i;
   for(i=1;i<=n-1;i++){
      vals[i][1]=y[i]-i*(y[i+1]-y[i]);
      vals[i][2]=y[i+1]-y[i];
   }
}

/* memory allocation functions */
double *vector(long nl,long nh)
/* allocate a vector with index range v[nl...nh] */
{
   double *v;
   v=(double *)malloc((size_t) ((nh-nl+2)*sizeof(double)));
   if (!v) nrerror("allocation failure in vector()");
   return v-nl+1;
}

void free_vector(double *v)
/* free a vector */
{
   free((FREE_ARG) (v));
}

int *ivector(long nl,long nh)
/* allocate a vector of integers with index range v[nl...nh] */
{
   int *v;
   v=(int *)malloc((size_t) ((nh-nl+2)*sizeof(int)));
   if (!v) nrerror("allocation failure in ivector()");
   return v-nl+1;
}

void free_ivector(int *v)
/* free a vector of integers */
{
   free((FREE_ARG) (v));
}

double **matrix(long nrl,long nrh,long ncl,long nch)
/* allocate a matrix */
{
   long i, nrow=nrh-nrl+1,ncol=nch-ncl+1;
   double **m;
   /* allocate pointers to rows */
   m=(double **) malloc((size_t)((nrow+1)*sizeof(double*)));
   if (!m) nrerror("allocation failure 1 in matrix()");
   m += 1;
   m -= nrl;
   /* allocate rows and set pointers to them */
   m[nrl]=(double *) malloc((size_t)((nrow*ncol+1)*sizeof(double)));
   if (!m[nrl]) nrerror("allocation failure 2 in matrix()");
   m[nrl] += 1;
   m[nrl] -= ncl;
   for(i=nrl+1;i<=nrh;i++) m[i]=m[i-1]+ncol;
   /* return pointer to array of pointers to rows */
   return m;
}

void free_matrix(double **m)
/* free a matrix */
{
   free((FREE_ARG) (m[1]));
   free((FREE_ARG) (m));
}

int **imatrix(long nrl,long nrh,long ncl,long nch)
/* allocate a matrix of integers */
{
   long i, nrow=nrh-nrl+1,ncol=nch-ncl+1;
   int **m;
   /* allocate pointers to rows */
   m=(int **) malloc((size_t)((nrow+1)*sizeof(int*)));
   if (!m) nrerror("allocation failure 1 in imatrix()");
   m += 1;
   m -= nrl;
   /* allocate rows and set pointers to them */
   m[nrl]=(int *) malloc((size_t)((nrow*ncol+1)*sizeof(int)));
   if (!m[nrl]) nrerror("allocation failure 2 in imatrix()");
   m[nrl] += 1;
   m[nrl] -= ncl;
   for(i=nrl+1;i<=nrh;i++) m[i]=m[i-1]+ncol;
   /* return pointer to array of pointers to rows */
   return m;
}

void free_imatrix(int **m)
/* free a matrix of integers */
{
   free((FREE_ARG) (m[1]));
   free((FREE_ARG) (m));
}

/* error handler */
void nrerror(char error_text[])
{
   fprintf(stderr,"Run-time error...\n");
   fprintf(stderr,"%s\n",error_text);
   fprintf(stderr,"...now exiting to system...\n");
   getch();
   exit(1);
}