#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include<string.h>
#include <mvcd.h>
#include <cres.h>
#include <matrixSHEN.h>  /*by SHEN*/

/* features for vertices */
#define BG    0            /* background */
#define CSF   10
#define VN    50
#define GM    150
#define WM    250

#define YYES    1
#define NNO     0


int HeadInField ;
int Last_image_size, Last_z_size ;

void MarcMain(int,char *[]);
void ShenMain(int argc,char *argv[]) ;
void show_usage_SHEN() ;


void WriteImg(char filename[80], unsigned char ***data, int image_size, int z_size)
{
  FILE  *fp;
  int   i, k ;

  /* write the smoothed image */
  fp=myopen(filename,"wb");
  for(k=0;k<z_size;k++)
    for(i=0;i<image_size;i++)
      fwrite(data[k][i],1,image_size,fp);
  fclose(fp);
}


void OpenDeformationField(char filename[80], Fvector3d ***DeformFld, int image_size, int z_size)
{
  FILE  *fp;
  Fvector3d ***Last_DeformFld ;
  float XYZ_ratio, deform_ratio, dx, dy, dz ;
  /* for linear interpolation */
  float ii,jj,kk, b,c,d, b1,c1,d1;
  int   i,j,k,ni,nj,nk, niP1,njP1,nkP1, GreyValue ;
  float alpha ;

  /* open deformation field in the last resolution */
  fp=myopen(filename,"rb");
  if( HeadInField==YYES )
    {
      fread(&Last_image_size,sizeof(int),1,fp); printf("Last_image_size=%d ", Last_image_size) ;
      fread(&Last_z_size,sizeof(int),1,fp);     printf("Last_z_size=%d\n", Last_z_size) ;
    }
  else
    {
      fseek(fp,0,SEEK_END);
      Last_z_size=ftell(fp)/(Last_image_size*Last_image_size*12);
      rewind(fp);
      printf("given: Last_image_size=%d ", Last_image_size) ;
      printf("Last_z_size=%d\n", Last_z_size) ;
    }

  Last_DeformFld = Fvector3dalloc3d(Last_image_size,Last_image_size,Last_z_size);
  
  for(k=0;k<Last_z_size;k++)
    for(i=0;i<Last_image_size;i++)
      fread(Last_DeformFld[k][i],sizeof(Fvector3d),Last_image_size,fp);
  fclose(fp);


  /* interpolation */
  XYZ_ratio = (float)Last_z_size/(float)z_size ;
  XYZ_ratio = 1.0 ; /* July 20, 2001 */

  deform_ratio = 1.0/XYZ_ratio ;
  alpha        = 1.0 ;
  deform_ratio = ( 1.0*alpha + deform_ratio*(1.0-alpha) ) ;  /* changed back June 18, 2001 */
  for(k=0; k<z_size; k++)
    for(i=0; i<image_size; i++)
      for(j=0; j<image_size; j++)
	{
	  ii = i*XYZ_ratio ;
	  jj = j*XYZ_ratio ;
	  kk = k*XYZ_ratio ;

	  ni = (int)ii ;
	  nj = (int)jj ;
	  nk = (int)kk ;

	  niP1 = ni+1 ;
	  njP1 = nj+1 ;
	  nkP1 = nk+1 ;

	  if(ni>=0 && ni<Last_image_size-1  &&  nj>=0 && nj<Last_image_size-1  &&  nk>=0 && nk<Last_z_size-1 )
	    {
	      b = ii-ni ;        b1 = 1.-b ;
	      c = jj-nj ;        c1 = 1.-c ;
	      d = kk-nk ;        d1 = 1.-d ;

	      dx = ( d1*(Last_DeformFld[nk][ni][nj].x*(b1*c1) + Last_DeformFld[nk][niP1][nj].x*(b*c1) + Last_DeformFld[nk][ni][njP1].x*(b1*c) + Last_DeformFld[nk][niP1][njP1].x*(b*c)) + d*(Last_DeformFld[nkP1][ni][nj].x*(b1*c1) + Last_DeformFld[nkP1][niP1][nj].x*(b*c1) + Last_DeformFld[nkP1][ni][njP1].x*(b1*c) + Last_DeformFld[nkP1][niP1][njP1].x*(b*c)) )/( d1*((b1*c1)+(b*c1)+(b1*c)+(b*c)) + d*((b1*c1)+(b*c1)+(b1*c)+(b*c)) ) ; 
	      dy = ( d1*(Last_DeformFld[nk][ni][nj].y*(b1*c1) + Last_DeformFld[nk][niP1][nj].y*(b*c1) + Last_DeformFld[nk][ni][njP1].y*(b1*c) + Last_DeformFld[nk][niP1][njP1].y*(b*c)) + d*(Last_DeformFld[nkP1][ni][nj].y*(b1*c1) + Last_DeformFld[nkP1][niP1][nj].y*(b*c1) + Last_DeformFld[nkP1][ni][njP1].y*(b1*c) + Last_DeformFld[nkP1][niP1][njP1].y*(b*c)) )/( d1*((b1*c1)+(b*c1)+(b1*c)+(b*c)) + d*((b1*c1)+(b*c1)+(b1*c)+(b*c)) ) ; 
	      dz = ( d1*(Last_DeformFld[nk][ni][nj].z*(b1*c1) + Last_DeformFld[nk][niP1][nj].z*(b*c1) + Last_DeformFld[nk][ni][njP1].z*(b1*c) + Last_DeformFld[nk][niP1][njP1].z*(b*c)) + d*(Last_DeformFld[nkP1][ni][nj].z*(b1*c1) + Last_DeformFld[nkP1][niP1][nj].z*(b*c1) + Last_DeformFld[nkP1][ni][njP1].z*(b1*c) + Last_DeformFld[nkP1][niP1][njP1].z*(b*c)) )/( d1*((b1*c1)+(b*c1)+(b1*c)+(b*c)) + d*((b1*c1)+(b*c1)+(b1*c)+(b*c)) ) ; 

	      DeformFld[k][i][j].x = dx*deform_ratio ; /* *deform_ratio, June 14, 2001*/
	      DeformFld[k][i][j].y = dy*deform_ratio ;
	      DeformFld[k][i][j].z = dz*deform_ratio ;
	  }

	  if(ni==Last_image_size-1 && nj>=0 && nj<Last_image_size-1 && nk>=0 && nk<Last_z_size-1 || ni>=0 && ni<Last_image_size-1 && nj==Last_image_size-1 && nk>=0 && nk<Last_z_size-1  || ni>=0 && ni<Last_image_size-1 && nj>=0 && nj<Last_image_size-1 && nk==Last_z_size-1)
	    {
	      DeformFld[k][i][j].x = Last_DeformFld[nk][ni][nj].x*deform_ratio ; /* *deform_ratio, June 14, 2001*/
	      DeformFld[k][i][j].y = Last_DeformFld[nk][ni][nj].y*deform_ratio ;
	      DeformFld[k][i][j].z = Last_DeformFld[nk][ni][nj].z*deform_ratio ;
	    }

	  /* boundary constraints */
	  /*if(DeformFld[k][i][j].x+i<0) DeformFld[k][i][j].x=-i ;
	  if(DeformFld[k][i][j].y+j<0) DeformFld[k][i][j].y=-j ; 
	  if(DeformFld[k][i][j].z+k<0) DeformFld[k][i][j].z=-k ; 
 
	  if(DeformFld[k][i][j].x+i>=(image_size-1)) DeformFld[k][i][j].x=(image_size-1)-i ; 
	  if(DeformFld[k][i][j].y+j>=(image_size-1)) DeformFld[k][i][j].y=(image_size-1)-j ; 
	  if(DeformFld[k][i][j].z+k>=(z_size-1))     DeformFld[k][i][j].z=(z_size-1)-k ;*/
	}

  /* free */
  Fvector3dfree3d(Last_DeformFld, Last_z_size, Last_image_size) ;
 }


unsigned char InterpolatedIntensity(float ii, float jj, float kk, unsigned char ***Img, int image_size, int z_size)
{
  float CurrentV ;
  float b,c,d, b1,c1,d1;
  int   ni,nj,nk, niP1,njP1,nkP1, GreyValue ;


  ni = (int)ii ;
  nj = (int)jj ;
  nk = (int)kk ;
  
  niP1 = ni+1 ;
  njP1 = nj+1 ;
  nkP1 = nk+1 ;
  
  GreyValue = 0;
  if(ni>=0 && ni<image_size-1  &&  nj>=0 && nj<image_size-1  &&  nk>=0 && nk<z_size-1 )
    {
      b = ii-ni ;        b1 = 1.-b ;
      c = jj-nj ;        c1 = 1.-c ;
      d = kk-nk ;        d1 = 1.-d ;

      CurrentV = ( d1*(Img[nk][ni][nj]*(b1*c1) + Img[nk][niP1][nj]*(b*c1) + Img[nk][ni][njP1]*(b1*c) + Img[nk][niP1][njP1]*(b*c)) + d*(Img[nkP1][ni][nj]*(b1*c1) + Img[nkP1][niP1][nj]*(b*c1) + Img[nkP1][ni][njP1]*(b1*c) + Img[nkP1][niP1][njP1]*(b*c)) )/( d1*((b1*c1)+(b*c1)+(b1*c)+(b*c)) + d*((b1*c1)+(b*c1)+(b1*c)+(b*c)) ) ; 

      if( CurrentV>255 )
	GreyValue = 255 ;
      else
	GreyValue = CurrentV ;
    }
  
  if(ni==image_size-1 && nj>=0 && nj<image_size-1 && nk>=0 && nk<z_size-1 || ni>=0 && ni<image_size-1 && nj==image_size-1 && nk>=0 && nk<z_size-1  || ni>=0 && ni<image_size-1 && nj>=0 && nj<image_size-1 && nk==z_size-1)
    GreyValue = Img[nk][ni][nj] ;

  return GreyValue ;
}

int INTERPOLATE /*=YYES*/ ;
void SaveCurrentWarpingResultOnOriginalObjImg( unsigned char ***ObjOriginalImg, Fvector3d ***DeformFld, int image_size, int z_size, char filename[80] ) 
{
  int s, i, j, k, x, y, z ;
  FILE  *fp;
  unsigned char ***resImg ;
  float  ii, jj, kk ;

  resImg = UCalloc3d(image_size,image_size,z_size); 

  printf("INTERPOLATE=%d\n", INTERPOLATE) ;
  for(k=0; k<z_size; k++)
    for(i=0; i<image_size; i++)
      for(j=0; j<image_size; j++)
	{
	  if(INTERPOLATE==YYES)
	    {
	      ii = i + DeformFld[k][i][j].x ;
	      jj = j + DeformFld[k][i][j].y ;
	      kk = k + DeformFld[k][i][j].z ;

	      resImg[k][i][j] = InterpolatedIntensity(ii, jj, kk, ObjOriginalImg, image_size, z_size) ;
	    }
	  else
	    {
	      x = i + DeformFld[k][i][j].x + 0.5 ;
	      y = j + DeformFld[k][i][j].y + 0.5 ;
	      z = k + DeformFld[k][i][j].z + 0.5 ;
	      
	      if( x<image_size && y<image_size && z<z_size && x>=0 && y>=0 && z>=0 )
		resImg[k][i][j] = ObjOriginalImg[z][x][y] ;
	      else
		resImg[k][i][j] = 0 ;
	    }
	}

  /* save */
  WriteImg(filename, resImg, image_size, z_size) ;

  /* free */
  UCfree3d(resImg, z_size, image_size) ;
}






int main(int argc,char *argv[])
{
  ShenMain(argc,argv);
  return 0;   
}



/* the following is edited by SHEN */
void ShenMain(int argc,char *argv[])
{
  int           image_size, z_size, nb_size;
  Fvector3d     ***DeformFld ;
  int           i,j,k,c,num;
  FILE          *fp;
  extern char   *optarg;
  char          filename[80] ;
  float         level, weight, thres ;
  unsigned char ***Img;


  num=4;

  if(argc<num)
    show_usage_SHEN() ;


  /* input image size */
  image_size= 256;
  z_size    = 124 ;
  /* vector fields size */
  Last_image_size= 256;
  Last_z_size    = 124 ;

  HeadInField = NNO  ; /* = YYES before Dec 5, 2003 */
  INTERPOLATE = YYES ;

  while((c=getopt(argc-3,argv+3,"v:s:I")) != -1)
    {
      switch(c)
	{
	case 'v':
	  sscanf(optarg, "%d", &Last_image_size) ; /* for vector field */
	  HeadInField = NNO ;
	  break ;

	case 's':
	  sscanf(optarg, "%d", &image_size) ; /* for input image field */
	  break ;

	case 'I':
	  INTERPOLATE = NNO ;
	  break ;

	default:
	  break;
	}
    }
  if(HeadInField==YYES) printf("There is size information in vector field file!\n") ;
  else                  printf("No head in vector field file!\n") ;


  /* image */
  fp=myopen(argv[2],"rb");
  fseek(fp,0,SEEK_END);
  z_size=ftell(fp)/(image_size*image_size);
  rewind(fp);
  Img = UCalloc3d(image_size,image_size,z_size);
  for(k=0;k<z_size;k++)
    for(i=0;i<image_size;i++)
      fread(Img[k][i],1,image_size,fp);
  fclose(fp);
  printf("\n\n^^^^  Original object image input!\n") ;
  printf("image size: image_size=%d  z_size=%d\n", image_size, z_size) ;



  /* vector field */ 
  DeformFld = Fvector3dalloc3d(image_size,image_size,z_size);
  OpenDeformationField(argv[1], DeformFld, image_size, z_size);
  printf("vector field size: image_size=%d  z_size=%d\n", Last_image_size, Last_z_size) ;

	   /* perform and save the result */
  SaveCurrentWarpingResultOnOriginalObjImg( Img, DeformFld, image_size, z_size, argv[3] ) ;
}


void show_usage_SHEN()
{
    char fullVersion[] = "$Rev: 83 $"; //This line will be automatically 
                                       //populated by svn and should be
                                       //of the form "$Rev: 83 $" else it could
                                       //cause a segmentation fault
    char *shortVersion = strchr(fullVersion,' ')+1; //Eliminate te word "$Rev: " 
    char *secondSpaceInVersion = strchr(shortVersion,' '); //Find the space after the number
    *secondSpaceInVersion ='\0';//Eliminate anything after the number, by zeroing the space
    printf("\t \n\performdeformationusingvectorfield version: 1.0.%s \n\ ",shortVersion);
  printf("USAGE: performdeformationusingvectorfield <VectorField> <input img> <output img>\n\
\t -v <int>             : xy_size in vector field, default : 256  \n\
\t -s <int>             : xy_size for input image image size, default : 256 \n\
\t -I                   : no interpolation on intensities\n\
");
  exit(1);
}
