/*---------------------------------------------------------------------------*/
/* 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                                                      */
/*                                                                           */
/* Date:     May 31, 1995                                                    */
/*                                                                           */
/* Note:     Copyright (C) The University of Kansas, 1995                    */
/*---------------------------------------------------------------------------*/
#include <IM.h>
#include "invert.c"

/*---------------------------------------------------------------------------*/
/* Purpose:  This function returns value of basis function.                  */
/*---------------------------------------------------------------------------*/
float H(float x1, float y1, float x2, float 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.                                       */
/*---------------------------------------------------------------------------*/
int main(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, 100);
   y1 = vector(1, 100);
   x2 = vector(1, 100);
   y2 = vector(1, 100);
   if ((fd = fopen(Name1, "r")) == NULL)
      Error("Could not open point file");
   for (i = 1; (i <= 100 && (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, (char *) &(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, (char *) &(Data2[0][0]));
   im_free2D((char **) Data1);
   im_free2D((char **) Data2);
   return (0);
}