/*---------------------------------------------------------------------------*/
/*                                                                           */
/* Program:  warp.c                                                          */
/*                                                                           */
/* Purpose:  This program takes in a collection of 2D landmark locations     */
/*           in one image and their corresponding locations in a second      */
/*           image and defines a global warping function from the first      */
/*           image to the second.  This function is used to "morph" the      */
/*           first image into a new image.                                   */
/*                                                                           */
/* Author:   John Gauch (based on code by Larry Lifshitz)                    */
/*                                                                           */
/* Date:     May 31, 1995                                                    */
/*                                                                           */
/* Note:     Copyright (C) The University of Kansas, 1994, 1995              */
/*---------------------------------------------------------------------------*/
#include <IM.h>
#include "invert.c"

/*---------------------------------------------------------------------------*/
/*                                                                           */
/* Purpose:  This function returns value of basis function.                  */
/*                                                                           */
/*---------------------------------------------------------------------------*/
float H(x1,y1,x2,y2)
   float x1,y1,x2,y2;
   {
   float dx, dy, dist;
   
   dx = x1-x2;
   dy = y1-y2;
   dist = dx*dx + dy*dy;
   if (dist>0.0001)
      return( dist*(float)log((double)dist)/(M_PI*8) );
   else
      return(0);
   }
 
/*---------------------------------------------------------------------------*/
/*                                                                           */
/* Purpose:  This is the main program.                                       */
/*                                                                           */
/*---------------------------------------------------------------------------*/
main(argc, argv)
   int argc;
   char *argv[];
   {
   /* Image variables */
   char Name1[50];
   char Name2[50];
   char Name3[50];
   IM_TYPE *Image1;
   IM_TYPE *Image2;
   FLOAT_TYPE **Data1;
   FLOAT_TYPE **Data2;
   int PixType, Xdim,Ydim,Zdim, DimCnt;

   /* Program variables */
   FILE *fd;
   int Debug = FALSE;
   int PtCnt;
   int i=0,j,x,y;
   float *x1, *y1;
   float *x2, *y2;
   float **Lmatrix;
   float **Linverse;
   float *Avector;
   float *Bvector;
   float Beta = 1000.0;
   float value, Xnew,Ynew;
   float v1,v2,v3,v4, w1,w2,w3,w4;

   /* Interpret program options */
   printf("WARP Program - KUIM Version 1.0\n\n");
   while ((++i<argc) && (argv[i][0] == '-'))
      switch (argv[i][1])
         {
         case 'd':
            Debug = TRUE;
            break;
         default:
            Error("Invalid option encountered");
            break;
         }
   
   /* Check number of file names */
   if (argc-i != 3) {
      fprintf(stderr,"Usage: warp [options] ptfile infile outfile\n");
      fprintf(stderr,"       [-d]    Print debugging information\n");
      exit(1);}

   /* Get image file names from argument list */
   if (sscanf(argv[i++],"%s",Name1)==0)
      Error("Could not get input file name");
   if (sscanf(argv[i++],"%s",Name2)==0)
      Error("Could not get input file name");
   if (sscanf(argv[i++],"%s",Name3)==0)
      Error("Could not get input file name");

   /* Read point file */
   x1 = vector(1,200); y1 = vector(1,200);
   x2 = vector(1,200); y2 = vector(1,200);
   if ((fd = fopen(Name1,"r")) == NULL)
      Error("Could not open point file");
   for (i=1; (i<=200 && (fscanf(fd, "%f %f %f %f\n", 
      &(x2[i]), &(y2[i]), &(x1[i]), &(y1[i])) != EOF)); i++);
   PtCnt = i-1;
   fclose(fd);

   /* Read first image */
   Image1 = im_open(Name2, &PixType, &Xdim, &Ydim, &Zdim, &DimCnt);
   if (DimCnt != 2) Error("Can not process 1D or 3D images");
   Data1 = (FLOAT_TYPE **)im_alloc2D(Image1, FLOAT);
   im_read(Image1, FLOAT, &(Data1[0][0]));

   /* Create output image */
   Image2 = im_create(Name3, FLOAT, Xdim, Ydim, Zdim);
   Data2 = (FLOAT_TYPE **)im_alloc2D(Image2, FLOAT);

   /* Initialize warping matrix */
   Lmatrix = matrix(1,PtCnt+3, 1,PtCnt+3);
   for (i=1; i<=PtCnt; i++)
   for (j=1; j<=PtCnt; j++)
      Lmatrix[i][j] = H(x1[i],y1[i],x1[j],y1[j]);
   for (i=1; i<=PtCnt; i++)
      {
      Lmatrix[i][i] = 1.0 / Beta;
      Lmatrix[i][PtCnt+1] = Lmatrix[PtCnt+1][i] = 1.0;
      Lmatrix[i][PtCnt+2] = Lmatrix[PtCnt+2][i] = x1[i];
      Lmatrix[i][PtCnt+3] = Lmatrix[PtCnt+3][i] = y1[i];
      }
   for (i=PtCnt+1; i<=PtCnt+3; i++)
   for (j=PtCnt+1; j<=PtCnt+3; j++)
      Lmatrix[i][j] = 0.0;

   /* Invert warping matrix */
   Linverse = matrix(1,PtCnt+3, 1,PtCnt+3);
   invert_matrix(Lmatrix, Linverse, PtCnt+3);

   /* Calculate basis function weights */
   Avector = vector(1, PtCnt+3);
   Bvector = vector(1, PtCnt+3);
   for (i=1; i<=PtCnt+3; i++)
      {
      value = 0.0;
      for (j=1; j<=PtCnt; j++)
         value += Linverse[i][j] * x2[j];
      Avector[i] = value;
      value = 0.0;
      for (j=1; j<=PtCnt; j++)
         value += Linverse[i][j] * y2[j];
      Bvector[i] = value;
      }

   /* Perform warping */
   for (y=0; y<Ydim; y++)
   for (x=0; x<Xdim; x++)
      {
      Xnew = Avector[PtCnt+1] + Avector[PtCnt+2]*x + Avector[PtCnt+3]*y;
      Ynew = Bvector[PtCnt+1] + Bvector[PtCnt+2]*x + Bvector[PtCnt+3]*y;
      for (i=1; i<=PtCnt; i++) 
         {
         value = H(x1[i],y1[i],(float)x,(float)y);
         Xnew += Avector[i] * value;
         Ynew += Bvector[i] * value;
         }

      if (Xnew>=0 && Ynew>=0 && Xnew<(Xdim-1) && Ynew<(Ydim-1))
         {
         v1 = v2 = v3 = v4 = Data1[(int)Ynew][(int)Xnew];
         if (Xnew+1 < Xdim) v2 = Data1[(int)Ynew][(int)Xnew+1];
         if (Ynew+1 < Ydim) v3 = Data1[(int)Ynew+1][(int)Xnew];
         if ((Xnew+1 < Xdim) && (Ynew+1 < Ydim))
            v4 = Data1[(int)Ynew+1][(int)Xnew+1];
         w4 = (Xnew-(int)Xnew)*(Ynew-(int)Ynew);
         w3 = (1-Xnew+(int)Xnew)*(Ynew-(int)Ynew);
         w2 = (Xnew-(int)Xnew)*(1-Ynew+(int)Ynew);
         w1 = (1-Xnew+(int)Xnew)*(1-Ynew+(int)Ynew);
         Data2[y][x] = w1*v1 + w2*v2 + w3*v3 + w4*v4;
         }
      else
         Data2[y][x] = 0;
      }

   /* Write information to output image */
   im_write(Image2, FLOAT, &(Data2[0][0]));
   im_free2D(Data1);
   im_free2D(Data2);
   exit(0);
   }