/**
 * @file  xzdgshenCLASSIC.cxx
 * @brief Image Processing Toolbox initally copied from CLASSIC.
 *
 * Copyright (c) 2008, 2009, 2012 University of Pennsylvania.
 *
 * This file is part of DTI-DROID.
 *
 * DTI-DROID is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * DTI-DROID is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with DTI-DROID.  If not, see <http://www.gnu.org/licenses/>.
 *
 * Contact: SBIA Group <sbia-software at uphs.upenn.edu>
 */

#include "matrixSHENCLASSIC.h"  //defines the basis matrix operations

#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <string.h>
#include <float.h>
#include <time.h>

#include <sys/stat.h>

int fileExists(char *filename)
{
  struct stat buf;
  int i = stat(filename, &buf);
  if (i==0)
    return 1;
  else
    return 0;
}

/***
image3d
 ***/

float * new_image3d(int x, int y, int z)
{
  float * mem;
  mem = (float *) malloc(x * y * z * sizeof(float));
  if (mem==NULL)
    {
      printf("error\n");
      exit(0);
    }
  return(mem);
}

void free_image3d(float *img)
{
  free(img);
}

void read_image3d(float *img, int x, int y, int z, char *filename, char *format)
{
  FILE *fp;
  int size;
  byte *bimg;
  int xyz, i;

  fp = fopen(filename, "rb");
  if (fp==NULL)
    {
      printf("file can not be opened: %s\n", filename);
      exit(0);
    }
  
  xyz = x*y*z;
  if (!strcmp(format, "byte"))
    {
      size = sizeof(byte);
      bimg = (byte *) malloc(xyz* sizeof(byte));
      if (bimg==NULL)
	{
	  printf("error\n");
	  exit(0);
	}
      fread(bimg, size, xyz, fp);
      
      for(i=0;i<xyz;i++)
	img[i] = bimg[i];       
    }
  else if (!strcmp(format, "float"))
    {
      size = sizeof(float);
      fread(img, size, xyz, fp);      
    }
  
  fclose(fp);
}

void write_image3d(float *img, int x, int y, int z, char *filename, char *format)
{
  FILE *fp;
  int size;
  byte *bimg;
  int i, xyz;
  
  fp = fopen(filename, "wb");
  if (fp==NULL)
    {
      printf("file can not be opened: %s\n", filename);
      exit(0);
    }
  
  if (!strcmp(format, "byte"))
    {
      size = sizeof(byte);
    }
  else if (!strcmp(format, "float"))
    {
      size = sizeof(float);
    }
  
  xyz = x * y * z;
  
  if (size==sizeof(byte))
    {
      bimg = (byte *) malloc(xyz* sizeof(byte));
      if (bimg==NULL)
	{
	  printf("error\n");
	  exit(0);
	}
      for(i=0;i<xyz;i++)
	bimg[i] = (byte) floorf(img[i]+0.5);
      
      fwrite(bimg, size, xyz, fp);        
    }
  else
    fwrite(img, size, xyz, fp);
  fclose(fp);
}

/*********************************
Handling the memory.
 *********************************/

double uniform_noise()
{
  double value;
  value = (double)rand()/((double)(RAND_MAX)+1);
  return(value);
}

double gaussian_noise(float mean, float sigma)
{
  double value1, value2;
  double u1, u2, S;
  double value;

  do{
    value1 = uniform_noise();
    value2 = uniform_noise();
    u1 = 2.0 * value1 - 1.0;
    u2 = 2.0 * value2 - 1.0;
    S = u1 * u1 + u2 * u2;    
  }
  while(S>=1.0);

  value = sqrt(-2.0 * log(S) / S) * u1;

  value = mean + sqrtf(sigma) * value;

  return(value);
}

void no_mem_exit(const char *where)
{
   printf("Error : Could not allocate memory: %s",where);
   exit(1);
}

//3D array

//3d float array

float ***Falloc3d(int X,int Y,int Z)
{
  float ***array;
  int i,j,k;

  if ((array=(float ***) calloc(Z,sizeof(float **)))==NULL)
    no_mem_exit("Falloc3d");

  for(k=0;k<Z;k++)
    if ((array[k]=(float **) calloc(Y,sizeof(float *)))==NULL)
      no_mem_exit("Falloc3d");

  for(k=0;k<Z;k++)
    for(i=0;i<Y;i++)
      if ((array[k][i]=(float *) calloc(X,sizeof(float)))==NULL)
	no_mem_exit("Falloc3d");	
  return(array);
}

void Ffree3d(float ***array,int Z,int Y)
{
  int k,i;

  for(k=0;k<Z;k++)
    for(i=0;i<Y;i++)
      free(array[k][i]);

  for(k=0;k<Z;k++)
    free(array[k]);

  free(array);
}

// 3D Unsigned Short Array

unshort ***USalloc3d(int X,int Y,int Z)
{
  unshort ***array;
  int i,j,k;

  if ((array=(unshort ***) calloc(Z,sizeof(unshort **)))==NULL)
    no_mem_exit("USalloc3d");

  for(k=0;k<Z;k++)
    if ((array[k]=(unshort **) calloc(Y,sizeof(unshort *)))==NULL)
      no_mem_exit("USalloc3d");

  for(k=0;k<Z;k++)
    for(i=0;i<Y;i++)
      if ((array[k][i]=(unshort *) calloc(X,sizeof(unshort)))==NULL)
         no_mem_exit("USalloc3d");
	
  return(array);
}

void USfree3d(unshort ***array,int Z,int Y)
{
  int k,i;

  for(k=0;k<Z;k++)
    for(i=0;i<Y;i++)
      free(array[k][i]);

  for(k=0;k<Z;k++)
    free(array[k]);

  free(array);
}

// 3D short array

short ***Salloc3d(int X,int Y,int Z)
{
  short ***array;
  int i,j,k;

  if ((array=(short ***) calloc(Z,sizeof(short **)))==NULL)
    no_mem_exit("Salloc3d");

  for(k=0;k<Z;k++)
    if ((array[k]=(short **) calloc(Y,sizeof(short *)))==NULL)
      no_mem_exit("Salloc3d");

  for(k=0;k<Z;k++)
    for(i=0;i<Y;i++)
      if ((array[k][i]=(short *) calloc(X,sizeof(short)))==NULL)
	no_mem_exit("Salloc3d");
	
  return(array);
}

void Sfree3d(short ***array,int Z,int Y)
{
  int k,i;

  for(k=0;k<Z;k++)
    for(i=0;i<Y;i++)
      free(array[k][i]);

  for(k=0;k<Z;k++)
    free(array[k]);

  free(array);
}

//3D Byte Array

byte ***Balloc3d(int X,int Y,int Z)
{
  byte ***array;
  int i,j,k;

  if ((array=(byte ***) calloc(Z,sizeof(byte **)))==NULL)
    no_mem_exit("Balloc3d");

  for(k=0;k<Z;k++)
    if ((array[k]=(byte **) calloc(Y,sizeof(byte *)))==NULL)
      no_mem_exit("Balloc3d");

  for(k=0;k<Z;k++)
    for(i=0;i<Y;i++)
      if ((array[k][i]=(byte *) calloc(X,sizeof(byte)))==NULL)
	no_mem_exit("Balloc3d");
	
  return(array);
}

void Bfree3d(byte ***array,int Z,int Y)
{
  int k,i;

  for(k=0;k<Z;k++)
    for(i=0;i<Y;i++)
      free(array[k][i]);

  for(k=0;k<Z;k++)
    free(array[k]);

  free(array);
}

// ******* 2D array

// 2D float array

float **Falloc2d(int X,int Y)
{
  float **array;
  int i,j;

  if ((array=(float **) calloc(Y,sizeof(float *)))==NULL)
      no_mem_exit("Falloc2d");

  for(i=0;i<Y;i++)
    if ((array[i]=(float *) calloc(X,sizeof(float )))==NULL)
      no_mem_exit("Falloc2d");

  return(array);
}

void Ffree2d(float **array,int Y)
{
  int i;

  for(i=0;i<Y;i++)
    free(array[i]);

  free(array);
}

// 2D byte array

byte **Balloc2d(int X,int Y)
{
  byte **array;
  int i,j;

  array=(byte **) calloc(Y,sizeof(byte *));

  for(i=0;i<Y;i++)
    array[i]=(byte *) calloc(X,sizeof(byte));

  return(array);
}

void Bfree2d(byte **array,int Y)
{
  int i;

  for(i=0;i<Y;i++)
    free(&array[i][0]);

  free(array);
}

//***************************************************************************
//*
//*  Volume Image Functions
//*
//***************************************************************************

void display_dbox(DBox *image)
{
   int x,y,z;
   for (z=0;z<image->Z;z++)
     {
       for (y=0;y<image->Y;y++)
	 {
	   for (x=0;x<image->X;x++)
	     printf("%f ", image->box[z][y][x]);
	   printf("\n");
	 }
       printf("\n");
     }
}

void threshold_dbox(DBox *image, float threshold)
{
  int x,y,z;
  for (z=0;z<image->Z;z++)
    for (y=0;y<image->Y;y++)
      for (x=0;x<image->X;x++)
	{
	  if (image->box[z][y][x]<threshold)
	    image->box[z][y][x] = 0;
	  else
	    image->box[z][y][x] = 255;
	}
}

DBox * new_dbox(int X, int Y, int Z)
{
  DBox *temp;
  int x,y,z;
  temp = (DBox *) calloc (1,sizeof(DBox));

  temp->X = X;
  temp->Y = Y;
  temp->Z = Z;
  temp->box = Falloc3d(X, Y, Z);

  for(z=0;z<Z;z++)
    for (y=0;y<Y;y++)
      for (x=0;x<X;x++)
	temp->box[z][y][x] = 0;

  return temp;
}

void half_intensity_bbox(BBox *image)
{
  int x,y,z;
  for (z=0;z<image->Z;z++)
    for (y=0;y<image->Y;y++)
      for (x=0;x<image->X;x++)
	image->box[z][y][x] = (int)floorf(image->box[z][y][x]/2.0+.5);
}

void free_dbox(DBox *tmp)
{
  Ffree3d(tmp->box, tmp->Z, tmp->Y);
  free(tmp);
}

void init_dbox(DBox * dbox, int X, int Y, int Z)
{
  dbox->X = X;
  dbox->Y = Y;
  dbox->Z = Z;
  dbox->box = Falloc3d(X, Y, Z);
}

// sub-sampling of dbox, sigma is the variance of the Gaussian function.

DBox * subsampling_dbox(DBox *oimage, int power, float sigma)
{
  DBox * dimage;
  int x,y,z, i,j,k;
  float temp;
  int Gaulen;
  float *Gau;
  int p, scale = 1;

  if ((power!=0)&&(power!=1)&&(power!=2))
    {
      printf("Error in subsampling_dbox(): power should be 1 or 2\n");
      exit(0);
    }

  for (p=0;p<power;p++)
    scale *= 2;

  x = oimage->X/scale;
  y = oimage->Y/scale;
  z = oimage->Z/scale;

  dimage = new_dbox(x,y,z);

  if (sigma == 0.0)
    {
      for (z=0;z<dimage->Z;z++)
	for (y=0;y<dimage->Y;y++)
	  for (x=0;x<dimage->X;x++)
	    dimage->box[z][y][x] = oimage->box[z*scale][y*scale][x*scale];
    }
  else
    {
      Gaulen = (int)(sigma * 6+1);
	if (Gaulen%2!=0)
		Gaulen ++; //xuezhong
      Gau = (float *) calloc (Gaulen, sizeof (float));
      temp = 0;
      for (i=0;i<Gaulen;i++)
	{
	  Gau[i]=1/sqrtf(2*M_PI)/sigma*exp(-(i-Gaulen/2.)*(i-Gaulen/2.)/2./sigma/sigma);
	  temp += Gau[i];
	}
      
      Gau[Gaulen/2] += (1-temp)/2.;
      Gau[Gaulen/2-1] += (1-temp)/2.;
      
      for (z=1;z<dimage->Z*scale;z+=scale)
	{
	  //	  printf("z = %d\n", z);
	  for (y=1;y<dimage->Y*scale;y+=scale)
	    for (x=1;x<dimage->X*scale;x+=scale)
	      {
		temp = 0;
		for (k=0;k<Gaulen;k++)
		  for (j=0;j<Gaulen;j++)
		    for (i=0;i<Gaulen;i++)
		      {
			if ((z+k-Gaulen/2>=0)&&(z+k-Gaulen/2<oimage->Z)&&(y+j-Gaulen/2>=0)&&(y+j-Gaulen/2<oimage->Y)&&(x+i-Gaulen/2>=0)&&(x+i-Gaulen/2<oimage->X))
			  temp += Gau[k]*Gau[j]*Gau[i]*oimage->box[z+k-Gaulen/2][y+j-Gaulen/2][x+i-Gaulen/2];
		      }
		dimage->box[z/scale][y/scale][x/scale] = temp;
	      }
	}
    }
  return(dimage);
}

//filtering a dbox using Gaussian filter, sigma is the variance of the Gaussian function

DBox * generate_3d_gaussian_mask(float sigma, float xres, float yres, float zres)
{
  int Gaulenx, Gauleny, Gaulenz;
  int HGaulenx, HGauleny, HGaulenz;
  float *Gaux, *Gauy, *Gauz;
  float temp;
  int i,j,k;
  DBox *mask;

  Gaulenx = (int)floorf(sigma * 6.0 / xres +1+0.5);
  Gauleny = (int)floorf(sigma * 6.0 / yres +1+0.5);
  Gaulenz = (int)floorf(sigma * 6.0 / zres +1+0.5);
  
  if (Gaulenx%2==0)
    Gaulenx ++;
  if (Gauleny%2==0)
    Gauleny ++;
  if (Gaulenz%2==0)
    Gaulenz ++;
  
  HGaulenx = Gaulenx/2;
  HGauleny = Gauleny/2;
  HGaulenz = Gaulenz/2;

  mask = new_dbox(Gaulenx, Gauleny, Gaulenz);

  Gaux = (float *) calloc (Gaulenx, sizeof (float));
  for (i=0;i<Gaulenx;i++)
    Gaux[i]=1/sqrtf(2*M_PI)/sigma*exp(-(i-HGaulenx)*(i-HGaulenx)*xres*xres/2./sigma/sigma);	 
  
  Gauy = (float *) calloc (Gauleny, sizeof (float));
  for (i=0;i<Gaulenx;i++)
    Gauy[i]=1/sqrtf(2*M_PI)/sigma*exp(-(i-HGauleny)*(i-HGauleny)*yres*yres/2./sigma/sigma);	 

  Gauz = (float *) calloc (Gaulenz, sizeof (float));
  for (i=0;i<Gaulenz;i++)
    Gauz[i]=1/sqrtf(2*M_PI)/sigma*exp(-(i-HGaulenz)*(i-HGaulenz)*zres*zres/2./sigma/sigma);	 

  temp = 0;

  for (k=0;k<Gaulenz;k++)
    for (j=0;j<Gauleny;j++)
      for (i=0;i<Gaulenx;i++)
	{
	  mask->box[k][j][i] = Gauz[k]*Gauy[j]*Gaux[i];
	  temp += mask->box[k][j][i];
	}

  mask->box[HGaulenz][HGauleny][HGaulenx] = mask->box[HGaulenz][HGauleny][HGaulenx] + 1 - temp;

  return(mask);
}

void generate_3d_gaussian_image(DBox *mask, int px, int py, int pz, float sigma, float xres, float yres, float zres)
{
  int Gaulenx, Gauleny, Gaulenz;
  int HGaulenx, HGauleny, HGaulenz;
  float *Gaux, *Gauy, *Gauz;
  float temp;
  int i,j,k;

  Gaulenx = mask->X;
  Gauleny = mask->Y;
  Gaulenz = mask->Z;
  
  Gaux = (float *) calloc (Gaulenx, sizeof (float));
  for (i=0;i<Gaulenx;i++)
    //    Gaux[i]=1/sqrtf(2*M_PI)/sigma*exp(-(i-px)*(i-px)*xres*xres/2./sigma/sigma);	 
    Gaux[i]=exp(-(i-px)*(i-px)*xres*xres/2./sigma/sigma);	 
  
  Gauy = (float *) calloc (Gauleny, sizeof (float));
  for (i=0;i<Gaulenx;i++)
    //    Gauy[i]=1/sqrtf(2*M_PI)/sigma*exp(-(i-py)*(i-py)*yres*yres/2./sigma/sigma);	 
    Gauy[i]=exp(-(i-py)*(i-py)*yres*yres/2./sigma/sigma);	 

  Gauz = (float *) calloc (Gaulenz, sizeof (float));
  for (i=0;i<Gaulenz;i++)
    //    Gauz[i]=1/sqrtf(2*M_PI)/sigma*exp(-(i-pz)*(i-pz)*zres*zres/2./sigma/sigma);	 
    Gauz[i]=exp(-(i-pz)*(i-pz)*zres*zres/2./sigma/sigma);	 

  temp = 0;

  for (k=0;k<Gaulenz;k++)
    for (j=0;j<Gauleny;j++)
      for (i=0;i<Gaulenx;i++)
	{
	  mask->box[k][j][i] = Gauz[k]*Gauy[j]*Gaux[i];
	  temp += mask->box[k][j][i];
	}

}


DBox * filtering_dbox_with_mask(DBox *oimg, DBox *mask)
{
  int x,y,z;
  int i,j,k;
  int nx,ny,nz;
  int Hx,Hy,Hz;

  float sum;

  Hx=mask->X/2;
  Hy=mask->Y/2;
  Hz=mask->Z/2;

  DBox *orimg, *nimg;

  orimg = new_dbox(oimg->X+mask->X/2*2, oimg->Y+mask->Y/2*2, oimg->Z+mask->Z/2*2);
  nimg = new_dbox(oimg->X, oimg->Y, oimg->Z);

  clear_dbox(orimg);

  for (z=0;z<oimg->Z;z++)
    for (y=0;y<oimg->Y;y++)
      for (x=0;x<oimg->X;x++)
	{
	  orimg ->box[z+Hz][y+Hy][x+Hx] = oimg->box[z][y][x];
	}

  for (z=0;z<oimg->Z;z++)
    {
      printf("z =%d\n", z);
    for (y=0;y<oimg->Y;y++)
      for (x=0;x<oimg->X;x++)
	{
	  sum = 0.0;

	  for (k=0;k<mask->Z;k++)
	    for (j=0;j<mask->Y;j++)
	      for (i=0;i<mask->X;i++)
		{
		      nx = x + i;
		      ny = y+ j;
		      nz = z + k;
		  sum += orimg->box[nz][ny][nx];
		}

	  if (sum>0.0)
	    {
	      sum = 0.0;
	      for (k=0;k<mask->Z;k++)
		for (j=0;j<mask->Y;j++)
		  for (i=0;i<mask->X;i++)
		    {
		      nx = x + i;
		      ny = y+ j;
		      nz = z + k;
		      sum += orimg->box[nz][ny][nx] * mask->box[k][j][i];
		    }
	      nimg->box[z][y][x] = sum;		
	    }
	  else
	    nimg->box[z][y][x] = 0.0;
	}
    }

  free_dbox(orimg);
  return(nimg);
}

float interpolate_dbox(DBox *db, float xx, float yy, float zz)
{
  float CurrentV ;
  int x,y,z, xu,yu,zu;
  float coeflx, coefly, coeflz, coefhx, coefhy, coefhz;

  x = (int) floorf(xx) ;
  y = (int) floorf(yy) ;
  z = (int) floorf(zz) ;
  xu = x+1;
  yu = y+1;
  zu = z+1;

  if (xu<=0 || yu<=0 || zu<=0 || x>=db->X-1 || y>=db->Y-1 || z>=db->Z-1)
    return 0;

  if (x>=0 && y>=0 && z>=0 && xu<db->X && yu<db->Y && zu<db->Z)
    {
      coefhx = xx-x;
      coefhy = yy-y;
      coefhz = zz-z;
      coeflx = 1-coefhx;
      coefly = 1-coefhy;
      coeflz = 1-coefhz;

      CurrentV = coeflz*(coefly*(coefhx * db->box[z][y][xu]   + coeflx * db->box[z][y][x])+
			 coefhy*(coefhx * db->box[z][yu][xu]  + coeflx * db->box[z][yu][x]))+
	coefhz*(coefly*(coefhx * db->box[zu][y][xu]  + coeflx * db->box[zu][y][x])+
		coefhy*(coefhx * db->box[zu][yu][xu] + coeflx * db->box[zu][yu][x]));
      return CurrentV;
      }
  else
    {
      return 100000;
    }

}

void clear_dbox(DBox *dbox)
{
  int x,y,z;
  for (z = 0; z < dbox->Z; z++)
    for (y=0;y<dbox->Y;y++)
      memset(&(dbox->box[z][y][0]), 0, dbox->X*sizeof(float));
}

void clear_bbox(BBox *dbox)
{
  int x,y,z;
  for (z = 0; z < dbox->Z; z++)
    for (y=0;y<dbox->Y;y++)
      memset(&(dbox->box[z][y][0]), 0, dbox->X*sizeof(byte));
}

void read_dbox(DBox *dbox, const char *filename, const char *flag)
{
  FILE *fp;

  int y,x,z;
  unshort ustemp;
  byte btemp;

  if ((fp = fopen(filename, "rb"))==NULL)
    {
      printf("file open failed: %s\n", filename);
      exit(0);
    };

  if (!strcmp(flag, "float"))

    for (z=0;z<dbox->Z;z++)
      for (y=0;y<dbox->Y;y++)
	fread(&(dbox->box[z][y][0]), sizeof(float), dbox->X, fp);  

  else if (!strcmp(flag, "unshort"))
    {
      for (z=0;z<dbox->Z;z++)
	for(y=0;y<dbox->Y;y++)
	  for(x=0;x<dbox->X;x++)
	    {
	      fread(&ustemp, sizeof(unshort), 1, fp);
	      dbox->box[z][y][x] = (float) ustemp;
	    }
    }

  else if (!strcmp(flag, "byte"))
    {
      for (z=0;z<dbox->Z;z++)
	for(y=0;y<dbox->Y;y++)
	  for(x=0;x<dbox->X;x++)
	    {
	      fread(&btemp,sizeof(byte), 1,  fp);
	      dbox->box[z][y][x] = (float) btemp;
	    }
    }
  else
    {
      printf("format error in read_dbox()\n");
      fclose(fp);
      exit(0);
    }

  fclose(fp);
}

/*

float: flag is no use.

unchar: flag==0 means dbox_data = abs(input)
        flag==1 means ???

byte: flag==0 means intensity normalization (0~255);
      flag==1 means (x>255)?255:x; (x<0)?0:x
*/

void write_dbox(DBox *dbox, const char *filename, const char *format, int flag)
{
  FILE *fp;
  int y,z,x;
  float max, min, ftemp;
  byte tmp;
  short shorttmp;
  unshort temp;

  max = 0; min = FLT_MAX;

  if ((fp = fopen(filename, "wb"))==NULL)
    {
      printf("file open failed: %s\n", filename);
      exit(0);
    };

  if (!strcmp(format, "float"))
    {
      for (z=0;z<dbox->Z;z++)
	for (y=0;y<dbox->Y;y++)
	  for (x=0;x<dbox->X;x++)
	    {
	      ftemp = dbox->box[z][y][x];
	      fwrite(&ftemp, sizeof(float), 1, fp);  
	    }
    }

  else if(!strcmp(format, "unshort"))
    {
      for (z=0;z<dbox->Z;z++)
	for(y=0;y<dbox->Y;y++)
	  for(x=0;x<dbox->X;x++)
	    {
	      switch(flag)
		{
		case 0:
		  if (dbox->box[z][y][x]<0)
		      temp = 0;
		  else
		    temp = (unshort)(dbox->box[z][y][x]+.5);
		  break;
		case 1:
		  temp = (unshort) (fabsf(dbox->box[z][y][x])+.5);
		  break;
		default:
		  printf("no such choice\n");
		  exit(0);
		}
	      fwrite(&temp,sizeof(unshort), 1,  fp);
	    }
    }
  else if (!strcmp(format, "byte"))
    {
      if (flag==0)
	{
	  for (z=0;z<dbox->Z;z++)
	    for(y=0;y<dbox->Y;y++)
	      for(x=0;x<dbox->X;x++)
		{
		  if (max<dbox->box[z][y][x])
		    max = dbox->box[z][y][x];
		  if (min>dbox->box[z][y][x])
		    min = dbox->box[z][y][x];
		}
	  printf("writing byte: max = %f, min = %f\n", max, min);
	  if (max==min)
	    {
	      printf("max==min!\n");
	      exit(0);
	    }
	  for (z=0;z<dbox->Z;z++)
	    for(y=0;y<dbox->Y;y++)
	      for(x=0;x<dbox->X;x++)
		{
		  tmp = (byte) ((dbox->box[z][y][x]-min)/(max-min)*255);
		  fwrite(&tmp, sizeof(byte), 1, fp);	      
		}
	}
      if (flag==1)
	{
	  for (z=0;z<dbox->Z;z++)
	    for(y=0;y<dbox->Y;y++)
	      for(x=0;x<dbox->X;x++)
		{
		  if (dbox->box[z][y][x]>255.0)
		    tmp = 255;
		  else if (dbox->box[z][y][x]<0.0)
		    tmp = 0;
		  else
		    tmp = (byte) floorf(dbox->box[z][y][x]+0.5);
		  fwrite(&tmp, sizeof(byte), 1, fp);	      
		}
	}	
    }
  else if (!strcmp(format, "short"))
    {
      for (z=0;z<dbox->Z;z++)
	for(y=0;y<dbox->Y;y++)
	  for(x=0;x<dbox->X;x++)
	    {
	      shorttmp = (short) (dbox->box[z][y][x]);
	      fwrite(&shorttmp, sizeof(short), 1, fp);	      
	    }      
    }
  
  //  printf("saved size: %d %d %d\n", dbox->X, dbox->Y, dbox->Z);
  fclose(fp);
}

//3x3x3 mask smooth, average

void smooth_dbox(DBox *dbox)
{
  int x,y,z;
  int m,n,l;
  DBox *temp;
  
  temp = new_dbox(dbox->X, dbox->Y, dbox->Z);
  
  for (z=1;z<dbox->Z-1;z++)
    for (y=1;y<dbox->Y-1;y++)
      for (x=1;x<dbox->X-1;x++)
	{
	  //	  if (dbox->box[z][y][x]!=0)
	    {
	      for (m=-1;m<2;m++)
		for (n=-1;n<2;n++)
		  for (l=-1;l<2;l++)
		    temp->box[z][y][x] += dbox->box[z+m][y+n][x+l];
	      temp->box[z][y][x] += dbox->box[z][y][x];
	      temp->box[z][y][x] = temp->box[z][y][x]/28.0;
	    }
	}

  for (z=0;z<dbox->Z;z++)
    for (y=0;y<dbox->Y;y++)
      for (x=0;x<dbox->X;x++)
	dbox->box[z][y][x] = temp->box[z][y][x];

  free_dbox(temp);
}

/*********************************************
Wavelet Transformation
*/

void iwt_3d_base_same(DBox * image, int x_l, int x_u, int y_l, int y_u, int z_l, int z_u)
{
  int i,j,k,n;
  int x,y,z;
  int dx, dy, dz;
  int hdx, hdy, hdz;
  float temp1,temp2;
  float  *bufferx, *buffery, *bufferz;
  int szx, szy, szz;
  float factor = 0.70710678118655;

  dx = x_u - x_l;
  dy = y_u - y_l;
  dz = z_u - z_l;

  bufferx = (float *) calloc (dx, sizeof(float));
  buffery = (float *) calloc (dy, sizeof(float));
  bufferz = (float *) calloc (dz, sizeof(float));

  hdx = dx / 2;
  hdy = dy / 2;
  hdz = dz / 2;

  for (x=x_l;x<x_u;x++)
    for(y=y_l;y<y_u;y++)
      {
	for (z=0;z<hdz;z++)
	  {
	    bufferz[2*z] = (image->box[z+z_l][y][x] - image->box[z+z_l+hdz][y][x])*factor;
	    bufferz[2*z+1] = (image->box[z+z_l][y][x] + image->box[z+z_l+hdz][y][x])*factor;
	  }
	for (z=z_l;z<z_u;z++)
	  image->box[z][y][x] = bufferz[z-z_l];
	}

  for (z=z_l;z<z_u;z++)
    for(x=x_l;x<x_u;x++)
      {
	for (y=0;y<hdy;y++)
	  {
	    buffery[2*y] = (image->box[z][y+y_l][x] - image->box[z][y+y_l+hdy][x])*factor;
	    buffery[2*y+1] = (image->box[z][y+y_l][x] + image->box[z][y+y_l+hdy][x])*factor;
	  }
	for (y=y_l;y<y_u;y++)
	  image->box[z][y][x] = buffery[y-y_l];
	}

  for (z=z_l;z<z_u;z++)
    for(y=y_l;y<y_u;y++)
      {
	for (x=0;x<hdx;x++)
	  {
	    bufferx[2*x] = (image->box[z][y][x+x_l] - image->box[z][y][x+x_l+hdx])*factor;
	    bufferx[2*x+1] = (image->box[z][y][x+x_l] + image->box[z][y][x+x_l+hdx])*factor;
	  }
	for (x=x_l;x<x_u;x++)
	  image->box[z][y][x] = bufferx[x-x_l];
	}

  free(bufferx);
  free(buffery);
  free(bufferz);
}


void wt_3d_base_same(DBox * image, int x_l, int x_u, int y_l, int y_u, int z_l, int z_u)
{
  int x,y,z;
  int hdx, hdy, hdz;
  float *bufferx, * buffery, * bufferz;
  int dx, dy, dz;

  //  DBox * wt;
  float K=0.35355339059328;

  dx = x_u - x_l;
  dy = y_u - y_l;
  dz = z_u - z_l;

  hdx = dx/2; 
  hdy = dy/2; 
  hdz = dz/2;

  //  wt = new_dbox(dx,dy,dz);

  if((bufferx=(float *)calloc(dx, sizeof(float)))==(float *)NULL)
    exit(1);  
  if((buffery=(float *)calloc(dy, sizeof(float)))==(float *)NULL)
    exit(1);  
  if((bufferz=(float *)calloc(dz, sizeof(float)))==(float *)NULL)
    exit(1);  
  
  for (z=z_l;z<dz+z_l;z++)
    for(y=y_l;y<dy+y_l;y++)
      {
	for (x=0;x<hdx;x++)
	  {
	    *(bufferx+x)= image->box[z][y][x*2+1+x_l] + image->box[z][y][x*2+x_l];
	    *(bufferx+x+hdx)= image->box[z][y][x*2+1+x_l] - image->box[z][y][x*2+x_l];
	  }
	for (x=0;x<dx;x++)
	  image->box[z][y][x+x_l] = *(bufferx+x);
      }
  
  for (z=z_l; z<dz+z_l; z++)
    for(x=x_l; x<dx+x_l; x++)
      {
	for (y=0;y<hdy;y++)
	  {
	    *(buffery+y)= image->box[z][1+y*2+y_l][x] + image->box[z][y*2+y_l][x];
	    *(buffery+y+hdy)= image->box[z][1+y*2+y_l][x] - image->box[z][y*2+y_l][x];
	  }
	for (y=0;y<dy;y++)
	  image->box[z][y+y_l][x] = *(buffery+y);
      }

 for(x=x_l; x<dx+x_l; x++)
    for(y=y_l; y<dy+y_l; y++)
      {
	for (z=0;z<hdz;z++)
	  {
	    *(bufferz+z)= image->box[z*2+1+z_l][y][x] + image->box[z*2+z_l][y][x];
	    *(bufferz+z+hdz)= image->box[z*2+1+z_l][y][x] - image->box[z*2+z_l][y][x];
	  }
	for (z=0;z<dz;z++)
	  image->box[z+z_l][y][x] = (*(bufferz+z))*K;
      }

  free(bufferx);
  free(buffery);
  free(bufferz);
}


/****************************************************************
Handling Signal

A Signal is defined as 1D float array
*****************************************************************/

Signal * new_signal(int Size)
{
  Signal *temp;
  temp = (Signal *) calloc (1, sizeof(Signal));
  temp->sig = (float *) calloc (Size, sizeof(float));
  temp->Size = Size;
  return(temp);
}

void copy_signal(Signal *a, Signal *b)
{
  int i;
  for (i=0;i<a->Size;i++)
    b->sig[i] = a->sig[i];
}

void free_signal(Signal *signal)
{
  free(signal->sig);
  free(signal);
}

void display_signal(Signal *sig)
{
  int i;
  printf("signal size is %d\n", sig->Size);
  for (i=0;i<sig->Size;i++)
    printf("%f\n", sig->sig[i]);  
}

void clear_signal(Signal *signal)
{
  int i;
  for (i=0;i<signal->Size;i++)
    signal->sig[i] = 0.0;
}

/*********************************************

Deformation_Field

**********************************************/

Deformation_Field *new_deformationfield(int X,int Y,int Z)
{
  Deformation_Field *df;

  int z,y,x;

  df = (Deformation_Field *) calloc (1, sizeof(Deformation_Field));

  df->point = (FPoint ***) calloc (Z, sizeof(FPoint **));
  if (df->point==NULL)
    no_mem_exit("new_deformationfield");    

  for (z=0;z<Z;z++)
    if ((df->point[z] = (FPoint **) calloc (Y, sizeof(FPoint *)))==NULL)
      no_mem_exit("new_deformationfield");    

  for (z = 0;z<Z;z++)
    for (y=0;y<Y;y++)
      if ((df->point[z][y] = (FPoint *) calloc (X, sizeof(FPoint)))==NULL)
	no_mem_exit("new_deformationfield");    

  df->X = X;
  df->Y = Y;
  df->Z = Z;

  for (z=0;z<df->Z;z++)
    for (y=0;y<df->Y;y++)
      for (x=0;x<df->X;x++)
	{
	  df->point[z][y][x].X = 0.0;
	  df->point[z][y][x].Y = 0.0;
	  df->point[z][y][x].Z = 0.0;
	}

  return(df);
}

void free_deformationfield(Deformation_Field *df)
{
  int z,y;

  for(z=0;z<df->Z;z++)
    for(y =0;y<df->Y;y++)
      free(df->point[z][y]);

  for(z=0;z<df->Z;z++)
    free(df->point[z]);

  free(df->point);

  free(df);
}

/**************************************************

Points, FPoints

 **************************************************/

Points * read_points(char *filename)
{
  Points *pts;
  FILE *fp;
  int N, i;
  pts = (Points *) calloc (1, sizeof(Points));
  fp = fopen(filename, "r");
  if (fp==NULL)
    {
      printf("file open failed, read_points(%s)\n", filename);
      exit(1);
    }
  fscanf(fp, "%d\n", &N);
  pts->Num = N;
  pts->point = (Point *) calloc (N, sizeof(Point));
  for (i=0;i<N;i++)
    fscanf(fp, "%d,%d,%d\n", &(pts->point[i].X), &(pts->point[i].Y), &(pts->point[i].Z)); 
  fclose (fp);
  return(pts);
}

Image * new_image(int X, int Y)
{
  Image * temp;
  temp = (Image *) calloc (1, sizeof(Image));
  temp->X = X;
  temp->Y = Y;
  temp->img = Falloc2d(X,Y);
  return temp;
}

void free_image(Image *tmp)
{
  Ffree2d(tmp->img, tmp->Y);
  free(tmp);
}


float distance_3d_f(FPoint p1, FPoint p2)
{
  return(sqrtf((p1.X-p2.X)*(p1.X-p2.X)+(p1.Y-p2.Y)*(p1.Y-p2.Y)+(p1.Z-p2.Z)*(p1.Z-p2.Z)));
}


FPoints * read_fpoints(char *filename)
{
  FPoints *pts;
  FILE *fp;
  int N, i;
  pts = (FPoints *) calloc (1, sizeof(FPoints));
  fp = fopen(filename, "r");
  if (fp==NULL)
    {
      printf("file open failed, read_points(%s)\n", filename);
      exit(1);
    }
  fscanf(fp, "%d\n", &N);
  pts->Num = N;
  pts->point = (FPoint *) calloc (N, sizeof(FPoint));
  for (i=0;i<N;i++)
    fscanf(fp, "%f,%f,%f\n", &(pts->point[i].X), &(pts->point[i].Y), &(pts->point[i].Z)); 
  fclose (fp);
  return(pts);
}
void write_points(Points *pts, char *filename)
{
  FILE *fp;
  int i;
  fp = fopen(filename, "w");
  if (fp==NULL)
    {
      printf("file open failed, write_points\n");
      exit(1);
    }
  fprintf(fp, "%d\n", pts->Num);
  for (i=0;i<pts->Num;i++)
    {
      fprintf(fp, "%d,%d,%d\n", (pts->point[i].X), (pts->point[i].Y), (pts->point[i].Z)); 
    }
  fclose (fp);
}

void write_fpoints(FPoints *pts, char *filename)
{
  FILE *fp;
  int i;
  fp = fopen(filename, "w");
  if (fp==NULL)
    {
      printf("file open failed, write_points\n");
      exit(1);
    }
  fprintf(fp, "%d\n", pts->Num);
  for (i=0;i<pts->Num;i++)
    {
      fprintf(fp, "%f,%f,%f\n", (pts->point[i].X), (pts->point[i].Y), (pts->point[i].Z)); 
    }
  fclose (fp);
}

void display_points(Points *pts)
{
  int i;
  printf("%d\n", pts->Num);
  for (i=0;i<pts->Num;i++)
    printf("%d,%d,%d\n", (pts->point[i].X), (pts->point[i].Y), (pts->point[i].Z)); 
}

void display_fpoints(FPoints *pts)
{
  int i;
  printf("%d\n", pts->Num);
  for (i=0;i<pts->Num;i++)
    printf("%f,%f,%f\n", (pts->point[i].X), (pts->point[i].Y), (pts->point[i].Z)); 
}

Points * new_points(int N)
{
  Points *npts;
  npts = (Points *) calloc (1, sizeof(Points));
  npts->Num = N;
  npts->point = (Point *) calloc (N, sizeof(Point));
  return(npts);
}

void free_points(Points *pts)
{
  free(pts->point);
  free(pts);
}

void copy_points(Points *pts1, Points *pts2)
{
  int i;
  for (i=0;i<pts1->Num;i++)
    {
      pts2->point[i].X = pts1->point[i].X;
      pts2->point[i].Y = pts1->point[i].Y;
      pts2->point[i].Z = pts1->point[i].Z;
    }
}

void copy_fpoints(FPoints *pts1, FPoints *pts2)
{
  int i;
  for (i=0;i<pts1->Num;i++)
    {
      pts2->point[i].X = pts1->point[i].X;
      pts2->point[i].Y = pts1->point[i].Y;
      pts2->point[i].Z = pts1->point[i].Z;
    }  
}

FPoints * new_fpoints(int N)
{
  FPoints *npts;
  int i;

  npts = (FPoints *) calloc (1, sizeof(FPoints));
  npts->Num = N;
  npts->point = (FPoint *) calloc (N, sizeof(FPoint));

  for (i=0;i<N;i++)
    {
      npts->point[i].X = 0;
      npts->point[i].Y = 0;
      npts->point[i].Z = 0;
    }

  return(npts);
}

void free_fpoints(FPoints *pts)
{
  free(pts->point);
  free(pts);
}

//xres is set to voxelsize/voxeldistance, if want 4 shell layers, and largest is 10 then res is 4/10;

Points * generate_hood_byIncreasingRadius(int HoodSize, float xres,float yres,float zres) 
{
  int i, j, k, l ;
  int x, y, z, t, r;
  int half ;

  Points *Hood;

  t = 0 ;
  t++;
  half = HoodSize/2;

  //  printf("in generate hood\n");

  for(r=1; r<=half; r++)
    {
      for(z=-r; z<=r; z+=2*r)
        for(x=-r; x<=r; x++)
          for(y=-r; y<=r; y++)
            {
              t++ ;
            }
      for(x=-r; x<=r; x+=2*r)
        for(z=-r+1; z<=r-1; z++)
          for(y=-r; y<=r; y++)
            {
              t++ ;
            }
      for(y=-r; y<=r; y+=2*r)
        for(z=-r+1; z<=r-1; z++)
          for(x=-r+1; x<=r-1; x++)
            {
              t++ ;
            }
    }

//  printf("t = %d\n", t);
  Hood = new_points(t);
//  printf("new t ok\n");

  t = 0;
  Hood->point[0].X = 0 ;  Hood->point[0].Y = 0 ;  Hood->point[0].Z = 0 ;
  t++ ;

  for(r=1; r<=half; r++)
    {
      for(z=-r; z<=r; z+=2*r)
        for(x=-r; x<=r; x++)
          for(y=-r; y<=r; y++)
            {
              Hood->point[t].X = (int) floorf(x/xres + .5) ;
              Hood->point[t].Y = (int) floorf(y/yres + .5) ;
              Hood->point[t].Z = (int) floorf(z/zres + .5) ;
              
              t++ ;
              /*printf("t=%d: (%d,%d,%d)\n", t,x,y,z) ;*/
            }
      for(x=-r; x<=r; x+=2*r)
        for(z=-r+1; z<=r-1; z++)
          for(y=-r; y<=r; y++)
            {
              Hood->point[t].X = (int) floorf(x/xres + .5) ;
              Hood->point[t].Y = (int) floorf(y/yres + .5) ;
              Hood->point[t].Z = (int) floorf(z/zres + .5) ;
              
              t++ ;
              /*printf("t=%d: (%d,%d,%d)\n", t,x,y,z) ;*/
            }
      for(y=-r; y<=r; y+=2*r)
        for(z=-r+1; z<=r-1; z++)
          for(x=-r+1; x<=r-1; x++)
            {
              Hood->point[t].X = (int) floorf(x/xres + .5) ;
              Hood->point[t].Y = (int) floorf(y/yres + .5) ;
              Hood->point[t].Z = (int) floorf(z/zres + .5) ;
              
              t++ ;
              /*printf("t=%d: (%d,%d,%d)\n", t,x,y,z) ;*/
            }
    }

//    printf("total %d in the search area!\n", t) ; 
  
  return(Hood);
}


/*********************************************

Gaussian and Others

 *********************************************/

float Gaussian_1d(float x, float sigma)
{
  return(exp(-x*x/2./sigma/sigma));  
}

float Gaussian(int x, int y, int z, float sigma)     
{
  float gx, gy, gz;
  if (sigma==0)
    return(1);
  gx = 1/sqrtf(2*M_PI)/sigma*exp(-x*x/2./sigma/sigma);
  gy = 1/sqrtf(2*M_PI)/sigma*exp(-y*y/2./sigma/sigma);
  gz = 1/sqrtf(2*M_PI)/sigma*exp(-z*z/2./sigma/sigma);
  return(gx*gy*gz);
}

float Gaussian_Similarity(int x, int y, int z, float sigma)     
{
  float gx, gy, gz;
  if (sigma==0)
    return(1);
  gx = exp(-x*x/2./sigma/sigma);
  gy = exp(-y*y/2./sigma/sigma);
  gz = exp(-z*z/2./sigma/sigma);
  return(gx*gy*gz);
}

/********************************************************

Deformation Field

 ********************************************************/

// update deformation field
//
// df               : current deformation field
// originalpoints   : model points
// newpoints        : newly detected subject points
// distance_sigma   : sigma of Gaussian function
// rate             : 0.0~1.0 the step to move.
// lambda           : delta = lambda*(one) + (1-lambda) * (two),
//                    one is determined from new subject points, two is determined from affine alignment(between model and new subject points).

void Smooth_DeformationField(Deformation_Field *df, float LocalRatio)
{
  int i,j,k,n ;
  int x,y,z ;

  Points *hood;

  int        nb_size, count, temp_num ;
  FPoint     nbr_tot ;

  printf("smoothing ...\n") ;

  nb_size = 5 ;
  hood = generate_hood_byIncreasingRadius(nb_size, 1.,1.,1.);

  printf("begin smoothing ...\n");
  for(k=0;k<df->Z;k++)
    for(j=0;j<df->Y;j++)
      for(i=0;i<df->X;i++)
       {
	      nbr_tot.X=0; 
		nbr_tot.Y=0; 
		nbr_tot.Z=0; 
		temp_num=0;

        for(n=1;n<hood->Num;n++) /* 26 neighbor */
          {
            x = i + hood->point[n].X ;
            y = j + hood->point[n].Y ;
            z = k + hood->point[n].Z ;

            if( x<df->X && y<df->Y && z<df->Z && x>=0 && y>=0 && z>=0 )
            {
              nbr_tot.X += df->point[z][y][x].X;
              nbr_tot.Y += df->point[z][y][x].Y;
              nbr_tot.Z += df->point[z][y][x].Z;
              temp_num++ ;
            }
          }

        if(temp_num>0)
          {
            nbr_tot.X /= temp_num ;
            nbr_tot.Y /= temp_num ;
            nbr_tot.Z /= temp_num ;
          }
	else
	  {
	    printf("why? there is something wrong in smooth field\n");
		exit(0);
	  }
        
        df->point[k][j][i].X += (nbr_tot.X - df->point[k][j][i].X)*LocalRatio ;
        df->point[k][j][i].Y += (nbr_tot.Y - df->point[k][j][i].Y)*LocalRatio ; 
        df->point[k][j][i].Z += (nbr_tot.Z - df->point[k][j][i].Z)*LocalRatio ; 

        /* boundary constraints */
        if(df->point[k][j][i].X+i<0) df->point[k][j][i].X=-i ;
        if(df->point[k][j][i].Y+j<0) df->point[k][j][i].Y=-j ;
        if(df->point[k][j][i].Z+k<0) df->point[k][j][i].Z=-k ;

        if(df->point[k][j][i].X+i>=df->X-1) df->point[k][j][i].X=df->X-i ;
        if(df->point[k][j][i].Y+j>=df->Y-1) df->point[k][j][i].Y=df->Y-j ;
        if(df->point[k][j][i].Z+k>=df->Z-1) df->point[k][j][i].Z=df->Z-k ;
      }
  //  printf("End\n") ;
  free_points(hood);
}

void Smooth_DeformationField_Without_BoundaryConstraints(Deformation_Field *df, float LocalRatio)
{
  int i,j,k,n ;
  int x,y,z ;

  Points *hood;

  int        nb_size, count, temp_num ;
  FPoint     nbr_tot ;

  Deformation_Field *dff;

  dff = new_deformationfield(df->X, df->Y, df->Z);

  printf("smoothing ...\n") ;

  nb_size = 3 ;
  hood = generate_hood_byIncreasingRadius(nb_size, 1.,1.,1.);

  printf("begin smoothing ...\n");
  for(k=0;k<df->Z;k++)
    for(j=0;j<df->Y;j++)
      for(i=0;i<df->X;i++)
       {
	 nbr_tot.X=0; 
	 nbr_tot.Y=0; 
	 nbr_tot.Z=0; 
	 temp_num=0;

        for(n=1;n<hood->Num;n++) /* 26 neighbor */
          {
            x = i + hood->point[n].X ;
            y = j + hood->point[n].Y ;
            z = k + hood->point[n].Z ;
	    
            if( x<df->X && y<df->Y && z<df->Z && x>=0 && y>=0 && z>=0 )
	      {
		nbr_tot.X += df->point[z][y][x].X;
		nbr_tot.Y += df->point[z][y][x].Y;
		nbr_tot.Z += df->point[z][y][x].Z;
		temp_num++ ;
	      }
          }
	
        if(temp_num>0)
          {
            nbr_tot.X /= temp_num ;
            nbr_tot.Y /= temp_num ;
            nbr_tot.Z /= temp_num ;
          }
	else
	  {
	    printf("why? there is something wrong in smooth field\n");
	    exit(0);
	  }
        
        dff->point[k][j][i].X = df->point[k][j][i].X * LocalRatio + nbr_tot.X * (1-LocalRatio);
        dff->point[k][j][i].Y = df->point[k][j][i].Y * LocalRatio + nbr_tot.Y * (1-LocalRatio);
        dff->point[k][j][i].Z = df->point[k][j][i].Z * LocalRatio + nbr_tot.Z * (1-LocalRatio);
      }

  for (z=0;z<df->Z;z++)
    for (y=0;y<df->Y;y++)
      for (x=0;x<df->X;x++)
	{
	  df->point[z][y][x].X = dff->point[z][y][x].X;
	  df->point[z][y][x].Y = dff->point[z][y][x].Y;
	  df->point[z][y][x].Z= dff->point[z][y][x].Z;
	}

  free_deformationfield(dff);
  //  printf("End\n") ;
  free_points(hood);
}

void Smooth_DeformationField_With_Edge_Preserving(Deformation_Field *df, float LocalRatio, FAVBox *mimg, FAVBox *simg)
{
  int i,j,k,n ;
  int x,y,z ;
  int nx,ny,nz;

  float max_weight, min_weight;

  Points *hood;

  DBox *weight_box;

  int        nb_size, count, temp_num ;
  FPoint     nbr_tot ;

  printf("smoothing with edge preserving\n") ;

  weight_box = new_dbox(df->X, df->Y, df->Z);

  nb_size = 3 ;
  hood = generate_hood_byIncreasingRadius(nb_size, 1.,1.,1.);

  max_weight = 0;
  min_weight = 65535;

  for (z=0;z<df->Z;z++)
    for (y=0;y<df->Y;y++)
      for (x=0;x<df->X;x++)
	{
	  nx = (int) floorf(x + df->point[z][y][x].X + .5);
	  ny = (int) floorf(y + df->point[z][y][x].Y + .5);
	  nz = (int) floorf(z + df->point[z][y][x].Z + .5);

	  if (nx>=simg->X) nx = simg->X-1;
	  if (ny>=simg->Y) ny = simg->Y-1;
	  if (nz>=simg->Z) nz = simg->Z-1;
	  if (nx<0) nx = 0;
	  if (ny<0) ny = 0;
	  if (nz<0) nz = 0;

	  weight_box->box[z][y][x] = mimg->grd[z][y][x] * simg->grd[nz][ny][nx];

	  if (max_weight<weight_box->box[z][y][x])
	    max_weight = weight_box->box[z][y][x];
	  if (min_weight>weight_box->box[z][y][x])
	    min_weight = weight_box->box[z][y][x];
	}

  for (z=0;z<df->Z;z++)
    for (y=0;y<df->Y;y++)
      for (x=0;x<df->X;x++)
	weight_box->box[z][y][x] = 1 - (weight_box->box[z][y][x]-min_weight)/max_weight;

  printf("begin smoothing ...\n");
  for(k=0;k<df->Z;k++)
    for(j=0;j<df->Y;j++)
      for(i=0;i<df->X;i++)
       {
	 nbr_tot.X=0; 
	 nbr_tot.Y=0; 
	 nbr_tot.Z=0; 
	 temp_num=0;
	 
        for(n=1;n<hood->Num;n++) /* 26 neighbor */
          {
            x = i + hood->point[n].X ;
            y = j + hood->point[n].Y ;
            z = k + hood->point[n].Z ;

            if( x<df->X && y<df->Y && z<df->Z && x>=0 && y>=0 && z>=0 )
            {
              nbr_tot.X += df->point[z][y][x].X;
              nbr_tot.Y += df->point[z][y][x].Y;
              nbr_tot.Z += df->point[z][y][x].Z;
              temp_num++ ;
            }
          }

        if(temp_num>0)
          {
            nbr_tot.X /= temp_num ;
            nbr_tot.Y /= temp_num ;
            nbr_tot.Z /= temp_num ;
          }
	else
	  {
	    printf("why? there is something wrong in smooth field\n");
		exit(0);
	  }
        
        df->point[k][j][i].X += (nbr_tot.X - df->point[k][j][i].X)*LocalRatio*weight_box->box[k][j][i] ;
        df->point[k][j][i].Y += (nbr_tot.Y - df->point[k][j][i].Y)*LocalRatio*weight_box->box[k][j][i] ; 
        df->point[k][j][i].Z += (nbr_tot.Z - df->point[k][j][i].Z)*LocalRatio*weight_box->box[k][j][i] ; 

        /* boundary constraints */
        if(df->point[k][j][i].X+i<0) df->point[k][j][i].X=-i ;
        if(df->point[k][j][i].Y+j<0) df->point[k][j][i].Y=-j ;
        if(df->point[k][j][i].Z+k<0) df->point[k][j][i].Z=-k ;

        if(df->point[k][j][i].X+i>=df->X-1) df->point[k][j][i].X=df->X-i ;
        if(df->point[k][j][i].Y+j>=df->Y-1) df->point[k][j][i].Y=df->Y-j ;
        if(df->point[k][j][i].Z+k>=df->Z-1) df->point[k][j][i].Z=df->Z-k ;
      }
  //  printf("End\n") ;
  free_points(hood);
  free_dbox(weight_box);
}

DBox * volume_transform(DBox *tag, Deformation_Field *df)
{
  int x,y,z;
  float nx,ny,nz;
  DBox *rst;

  FPoint zero;

  rst = new_dbox(df->X, df->Y, df->Z);

  zero.X = 0.0;
  zero.Y = 0.0;
  zero.Z = 0.0;

  for (z=0;z<df->Z;z++)
    for (y=0;y<df->Y;y++)
      for (x=0;x<df->X;x++)
	{
	  if (distance_3d_f(zero, df->point[z][y][x])<100.0)
	    {
	      nx = df->point[z][y][x].X + x;
	      ny = df->point[z][y][x].Y + y;
	      nz = df->point[z][y][x].Z + z;
	      
	      rst->box[z][y][x] = interpolate_dbox(tag, nx, ny, nz);
	    }
	}

  return(rst);
}

void volume_transform_nointerpolation(DBox *tag, Deformation_Field *df, DBox *rst)
{
  int x,y,z;
  int nx,ny,nz;

  FPoint zero;

  zero.X = 0.0;
  zero.Y = 0.0;
  zero.Z = 0.0;

  for (z=0;z<df->Z;z++)
    for (y=0;y<df->Y;y++)
      for (x=0;x<df->X;x++)
	{
	  //	  if (distance_3d_f(zero, df->point[z][y][x])<100.0)
	    {
	      nx = (int) floorf(df->point[z][y][x].X + x +.5);
	      ny = (int) floorf(df->point[z][y][x].Y + y +.5);
	      nz = (int) floorf(df->point[z][y][x].Z + z +.5);
	      
	      if ((nx>=0)&&(ny>=0)&&(nz>=0)&&(nx<df->X)&&(ny<df->Y)&&(nz<df->Z))
		rst->box[z][y][x] = tag->box[nz][ny][nx];
	    }
	}
}

void volume_transform_interpolation(DBox *tag, Deformation_Field *df, DBox *rst)
{
  int x,y,z;
  float nx,ny,nz;
  FPoint zero;

  zero.X = 0.0;
  zero.Y = 0.0;
  zero.Z = 0.0;

  for (z=0;z<df->Z;z++)
    for (y=0;y<df->Y;y++)
      for (x=0;x<df->X;x++)
	{
	  //	  if (distance_3d_f(zero, df->point[z][y][x])<100.0)
	    {
	      nx = df->point[z][y][x].X + x;
	      ny = df->point[z][y][x].Y + y;
	      nz = df->point[z][y][x].Z + z;
	      
	      rst->box[z][y][x] = interpolate_dbox(tag, nx, ny, nz);
	    }
	}

}

DBox * volume_transform_forward(DBox *tag, Deformation_Field *df)
{
  int x,y,z;
  int xxx,yyy,zzz;
  int nx,ny,nz;
  int nnx,nny,nnz;
  DBox *rst;
  DBox *smooth;
  int graynumber, csfnumber;
  FPoint ZP;
  float rate;
  FPoint vector;

  rst = new_dbox(df->X, df->Y, df->Z);
  smooth = new_dbox(df->X, df->Y, df->Z);
  copy_dbox(tag, rst);

  for (z=0;z<df->Z;z++)
    for (y=0;y<df->Y;y++)
      for (x=0;x<df->X;x++)
	{
	  if (tag->box[z][y][x]>0.0)
	    {
	      nx = (int) floorf(df->point[z][y][x].X + x +.5);
	      ny = (int) floorf(df->point[z][y][x].Y + y +.5);
	      nz = (int) floorf(df->point[z][y][x].Z + z +.5);
	      
	      if ((nx>=0)&&(ny>=0)&&(nz>=0)&&(nx<rst->X)&&(ny<rst->Y)&&(nz<rst->Z))
		{
		  if (tag->box[z][y][x] < rst->box[nz][ny][nx])
		  rst->box[nz][ny][nx] = tag->box[z][y][x];
		}
	    }
	}

  copy_dbox(rst,smooth);
  ZP.X = 0;
  ZP.Y = 0;
  ZP.Z = 0;
 
  for (z=1;z<df->Z-1;z++)
    for (y=1;y<df->Y-1;y++)
      for (x=1;x<df->X-1;x++)
	{
	  if (distance_3d_f(ZP, df->point[z][y][x])>0.0)
	  if (rst->box[z][y][x]<160)
	    {
	      graynumber = 0;
	      csfnumber = 0;
	      for (zzz=-1;zzz<2;zzz++)
		for (yyy=-1;yyy<2;yyy++)
		  for (xxx=-1;xxx<2;xxx++)
		    {
		      if (rst->box[z+zzz][y+yyy][x+xxx] == 150.0)
			graynumber ++;
		      else if (rst->box[z+zzz][y+yyy][x+xxx] < 150.0)
			csfnumber ++;
		    }
	      if (graynumber>csfnumber*2)
		smooth->box[z][y][x] = 150;
	      else
		smooth->box[z][y][x] = 10;
	    }
	}

  copy_dbox(smooth, rst);

  for (z=1;z<df->Z-1;z++)
    for (y=1;y<df->Y-1;y++)
      for (x=1;x<df->X-1;x++)
	{
	  if (distance_3d_f(ZP, df->point[z][y][x])>0.0)
	  if (rst->box[z][y][x]<100)
	    {
	      graynumber = 0;
	      csfnumber = 0;
	      for (zzz=-1;zzz<2;zzz++)
		for (yyy=-1;yyy<2;yyy++)
		  for (xxx=-1;xxx<2;xxx++)
		    {
		      if (rst->box[z+zzz][y+yyy][x+xxx] == 150.0)
			graynumber ++;
		      else if (rst->box[z+zzz][y+yyy][x+xxx] < 150.0)
			csfnumber ++;
		    }
	      if (graynumber>csfnumber*2)
		smooth->box[z][y][x] = 150;
	      else
		smooth->box[z][y][x] = 10;
	    }
	}

  free_dbox(rst);
  return(smooth);
}

DBox * volume_transform_forward_bilge(DBox *tag, Deformation_Field *df)
{
  int x,y,z;
  int lx,ly,lz;
  int ux,uy,uz;
  int i,j,k;
  int *gotit;
  float temp;
  int nonzeronum;

  int tissue_no, histo[256];

  float fx, fy, fz;
  float rx, ry, rz;
  float nrx, nry, nrz;
  float distance[5], minvalue;
  int index;

  DBox *rst;
  BBox *flag;
  DBox *sumweight;
  float *value, *intensity;

  Points *zeropoints;
  Points *NBr;

  rst = new_dbox(df->X, df->Y, df->Z);
  flag = new_bbox(df->X, df->Y, df->Z);
  sumweight = new_dbox(df->X, df->Y, df->Z);
  //  copy_dbox(tag, rst);

  for (z=0;z<df->Z;z++)
    for (y=0;y<df->Y;y++)
      for (x=0;x<df->X;x++)
	//	if (fabsf(tag->box[z][y][x])>0.000001)
	{
	  flag->box[z][y][x] = 0;

	  fx = df->point[z][y][x].X + x;
	  fy = df->point[z][y][x].Y + y;
	  fz = df->point[z][y][x].Z + z;
	  
	  lx = (int) floorf(fx);
	  ly = (int) floorf(fy);
	  lz = (int) floorf(fz);

	  rx = fx - lx;
	  ry = fy - ly;
	  rz = fz - lz;
	  
	  for (k=0;k<2;k++)
	    for (j=0;j<2;j++)
	      for (i=0;i<2;i++)
		{
		  ux = lx + i;
		  uy = ly + j;
		  uz = lz + k;

		  if ((ux>=0)&&(uy>=0)&&(uz>=0)&&(ux<df->X)&&(uy<df->Y)&&(uz<df->Z))
		    {
		      nrx = (1-rx) * (1-i) + rx * i;
		      nry = (1-ry) * (1-j) + ry * j;
		      nrz = (1-rz) * (1-k) + rz * k;

		      rst->box[uz][uy][ux] += nrx * nry * nrz * tag->box[z][y][x];
		      flag->box[uz][uy][ux] = 1;
		      sumweight->box[uz][uy][ux] += nrx * nry * nrz;
		    }
		}
	}

  i = 0;
  for (z=0;z<df->Z;z++)
    for (y=0;y<df->Y;y++)
      for (x=0;x<df->X;x++)
	{
	  if ((sumweight->box[z][y][x] > 0.1)&&(flag->box[z][y][x]))
	    {
	      rst->box[z][y][x] = rst->box[z][y][x] / sumweight->box[z][y][x];
	    }
	  else
	    {
	      i++;
	      flag->box[z][y][x] = 0;
	    }
	}

  //  write_bbox(flag, "flag.img");

  zeropoints = new_points(i);
  i = 0;
  for (z=0;z<df->Z;z++)
    for (y=0;y<df->Y;y++)
      for (x=0;x<df->X;x++)
	{
	  if (flag->box[z][y][x] == 0)
	    {
	      zeropoints->point[i].X = x;
	      zeropoints->point[i].Y = y;
	      zeropoints->point[i].Z = z;
	      i++;
	    }
	}

  NBr = new_points(6);
  NBr->point[0].X = -1;NBr->point[0].Y = 0;NBr->point[0].Z = 0;
  NBr->point[1].X = 1;NBr->point[1].Y = 0;NBr->point[1].Z = 0;
  NBr->point[2].X = 0;NBr->point[2].Y = -1;NBr->point[2].Z = 0;
  NBr->point[3].X = 0;NBr->point[3].Y = 1;NBr->point[3].Z = 0;
  NBr->point[4].X = 0;NBr->point[4].Y = 0;NBr->point[4].Z = -1;
  NBr->point[5].X = 0;NBr->point[5].Y = 0;NBr->point[5].Z = 1;
  intensity = (float *) calloc(6, sizeof(float));
 
  while(1)
    {
      //      printf("%d \n", zeropoints->Num);

      //      display_points(zeropoints);

      value = (float *) calloc (zeropoints->Num, sizeof(float));
      gotit = (int *) calloc (zeropoints->Num, sizeof(int));

      for (i=0;i<zeropoints->Num;i++)
	{
	  gotit[i] = 0;
	  nonzeronum = 0;
	  for (j=0;j<6;j++)
	    {
	      x = zeropoints->point[i].X + NBr->point[j].X;
	      y = zeropoints->point[i].Y + NBr->point[j].Y;
	      z = zeropoints->point[i].Z + NBr->point[j].Z;
	      
	      if ((x>=0)&&(y>=0)&&(z>=0)&&(x<df->X)&&(y<df->Y)&&(z<df->Z))
		{
		  intensity[j] = rst->box[z][y][x];
		  if (flag->box[z][y][x])
		    gotit[i] += 1;
		  if (intensity[j]>0)
		    nonzeronum ++;
		}
	      else
		intensity[j] = 0;
	    }

	  if (nonzeronum>2)
	    {
	      for (j=0;j<5;j++)
		for (k=j+1;k<6;k++)
		  {
		    if (intensity[j]<intensity[k])
		      {
			temp = intensity[j];
			intensity[j] = intensity[k];
			intensity[k] = temp;
		      }
		  }
	      
	      value[i] = intensity[j/2];
	    }	
	  else
	    value[i] = 0;
	}
      
      j = 0;
      for (i=0;i<zeropoints->Num;i++)
	{
	  if (gotit[i])
	    {
	      rst->box[zeropoints->point[i].Z][zeropoints->point[i].Y][zeropoints->point[i].X] = value[i];
	      flag->box[zeropoints->point[i].Z][zeropoints->point[i].Y][zeropoints->point[i].X] = 1;
	    }
	  else
	    j++;
	}

      printf("got = %d\n", j);
      //      getchar();

      if (j==0)
	{
	  free_points(zeropoints);
	  free(gotit);
	  free(value);
	  break;
	}      
      
      free_points(zeropoints);
      zeropoints = new_points(j);
      j = 0;
      for (i=0;i<zeropoints->Num;i++)
	{
	  if (!gotit[i])
	    {
	      zeropoints->point[j].X = zeropoints->point[i].X;
	      zeropoints->point[j].Y = zeropoints->point[i].Y;
	      zeropoints->point[j].Z = zeropoints->point[i].Z;
	      j++;
	    }
	}

      free(value);
      free(gotit);
    }     

  free(intensity);
  free_points(NBr);

  free_dbox(sumweight); 

  for (i=0;i<256;i++)
    histo[i] = 0;

  for (z=0;z<tag->Z;z++)
    for (y=0;y<tag->Y;y++)
      for (x=0;x<tag->X;x++)
	histo[(int)(tag->box[z][y][x])]++;

  tissue_no = 0;
  for (i=0;i<256;i++)
    if (histo[i]!=0)
      tissue_no++;

  if (tissue_no<6)
    {
      for (z=0;z<rst->Z;z++)
	for (y=0;y<rst->Y;y++)
	  for (x=0;x<rst->X;x++)
	    {
	      if ((rst->box[z][y][x]!=0)
		&& (rst->box[z][y][x]!=50)
		  && (rst->box[z][y][x]!=150)
		    && (rst->box[z][y][x]!=10)
		  && (rst->box[z][y][x]!=250))		    
		{
		  distance[0] = fabsf(rst->box[z][y][x] - 0);
		  distance[1] = fabsf(rst->box[z][y][x] - 10);
		  distance[2] = fabsf(rst->box[z][y][x] - 50);
		  distance[3] = fabsf(rst->box[z][y][x] - 150);
		  distance[4] = fabsf(rst->box[z][y][x] - 250);
		  index = 0;
		  minvalue = distance[0];
		  for (i=1;i<5;i++)
		    if (minvalue>distance[i])
		      {
			minvalue=distance[i];
			index = i;
		      }
		  switch(index)
		    {
		    case 0:
		      rst->box[z][y][x] = 0;
		      break;
		    case 1:
		      rst->box[z][y][x] = 10;
		      break;
		    case 2:
		      rst->box[z][y][x] = 50;
		      break;
		    case 3:
		      rst->box[z][y][x] = 150;
		      break;
		    case 4:
		      rst->box[z][y][x] = 250;
		      break;
		    }
		}
		
	    }
    }

  return(rst);
}


DBox * volume_transform_forward_bilge_simple(DBox *tag, Deformation_Field *df, int do_segmentation)
{
  int x,y,z;
  int lx,ly,lz;
  int ux,uy,uz;
  int i,j,k;
  int *gotit;
  int temp;
  int nonzeronum;

  int tissue_no, histo[256];

  float fx, fy, fz;
  float rx, ry, rz;
  float nrx, nry, nrz;
  float distance[5], minvalue;
  int index;

  DBox *rst;
  BBox *flag;
  DBox *sumweight;
  float *value, *intensity;

  Points *zeropoints;
  Points *NBr;

  rst = new_dbox(df->X, df->Y, df->Z);
  //  flag = new_bbox(df->X, df->Y, df->Z);
  sumweight = new_dbox(df->X, df->Y, df->Z);
  //  copy_dbox(tag, rst);

  for (z=0;z<df->Z;z++)
    for (y=0;y<df->Y;y++)
      for (x=0;x<df->X;x++)
	//	if (fabsf(tag->box[z][y][x])>0.000001)
	{
	  //	  flag->box[z][y][x] = 0;

	  fx = df->point[z][y][x].X + x;
	  fy = df->point[z][y][x].Y + y;
	  fz = df->point[z][y][x].Z + z;
	  
	  lx = (int) floorf(fx);
	  ly = (int) floorf(fy);
	  lz = (int) floorf(fz);

	  rx = fx - lx;
	  ry = fy - ly;
	  rz = fz - lz;
	  
	  for (k=0;k<2;k++)
	    for (j=0;j<2;j++)
	      for (i=0;i<2;i++)
		{
		  ux = lx + i;
		  uy = ly + j;
		  uz = lz + k;

		  if ((ux>=0)&&(uy>=0)&&(uz>=0)&&(ux<df->X)&&(uy<df->Y)&&(uz<df->Z))
		    {
		      nrx = (1-rx) * (1-i) + rx * i;
		      nry = (1-ry) * (1-j) + ry * j;
		      nrz = (1-rz) * (1-k) + rz * k;

		      rst->box[uz][uy][ux] += nrx * nry * nrz * tag->box[z][y][x];
		      //		      flag->box[uz][uy][ux] = 1;
		      sumweight->box[uz][uy][ux] += nrx * nry * nrz;
		    }
		}
	}

  i = 0;
  for (z=0;z<df->Z;z++)
    for (y=0;y<df->Y;y++)
      for (x=0;x<df->X;x++)
	{
	  if ((sumweight->box[z][y][x] > 0.1))
	    {
	      rst->box[z][y][x] = rst->box[z][y][x] / sumweight->box[z][y][x];
	    }
	  else
	    {
	      i++;
	      //	      flag->box[z][y][x] = 0;
	    }
	}

  free_dbox(sumweight); 

  if (do_segmentation)
  for (z=0;z<rst->Z;z++)
    for (y=0;y<rst->Y;y++)
      for (x=0;x<rst->X;x++)
	{
	  if (rst->box[z][y][x]==50)
	    rst->box[z][y][x] = 10;

	  if ((rst->box[z][y][x]!=0)
	      && (rst->box[z][y][x]!=50)
	      && (rst->box[z][y][x]!=150)
	      && (rst->box[z][y][x]!=10)
	      && (rst->box[z][y][x]!=250))		    
	    {
	      distance[0] = fabsf(rst->box[z][y][x] - 0);
	      distance[1] = fabsf(rst->box[z][y][x] - 10);
	      distance[2] = fabsf(rst->box[z][y][x] - 50);
	      distance[3] = fabsf(rst->box[z][y][x] - 150);
	      distance[4] = fabsf(rst->box[z][y][x] - 250);
	      index = 0;
	      minvalue = distance[0];
	      for (i=1;i<5;i++)
		if (minvalue>distance[i])
		  {
		    minvalue=distance[i];
		    index = i;
		  }
	      switch(index)
		{
		case 0:
		  rst->box[z][y][x] = 0;
		  break;
		case 1:
		  rst->box[z][y][x] = 10;
		  break;
		case 2:
		  rst->box[z][y][x] = 10;
		  break;
		case 3:
		  rst->box[z][y][x] = 150;
		  break;
		case 4:
		  rst->box[z][y][x] = 250;
		  break;
		}
	    }
	  
	}  

  return(rst);
}

void volume_transformation(DBox *org, DBox *tag, Deformation_Field *df)
{
  int x,y,z,nx,ny,nz;
  int xx,yy,zz;

  float minimal, temp;
  int rx, ry,rz;

  int all_zero;

  DBox *tmp;

  tmp = new_dbox(org->X, org->Y, org->Z);

  for (z=0;z<tag->Z;z++)
    for (y=0;y<tag->Y;y++)
      for (x=0;x<tag->X;x++)
	{
	  all_zero = 1;

	  for (zz=-1;zz<2;zz++)
	    for (yy=-1;yy<2;yy++)
	      for (xx=-1;xx<2;xx++)
		{
		  nx = x +xx;
		  ny = y +yy;
		  nz = z +zz;
		  if (nx<0)
		    nx = 0;
		  if (ny<0)
		    ny = 0;
		  if (nz<0)
		    nz = 0;
		  if (nx>=df->X)
		    nx = df->X-1;
		  if (ny>=df->Y)
		    ny = df->Y-1;
		  if (nz>=df->Z)
		    nz = df->Z-1;
		  if (org->box[nz][ny][nx]!=0)
		    all_zero = 0;
		}
		  
	  if (!all_zero)
	    {
	      /*
	      minimal = 255.0;
	      for (zz=-2;zz<3;zz++)
		for (yy=-2;yy<3;yy++)
		  for (xx=-2;xx<3;xx++)
		    {
		      nx = (int) floorf(df->point[z][y][x].X + x+.5 +xx);
		      ny = (int) floorf(df->point[z][y][x].Y + y+.5 +yy);
		      nz = (int) floorf(df->point[z][y][x].Z + z+.5 +zz);
		      if (nx<0)
			nx = 0;
		      if (ny<0)
			ny = 0;
		      if (nz<0)
			nz = 0;
		      if (nx>=df->X)
			nx = df->X-1;
		      if (ny>=df->Y)
			ny = df->Y-1;
		      if (nz>=df->Z)
			nz = df->Z-1;
		      temp = fabsf(org->box[z][y][x] - tag->box[nz][ny][nx]);
		      if (temp<minimal)
			{
			  minimal = temp;
			  rx = xx;
			  ry = yy;
			  rz = zz;
			}
		    }
	      */
	      rx = ry = rz = 0;

	      nx = (int) floorf(df->point[z][y][x].X + x+.5 +rx);
	      ny = (int) floorf(df->point[z][y][x].Y + y+.5 +ry);
	      nz = (int) floorf(df->point[z][y][x].Z + z+.5+ rz);

	      if (nx<0)
		nx = 0;
	      if (ny<0)
		ny = 0;
	      if (nz<0)
		nz = 0;
	      if (nx>=df->X)
		nx = df->X-1;
	      if (ny>=df->Y)
		ny = df->Y-1;
	      if (nz>=df->Z)
		nz = df->Z-1;

	      tmp->box[z][y][x] = tag->box[nz][ny][nx];
	    }
	}

  for (z=0;z<tag->Z;z++)
    for (y=0;y<tag->Y;y++)
      for (x=0;x<tag->X;x++)
	org->box[z][y][x] = tmp->box[z][y][x];

  free_dbox(tmp);
}

// update deformation field
//
// df               : current deformation field
// originalpoints   : model points
// newpoints        : newly detected subject points
// distance_sigma   : sigma of Gaussian function
// rate             : 0.0~1.0 the step to move.
// lambda           : delta = lambda*(one) + (1-lambda) * (two),
//                    one is determined from new subject points, two is determined from affine alignment(between model and new subject points).

void write_deformationfield(Deformation_Field *df, char *filename)
{
  int z,y,x;
  FILE *fp;
  FPoint zero;
  float maxdf, temp;

  zero.X = 0.0;
  zero.Y = 0.0;
  zero.Z = 0.0;

  fp = fopen(filename, "wb");
  if (fp == NULL)
	{
	  printf("file open failed in write_deformationfield()\n");
	  exit(0);
      }

  maxdf = 0.0;
  if (df->Z>1)
    for(z=0;z<df->Z;z++)
      for(y =0;y<df->Y;y++)
	for (x=0;x<df->X;x++)
	  {
	    fwrite(&(df->point[z][y][x]), sizeof(FPoint), 1, fp);
	    temp = distance_3d_f(df->point[z][y][x], zero);
	    if (maxdf<temp)
	      maxdf = temp;
	    //	  if (temp>1000)
	    //	    printf("%d %d %d, %f %f %f\n", z,y,x, df->point[z][y][x].Z, df->point[z][y][x].Y, df->point[z][y][x].X);
	  }
  else if (df->Z==1)
    {
      for(y =0;y<df->Y;y++)
	for (x=0;x<df->X;x++)
	  {
	    temp = df->point[0][y][x].X;
	    fwrite(&temp, sizeof(float),1, fp);
	    temp = df->point[0][y][x].Y;
	    fwrite(&temp, sizeof(float),1, fp);
	  }
    }

  //  printf("deformation field %s saved, and its size is: %d %d %d\n", filename, df->X, df->Y, df->Z);
  //  printf("max df = %f\n", maxdf);
  fclose(fp);
}

void write_deformationfield_with_switch(Deformation_Field *df, char *filename, int sw)
{
  int z,y,x;
  FILE *fp;
  FPoint zero;
  float maxdf, temp;

  zero.X = 0.0;
  zero.Y = 0.0;
  zero.Z = 0.0;

  fp = fopen(filename, "wb");
  if (fp == NULL)
	{
	  printf("file open failed in write_deformationfield()\n");
	  exit(0);
      }

  if (sw)
    printf("switching X<->Y\n");

  maxdf = 0.0;
  for(z=0;z<df->Z;z++)
    for(y =0;y<df->Y;y++)
      for (x=0;x<df->X;x++)
	{
	  temp = distance_3d_f(df->point[z][y][x], zero);
	  if (maxdf<temp)
	    maxdf = temp;
	  //	  if (temp>1000)
	  //	    printf("%d %d %d, %f %f %f\n", z,y,x, df->point[z][y][x].Z, df->point[z][y][x].Y, df->point[z][y][x].X);
	  if (!sw)
	    fwrite(&(df->point[z][y][x]), sizeof(FPoint), 1, fp);
	  else
	    {
	      fwrite(&(df->point[z][y][x].Y), sizeof(float), 1, fp);
	      fwrite(&(df->point[z][y][x].X), sizeof(float), 1, fp);
	      fwrite(&(df->point[z][y][x].Z), sizeof(float), 1, fp);	      
	    }
	}

  printf("max df = %f\n", maxdf);
  fclose(fp);
}


void read_deformationfield(Deformation_Field *df, char *filename)
{
  int z,y,x;
  FILE *fp;

  float maxv;
  float ok;
  float tempf;

  FPoint zero;

  zero.X = 0.0;
  zero.Y = 0.0;
  zero.Z = 0.0;

  //  printf("reading file:%s\n", filename);

  fp = fopen(filename, "rb");
  if (fp==NULL)
    {
      printf("file open error in read_deformationfield\n");
      exit(0);
    }

  maxv = 0.0;

  if (df->Z>1)
    for(z=0;z<df->Z;z++) 
      for(y =0;y<df->Y;y++) 
	for (x=0;x<df->X;x++) 
	  {
	    fread(&(df->point[z][y][x]), sizeof(FPoint), 1, fp); 
	    ok = distance_3d_f(df->point[z][y][x],zero);
	    if (maxv<ok)
	      maxv = ok;
	    /*
	      ok = distance_3d_f(df->point[z][y][x],zero);
	      if (ok<1500.0)
	      {
	      if (maxv<ok)
	      maxv = ok;
	      }
	      else
	      {
	      df->point[z][y][x].X = 1500.0;
	      df->point[z][y][x].Y = 1500.0;
	      df->point[z][y][x].Z = 1500.0;
	      }
	    */
	  }
  else if (df->Z==1)
    {
      for(y =0;y<df->Y;y++)
	for (x=0;x<df->X;x++)
	  {
	    fread(&tempf, sizeof(float), 1, fp);
	    df->point[0][y][x].X = tempf;
	    fread(&tempf, sizeof(float), 1, fp);
	    df->point[0][y][x].Y = tempf;
	  }
    }    

  fclose(fp);
  //  printf("max df = %f\n", maxv);
}

void read_deformationfield_with_switch(Deformation_Field *df, char *filename, int sw)
{
  int z,y,x;
  FILE *fp;

  float maxv;
  float ok, okok;

  int width, k;

  int xx,yy,zz, l,m,n,goout, XX,YY,ZZ;

  FPoint zero;
  Points *hood;

  zero.X = 0.0;
  zero.Y = 0.0;
  zero.Z = 0.0;

  XX = df->X;
  YY = df->Y;
  ZZ = df->Z;

  width = 35;

  maxv = 0.0;

  if (sw)
    printf("switch X<->Y is ON\n");

  read_deformationfield(df, filename);

  if (sw)
    {      
      for(z=0;z<df->Z;z++)
	for(y =0;y<df->Y;y++)
	  for (x=0;x<df->X;x++)
	    {
	      ok = df->point[z][y][x].X;
	      df->point[z][y][x].X = df->point[z][y][x].Y;
	      df->point[z][y][x].Y = ok;
	    }
    }

  /*

  for(z=0;z<df->Z;z++)
    for(y =0;y<df->Y;y++)
      for (x=0;x<df->X;x++)
	{
	  ok = distance_3d_f(tdf->point[z][y][x],zero);
	  if (ok>1500)
	    {
	      n = 0;
	      goout = 0;
	      for (k=0;k<hood->Num;k++)
		{
		  xx = x+hood->point[k].X;
		  yy = y+hood->point[k].Y;
		  zz = z+hood->point[k].Z;
		  
		  xx = (xx<0)?0:xx;
		  xx = (xx>=XX)?XX-1:xx;
		  yy = (yy<0)?0:yy;
		  yy = (yy>=YY)?YY-1:yy;
		  zz = (zz<0)?0:zz;
		  zz = (zz>=ZZ)?ZZ-1:zz;
		  
		  okok = distance_3d_f(tdf->point[zz][yy][xx],zero);
		  if (okok<1500.0)
		    {
		      //			      printf("here %d %d, %d %d\n", l, m, yy, xx);
		      df->point[z][y][x].X = tdf->point[zz][yy][xx].X;
		      df->point[z][y][x].Y = tdf->point[zz][yy][xx].Y;			      
		      df->point[z][y][x].Z = tdf->point[zz][yy][xx].Z;
		      goout = 1;
		      break;
		    }			
		}		  
	    }
	  else
	    {
	      df->point[z][y][x].X = tdf->point[z][y][x].X;
	      df->point[z][y][x].Y = tdf->point[z][y][x].Y;			      
	      df->point[z][y][x].Z = tdf->point[z][y][x].Z;
	      
	      if (maxv<ok)
		maxv = ok;
	    }		  
	}      

  free_deformationfield(tdf);
  free_points(hood);
  fclose(fp);
  printf("max df = %f\n", maxv);
  */
}
/**************************

Rotation Invariant Mask

 **************************/


BBox * generate_rotation_invariant_mask(int width)
{
  int x,y,z;
  BBox *tmp;
  float c;
  float temp;
  float k;

  tmp = new_bbox(width, width, width);

  if (width==1)
    {
      tmp->box[0][0][0] = 1;
      return(tmp);
    }

  c = (width-1)/2.;

  for (x=0;x<width;x++)
    for (y=0;y<width;y++)
      for (z=0;z<width;z++)
	{
	  temp = sqrtf((x-c)*(x-c)+(y-c)*(y-c)+(z-c)*(z-c));
	  if (temp<=c+1.0)
		tmp->box[z][y][x] = (int) floorf(temp + .5) + 1;
	}

  if (width%2==0)
    for (x=0;x<width;x++)
      for (y=0;y<width;y++)
	for (z=0;z<width;z++)
	  {
	    if (tmp->box[z][y][x] !=0)
	      {
		tmp->box[z][y][x] -= 1;
		if (tmp->box[z][y][x] ==0)
		  tmp->box[z][y][x] = 1;
	      }
	  }

  return(tmp);
}

// currently no use
BBox * generate_rotation_invariant_mask_shift(int width, int dx, int dy, int dz)
{
  int x,y,z;
  BBox *tmp;
  int c;
  float temp;
  float k;

  tmp = new_bbox(width, width, width);

  if (width==1)
    {
      tmp->box[0][0][0] = 1;
      return(tmp);
    }

  c = (width-1)/2;

  dx = c+dx;
  dy = c+dy;
  dz = c+dz;

  for (x=0;x<width;x++)
    for (y=0;y<width;y++)
      for (z=0;z<width;z++)
	{
	  temp = sqrtf((x-dx)*(x-dx)+(y-dy)*(y-dy)+(z-dz)*(z-dz));
	  for (k=0;k<c+1.0;k+=.5)
	    if (fabsf(temp-k)<.4)
		tmp->box[z][y][x] = (int) floorf(k+.5)+1;
	}

  return(tmp);
}

void copy_bbox(BBox *img1, BBox *img2)
{
  int x,y,z;
  
  for (z=0;z<img1->Z;z++)
    for (y=0;y<img1->Y;y++)
      for (x=0;x<img1->X;x++)
	{
	  img2->box[z][y][x] = img1->box[z][y][x];
	}
}

BBox * new_bbox(int X, int Y, int Z)
{
  int i;
  BBox *temp;
  int x,y,z,d;

  temp = (BBox *) calloc (1, sizeof(BBox));

  temp->X = X;
  temp->Y = Y;
  temp->Z = Z;

  temp->box = Balloc3d(X,Y,Z);

  for (z=0;z<Z;z++)
    for (y=0;y<Y;y++)
      for (x=0;x<X;x++)
	temp->box[z][y][x] = 0;

  return temp;
}

void calloc_bbox(BBox *temp, int X, int Y, int Z)
{
  int i;
  int x,y,z,d;

  temp->X = X;
  temp->Y = Y;
  temp->Z = Z;

  temp->box = Balloc3d(X,Y,Z);

  for (z=0;z<Z;z++)
    for (y=0;y<Y;y++)
      for (x=0;x<X;x++)
	temp->box[z][y][x] = 0;
}

void minux_bbox(BBox *bbox1, BBox*bbox2, BBox *rst)
{
  int x,y,z;
  byte temp;

  for(z=0;z<bbox1->Z;z++)
    for (y=0;y<bbox1->Y;y++)
      for (x=0;x<bbox1->X;x++)
	{
	  temp = bbox1->box[z][y][x] - bbox2->box[z][y][x];
	  rst->box[z][y][x] = (temp>0)? temp:-temp;
	}
}

long difference_bbox(BBox *bbox1, BBox*bbox2)
{
  int x,y,z;
  long temp;

  temp = 0;
  for(z=0;z<bbox1->Z;z++)
    for (y=0;y<bbox1->Y;y++)
      for (x=0;x<bbox1->X;x++)
	{
	  if (bbox1->box[z][y][x]-bbox2->box[z][y][x]!=0)
	    temp++;
	}

  return(temp);
}


void free_bbox(BBox * tmp)
{
  Bfree3d(tmp->box, tmp->Z, tmp->Y);
  free(tmp);  
}

void free_bbox_data(BBox * tmp)
{
  Bfree3d(tmp->box, tmp->Z, tmp->Y);
}


void display_bbox(BBox *tmp)
{
  int x,y,z;
  for (z=0;z<tmp->Z;z++)
    {
      for (y=0;y<tmp->Y;y++)
	{
	  for (x=0;x<tmp->X;x++)
	    printf("%3d ", tmp->box[z][y][x]);
	  printf("\n");
	}
      printf("\n");
    }
  
}

void mask_bbox(BBox *iimg,BBox *msk,BBox *oimg, int threshold)
{
  int x,y,z;
  for (z=0;z<iimg->Z;z++)
    for (y=0;y<iimg->Y;y++)
      for (x=0;x<iimg->X;x++)
	if (iimg->box[z][y][x]>threshold)
	  oimg->box[z][y][x] = iimg->box[z][y][x];
	else
	  oimg->box[z][y][x] = 0;
}

void read_bbox(BBox *dbox, char *filename)
{
  FILE *fp;

  int y,x,z;
  unshort ustemp;
  byte btemp;

  if ((fp = fopen(filename, "rb"))==NULL)
    {
      printf("file open failed: %s\n", filename);
      exit(0);
    };

  for (z=0;z<dbox->Z;z++)
    for(y=0;y<dbox->Y;y++)
      fread(dbox->box[z][y], sizeof(byte), dbox->X, fp);

  fclose(fp);
}

void write_bbox(BBox *dbox, char *filename)
{
  FILE *fp;
  int y,z,x;

  if ((fp = fopen(filename, "wb"))==NULL)
    {
      printf("file open failed: %s\n", filename);
      exit(0);
    };

  for (z=0;z<dbox->Z;z++)
    for(y=0;y<dbox->Y;y++)
      fwrite(dbox->box[z][y], sizeof(byte), dbox->X, fp);

  //  printf("saved size: %d %d %d\n", dbox->X, dbox->Y, dbox->Z);
  fclose(fp);
}


float interpolate_bbox(BBox *db, float xx, float yy, float zz)
{
  float CurrentV ;
  int x,y,z, xu,yu,zu;
  float coeflx, coefly, coeflz, coefhx, coefhy, coefhz;

  x = (int) floorf(xx) ;
  y = (int) floorf(yy) ;
  z = (int) floorf(zz) ;
  xu = x+1;
  yu = y+1;
  zu = z+1;

  if (xu<=0 || yu<=0 || zu<=0 || x>=db->X-1 || y>=db->Y-1 || z>=db->Z-1)
    return 0;

  if (x>=0 && y>=0 && z>=0 && xu<db->X && yu<db->Y && zu<db->Z)
    {
      coefhx = xx-x;
      coefhy = yy-y;
      coefhz = zz-z;
      coeflx = 1-coefhx;
      coefly = 1-coefhy;
      coeflz = 1-coefhz;

      CurrentV = coeflz*(coefly*(coefhx * db->box[z][y][xu]   + coeflx * db->box[z][y][x])+
			 coefhy*(coefhx * db->box[z][yu][xu]  + coeflx * db->box[z][yu][x]))+
	coefhz*(coefly*(coefhx * db->box[zu][y][xu]  + coeflx * db->box[zu][y][x])+
		coefhy*(coefhx * db->box[zu][yu][xu] + coeflx * db->box[zu][yu][x]));
      return CurrentV;
      }
  else
    {
      return 100000;
    }
}


/************************************
Histogram
 ************************************/

float * calculate_histogram_bbox(BBox *dbox)
{
  int x,y,z;
  float *histo;
  int number;

  histo = (float *) calloc (256, sizeof(float));
  
  number = 0;
  for (z=0;z<dbox->Z;z++)
    for (y=0;y<dbox->Y;y++)
      for (x=0;x<dbox->X;x++)
	{
	  histo[dbox->box[z][y][x]] ++;
	  number ++;
	}

  for (x=0;x<256;x++)
    histo[x] /= (float)number;
  
  return(histo);
}

void calculate_histogram_bbox_void(BBox *dbox, float *histo)
{
  int x,y,z;
  int number;

  number = 0;
  for (x=0;x<256;x++)
    histo[x] = 0;

  for (z=0;z<dbox->Z;z++)
    for (y=0;y<dbox->Y;y++)
      for (x=0;x<dbox->X;x++)
	{	  
	  histo[dbox->box[z][y][x]] ++;
	  number ++;
	}

  for (x=0;x<256;x++)
    histo[x] /= (float)number;  
}


float * calculate_histogram(DBox *dbox)
{
  int x,y,z;
  float *histo;
  int number;

  histo = (float *) calloc (256, sizeof(float));
  
  number = 0;
  for (z=0;z<dbox->Z;z++)
    for (y=0;y<dbox->Y;y++)
      for (x=0;x<dbox->X;x++)
	{
	  histo[(int) floorf(dbox->box[z][y][x]+.5)] ++;
	  number ++;
	}

  for (x=0;x<256;x++)
    histo[x] /= (float)number;
  
  return(histo);
}

float * calculate_histogram_mask(DBox *dbox, BBox *mask)
{
  int x,y,z;
  float *histo;
  int number;

  histo = (float *) calloc (256, sizeof(float));
  
  number = 0;
  for (z=0;z<dbox->Z;z++)
    for (y=0;y<dbox->Y;y++)
      for (x=0;x<dbox->X;x++)
	if (mask->box[z][y][x])
	  {	  
	    histo[(int) floorf(dbox->box[z][y][x]+.5)] ++;
	    number ++;
	  }

  for (x=0;x<256;x++)
    histo[x] /= (float)number;
  
  return(histo);
}

float * calculate_histogram_mask128(DBox *dbox, BBox *mask)
{
  int x,y,z;
  float *histo;
  int number;

  histo = (float *) calloc (128, sizeof(float));
  
  number = 0;
  for (z=0;z<dbox->Z;z++)
    for (y=0;y<dbox->Y;y++)
      for (x=0;x<dbox->X;x++)
	if (mask->box[z][y][x])
	  {	  
	    histo[(int) floorf(dbox->box[z][y][x]+.5)] ++;
	    number ++;
	  }

  for (x=0;x<128;x++)
    histo[x] /= (float)number;
  
  return(histo);
}


void calculate_histogram_void(DBox *dbox, float *histo)
{
  int x,y,z;
  int number;
  
  number = 0;
  for (x=0;x<256;x++)
    histo[x] = 0;

  for (z=0;z<dbox->Z;z++)
    for (y=0;y<dbox->Y;y++)
      for (x=0;x<dbox->X;x++)
	{
	  histo[(int) floorf(dbox->box[z][y][x]+.5)] ++;
	  number ++;
	}

  for (x=0;x<256;x++)
    histo[x] /= (float)number;
  
}

float vec_dist2(float *a, float *b, int N)
{
  float v;
  int i;
  v = 0.0;
  for (i=0;i<N;i++)
    v += (a[i]-b[i])*(a[i]-b[i]);
  return(v);
}

DBox * calculate_3d_gradient_dd(DBox *dbox)
{
  DBox *ndb;
  int x,y,z;
  int lx, ly, lz, ux, uy, uz;
  float fx, fy, fz;

  ndb = new_dbox(dbox->X, dbox->Y, dbox->Z);

  for (z=1;z<dbox->Z-1;z++)
    for (y=1;y<dbox->Y-1;y++)
      for (x=1;x<dbox->X-1;x++)
	{
	  ux = x+1;
	  uy = y+1;
	  uz = z+1;
	  lx = x-1;
	  ly = y-1;
	  lz = z-1;
	  fx = (dbox->box[z][y][ux] - dbox->box[z][y][lx])/2.0;
	  fy = (dbox->box[z][uy][x] - dbox->box[z][ly][x])/2.0;
	  fz = (dbox->box[uz][y][x] - dbox->box[lz][y][x])/2.0;
	  ndb->box[z][y][x] = sqrtf(fx*fx+fy*fy+fz*fz);	  	  
	}
  
  return(ndb);
}



Image * histogram_gi(DBox *dbox, DBox *flag)
{
  int x,y,z;
  Image *histo;
  DBox *grd;
  int number;

  histo = new_image(256,256);

  grd = calculate_3d_gradient_dd(dbox);
  
  number = 0;
  for (z=0;z<dbox->Z;z++)
    for (y=0;y<dbox->Y;y++)
      for (x=0;x<dbox->X;x++)
	{
	  if (flag->box[z][y][x]>0.0)
	    {
	      histo->img[(int) floorf(dbox->box[z][y][x]+.5)][(int) floorf(grd->box[z][y][x]+.5)] ++;
	      number ++;
	    }
	}

  for (x=0;x<256;x++)
    for (y=0;y<256;y++)
      histo->img[x][y] /= (float)number;

  free_dbox(grd);
  return(histo);
}


void write_matrix(Matrix *matrix, char *filename)
{
  FILE *fp;
  int i,j;
  float test;
  fp = fopen(filename, "wb");
  if (fp==NULL)
    {
      printf("file open failed\n");
      exit(0);
    }
  test = (float) matrix->height;
  fwrite(&test, sizeof(float), 1, fp);

  test = (float) matrix->width;
  fwrite(&test, sizeof(float), 1, fp);

  for (i=0;i<matrix->height;i++)
    for (j=0;j<matrix->width;j++)
      {
	test = (float)matrix->data[i][j];
	fwrite(&test, sizeof(float), 1, fp);
      }

  fclose(fp);
}

Matrix * read_matrix(char *filename)
{
  FILE *fp;
  int i,j;
  float test, height, width;
  Matrix *matrix;

  fp = fopen(filename, "rb");
  if (fp==NULL)
    {
      printf("file open failed\n");
      exit(0);
    }

  matrix = (Matrix *) calloc (1, sizeof(Matrix));

  fread(&height, sizeof(float), 1, fp);
  fread(&width, sizeof(float), 1, fp);

  new_matrix(matrix, static_cast<int>(height), static_cast<int>(width));

  for (i=0;i<matrix->height;i++)
    for (j=0;j<matrix->width;j++)
      {
	fread(&test, sizeof(float), 1, fp);
	matrix->data[i][j] = test;
      }

  fclose(fp);
  return(matrix);
}

void copy_matrix(Matrix *a, Matrix *b)
{
  int i,j;
  for (i=0;i<a->height;i++)
    for (j=0;j<a->width;j++)
      b->data[i][j] = a->data[i][j];
}

Transformation new_transformation(void)
{
  Transformation T;
  T.A = (Matrix *) calloc (1, sizeof(Matrix));
  new_matrix(T.A, 3,3);
  return(T);
}

Transformation read_transformation_fsl(char *filename)
{
  Transformation T;
  FILE *fp;
  float tempfloat;
  float *translation;
  int i,j;

  fp = fopen(filename, "r");
  if (fp==NULL)
    {
      printf("file open failed\n");
      exit(0);
    }
  
  T = new_transformation();
  for (i=0;i<T.A->height;i++)
    {
      for (j=0;j<T.A->width;j++)
	{
	  fscanf(fp, "%f", &tempfloat);
	  T.A->data[i][j] = tempfloat;
	}
      fscanf(fp, "%f", &(T.Ty[i]));
      T.Tx[i] = 0;
    }
  fclose(fp);
  return(T);
}

void adjust_fsl_transform_to_myformat(Transformation *T, int XX, int YY, int ZZ)
{
  int i,j;
  float rx,ry,rz;
  float Rx,Ry,Rz;

  rx = .9375;
  ry = .9375;
  rz = 1.5;
  Rx = .9375;
  Ry = .9375;
  Rz = 1.5;

  for(j=0; j<3; j++)
    {
      T->A->data[0][j] /= rx ;
      T->A->data[1][j] /= ry ;
      T->A->data[2][j] /= rz ;
    }
  for(i=0; i<3; i++)
    {
      T->A->data[i][0] *= Rx ;
      T->A->data[i][1] *= Ry ;
      T->A->data[i][2] *= Rz ;
    }

  T->Ty[0] /= rx;
  T->Ty[1] /= rx;
  T->Ty[2] /= rx;

  printf("image size is %d,%d,%d\n", XX,YY,ZZ);
  T->Tx[0] = XX/2.0;
  T->Tx[1] = YY/2.0;
  T->Tx[2] = ZZ/2.0;
  //  printf("%f %f %f\n", T->Tx[0],T->Tx[1],T->Tx[2]);

  for (i=0;i<3;i++)
    {
      for (j=0;j<3;j++)
	T->Ty[i] += T->A->data[i][j] * T->Tx[i];
      //      printf("%f %f %f\n", T->Ty[0],T->Ty[1],T->Ty[2]);
    }
}

// y = A (x - Tx) + Ty
// Forward: here you have A,T,x and try to solve y;
// Backward: here you have A,T, and y, and try to solve x;

void perform_global_image_transformation(BBox *iimg, BBox *oimg, Transformation T, int is_forward, int do_interpolation)
{
  int x,y,z;
  int oZZ,oYY,oXX;
  Matrix *IA;
  float vector0, vector2, vector1;
  float tmp0, tmp2, tmp1;
  int nx,ny,nz;

  oXX = oimg->X;
  oYY = oimg->Y;
  oZZ = oimg->Z;

  IA = (Matrix *) calloc (1, sizeof(Matrix));
  new_matrix(IA, 3,3);
  Mat_Inverse(T.A, IA);

  if (!is_forward)  // if the transformation is backward
    for (z=0;z<oZZ;z++)
      for (y=0;y<oYY;y++)
	for (x=0;x<oXX;x++)
	  {
	    oimg->box[z][y][x] = 0;
	    vector0 = x - T.Tx[0];
	    vector1 = y - T.Tx[1];
	    vector2 = z - T.Tx[2];
	    
	    tmp0 = (float) (T.A->data[0][0] * vector0 + T.A->data[0][1] * vector1 + T.A->data[0][2] * vector2 + T.Ty[0]);
	    tmp1 = (float) (T.A->data[1][0] * vector0 + T.A->data[1][1] * vector1 + T.A->data[1][2] * vector2 + T.Ty[1]);
	    tmp2 = (float) (T.A->data[2][0] * vector0 + T.A->data[2][1] * vector1 + T.A->data[2][2] * vector2 + T.Ty[2]);
	    
	    if (do_interpolation)
	      oimg->box[z][y][x] = (unsigned char)interpolate_bbox(iimg, tmp0, tmp1, tmp2);	      
	    else
	      {
		nx = (int)floorf(tmp0+.5);
		ny = (int)floorf(tmp1+.5);
		nz = (int)floorf(tmp2+.5);

		if ((nx>=0)&&(ny>=0)&&(nz>=0)&&(nx<iimg->X)&&(ny<iimg->Y)&&(nz<iimg->Z))
		  oimg->box[z][y][x] = iimg->box[nz][ny][nx];
	      }
	  }

  else // if the transformation is forward transformation
    {
      for (z=0;z<oZZ;z++)
	for (y=0;y<oYY;y++)
	  for (x=0;x<oXX;x++)
	    {
	      oimg->box[z][y][x] = 0;
	      vector0 = x - T.Ty[0];
	      vector1 = y - T.Ty[1];
	      vector2 = z - T.Ty[2];
	      
	      tmp0 = (float) (IA->data[0][0] * vector0 + IA->data[0][1] * vector1 + IA->data[0][2] * vector2 + T.Tx[0]);
	      tmp1 = (float) (IA->data[1][0] * vector0 + IA->data[1][1] * vector1 + IA->data[1][2] * vector2 + T.Tx[1]);
	      tmp2 = (float) (IA->data[2][0] * vector0 + IA->data[2][1] * vector1 + IA->data[2][2] * vector2 + T.Tx[2]);
	      
	      if (do_interpolation)
		oimg->box[z][y][x] = (unsigned char)interpolate_bbox(iimg, tmp0, tmp1, tmp2);	      
	      else
		{
		  nx = (int)floorf(tmp0+.5);
		  ny = (int)floorf(tmp1+.5);
		  nz = (int)floorf(tmp2+.5);
		  
		  if ((nx>=0)&&(ny>=0)&&(nz>=0)&&(nx<iimg->X)&&(ny<iimg->Y)&&(nz<iimg->Z))
		    oimg->box[z][y][x] = iimg->box[nz][ny][nx];
		}		  		  
	    }	  
    }
}


Transformation read_transformation(char *filename)
{
  Transformation T;
  double test;
  int i,j;
  FILE *fp;
  T = new_transformation();
  fp = fopen(filename, "rb");
  if (fp == NULL)
    {
      printf("file open failed, in read_transformation, filename = %s\n", filename);
      exit(0);
    }
  for (i=0;i<T.A->height;i++)
    for (j=0;j<T.A->width;j++)
      {
	fread(&test, sizeof(double), 1, fp);
	T.A->data[i][j] = test;
      }
  fread(T.Tx, sizeof(float), 3, fp);
  fread(T.Ty, sizeof(float), 3, fp);
  fclose(fp);
  return(T);
}

void write_transformation(Transformation T, char *filename)
{
  double test;
  int i,j;
  FILE *fp;
  fp = fopen(filename, "wb");
  if (fp == NULL)
    {
      printf("file open failed, in write_transformation, filename = %s\n", filename);
      exit(0);
    }
  for (i=0;i<T.A->height;i++)
    for (j=0;j<T.A->width;j++)
      {
	test = T.A->data[i][j];
	fwrite(&test, sizeof(double), 1, fp);
      }
  fwrite(T.Tx, sizeof(float), 3, fp);
  fwrite(T.Ty, sizeof(float), 3, fp);
  fclose(fp);
}

void display_transformation(Transformation T)
{
  int i,j;

  printf("A:\n");
  for (i=0;i<T.A->height;i++)
    {
      for (j=0;j<T.A->width;j++)
	printf("%9.5f ", T.A->data[i][j]);
      printf("\n");
    }

  printf("Ty (center2): %9.5f %9.5f %9.5f \n", T.Ty[0], T.Ty[1], T.Ty[2]);
  printf("Tx (center1): %9.5f %9.5f %9.5f \n", T.Tx[0], T.Tx[1], T.Tx[2]);

}


FPoint subtract_fpoint(FPoint A, FPoint B)
{
  FPoint C;
  C.X = A.X-B.X;
  C.Y = A.Y-B.Y;
  C.Z = A.Z-B.Z;
  return(C);
}

FPoint normalize_fpoint(FPoint A)
{
  FPoint FZ;
  float mag;
  FZ.X = 0;
  FZ.Y = 0;
  FZ.Z = 0;
  mag = distance_3d_f(A, FZ);  
  if (mag==0)
    return(FZ);
  FZ.X = A.X/mag;
  FZ.Y = A.Y/mag;
  FZ.Z = A.Z/mag;
  return(FZ);
 }

FPoint scale_fpoint(FPoint A, float s)
{
  FPoint FZ;
  FZ.X = A.X*s;
  FZ.Y = A.Y*s;
  FZ.Z = A.Z*s;
  return(FZ);
}

// output will be still df
void add_deformationfield(Deformation_Field *df, Deformation_Field *pdf, Deformation_Field *rst)
{
  int x,y,z;
  FPoint fp, p;  

  for (z=0;z<df->Z;z++)
    for (y=0;y<df->Y;y++)
      for (x=0;x<df->X;x++)
	{
	  p.X = x + df->point[z][y][x].X;
	  p.Y = y + df->point[z][y][x].Y;
	  p.Z = z + df->point[z][y][x].Z;
	  fp = get_deformation_fpoint(pdf, p);
	  rst->point[z][y][x].X = df->point[z][y][x].X + fp.X;
	  rst->point[z][y][x].Y = df->point[z][y][x].Y + fp.Y;
	  rst->point[z][y][x].Z = df->point[z][y][x].Z + fp.Z;
	}
}

void subtract_deformationfield(Deformation_Field *df, Deformation_Field *pdf, Deformation_Field *rst)
{
  int x,y,z;
  int xx,yy,zz;
  int nx,ny,nz;
  int XX,YY,ZZ;
  FPoint fp, p, np, deltaf;  
  float distance;
  float w;

  int lx,ly,lz, ux,uy,uz;
  float rx, ry, rz;

  DBox *weights;

  XX = df->X; 
  YY = df->Y;
  ZZ = df->Z;

  weights = new_dbox(XX,YY,ZZ);

  clear_dbox(weights);
  clear_deformationfield(rst);

  for (z=0;z<df->Z;z++)
    {
      //      printf("%d\n", z);
    for (y=0;y<df->Y;y++)
      for (x=0;x<df->X;x++)
	{
	  p.X = x + df->point[z][y][x].X;
	  p.Y = y + df->point[z][y][x].Y;
	  p.Z = z + df->point[z][y][x].Z;

	  np.X = x + pdf->point[z][y][x].X;
	  np.Y = y + pdf->point[z][y][x].Y;
	  np.Z = z + pdf->point[z][y][x].Z;

	  lx = (int)floorf(np.X);
	  ly = (int)floorf(np.Y);
	  lz = (int)floorf(np.Z);	  
	  ux = (int)ceilf(np.X);
	  uy = (int)ceilf(np.Y);
	  uz = (int)ceilf(np.Z);	  

	  //	  printf("begin\n");
	  for (nz = lz-1; nz<=uz+1; nz++)
	    for (ny = ly-1; ny<=uy+1; ny++)
	      for (nx = lx-1; nx<=ux+1; nx++)
		{
		  if ((nx>=0)&&(ny>=0)&&(nz>=0)&&(nx<XX)&&(ny<YY)&&(nz<ZZ))
		    {
		      distance = sqrtf((nx-np.X)*(nx-np.X)+(ny-np.Y)*(ny-np.Y)+(nz-np.Z)*(nz-np.Z));
		      w = expf(-1.0 * distance / 1.0);
		      //		      printf("%f  %f\n", distance, w);
		      rst->point[nz][ny][nx].X = w * (p.X-nx);
		      rst->point[nz][ny][nx].Y = w * (p.Y-ny);
		      rst->point[nz][ny][nx].Z = w * (p.Z-nz);
		      weights->box[nz][ny][nx] += w;		      
		    }
		}
	}
    }
  
  write_dbox(weights, "test.img", "float", 1);

  for (z=0;z<df->Z;z++)
    for (y=0;y<df->Y;y++)
      for (x=0;x<df->X;x++)
	if (weights->box[z][y][x]>0.01)
	  {
	    rst->point[z][y][x].X = rst->point[z][y][x].X / weights->box[z][y][x];
	    rst->point[z][y][x].Y = rst->point[z][y][x].Y / weights->box[z][y][x];
	    rst->point[z][y][x].Z = rst->point[z][y][x].Z / weights->box[z][y][x];
	  }
	else
	  {
	    rst->point[z][y][x].X = 0.0;
	    rst->point[z][y][x].Y = 0.0;
	    rst->point[z][y][x].Z = 0.0;
	  }
  
  free_dbox(weights);
}


void subtract_deformationfield_smallsize(Deformation_Field *df, Deformation_Field *pdf, Deformation_Field *rst, int BX, int EX, int BY, int EY, int BZ, int EZ)
{
  int x,y,z;
  int xx,yy,zz;
  int nx,ny,nz;
  int XX,YY,ZZ;
  FPoint fp, p, np, deltaf;  
  float distance;
  float w;

  int lx,ly,lz, ux,uy,uz;
  float rx, ry, rz;

  DBox *weights;

  XX = df->X; 
  YY = df->Y;
  ZZ = df->Z;

  weights = new_dbox(XX,YY,ZZ);

  clear_dbox(weights);
  clear_deformationfield(rst);

  for (z=BZ+1;z<=EZ-1;z++)
    {
      //      printf("%d\n", z);
    for (y=BY+1;y<=EY-1;y++)
      for (x=BX+1;x<=EX-1;x++)
	{
	  p.X = x + df->point[z][y][x].X;
	  p.Y = y + df->point[z][y][x].Y;
	  p.Z = z + df->point[z][y][x].Z;

	  np.X = x + pdf->point[z][y][x].X;
	  np.Y = y + pdf->point[z][y][x].Y;
	  np.Z = z + pdf->point[z][y][x].Z;

	  lx = (int)floorf(np.X);
	  ly = (int)floorf(np.Y);
	  lz = (int)floorf(np.Z);	  
	  ux = (int)ceilf(np.X);
	  uy = (int)ceilf(np.Y);
	  uz = (int)ceilf(np.Z);	  

	  //	  printf("begin\n");
	  for (nz = lz-1; nz<=uz+1; nz++)
	    for (ny = ly-1; ny<=uy+1; ny++)
	      for (nx = lx-1; nx<=ux+1; nx++)
		{
		  if ((nx>=BX+1)&&(ny>=BY+1)&&(nz>=BZ+1)&&(nx<=EX-1)&&(ny<=EY-1)&&(nz<=EZ-1))
		    {
		      distance = sqrtf((nx-np.X)*(nx-np.X)+(ny-np.Y)*(ny-np.Y)+(nz-np.Z)*(nz-np.Z));
		      w = expf(-1.0 * distance / 1.0);
		      //		      printf("%f  %f\n", distance, w);
		      rst->point[nz][ny][nx].X = w * (p.X-nx);
		      rst->point[nz][ny][nx].Y = w * (p.Y-ny);
		      rst->point[nz][ny][nx].Z = w * (p.Z-nz);
		      weights->box[nz][ny][nx] += w;		      
		    }
		}
	}
    }
  
  write_dbox(weights, "test.img", "float", 1);

  for (z=BZ+1;z<=EZ-1;z++)
    for (y=BY+1;y<=EY-1;y++)
      for (x=BX+1;x<=EX-1;x++)
	if (weights->box[z][y][x]>0.01)
	  {
	    rst->point[z][y][x].X = rst->point[z][y][x].X / weights->box[z][y][x];
	    rst->point[z][y][x].Y = rst->point[z][y][x].Y / weights->box[z][y][x];
	    rst->point[z][y][x].Z = rst->point[z][y][x].Z / weights->box[z][y][x];
	  }
	else
	  {
	    rst->point[z][y][x].X = 0.0;
	    rst->point[z][y][x].Y = 0.0;
	    rst->point[z][y][x].Z = 0.0;
	  }
  
  free_dbox(weights);
}


void minus_deformationfield(Deformation_Field *odf, Deformation_Field *sdf, Deformation_Field *rst)
{
  int x,y,z;
  FPoint fp, p;  

  for (z=0;z<odf->Z;z++)
    for (y=0;y<odf->Y;y++)
      for (x=0;x<odf->X;x++)
	{
	  p.X = x + odf->point[z][y][x].X;
	  p.Y = y + odf->point[z][y][x].Y;
	  p.Z = z + odf->point[z][y][x].Z;

	  fp.X = x + sdf->point[z][y][x].X;
	  fp.Y = y + sdf->point[z][y][x].Y;
	  fp.Z = z + sdf->point[z][y][x].Z;

	  rst->point[z][y][x].X = p.X - fp.X;
	  rst->point[z][y][x].Y = p.Y - fp.Y;
	  rst->point[z][y][x].Z = p.Z - fp.Z;
	}
}


FPoint get_deformation_point(Deformation_Field *df, Point p)
{
  FPoint op;
  op.X = p.X + df->point[p.Z][p.Y][p.X].X;
  op.Y = p.Y + df->point[p.Z][p.Y][p.X].Y;
  op.Z = p.Z + df->point[p.Z][p.Y][p.X].Z;
  return(op);
}

FPoint get_deformation_fpoint(Deformation_Field *df, FPoint p)
{
  int lx,ly,lz;
  int ux,uy,uz;
  float rx,ry,rz;

  FPoint op;

  op.X = 0.0;
  op.Y = 0.0;
  op.Z = 0.0;
  
  if ((p.X<0.0)||(p.Y<0.0)||(p.Z<0.0))
    return(op);

  lx = (int) floorf(p.X);
  ly = (int) floorf(p.Y);
  lz = (int) floorf(p.Z);

  ux = lx + 1;
  uy = ly + 1;
  uz = lz + 1;

  rx = p.X - lx;
  ry = p.Y - ly;
  rz = p.Z - lz;

  if ((lx>=0)&&(ly>=0)&&(lz>=0)&&(ux<df->X)&&(uy<df->Y)&&(uz<df->Z))
    {
      op.X = 
	(1-rz)*((1-ry)*(df->point[lz][ly][lx].X*(1-rx) + df->point[lz][ly][ux].X*(rx)) +
	  	    ry*(df->point[lz][uy][lx].X*(1-rx) + df->point[lz][uy][ux].X*(rx)))
	 +(rz)*((1-ry)*(df->point[uz][ly][lx].X*(1-rx) + df->point[uz][ly][ux].X*(rx)) +
	          ry * (df->point[uz][uy][lx].X*(1-rx) + df->point[uz][uy][ux].X*(rx)));      
       op.Y = 
	df->point[lz][ly][lx].Y*(1-rx)*(1-ry)*(1-rz) + df->point[lz][ly][ux].Y*(rx)*(1-ry)*(1-rz) +
	df->point[uz][ly][lx].Y*(1-rx)*(1-ry)*(rz)   + df->point[uz][ly][ux].Y*(rx)*(1-ry)*(rz) +
	df->point[lz][uy][lx].Y*(1-rx)*(ry)*(1-rz)   + df->point[lz][uy][ux].Y*(rx)*(ry)*(1-rz) +
	df->point[uz][uy][lx].Y*(1-rx)*(ry)*(rz)     + df->point[uz][uy][ux].Y*(rx)*(ry)*(rz);      
       op.Z = 
	df->point[lz][ly][lx].Z*(1-rx)*(1-ry)*(1-rz) + df->point[lz][ly][ux].Z*(rx)*(1-ry)*(1-rz) +
	df->point[uz][ly][lx].Z*(1-rx)*(1-ry)*(rz)   + df->point[uz][ly][ux].Z*(rx)*(1-ry)*(rz) +
	df->point[lz][uy][lx].Z*(1-rx)*(ry)*(1-rz)   + df->point[lz][uy][ux].Z*(rx)*(ry)*(1-rz) +
	df->point[uz][uy][lx].Z*(1-rx)*(ry)*(rz)     + df->point[uz][uy][ux].Z*(rx)*(ry)*(rz);      
       /*
  if ((op.X!=0)&&(op.Y!=0)&&(op.Z!=0))
    {
      printf("I am here\n");
      printf("%3d,%3d,%3d --- %6.3f\n", lz,ly,lx, df->point[lz][ly][lx].X);
      printf("%3d,%3d,%3d --- %6.3f\n", lz,ly,ux, df->point[lz][ly][ux].X);
      printf("%3d,%3d,%3d --- %6.3f\n", lz,uy,lx, df->point[lz][uy][lx].X);
      printf("%3d,%3d,%3d --- %6.3f\n", lz,uy,ux, df->point[lz][uy][ux].X);
      printf("%3d,%3d,%3d --- %6.3f\n", uz,ly,lx, df->point[uz][ly][lx].X);
      printf("%3d,%3d,%3d --- %6.3f\n", uz,ly,ux, df->point[uz][ly][ux].X);
      printf("%3d,%3d,%3d --- %6.3f\n", uz,uy,lx, df->point[uz][uy][lx].X);
      printf("%3d,%3d,%3d --- %6.3f\n", uz,uy,ux, df->point[uz][uy][ux].X);
      printf("\n%4.1f,%4.1f,%4.1f --- %6.3f\n\n", p.X,p.Y,p.Z, op.X);
      getchar();
    }
       */

    }

  return(op);
}


void copy_deformationfield(Deformation_Field *dfa, Deformation_Field *dfb)
{
  int x,y,z;
  for (z=0;z<dfa->Z;z++)
    for (y=0;y<dfa->Y;y++)
      for (x=0;x<dfa->X;x++)
	{
	  dfb->point[z][y][x].X = dfa->point[z][y][x].X;
	  dfb->point[z][y][x].Y = dfa->point[z][y][x].Y;
	  dfb->point[z][y][x].Z = dfa->point[z][y][x].Z;
	}
}

void clear_deformationfield(Deformation_Field *df)
{
  int x,y,z;
  for (z=0;z<df->Z;z++)
    for (y=0;y<df->Y;y++)
      for (x=0;x<df->X;x++)
	{
	  df->point[z][y][x].X = 0.0;
	  df->point[z][y][x].Y = 0.0;
	  df->point[z][y][x].Z = 0.0;
	}
}

void copy_dbox(DBox *dbx1, DBox *dbx2)
{
  int x,y,z;
  for (z=0;z<dbx1->Z;z++)
    for (y=0;y<dbx1->Y;y++)
      for (x=0;x<dbx1->X;x++)
	dbx2->box[z][y][x] = dbx1->box[z][y][x];
}



// Principal Component Analysis (PCA)

// use XZPCA when the number of features is larger than the number of samples.

void XZPCA(Matrix *Samples, Matrix * Phi, Vector * lambda, Vector *AX)
{
  int i,j,s;

  float total, subvalue;
  double *sum;

  Matrix * S, *eigenvector, *D;
  float * eigenvalue;

  S = (Matrix *) calloc (1, sizeof(Matrix));
  new_matrix(S, Samples->width, Samples->width);

  D = (Matrix *) calloc (1, sizeof(Matrix));
  new_matrix(D, Samples->height, Samples->width);

  for (i=0;i<Samples->height;i++)
    {
      AX->value[i] = 0;
      for (j=0;j<Samples->width;j++)
	AX->value[i] += Samples->data[i][j];
      AX->value[i] /= Samples->width;
    }

  for (i=0;i<Samples->height;i++)
    for (j=0;j<Samples->width;j++)
      D->data[i][j] = Samples->data[i][j] - AX->value[i];

  for (i=0;i<S->height;i++)
    for (j=0;j<S->width;j++)
      {
	S->data[i][j] = 0;
	for (s=0;s<Samples->height;s++)
	  S->data[i][j] += D->data[s][i] * D->data[s][j];
	S->data[i][j] /= (Samples->width-1);
      }

  CreateMatrix(&eigenvector,  S->height, S->width);
  eigenvalue = (float *) calloc (S->height, sizeof(float));

  Mat_Calculate_EigenVectors_EigenValues(S, eigenvalue, eigenvector, 0);

  for (i=0;i<D->height;i++)
    for (j=0;j<D->width;j++)
      {
	Phi->data[i][j] = 0;
	for (s=0;s<D->width;s++)
	  Phi->data[i][j] += D->data[i][s] * eigenvector->data[s][j];
      }

  sum = (double *) calloc(D->width, sizeof(double));

  for (j=0;j<D->width;j++)
    {
      sum[j] = 0;
      for (i=0;i<D->height;i++)
	sum[j] += Phi->data[i][j] * Phi->data[i][j];
      sum[j] = sqrtf(sum[j]);
      for (i=0;i<D->height;i++)
	Phi->data[i][j] /= sum[j];
    }

  for (i=0;i<S->height;i++)
    lambda->value[i] = eigenvalue[i];

  FreeMatrix(eigenvector);
  free(eigenvalue);
  free(sum);
  FreeMatrix(S);
  FreeMatrix(D);
}

void XZPCA_small_memory(Matrix *Samples, Matrix * Phi, Vector * lambda, Vector *AX)
{
  int i,j,s;

  float total, subvalue;
  double *sum;

  Matrix * S, *eigenvector;
  float * eigenvalue;

  S = (Matrix *) calloc (1, sizeof(Matrix));
  new_matrix(S, Samples->width, Samples->width);

  for (i=0;i<Samples->height;i++)
    {
      AX->value[i] = 0;
      for (j=0;j<Samples->width;j++)
	AX->value[i] += Samples->data[i][j];
      AX->value[i] /= Samples->width;
    }

  //  printf("calculating corr matrix\n");
  for (i=0;i<S->height;i++)
    {
      for (j=0;j<=i;j++)
	{
	  //	  printf("%3d ", j);
	  S->data[i][j] = 0;
	  for (s=0;s<Samples->height;s++)
	    S->data[i][j] += (Samples->data[s][i] - AX->value[s]) * (Samples->data[s][j] - AX->value[s]);
	  S->data[i][j] /= (Samples->width-1);
	}
      //      printf("\n");
    }

  for (i=0;i<S->height;i++)
    {
      for (j=i+1;j<S->width;j++)
	{
	  //	  printf("%3d ", j);
	  S->data[i][j] = S->data[j][i];
	}
      //      printf("\n");
    }

  //  printf("calculating eigenvector eigenvalues\n");
  eigenvector = (Matrix *) calloc (1, sizeof(Matrix));
  new_matrix(eigenvector, S->height, S->width);
  //  CreateMatrix(&eigenvector,  S->height, S->width);
  eigenvalue = (float *) calloc (S->height, sizeof(float));

  Mat_Calculate_EigenVectors_EigenValues(S, eigenvalue, eigenvector, 0);

  //  printf("normalizing Phi\n");
  for (i=0;i<Samples->height;i++)
    for (j=0;j<Samples->width;j++)
      {
	Phi->data[i][j] = 0;
	for (s=0;s<Samples->width;s++)
	  Phi->data[i][j] += (Samples->data[i][s]-AX->value[i]) * eigenvector->data[s][j];
      }

  sum = (double *) calloc(Samples->width, sizeof(double));

  for (j=0;j<Samples->width;j++)
    {
      sum[j] = 0;
      for (i=0;i<Samples->height;i++)
	sum[j] += Phi->data[i][j] * Phi->data[i][j];
      sum[j] = sqrtf(sum[j]);
      for (i=0;i<Samples->height;i++)
	Phi->data[i][j] /= sum[j];
    }

  for (i=0;i<S->height;i++)
    lambda->value[i] = eigenvalue[i];

  FreeMatrix(eigenvector);
  free(eigenvalue);
  free(sum);
  FreeMatrix(S);
}

// if number of samples is larger than number of features.
// i.e. matrix Samples' height is smaller than width.
// normal PCA

void XZPCA1(Matrix *Samples, Matrix * Phi, Vector * lambda, Vector *AX)
{
  int i,j,s;

  float total, subvalue;
  double *sum;

  Matrix * S, *eigenvector, *D;
  float * eigenvalue;

  if (Samples->width<2)
    {
      printf("you have too small samples\n");
      exit(0);
    }

  S = (Matrix *) calloc (1, sizeof(Matrix));
  new_matrix(S, Samples->height, Samples->height);

  D = (Matrix *) calloc (1, sizeof(Matrix));
  new_matrix(D, Samples->height, Samples->width);

  for (i=0;i<Samples->height;i++)
    {
      AX->value[i] = 0;
      for (j=0;j<Samples->width;j++)
	AX->value[i] += Samples->data[i][j];
      AX->value[i] /= Samples->width;
    }

  for (i=0;i<Samples->height;i++)
    for (j=0;j<Samples->width;j++)
      D->data[i][j] = Samples->data[i][j] - AX->value[i];

  for (i=0;i<S->height;i++)
    for (j=0;j<S->width;j++)
      {
	S->data[i][j] = 0;
	for (s=0;s<D->width;s++)
	  S->data[i][j] += D->data[i][s] * D->data[j][s];
	S->data[i][j] /= (D->width-1);
      }

  CreateMatrix(&eigenvector,  S->height, S->width);
  eigenvalue = (float *) calloc (S->height, sizeof(float));

  //  printf("calculating eigenvector, eigenvalues\n");

  Mat_Calculate_EigenVectors_EigenValues(S, eigenvalue, eigenvector, 0);

  for (i=0;i<eigenvector->height;i++)
    for(j=0;j<eigenvector->width;j++)
      Phi->data[i][j] = eigenvector->data[i][j];

  for (i=0;i<S->height;i++)
    lambda->value[i] = eigenvalue[i];

  FreeMatrix(eigenvector);
  free(eigenvalue);
  FreeMatrix(S);
  FreeMatrix(D);
}

int clip(Vector * lambda, double rate)
{
  double total;
  int i,s;
  double subtotal;

  total = 0.0;
  for (i=0;i<lambda->size;i++)
    total += sqrtf(lambda->value[i]);

  subtotal = 0.0;

  //  printf("total = %f\n", total);

  s = 0;
  while (subtotal/total<rate)
    {
      subtotal += sqrtf(lambda->value[s]);
      s++;
      if (s==lambda->size)
	break;
      //     printf("subtotal = %f\n", subtotal);
    }
  //  printf("s = %d\n",s);

  return(s);
}

// input sample matrix, output phi, lambda, and AX.
int return_small(int x, int y)
{
  if (x>y)
    return(y);
  else
    return(x);
}

void PrincipalComponentAnalysis(Matrix *Samples, Matrix *Phi, Vector *lambda, Vector *AX)
{  
  int small, big;

  //  printf("Now enter PCA\n");

  AX->value = (double *) calloc (Samples->height, sizeof(double));

  AX->size = Samples->height;

  //  printf("AX size alloc\n");

  if (Samples->height > Samples->width)
    {
      //      printf("I am using XZPCA\n");
      new_matrix(Phi, Samples->height, Samples->width);
      lambda->value= (double *) calloc (Samples->width, sizeof(double));
      lambda->size = Samples->width;
      XZPCA_small_memory(Samples, Phi, lambda, AX);
    }
  else
    {
      //      printf("I am using XZPCA1\n");
      new_matrix(Phi, Samples->height, Samples->height);
      lambda->value= (double *) calloc (Samples->height, sizeof(double));
      lambda->size = Samples->height;
      XZPCA1(Samples, Phi, lambda, AX);
    }
}

void PrincipalComponentAnalysis_NoNewMemory(Matrix *Samples, Matrix *Phi, Vector *lambda, Vector *AX)
{  
  int small, big;

  //  printf("Now enter PCA\n");

  //  printf("AX size alloc\n");

  if (Samples->height > Samples->width)
    {
      XZPCA_small_memory(Samples, Phi, lambda, AX);
    }
  else
    {
      XZPCA1(Samples, Phi, lambda, AX);
    }
}


void free_vector(Vector *vec)
{
  free(vec->value);
  free(vec);
}

void display_vector(Vector *vec)
{
  int i;
  for (i=0;i<vec->size;i++)
    printf("%10.6f\n", vec->value[i]);
}

Vector * new_vector(int N)
{
  Vector *tmp;
  tmp = (Vector *) calloc (1, sizeof(Vector));
  if (tmp == NULL)
    {
      printf("mem error in new_vector()\n");
      exit(0);
    }
  tmp->value = (double *) calloc (N, sizeof(double));
  if (tmp->value == NULL)
    {
      printf("mem error in new_vector()\n");
      exit(0);
    }
  tmp->size = N;
  return(tmp);
}

Deformation_Field * inverse_deformationfield(Deformation_Field *df)
{
  Deformation_Field *idf;
  int x,y,z,i,j,k;
  FPoint fp, sp;
  Point dp;
  DBox *multiple;  
  int number;

  float rate = 3.0;
  int intrate = 3;

  printf("begin inverse deformationfield\n");

  idf = new_deformationfield(df->X, df->Y, df->Z);
  multiple = new_dbox(df->X, df->Y, df->Z);

  for (z=0;z<multiple->Z;z++)
    for (y=0;y<multiple->Y;y++)
      for (x=0;x<multiple->X;x++)
	multiple->box[z][y][x] = 0.0;

  for (z=0;z<df->Z;z++)
    {
      //    printf("z = %d\n",z);
    for (y=0;y<df->Y;y++)
      for (x=0;x<df->X;x++)
	{
	  for (k=0;k<intrate;k++)
	    for (j=0;j<intrate;j++)
	      for (i=0;i<intrate;i++)
		{
		  fp.X = x + i/rate;
		  fp.Y = y + j/rate;
		  fp.Z = z + k/rate;
		  
		  sp = get_deformation_fpoint(df, fp);
		  dp.X = (int) floorf(sp.X + .5 + fp.X);
		  dp.Y = (int) floorf(sp.Y + .5 + fp.Y);
		  dp.Z = (int) floorf(sp.Z + .5 + fp.Z);		  

		  //		  printf("%d %d %d --- %d %d %d\n", dp.X, dp.Y, dp.Z, (int) fp.X, (int) fp.Y, (int)fp.Z);

		  if ((dp.X>=0)&&(dp.Y>=0)&&(dp.Z>=0)&&(dp.X<df->X)&&(dp.Y<df->Y)&&(dp.Z<df->Z))
		    {
		      multiple->box[dp.Z][dp.Y][dp.X] += 1.0;
		      idf->point[dp.Z][dp.Y][dp.X].X += 0 - sp.X;
		      idf->point[dp.Z][dp.Y][dp.X].Y += 0 - sp.Y;
		      idf->point[dp.Z][dp.Y][dp.X].Z += 0 - sp.Z;
		    }
		}
	}
    }

  number = 0;
  for (z=0;z<idf->Z;z++)
    for (y=0;y<idf->Y;y++)
      for (x=0;x<idf->X;x++)
	if (multiple->box[z][y][x]>0.0)
	  {
	    number ++;
	    if (multiple->box[z][y][x]>0.0)
	      {
		idf->point[z][y][x].X /= multiple->box[z][y][x];
		idf->point[z][y][x].Y /= multiple->box[z][y][x];
		idf->point[z][y][x].Z /= multiple->box[z][y][x];	    
	      }
	  }

  printf("number = %d\n", number);

  free_dbox(multiple);

  //  Smooth_DeformationField(idf, 0.5);

  return(idf);
}
// get a smaller tvol

void get_a_smaller_size(DBox *img, int *oBX, int *oEX, int *oBY, int *oEY, int *oBZ, int *oEZ)
{
  int BX,BY,BZ,EX,EY,EZ;
  int XX,YY,ZZ;
  int number;
  int x,y,z;

  XX = img->X;
  YY = img->Y;
  ZZ = img->Z;

  BX = 0;
  BY = 0;
  BZ = 0;
  EX = XX-1;
  EY = YY-1;
  EZ = ZZ-1;

  number = 0;
  while (number==0)
    {
      for (y=0;y<img->Y;y++)
	for (x=0;x<img->X;x++)
	  if (img->box[BZ][y][x]!=0)
	    {
	      number ++;
	    }
      BZ++;
    }
  BZ = BZ-5;
  if (BZ<0)
    BZ = 0;

  number = 0;
  while (number==0)
    {
      for (z=0;z<img->Z;z++)
	for (x=0;x<img->X;x++)
	  if (img->box[z][BY][x]!=0)
	    {
	      number ++;
	    }
      BY++;
    }
  BY = BY-5;
  if (BY<0)
    BY = 0;

  number = 0;
  while (number==0)
    {
      for (y=0;y<img->Y;y++)
	for (z=0;z<img->Z;z++)
	  if (img->box[z][y][BX]!=0)
	    {
	      number ++;
	    }
      BX++;
    }
  BX = BX-5;
  if (BX<0)
    BX = 0;
  
  number = 0;
  while (number==0)
    {
      for (y=0;y<img->Y;y++)
	for (x=0;x<img->X;x++)
	  if (img->box[EZ][y][x]!=0)
	    {
	      number ++;
	    }
      EZ--;
    }
  EZ = EZ+5;
  if (EZ>=ZZ)
    EZ = ZZ-1;

  number = 0;
  while (number==0)
    {
      for (z=0;z<img->Z;z++)
	for (x=0;x<img->X;x++)
	  if (img->box[z][EY][x]!=0)
	    {
	      number ++;
	    }
      EY--;
    }
  EY = EY+5;
  if (EY>=YY)
    EY = YY-1;

  number = 0;
  while (number==0)
    {
      for (y=0;y<img->Y;y++)
	for (z=0;z<img->Z;z++)
	  if (img->box[z][y][EX]!=0)
	    {
	      number ++;
	    }
      EX--;
    }
  EX = EX+5;
  if (EX>=XX)
    EX = XX-1;

  *oBX = BX;
  *oBY = BY;
  *oBZ = BZ;
  *oEX = EX;
  *oEY = EY;
  *oEZ = EZ;
  
  //  printf("new image size is %d-%d, %d-%d, %d-%d\n", BX,EX,BY,EY,BZ,EZ);
}

void get_a_smaller_size_bbox(BBox *img, int *oBX, int *oEX, int *oBY, int *oEY, int *oBZ, int *oEZ)
{
  int BX,BY,BZ,EX,EY,EZ;
  int XX,YY,ZZ;
  int number;
  int x,y,z;

  XX = img->X;
  YY = img->Y;
  ZZ = img->Z;

  BX = 0;
  BY = 0;
  BZ = 0;
  EX = XX-1;
  EY = YY-1;
  EZ = ZZ-1;

  number = 0;
  while (number==0)
    {
      for (y=0;y<img->Y;y++)
	for (x=0;x<img->X;x++)
	  if (img->box[BZ][y][x]!=0)
	    {
	      number ++;
	    }
      BZ++;
    }
  BZ = BZ-5;
  if (BZ<0)
    BZ = 0;

  number = 0;
  while (number==0)
    {
      for (z=0;z<img->Z;z++)
	for (x=0;x<img->X;x++)
	  if (img->box[z][BY][x]!=0)
	    {
	      number ++;
	    }
      BY++;
    }
  BY = BY-3;
  if (BY<0)
    BY = 0;

  number = 0;
  while (number==0)
    {
      for (y=0;y<img->Y;y++)
	for (z=0;z<img->Z;z++)
	  if (img->box[z][y][BX]!=0)
	    {
	      number ++;
	    }
      BX++;
    }
  BX = BX-5;
  if (BX<0)
    BX = 0;
  
  number = 0;
  while (number==0)
    {
      for (y=0;y<img->Y;y++)
	for (x=0;x<img->X;x++)
	  if (img->box[EZ][y][x]!=0)
	    {
	      number ++;
	    }
      EZ--;
    }
  EZ = EZ+5;
  if (EZ>=ZZ)
    EZ = ZZ-1;

  number = 0;
  while (number==0)
    {
      for (z=0;z<img->Z;z++)
	for (x=0;x<img->X;x++)
	  if (img->box[z][EY][x]!=0)
	    {
	      number ++;
	    }
      EY--;
    }
  EY = EY+5;
  if (EY>=YY)
    EY = YY-1;

  number = 0;
  while (number==0)
    {
      for (y=0;y<img->Y;y++)
	for (z=0;z<img->Z;z++)
	  if (img->box[z][y][EX]!=0)
	    {
	      number ++;
	    }
      EX--;
    }
  EX = EX+5;
  if (EX>=XX)
    EX = XX-1;

  *oBX = BX;
  *oBY = BY;
  *oBZ = BZ;
  *oEX = EX;
  *oEY = EY;
  *oEZ = EZ;
  
  //  printf("new image size is %d-%d, %d-%d, %d-%d\n", BX,EX,BY,EY,BZ,EZ);
}

void get_a_smaller_size_bbox_samexysize(BBox *img, int *oBX, int *oEX, int *oBY, int *oEY, int *oBZ, int *oEZ)
{
  int BX,BY,BZ,EX,EY,EZ;
  int XX,YY,ZZ;
  int nXX,nYY,nZZ;
  int dX,dY;
  int number;
  int x,y,z;

  XX = img->X;
  YY = img->Y;
  ZZ = img->Z;

  BX = 0;
  BY = 0;
  BZ = 0;
  EX = XX-1;
  EY = YY-1;
  EZ = ZZ-1;

  number = 0;
  while (number==0)
    {
      for (y=0;y<img->Y;y++)
	for (x=0;x<img->X;x++)
	  if (img->box[BZ][y][x]!=0)
	    {
	      number ++;
	    }
      BZ++;
    }
  BZ = BZ-5;
  if (BZ<0)
    BZ = 0;

  number = 0;
  while (number==0)
    {
      for (z=0;z<img->Z;z++)
	for (x=0;x<img->X;x++)
	  if (img->box[z][BY][x]!=0)
	    {
	      number ++;
	    }
      BY++;
    }
  BY = BY-5;
  if (BY<0)
    BY = 0;

  number = 0;
  while (number==0)
    {
      for (y=0;y<img->Y;y++)
	for (z=0;z<img->Z;z++)
	  if (img->box[z][y][BX]!=0)
	    {
	      number ++;
	    }
      BX++;
    }
  BX = BX-5;
  if (BX<0)
    BX = 0;
  
  number = 0;
  while (number==0)
    {
      for (y=0;y<img->Y;y++)
	for (x=0;x<img->X;x++)
	  if (img->box[EZ][y][x]!=0)
	    {
	      number ++;
	    }
      EZ--;
    }
  EZ = EZ+5;
  if (EZ>=ZZ)
    EZ = ZZ-1;

  number = 0;
  while (number==0)
    {
      for (z=0;z<img->Z;z++)
	for (x=0;x<img->X;x++)
	  if (img->box[z][EY][x]!=0)
	    {
	      number ++;
	    }
      EY--;
    }
  EY = EY+5;
  if (EY>=YY)
    EY = YY-1;

  number = 0;
  while (number==0)
    {
      for (y=0;y<img->Y;y++)
	for (z=0;z<img->Z;z++)
	  if (img->box[z][y][EX]!=0)
	    {
	      number ++;
	    }
      EX--;
    }
  EX = EX+5;
  if (EX>=XX)
    EX = XX-1;

  nXX = EX - BX +1;
  nYY = EY - BY +1;

  if (nXX<nYY)
    {
      nZZ = nYY-nXX;
      nZZ = nZZ/2;
      BX = BX - nZZ;
      EX = BX + nYY - 1;
    }
  else
    {
      nZZ = nXX-nYY;
      nZZ = nZZ/2;
      BY = BY-nZZ;
      EY = BY + nXX - 1;
    }

  *oBX = BX;
  *oBY = BY;
  *oBZ = BZ;
  *oEX = EX;
  *oEY = EY;
  *oEZ = EZ;
  
  //  printf("new image size is %d-%d, %d-%d, %d-%d\n", BX,EX,BY,EY,BZ,EZ);
}


float image_similarity(DBox *mdl, DBox *warped_sub)
{
  int x,y,z;
  int number;
  float similarity;
  similarity = 0.0;
  for (z=0;z<mdl->Z;z++)
    for (y=0;y<mdl->Y;y++)
      for (x=0;x<mdl->X;x++)
	{
	  if ((mdl->box[z][y][x]>0.0)||(warped_sub->box[z][y][x]>0.0))
	    {
	      similarity += fabsf(mdl->box[z][y][x] - warped_sub->box[z][y][x]);
	      number ++;
	    }
	}

  similarity = similarity / number;

  return(similarity);
}


Short_Deformation_Field * new_short_deformationfield(int X,int Y,int Z)
{
  Short_Deformation_Field * sdf;
  int x,y,z;
  sdf = (Short_Deformation_Field *) calloc (1, sizeof(Short_Deformation_Field));
  sdf->X = X;
  sdf->Y = Y;
  sdf->Z = Z;

  sdf->point = (ShortPoint ***) calloc (Z, sizeof(ShortPoint **));
  if (sdf->point==NULL)
    no_mem_exit("new_deformationfield");    

  for (z=0;z<Z;z++)
    if ((sdf->point[z] = (ShortPoint **) calloc (Y, sizeof(ShortPoint *)))==NULL)
      no_mem_exit("new_deformationfield");    

  for (z = 0;z<Z;z++)
    for (y=0;y<Y;y++)
      if ((sdf->point[z][y] = (ShortPoint *) calloc (X, sizeof(ShortPoint)))==NULL)
	no_mem_exit("new_deformationfield");    

  return(sdf);
}

void free_short_deformationfield(Short_Deformation_Field * df)
{
  int x,y,z;
  for(z=0;z<df->Z;z++)
    for(y =0;y<df->Y;y++)
      free(df->point[z][y]);

  for(z=0;z<df->Z;z++)
    free(df->point[z]);

  free(df->point);

  free(df);
}

void read_short_deformationfield_with_switch(Short_Deformation_Field *df, char *filename, char *format, int swch)
{
  int z,y,x;
  FILE *fp;

  FPoint zero;
  ShortPoint szero;
  short tempshort;

  zero.X = 0.0;
  zero.Y = 0.0;
  zero.Z = 0.0;
  szero.X = 0;
  szero.Y = 0;
  szero.Z = 0;

  printf("reading file:%s\n", filename);

  fp = fopen(filename, "rb");
  if (fp==NULL)
    {
      printf("file open error in read_deformationfield\n");
      exit(0);
    }

  if (!strcmp(format, "float"))
    for(z=0;z<df->Z;z++)
      for(y =0;y<df->Y;y++)
	for (x=0;x<df->X;x++)
	  {
	    fread(&zero, sizeof(FPoint), 1, fp);
	    
	    df->point[z][y][x].X = (short) floorf(zero.X + 0.5);
	    df->point[z][y][x].Y = (short) floorf(zero.Y + 0.5);
	    df->point[z][y][x].Z = (short) floorf(zero.Z + 0.5);
	  }
  else  if (!strcmp(format, "byte"))
    for(z=0;z<df->Z;z++)
      for(y =0;y<df->Y;y++)
	for (x=0;x<df->X;x++)
	  {
	    fread(&szero, sizeof(ShortPoint), 1, fp);
	    
	    df->point[z][y][x].X = szero.X;
	    df->point[z][y][x].Y = szero.Y;
	    df->point[z][y][x].Z = szero.Z;
	  }
  else
    {
      printf("wrong option in function read_short_deformationfield\n");
      exit(0);
    }
  
  fclose(fp);

  if (swch)
    {
      for(z=0;z<df->Z;z++)
	for(y =0;y<df->Y;y++)
	  for (x=0;x<df->X;x++)
	    {
	      tempshort = df->point[z][y][x].X;
	      df->point[z][y][x].X = df->point[z][y][x].Y;
	      df->point[z][y][x].Y = tempshort;
	    }
    }
}

void read_short_deformationfield_with_switch_and_scale(Short_Deformation_Field *df, char *filename, char *format, int swch, float scale)
{
  int z,y,x;
  FILE *fp;

  FPoint zero;
  ShortPoint szero;
  short tempshort;

  zero.X = 0.0;
  zero.Y = 0.0;
  zero.Z = 0.0;
  szero.X = 0;
  szero.Y = 0;
  szero.Z = 0;

  printf("reading file:%s\n", filename);

  fp = fopen(filename, "rb");
  if (fp==NULL)
    {
      printf("file open error in read_deformationfield\n");
      exit(0);
    }

  if (!strcmp(format, "float"))
    for(z=0;z<df->Z;z++)
      for(y =0;y<df->Y;y++)
	for (x=0;x<df->X;x++)
	  {
	    fread(&zero, sizeof(FPoint), 1, fp);
	    
	    df->point[z][y][x].X = (short) floorf(zero.X * scale + 0.5);
	    df->point[z][y][x].Y = (short) floorf(zero.Y * scale + 0.5);
	    df->point[z][y][x].Z = (short) floorf(zero.Z * scale + 0.5);
	  }
  else  if (!strcmp(format, "byte"))
    for(z=0;z<df->Z;z++)
      for(y =0;y<df->Y;y++)
	for (x=0;x<df->X;x++)
	  {
	    fread(&szero, sizeof(ShortPoint), 1, fp);
	    
	    df->point[z][y][x].X = (short) (szero.X * scale);
	    df->point[z][y][x].Y = (short) (szero.Y * scale);
	    df->point[z][y][x].Z = (short) (szero.Z * scale);
	  }
  else
    {
      printf("wrong option in function read_short_deformationfield\n");
      exit(0);
    }
  
  fclose(fp);

  if (swch)
    {
      for(z=0;z<df->Z;z++)
	for(y =0;y<df->Y;y++)
	  for (x=0;x<df->X;x++)
	    {
	      tempshort = df->point[z][y][x].X;
	      df->point[z][y][x].X = df->point[z][y][x].Y;
	      df->point[z][y][x].Y = tempshort;
	    }
    }
}

void Calculate_Jakobian(Deformation_Field *df, DBox *jcb)
{
  int x,y,z,i;
  Point OU, U;
  FPoint cur;
  float result[9], ok;
  float max_ok, min_ok;

  int ux,lx, uy,ly,uz,lz;

  float fxx, fxy, fxz, fyx, fyy, fyz, fzx, fzy, fzz;

  // calculate gradient;

  for (z=0;z<df->Z;z++)
    for (y=0;y<df->Y;y++)
      for (x=0;x<df->X;x++)
	{
	  df->point[z][y][x].X += x;
	  df->point[z][y][x].Y += y;
	  df->point[z][y][x].Z += z;
	}

  max_ok = 0;
  min_ok = 1;
  for (z=0;z<df->Z;z++)
    for (y=0;y<df->Y;y++)
      for (x=0;x<df->X;x++)
	{
	  ux = x+1;
	  uy = y+1;
	  uz = z+1;
	  lx = x-1;
	  ly = y-1;
	  lz = z-1;
	  
	  if (ux>=df->X) ux = df->X-1;
	  if (uy>=df->Y) uy = df->Y-1;
	  if (uz>=df->Z) uz = df->Z-1;
	  if (lx<0) lx = 0;
	  if (ly<0) ly = 0;
	  if (lz<0) lz = 0;	  	  fxx = (df->point[z][y][ux].X - df->point[z][y][lx].X)/(ux - lx);
	  fxy = (df->point[z][uy][x].X - df->point[z][ly][x].X)/(uy - ly);
	  fxz = (df->point[uz][y][x].X - df->point[lz][y][x].X)/(uz - lz);


	  fxx = (df->point[z][y][ux].X - df->point[z][y][lx].X)/(ux - lx);
	  fxy = (df->point[z][uy][x].X - df->point[z][ly][x].X)/(uy - ly);
	  fxz = (df->point[uz][y][x].X - df->point[lz][y][x].X)/(uz - lz);

	  fyx = (df->point[z][y][ux].Y - df->point[z][y][lx].Y)/(ux - lx);
	  fyy = (df->point[z][uy][x].Y - df->point[z][ly][x].Y)/(uy - ly);
	  fyz = (df->point[uz][y][x].Y - df->point[lz][y][x].Y)/(uz - lz);

	  fzx = (df->point[z][y][ux].Z - df->point[z][y][lx].Z)/(ux - lx);
	  fzy = (df->point[z][uy][x].Z - df->point[z][ly][x].Z)/(uy - ly);
	  fzz = (df->point[uz][y][x].Z - df->point[lz][y][x].Z)/(uz - lz);

	  jcb->box[z][y][x] = fxx * fyy * fzz + fxy * fyz * fzx + fyx * fzy * fxz - fxz*fyy*fzx - fxy*fyx*fzz - fxx*fyz*fzy;
	  if (max_ok<jcb->box[z][y][x]) max_ok=jcb->box[z][y][x];
	  if (min_ok>jcb->box[z][y][x]) min_ok=jcb->box[z][y][x];
	}

  for (z=0;z<df->Z;z++)
    for (y=0;y<df->Y;y++)
      for (x=0;x<df->X;x++)
	{
	  df->point[z][y][x].X -= x;
	  df->point[z][y][x].Y -= y;
	  df->point[z][y][x].Z -= z;
	}

  printf("max is %d\nmin is %d\n", (int)max_ok, (int)min_ok);
}


Signal * fcm(float *histo, int Mx)
{
  Matrix *U;
  float *center;
  float *obfun;

  Signal *ct;

  int kk,x,k,m,C,KK,i;
  float pp, upper, lower, q, ok,temp;

  q = 2;
  C = 4;
  KK = 30;

  U = (Matrix *) calloc(1, sizeof(Matrix));
  new_matrix(U,Mx,C);

  center = (float *) calloc( C, sizeof(float));

  for (k=0;k<C;k++)
    {
      center[k] = Mx/C*k+10;
    }

  obfun = (float *) calloc( KK, sizeof(float));

  for (kk=0;kk<KK;kk++)
    {
      for (x=0;x<Mx;x++)
	{
	  for (k=0;k<C;k++)
	    {
	      pp = 1.0/(q-1);
	      upper = 0.0;
	      upper = powf((x-center[k])*(x-center[k]), pp);
	      if (upper!=0)
		upper = 1/upper;
	      else
		upper = 10000;
	      
	      lower = 0.0;
	      for (m=0;m<C;m++)
		{
		  ok = powf((x-center[m])*(x-center[m]), pp);
		  if (ok==0)
		    ok = 10000;
		  else
		    ok = 1/ok;
		  lower += ok;
		}
	      
	      U->data[x][k] = upper / lower;
	    }
	}
      
      for (k=0;k<C;k++)
	{
	  upper = 0;
	  lower = 0;
	  for (x=0;x<Mx;x++)
	    {
	      upper += powf(U->data[x][k], q)*x*histo[x];
	      lower += powf(U->data[x][k], q)*histo[x];
	    }

	  center[k] = upper / lower;	  
	}
      
      //calculating objectvie function
      
      obfun[kk] = 0.0;
      for (x=0;x<Mx;x++)
	for (k=0;k<C;k++)
	  {
	    obfun[kk] += powf(U->data[x][k], q)*(x-center[k])*(x-center[k])*histo[x];
	  }
      
      if (kk>1)
	if (fabsf(obfun[kk]-obfun[kk-1])/obfun[kk-1]<0.00001)
	  break;
      
    }
  
  // rearrange center

  for (k=0;k<C-1;k++)
    for (m=k+1;m<C;m++)
      {
	if (center[k]>center[m])
	  {
	    temp = center[k];
	    center[k] = center[m];
	    center[m] = temp;
	  }
      }

  ct = new_signal(3);
  for (i=1;i<4;i++)
    ct->sig[i-1] = center[i];

  FreeMatrix(U);
  free(center);
  free(obfun);

  return(ct);
}

Signal * fcm_center_no(float *histo, int Mx, int C)
{
  Matrix *U;
  float *center;
  float *obfun;

  Signal *ct;
  int lastm;
  int kk,x,k,m,KK,i;
  float pp, upper, lower, q, ok,temp;

  q = 2;

  KK = 30;

  U = (Matrix *) calloc(1, sizeof(Matrix));
  new_matrix(U,Mx,C);

  center = (float *) calloc( C, sizeof(float));

  for (k=0;k<C;k++)
    {
      center[k] = Mx/C*k+10;
    }

  obfun = (float *) calloc( KK, sizeof(float));

  for (kk=0;kk<KK;kk++)
    {
      for (x=0;x<Mx;x++)
	{
	  for (k=0;k<C;k++)
	    {
	      pp = 1.0/(q-1);
	      upper = 0.0;
	      upper = powf((x-center[k])*(x-center[k]), pp);
	      if (upper!=0)
		upper = 1/upper;
	      else
		upper = 10000;
	      
	      lower = 0.0;
	      for (m=0;m<C;m++)
		{
		  ok = powf((x-center[m])*(x-center[m]), pp);
		  if (ok==0)
		    ok = 10000;
		  else
		    ok = 1/ok;
		  lower += ok;
		}
	      
	      U->data[x][k] = upper / lower;
	    }
	}
      
      for (k=0;k<C;k++)
	{
	  upper = 0;
	  lower = 0;
	  for (x=0;x<Mx;x++)
	    {
	      upper += powf(U->data[x][k], q)*x*histo[x];
	      lower += powf(U->data[x][k], q)*histo[x];
	    }

	  center[k] = upper / lower;	  
	}
      
      //calculating objectvie function
      
      obfun[kk] = 0.0;
      for (x=0;x<Mx;x++)
	for (k=0;k<C;k++)
	  {
	    obfun[kk] += powf(U->data[x][k], q)*(x-center[k])*(x-center[k])*histo[x];
	  }
      
      if (kk>1)
	if (fabsf(obfun[kk]-obfun[kk-1])/obfun[kk-1]<0.00001)
	  break;
      

      for (k=0;k<C-1;k++)
	for (m=k+1;m<C;m++)
	  {
	    if (center[k]>center[m])
	      {
		temp = center[k];
		center[k] = center[m];
		center[m] = temp;
	      }
	  }
    }
  
  // rearrange center

  lastm = 0;
  for (x=0;x<Mx;x++)
    {
      temp = U->data[x][0];
      m = 0;
      for (i=1;i<C;i++)
	{
	  //	  printf("%f ", U->data[x][i]);
	  if (U->data[x][i]>temp)
	    {
	      m = i;
	      temp = U->data[x][i];
	    }
	}

      /*
      switch(m)
	{
	case 0:
	  printf("0.3");
	  break;
	case 1:
	  printf("0.6");
	  break;
	case 2:
	  printf("1.0");
	  break;
	default:
	  break;
	}
      printf("\n");
      */

      if (m!=lastm)
	{
	  printf("x = %d\n", x);
	  lastm = m;
	}
      
    }


  ct = new_signal(C);
  for (i=0;i<C;i++)
    ct->sig[i] = center[i];

  FreeMatrix(U);
  free(center);
  free(obfun);

  return(ct);
}

Signal * fcm_center_no_prior(float *histo, int Mx, int C, float *prior, float lambda)
{
  Matrix *U;
  float *center;
  float *obfun;

  Signal *ct;
  int lastm;
  int kk,x,k,m,KK,i;
  float pp, upper, lower, q, ok,temp;

  q = 2;

  KK = 30;

  U = (Matrix *) calloc(1, sizeof(Matrix));
  new_matrix(U,Mx,C);

  center = (float *) calloc( C, sizeof(float));

  for (k=0;k<C;k++)
    {
      center[k] = Mx/C*k+10;
    }

  obfun = (float *) calloc( KK, sizeof(float));

  for (kk=0;kk<KK;kk++)
    {
      for (x=0;x<Mx;x++)
	{
	  for (k=0;k<C;k++)
	    {
	      pp = 1.0/(q-1);
	      upper = 0.0;
	      upper = powf((x-center[k])*(x-center[k]), pp) + lambda * (1- powf(prior[k], q))/Mx;
	      if (upper!=0)
		upper = 1/upper;
	      else
		upper = 10000;
	      
	      lower = 0.0;
	      for (m=0;m<C;m++)
		{
		  ok = powf((x-center[m])*(x-center[m]), pp) + lambda * (1- powf(prior[m], q))/Mx;
		  if (ok==0)
		    ok = 10000;
		  else
		    ok = 1/ok;
		  lower += ok;
		}
	      
	      U->data[x][k] = upper / lower;
	    }
	}
      
      for (k=0;k<C;k++)
	{
	  upper = 0;
	  lower = 0;
	  for (x=0;x<Mx;x++)
	    {
	      upper += powf(U->data[x][k], q)*x*histo[x];
	      lower += powf(U->data[x][k], q)*histo[x];
	    }

	  center[k] = upper / lower;	  
	}
      
      //calculating objectvie function
      
      obfun[kk] = 0.0;
      for (x=0;x<Mx;x++)
	for (k=0;k<C;k++)
	  {
	    obfun[kk] += powf(U->data[x][k], q)*(x-center[k])*(x-center[k])*histo[x];
	  }
      
      if (kk>1)
	if (fabsf(obfun[kk]-obfun[kk-1])/obfun[kk-1]<0.00001)
	  break;
      
        // rearrange center

      for (k=0;k<C-1;k++)
	for (m=k+1;m<C;m++)
	  {
	    if (center[k]>center[m])
	      {
		temp = center[k];
		center[k] = center[m];
		center[m] = temp;
	      }
	  }
    }
  
  ct = new_signal(C);
  for (i=0;i<C;i++)
    ct->sig[i] = center[i];

  lastm = 0;
  for (x=0;x<Mx;x++)
    {
      temp = U->data[x][0];
      m = 0;
      for (i=1;i<C;i++)
	{
	  //	  printf("%f ", U->data[x][i]);
	  if (U->data[x][i]>temp)
	    {
	      m = i;
	      temp = U->data[x][i];
	    }
	}

      /*
      switch(m)
	{
	case 0:
	  printf("0.3");
	  break;
	case 1:
	  printf("0.6");
	  break;
	case 2:
	  printf("1.0");
	  break;
	default:
	  break;
	}
      printf("\n");
      */
      if (m!=lastm)
	{
	  printf("x = %d\n", x);
	  lastm = m;
	}
    }

  FreeMatrix(U);
  free(center);
  free(obfun);

  //  display_signal(ct);

  return(ct);
}

void read_small_bbox(BBox *smbox, char *filename, int BX, int EX, int BY, int EY, int BZ, int EZ, int XX, int YY, int ZZ)
{
  int x,y,z;
  BBox *tmp;

  tmp = new_bbox(XX,YY,ZZ);
  read_bbox(tmp, filename);

   for (z=BZ;z<=EZ;z++)
     for (y=BY;y<=EY;y++)
       for (x=BX;x<=EX;x++)
	 {
	   if ((x>=0)&&(y>=0)&&(z>=0)&&(x<XX)&&(y<YY)&&(z<ZZ))
	     smbox->box[z-BZ][y-BY][x-BX] = tmp->box[z][y][x];
	   else
	     smbox->box[z-BZ][y-BY][x-BX] = 0;
	 }
  
  free_bbox(tmp);
}

void read_small_dbox(DBox *smbox, char *filename, int BX, int EX, int BY, int EY, int BZ, int EZ, int XX, int YY, int ZZ)
{
  int x,y,z;
  DBox *tmp;

  tmp = new_dbox(XX,YY,ZZ);
  read_dbox(tmp, filename, "byte");

   for (z=BZ;z<=EZ;z++)
     for (y=BY;y<=EY;y++)
       for (x=BX;x<=EX;x++)
	 {
	   if ((x>=0)&&(y>=0)&&(z>=0)&&(x<XX)&&(y<YY)&&(z<ZZ))
	     smbox->box[z-BZ][y-BY][x-BX] = tmp->box[z][y][x];
	   else
	     smbox->box[z-BZ][y-BY][x-BX] = 0;
	 }
  
  free_dbox(tmp);
}

void read_small_deformationfield(Deformation_Field *smdf, char *filename, int BX, int EX, int BY, int EY, int BZ, int EZ, int XX, int YY, int ZZ)
{
  int x,y,z;
  Deformation_Field *df;

  df = new_deformationfield(XX,YY,ZZ);
  read_deformationfield(df, filename);

  for (z=BZ;z<=EZ;z++)
    for (y=BY;y<=EY;y++)
      for (x=BX;x<=EX;x++)
	{
	  smdf->point[z-BZ][y-BY][x-BX].X = df->point[z][y][x].X;
	  smdf->point[z-BZ][y-BY][x-BX].Y = df->point[z][y][x].Y;
	  smdf->point[z-BZ][y-BY][x-BX].Z = df->point[z][y][x].Z;
	}
  
  free_deformationfield(df);     
}




void xz_randomize()
{
  srand (time (NULL));
}

void file_open_error(char *filename)
{
  printf("file open error : %s\n", filename);
  exit(0);
}

void Markov_Smoothing(Deformation_Field *tdf, Deformation_Field *tndf, float lambda, int Nsize)
{
  int x,y,z;
  float Jkx, Jky, Jkz;
  float Jx, Jy, Jz;
  float dx, dy, dz;
  float f;
  int K;
  int nx,ny,nz;
  int xx,yy,zz;
  int NUMBER;
  int index;
  Matrix *P, *IP;

  float *objvalue;

  Deformation_Field *pdf;
  
  P = (Matrix *) calloc (1, sizeof(Matrix));
  new_matrix(P, 3,3);
  IP = (Matrix *) calloc (1, sizeof(Matrix));
  new_matrix(IP, 3,3);

  pdf = new_deformationfield(tdf->X, tdf->Y, tdf->Z);

  NUMBER = 30;

  objvalue = (float *) calloc (NUMBER, sizeof(float));

  for (index=0;index<NUMBER;index++)
    {
      objvalue[index] = 0;
      for (z=0;z<tdf->Z;z++)
	for (y=0;y<tdf->Y;y++)
	  for (x=0;x<tdf->X;x++)
	    {
	      Jkx = 0;
	      Jky = 0;
	      Jkz = 0;
	      Jx = 0;
	      Jy = 0;
	      Jz = 0;
	      f = 0;
	      K = 0;
	      for (xx = -Nsize; xx<=Nsize; xx++)
		for (yy = -Nsize; yy<=Nsize; yy++)
		  for (zz = -Nsize; zz<=Nsize; zz++)
		    if (!((xx==0)&&(yy==0)&&(zz==0)))
		      {
			nx = x + xx;
			ny = y + yy;
			nz = z + zz;
			if ((nz>=0)&&((ny)>=0)&&((nx)>=0)&&((nz)<tdf->Z)&&((ny)<tdf->Y)&&((nx)<tdf->X))
			  {
			    Jx = tndf->point[z][y][x].X - tndf->point[nz][ny][nx].X;
			    Jy = tndf->point[z][y][x].Y - tndf->point[nz][ny][nx].Y;
			    Jz = tndf->point[z][y][x].Z - tndf->point[nz][ny][nx].Z;
			    Jkx += Jx;
			    Jky += Jy;
			    Jkz += Jz;
			    K ++;	
			    f += Jx * Jx + Jy * Jy + Jz * Jz;
			  }
		      }
	      dx = tndf->point[z][y][x].X - tdf->point[z][y][x].X;
	      dy = tndf->point[z][y][x].Y - tdf->point[z][y][x].Y;
	      dz = tndf->point[z][y][x].Z - tdf->point[z][y][x].Z;
	      
	      Jkx = Jkx * lambda / (float)K + dx;
	      Jky = Jky * lambda / (float)K + dy;
	      Jkz = Jkz * lambda / (float)K + dz;  
	      
	      f = f * lambda / (float )K + dx * dx + dy * dy + dz * dz;

	      objvalue[index] += f;
	      
	      Jx = -1 * Jkx ;
	      Jy = -1 * Jky ;
	      Jz = -1 * Jkz ;
	      
	      P->data[0][0] = 2 + 2 * lambda / (float)K;
	      P->data[0][1] = 0;
	      P->data[0][2] = 0;
	      P->data[1][0] = 0;
	      P->data[1][1] = 2 + 2 * lambda / (float)K;
	      P->data[1][2] = 0;
	      P->data[2][0] = 0;
	      P->data[2][1] = 0;
	      P->data[2][2] = 2 + 2 * lambda / (float)K;
	      
	      Mat_Inverse(P, IP);
	      
	      pdf->point[z][y][x].X = IP->data[0][0] * Jx + IP->data[0][1] * Jy + IP->data[0][2] * Jz;
	      pdf->point[z][y][x].Y = IP->data[1][0] * Jx + IP->data[1][1] * Jy + IP->data[1][2] * Jz;
	      pdf->point[z][y][x].Z = IP->data[2][0] * Jx + IP->data[2][1] * Jy + IP->data[2][2] * Jz;
	    }

      for (z=0;z<tdf->Z;z++)
	for (y=0;y<tdf->Y;y++)
	  for (x=0;x<tdf->X;x++)
	    {
	      tndf->point[z][y][x].X = tndf->point[z][y][x].X + pdf->point[z][y][x].X;
	      tndf->point[z][y][x].Y = tndf->point[z][y][x].Y + pdf->point[z][y][x].Y;
	      tndf->point[z][y][x].Z = tndf->point[z][y][x].Z + pdf->point[z][y][x].Z;
	    }

      printf("iteration: %d, value %f\n", index, objvalue[index]);
      if (index>1)
	{
	  if (fabsf(objvalue[index]-objvalue[index-1])/objvalue[index]<0.0001)
	    break;
	}
    }  
  free_deformationfield(pdf);
}

void Markov_Smoothing_Prior(Deformation_Field *tdf, Deformation_Field *tndf, float lambda, int Nsize, DBox *sigma)
{
  int x,y,z;
  float Jkx, Jky, Jkz;
  float Jx, Jy, Jz;
  float dx, dy, dz;
  float fx, fy, fz;
  float average_sigma;
  int K;
  int nx,ny,nz;
  int xx,yy,zz;
  int NUMBER;
  int index;

  int iteration_no, bg_idx, ed_idx;

  float eta;
  float new_obj;
  Matrix *P, *IP;
  Deformation_Field *ttdf;

  float *objvalue;
  float ratio;

  Deformation_Field *pdf;

  eta = 0.05;
  
  P = (Matrix *) calloc (1, sizeof(Matrix));
  new_matrix(P, 3,3);
  IP = (Matrix *) calloc (1, sizeof(Matrix));
  new_matrix(IP, 3,3);

  pdf = new_deformationfield(tdf->X, tdf->Y, tdf->Z);
  ttdf = new_deformationfield(tdf->X, tdf->Y, tdf->Z);

  NUMBER = 30;

  objvalue = (float *) calloc (NUMBER, sizeof(float));

  average_sigma = 0.0;
  for (z=0;z<tdf->Z;z++)
    for (y=0;y<tdf->Y;y++)
      for (x=0;x<tdf->X;x++)
	{
	  if (sigma->box[z][y][x] <=0.005)
	    sigma->box[z][y][x] = 0.005;
	  average_sigma += sqrtf(sigma->box[z][y][x]);

	}
  average_sigma = average_sigma / (tdf->X*tdf->Y*tdf->Z);
  average_sigma = average_sigma * average_sigma;

  index = 0;
  while (1)
    {
      bg_idx = index;
      objvalue[index] = 0;
      for (z=0;z<tdf->Z;z++)
	for (y=0;y<tdf->Y;y++)
	  for (x=0;x<tdf->X;x++)
	    {
	      Jkx = 0;
	      Jky = 0;
	      Jkz = 0;
	      Jx = 0;
	      Jy = 0;
	      Jz = 0;
	      K = 0;
	      for (xx = -Nsize; xx<=Nsize; xx++)
		for (yy = -Nsize; yy<=Nsize; yy++)
		  for (zz = -Nsize; zz<=Nsize; zz++)
		    if (!((xx==0)&&(yy==0)&&(zz==0)))
		      {
			nx = x + xx;
			ny = y + yy;
			nz = z + zz;
			if ((nz>=0)&&((ny)>=0)&&((nx)>=0)&&((nz)<tdf->Z)&&((ny)<tdf->Y)&&((nx)<tdf->X))
			  {
			    Jx = tndf->point[nz][ny][nx].X;
			    Jy = tndf->point[nz][ny][nx].Y;
			    Jz = tndf->point[nz][ny][nx].Z;
			    Jkx += Jx;
			    Jky += Jy;
			    Jkz += Jz;
			    K ++;	
			  }
		      }

	      Jkx = tndf->point[z][y][x].X - Jkx / K;
	      Jky = tndf->point[z][y][x].Y - Jky / K;
	      Jkz = tndf->point[z][y][x].Z - Jkz / K;

	      dx = tndf->point[z][y][x].X - tdf->point[z][y][x].X;
	      dy = tndf->point[z][y][x].Y - tdf->point[z][y][x].Y;
	      dz = tndf->point[z][y][x].Z - tdf->point[z][y][x].Z;

	      ratio = 1.0/ sigma->box[z][y][x] / 2.0;

	      fx = Jkx * Jkx * ratio;
	      fx = expf(fx);

	      fy = Jky * Jky * ratio;
	      fy = expf(fy);

	      fz = Jkz * Jkz * ratio;
	      fz = expf(fz);
	      
	      Jkx = 2* dx - lambda * Jkx /sigma->box[z][y][x] / fx;
	      Jky = 2* dy - lambda * Jky /sigma->box[z][y][x] / fy;
	      Jkz = 2* dz - lambda * Jkz /sigma->box[z][y][x] / fz;
	      
	      objvalue[index] += lambda / fx + lambda / fy + lambda / fz + dx * dx + dy * dy + dz * dz;

	      Jx = -1 * Jkx ;
	      Jy = -1 * Jky ;
	      Jz = -1 * Jkz ;

	      /*	      
	      P->data[0][0] = 2 - lambda *(1.0/ sigma->box[z][y][x] - Jkx / sigma->box[z][y][x] * Jkx / sigma->box[z][y][x]) / fx;
	      P->data[0][1] = 0;
	      P->data[0][2] = 0;
	      P->data[1][0] = 0;
	      P->data[1][1] = 2 - lambda *(1.0/ sigma->box[z][y][x] - Jky / sigma->box[z][y][x] * Jky / sigma->box[z][y][x]) / fy;
	      P->data[2][0] = 0;
	      P->data[2][1] = 0;
	      P->data[2][2] = 2 - lambda *(1.0/ sigma->box[z][y][x] - Jkz / sigma->box[z][y][x] * Jkz / sigma->box[z][y][x]) / fz;
	      
	      Mat_Inverse(P, IP);
	      
	      pdf->point[z][y][x].X = IP->data[0][0] * Jx + IP->data[0][1] * Jy + IP->data[0][2] * Jz;
	      pdf->point[z][y][x].Y = IP->data[1][0] * Jx + IP->data[1][1] * Jy + IP->data[1][2] * Jz;
	      pdf->point[z][y][x].Z = IP->data[2][0] * Jx + IP->data[2][1] * Jy + IP->data[2][2] * Jz;
	      */

	      pdf->point[z][y][x].X = eta * Jx;
	      pdf->point[z][y][x].Y = eta * Jy;
	      pdf->point[z][y][x].Z = eta * Jz;
	    }

      //      printf("index: %d, %f\n", index, objvalue[index]);
      if (index>0)
	{
	  for (z=0;z<tdf->Z;z++)
	    for (y=0;y<tdf->Y;y++)
	      for (x=0;x<tdf->X;x++)
		{
		  ttdf->point[z][y][x].X = tndf->point[z][y][x].X + pdf->point[z][y][x].X;
		  ttdf->point[z][y][x].Y = tndf->point[z][y][x].Y + pdf->point[z][y][x].Y;
		  ttdf->point[z][y][x].Z = tndf->point[z][y][x].Z + pdf->point[z][y][x].Z;
		}
	  
	  new_obj = 0.0;
	  
	  for (z=0;z<tdf->Z;z++)
	    for (y=0;y<tdf->Y;y++)
	      for (x=0;x<tdf->X;x++)
		{
		  Jkx = 0;
		  Jky = 0;
		  Jkz = 0;
		  Jx = 0;
		  Jy = 0;
		  Jz = 0;
		  K = 0;
		  for (xx = -Nsize; xx<=Nsize; xx++)
		    for (yy = -Nsize; yy<=Nsize; yy++)
		      for (zz = -Nsize; zz<=Nsize; zz++)
			if (!((xx==0)&&(yy==0)&&(zz==0)))
			  {
			    nx = x + xx;
			    ny = y + yy;
			    nz = z + zz;
			    if ((nz>=0)&&((ny)>=0)&&((nx)>=0)&&((nz)<tdf->Z)&&((ny)<tdf->Y)&&((nx)<tdf->X))
			      {
				Jx = ttdf->point[nz][ny][nx].X;
				Jy = ttdf->point[nz][ny][nx].Y;
				Jz = ttdf->point[nz][ny][nx].Z;
				Jkx += Jx;
				Jky += Jy;
				Jkz += Jz;
				K ++;	
			      }
			  }
		  
		  Jkx = ttdf->point[z][y][x].X - Jkx / K;
		  Jky = ttdf->point[z][y][x].Y - Jky / K;
		  Jkz = ttdf->point[z][y][x].Z - Jkz / K;
		  
		  dx = ttdf->point[z][y][x].X - tdf->point[z][y][x].X;
		  dy = ttdf->point[z][y][x].Y - tdf->point[z][y][x].Y;
		  dz = ttdf->point[z][y][x].Z - tdf->point[z][y][x].Z;
		  
		  ratio = 1.0/ sigma->box[z][y][x] / 2.0;
		  
		  fx = Jkx * Jkx * ratio;
		  fx = expf(fx);
		  
		  fy = Jky * Jky * ratio;
		  fy = expf(fy);
		  
		  fz = Jkz * Jkz * ratio;
		  fz = expf(fz);
		  
		  new_obj += lambda / fx + lambda / fy + lambda / fz + dx * dx + dy * dy + dz * dz;
		}

	  //	  printf("new obj = %f\n", new_obj);

	  if (new_obj<objvalue[index-1])
	    {
	      for (z=0;z<tdf->Z;z++)
		for (y=0;y<tdf->Y;y++)
		  for (x=0;x<tdf->X;x++)
		    {
		      tndf->point[z][y][x].X = ttdf->point[z][y][x].X;
		      tndf->point[z][y][x].Y = ttdf->point[z][y][x].Y;
		      tndf->point[z][y][x].Z = ttdf->point[z][y][x].Z;
		  }
	      eta = eta * 1.01;	  
	      index ++;
	    }
	  else
	    {
	      eta = eta * .5;
	    }
	}
      else
	{
	  for (z=0;z<tdf->Z;z++)
	    for (y=0;y<tdf->Y;y++)
	      for (x=0;x<tdf->X;x++)
		{
		  tndf->point[z][y][x].X = tndf->point[z][y][x].X + pdf->point[z][y][x].X;
		  tndf->point[z][y][x].Y = tndf->point[z][y][x].Y + pdf->point[z][y][x].Y;
		  tndf->point[z][y][x].Z = tndf->point[z][y][x].Z + pdf->point[z][y][x].Z;
		}
	  index ++;
	}

      if (index>1)
	{
	  if (fabsf(objvalue[index]-objvalue[index-1])/objvalue[index]<0.0001)
	    break;
	}
      ed_idx = index;

      if (bg_idx==ed_idx)
	iteration_no ++;
      else
	{
	  iteration_no = 0;
	  //	  printf("iteration: %d, %f\n", bg_idx, objvalue[index-1]);
	}

      if (iteration_no>10)
	break;
    }  
  free_deformationfield(pdf);
  free_deformationfield(ttdf);
}

void Markov_Smoothing_Weights(Deformation_Field *tdf, Deformation_Field *tndf, float lambda, int Nsize, DBox *sigma)
{
  int x,y,z;
  float Jkx, Jky, Jkz;
  float Jx, Jy, Jz;
  float dx, dy, dz;
  float f;
  int K;
  int nx,ny,nz;
  int xx,yy,zz;
  int NUMBER;
  int index;
  int number;
  Matrix *P, *IP;

  float average_sigma;
  float *objvalue;

  DBox *weights;
  Deformation_Field *pdf;
  
  P = (Matrix *) calloc (1, sizeof(Matrix));
  new_matrix(P, 3,3);
  IP = (Matrix *) calloc (1, sizeof(Matrix));
  new_matrix(IP, 3,3);

  pdf = new_deformationfield(tdf->X, tdf->Y, tdf->Z);
  weights = new_dbox(sigma->X, sigma->Y, sigma->Z);

  NUMBER = 30;

  objvalue = (float *) calloc (NUMBER, sizeof(float));


  number = 0;
  average_sigma = 0.0;
  for (z=0;z<tdf->Z;z++)
    for (y=0;y<tdf->Y;y++)
      for (x=0;x<tdf->X;x++)
	{
	  if (sigma->box[z][y][x] > 0.0001)
	    {
	      average_sigma += sqrtf(sigma->box[z][y][x]);
	      number ++;
	    }
	}

  average_sigma = average_sigma / number;
  average_sigma = average_sigma * average_sigma;

  for (z=0;z<tdf->Z;z++)
    for (y=0;y<tdf->Y;y++)
      for (x=0;x<tdf->X;x++)
	{
	  weights->box[z][y][x] = lambda * expf(-sigma->box[z][y][x]/average_sigma/9.0);
	}

  write_dbox(weights, "temp.weight", "float", 1);

  for (index=0;index<NUMBER;index++)
    {
      objvalue[index] = 0;
      for (z=0;z<tdf->Z;z++)
	for (y=0;y<tdf->Y;y++)
	  for (x=0;x<tdf->X;x++)
	    {
	      Jkx = 0;
	      Jky = 0;
	      Jkz = 0;
	      Jx = 0;
	      Jy = 0;
	      Jz = 0;
	      f = 0;
	      K = 0;
	      for (xx = -Nsize; xx<=Nsize; xx++)
		for (yy = -Nsize; yy<=Nsize; yy++)
		  for (zz = -Nsize; zz<=Nsize; zz++)
		    if (!((xx==0)&&(yy==0)&&(zz==0)))
		      {
			nx = x + xx;
			ny = y + yy;
			nz = z + zz;
			if ((nz>=0)&&((ny)>=0)&&((nx)>=0)&&((nz)<tdf->Z)&&((ny)<tdf->Y)&&((nx)<tdf->X))
			  {
			    Jx = tndf->point[z][y][x].X - tndf->point[nz][ny][nx].X;
			    Jy = tndf->point[z][y][x].Y - tndf->point[nz][ny][nx].Y;
			    Jz = tndf->point[z][y][x].Z - tndf->point[nz][ny][nx].Z;
			    Jkx += Jx;
			    Jky += Jy;
			    Jkz += Jz;
			    K ++;	
			    f += Jx * Jx + Jy * Jy + Jz * Jz;
			  }
		      }
	      dx = tndf->point[z][y][x].X - tdf->point[z][y][x].X;
	      dy = tndf->point[z][y][x].Y - tdf->point[z][y][x].Y;
	      dz = tndf->point[z][y][x].Z - tdf->point[z][y][x].Z;
	      
	      Jkx = Jkx * weights->box[z][y][x] / (float)K + dx;
	      Jky = Jky * weights->box[z][y][x] / (float)K + dy;
	      Jkz = Jkz * weights->box[z][y][x] / (float)K + dz;  
	      
	      f = f * weights->box[z][y][x] / (float )K + dx * dx + dy * dy + dz * dz;

	      objvalue[index] += f;
	      
	      Jx = -1 * Jkx ;
	      Jy = -1 * Jky ;
	      Jz = -1 * Jkz ;
	      
	      P->data[0][0] = 2 + 2 * weights->box[z][y][x] / (float)K;
	      P->data[0][1] = 0;
	      P->data[0][2] = 0;
	      P->data[1][0] = 0;
	      P->data[1][1] = 2 + 2 * weights->box[z][y][x] / (float)K;
	      P->data[1][2] = 0;
	      P->data[2][0] = 0;
	      P->data[2][1] = 0;
	      P->data[2][2] = 2 + 2 * weights->box[z][y][x] / (float)K;
	      
	      Mat_Inverse(P, IP);
	      
	      pdf->point[z][y][x].X = IP->data[0][0] * Jx + IP->data[0][1] * Jy + IP->data[0][2] * Jz;
	      pdf->point[z][y][x].Y = IP->data[1][0] * Jx + IP->data[1][1] * Jy + IP->data[1][2] * Jz;
	      pdf->point[z][y][x].Z = IP->data[2][0] * Jx + IP->data[2][1] * Jy + IP->data[2][2] * Jz;
	    }

      for (z=0;z<tdf->Z;z++)
	for (y=0;y<tdf->Y;y++)
	  for (x=0;x<tdf->X;x++)
	    {
	      tndf->point[z][y][x].X = tndf->point[z][y][x].X + pdf->point[z][y][x].X;
	      tndf->point[z][y][x].Y = tndf->point[z][y][x].Y + pdf->point[z][y][x].Y;
	      tndf->point[z][y][x].Z = tndf->point[z][y][x].Z + pdf->point[z][y][x].Z;
	    }

      printf("iteration: %d, value %f\n", index, objvalue[index]);
      if (index>1)
	{
	  if (fabsf(objvalue[index]-objvalue[index-1])/objvalue[index]<0.0001)
	    break;
	}
    }  
  free_deformationfield(pdf);
}



void vectorfcm_center_no(float **data, int Mx, int My, int C, float **center)
{
  Matrix *U;
  float *obfun;
  Signal *ct;
  int rok;
  int kk,x,k,m,KK,i;
  float pp, upper, lower, q, ok,temp;
  float *upperv;

  upperv = (float *) calloc (My, sizeof(float));

  q = 2;

  KK = 30;

  U = (Matrix *) calloc(1, sizeof(Matrix));
  new_matrix(U,Mx,C);

  for (k=0;k<C;k++)
    {
      rok = (int)floorf(((float)rand())/RAND_MAX * Mx);
      for (i=0;i<My;i++)
	center[k][i] = data[rok][i];
    }

  obfun = (float *) calloc( KK, sizeof(float));

  for (kk=0;kk<KK;kk++)
    {
      for (x=0;x<Mx;x++)
	{
	  for (k=0;k<C;k++)
	    {
	      pp = 1.0/(q-1);
	      upper = 0.0;
	      upper = powf(vec_dist2(center[k], data[x], My), pp);
	      if (upper!=0)
		upper = 1/upper;
	      else
		upper = 10000;
	      
	      lower = 0.0;
	      for (m=0;m<C;m++)
		{
		  ok = powf(vec_dist2(center[m], data[x], My), pp);
		  if (ok==0)
		    ok = 10000;
		  else
		    ok = 1/ok;
		  lower += ok;
		}
	      
	      U->data[x][k] = upper / lower;
	    }
	}
      
      for (k=0;k<C;k++)
	{
	  for (i=0;i<My;i++)
	    upperv[i] = 0.0;

	  lower = 0;
	  for (x=0;x<Mx;x++)
	    {
	      lower += powf(U->data[x][k], q);
	      for (i=0;i<My;i++)
		upperv[i] = powf(U->data[x][k], q) * data[x][i];
	    }

	  for (i=0;i<My;i++)
	    {
	      center[k][i] = upperv[i] / lower;	  
	      printf("%f ", center[k][i]);
	    }
	  printf("\n");
	}
      
      //calculating objectvie function
      
      obfun[kk] = 0.0;
      for (x=0;x<Mx;x++)
	for (k=0;k<C;k++)
	  {
	    obfun[kk] += powf(U->data[x][k], q)*vec_dist2(data[x], center[k], My);
	  }
      
      if (kk>1)
	if (fabsf(obfun[kk]-obfun[kk-1])/obfun[kk-1]<0.00001)
	  break;


      
    }
  
  FreeMatrix(U);
  free(obfun);
}

FPoint fpoint_affine_transform(FPoint M, Matrix *A, FPoint *T)
{
  FPoint S;
  S.X = A->data[0][0] * M.X + A->data[0][1] * M.Y + A->data[0][2] * M.Z + T->X;
  S.Y = A->data[1][0] * M.X + A->data[1][1] * M.Y + A->data[1][2] * M.Z + T->Y;
  S.Z = A->data[2][0] * M.X + A->data[2][1] * M.Y + A->data[2][2] * M.Z + T->Z;
  return(S);
}



DBox * calculate_brainboundary(DBox *img)
{
  int x,y,z;
  int xx,yy,zz;
  int nCSF;
  float ratio;

  DBox *map;

  map = new_dbox(img->X, img->Y, img->Z);

  for (z=2;z<img->Z-2;z++)
    for (y=2;y<img->Y-2;y++)
      for (x=2;x<img->X-2;x++)
	{
	  nCSF = 0;
	  for (xx=-2;xx<3;xx++)
	    for (yy=-2;yy<3;yy++)
	      for (zz=-2;zz<3;zz++)
		{
		  if (img->box[zz+z][yy+y][xx+x]==0)
		    nCSF ++;
		}
	  ratio = nCSF/125.0;
	  if (ratio>0.5)
	    map->box[z][y][x] = (1-(ratio - 0.5)/0.5)*255.0;
	  else
	    map->box[z][y][x] = ratio/0.5*255.0;
	    
	  //	  map->box[z][y][x] = exp(-1*(ratio-0.5)*(ratio-0.5)/0.16) * 255.0;
	}

  return(map);
}


void set_I_matrix(Matrix *A)
{
  int i,j;
  for (i=0;i<A->height;i++)
    for (j=0;j<A->width;j++)
      {
	if (i==j)
	  A->data[i][j] = 1.0;
	else
	  A->data[i][j] = 0;
      }
}


