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

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

#define YYES    1
#define NNO     0

#define Zscale    1.5

int    HeadInField ;
float  CompareAxis ;
int    SoftlyDistributeVolume, Factor4Scaling, OutputShortImg ;

void MarcMain(int,char *[]);
void ShenMain(int argc,char *argv[]) ;
void show_usage_SHEN() ;
void OutPutRAVENSmapByShort(float ***Mass, int image_size, int z_size, char filename[180]);



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

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


void WriteShortImg(char filename[180], short ***data, int image_size, int z_size)
{
  FILE  *fp;
  int   i, k ;

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


void OpenDeformationField(char filename[180], Fvector3d ***DeformFld, int image_size, int z_size)
{
  FILE  *fp;
  float XYZ_ratio, deform_ratio, dx, dy, dz ;
  int Last_image_size, Last_z_size ;
  int k,i ;

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

  for(k=0;k<z_size;k++)
    for(i=0;i<image_size;i++)
      fread(DeformFld[k][i],sizeof(Fvector3d),image_size,fp);
  fclose(fp);
 }



void DistributingVolume_HardCase(float weight, float ii, float jj, float kk, float ***Mass, int image_size, int z_size)
{
  int   ni,nj,nk, GreyValue ;


  ni = (int)(ii+0.5) ;
  nj = (int)(jj+0.5) ;
  nk = (int)(kk+0.5) ;
  
  if(ni>=0 && ni<image_size  &&  nj>=0 && nj<image_size  &&  nk>=0 && nk<z_size )
    Mass[nk][ni][nj] += weight ;
}


void DistributingVolume(float weight, float ii, float jj, float kk, float ***Mass, int image_size, int z_size)
{
  float CurrentV ;
  float b,c,d, b1,c1,d1, combined;
  int   ni,nj,nk, niP1,njP1,nkP1, GreyValue ;


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

      combined = ( d1*((b1*c1)+(b*c1)+(b1*c)+(b*c)) + d*((b1*c1)+(b*c1)+(b1*c)+(b*c)) ) ;

      Mass[nk][ni][nj]      += weight*d1*(b1*c1)/combined ;
      Mass[nk][niP1][nj]    += weight*d1*(b*c1)/combined ;
      Mass[nk][ni][njP1]    += weight*d1*(b1*c)/combined ;
      Mass[nk][niP1][njP1]  += weight*d1*(b*c)/combined ;
      Mass[nkP1][ni][nj]    += weight*d*(b1*c1)/combined ;
      Mass[nkP1][niP1][nj]  += weight*d*(b*c1)/combined ;
      Mass[nkP1][ni][njP1]  += weight*d*(b1*c)/combined ;
      Mass[nkP1][niP1][njP1]+= weight*d*(b*c)/combined ;
    }
  
  if(ni==image_size-1 && nj>=0 && nj<image_size-1 && nk>=0 && nk<z_size-1 || ni>=0 && ni<image_size-1 && nj==image_size-1 && nk>=0 && nk<z_size-1  || ni>=0 && ni<image_size-1 && nj>=0 && nj<image_size-1 && nk==z_size-1)
    Mass[nk][ni][nj]      += weight*1 ;
}


void RAVENSmap_DirectWay( unsigned char ***ObjOriginalImg, Fvector3d ***DeformFld, int Intensity, int image_size, int z_size, char filename[180] ) 
{
  int s, i, j, k, x, y, z ;
  FILE  *fp;
  unsigned char ***resImg ;
  float         ***Mass, max, min ;
  float  ii, jj, kk ;
  int    temp ;

  resImg = UCalloc3d(image_size,image_size,z_size); 
  Mass   = Falloc3d(image_size,image_size,z_size);
  for(k=0; k<z_size; k++)
    for(i=0; i<image_size; i++)
      for(j=0; j<image_size; j++)
	Mass[k][i][j] = 0 ;

  for(k=0; k<z_size; k++)
    for(i=0; i<image_size; i++)
      for(j=0; j<image_size; j++)
	{
	  ii = i + DeformFld[k][i][j].x ;
	  jj = j + DeformFld[k][i][j].y ;
	  kk = k + DeformFld[k][i][j].z ;
	  
	  if( ObjOriginalImg[k][i][j]==Intensity) 
	    DistributingVolume(1.0, ii, jj, kk, Mass, image_size, z_size) ;
	}

  /* find max */
  max = 0 ;
  min = 10000.0 ;
  for(k=0; k<z_size; k++)
    for(i=0; i<image_size; i++)
      for(j=0; j<image_size; j++)
	{
	  if( Mass[k][i][j]>max )
	    max = Mass[k][i][j] ;
	  if( Mass[k][i][j]<min )
	    min = Mass[k][i][j] ;
	}
  printf("max=%f  min=%f\n", max, min) ;
  /* normalize */
  for(k=0; k<z_size; k++)
    for(i=0; i<image_size; i++)
      for(j=0; j<image_size; j++)
	{
	  temp = Mass[k][i][j]*100 ;
	  if( temp>255 ) temp = 255 ;
	  resImg[k][i][j]= temp ;
	}

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

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


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


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

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

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

  return GreyValue ;
}

void InterpolatedDisplacement(Fvector3d *Displace_subVoxel, float ii, float jj, float kk, Fvector3d ***DeformFld, int image_size, int z_size)
{
  float CurrentV ;
  float b,c,d, b1,c1,d1;
  int   ni,nj,nk, niP1,njP1,nkP1, GreyValue ;


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

      (*Displace_subVoxel).x = ( d1*(DeformFld[nk][ni][nj].x*(b1*c1) + DeformFld[nk][niP1][nj].x*(b*c1) + DeformFld[nk][ni][njP1].x*(b1*c) + DeformFld[nk][niP1][njP1].x*(b*c)) + d*(DeformFld[nkP1][ni][nj].x*(b1*c1) + DeformFld[nkP1][niP1][nj].x*(b*c1) + DeformFld[nkP1][ni][njP1].x*(b1*c) + DeformFld[nkP1][niP1][njP1].x*(b*c)) )/( d1*((b1*c1)+(b*c1)+(b1*c)+(b*c)) + d*((b1*c1)+(b*c1)+(b1*c)+(b*c)) ) ; 

      (*Displace_subVoxel).y = ( d1*(DeformFld[nk][ni][nj].y*(b1*c1) + DeformFld[nk][niP1][nj].y*(b*c1) + DeformFld[nk][ni][njP1].y*(b1*c) + DeformFld[nk][niP1][njP1].y*(b*c)) + d*(DeformFld[nkP1][ni][nj].y*(b1*c1) + DeformFld[nkP1][niP1][nj].y*(b*c1) + DeformFld[nkP1][ni][njP1].y*(b1*c) + DeformFld[nkP1][niP1][njP1].y*(b*c)) )/( d1*((b1*c1)+(b*c1)+(b1*c)+(b*c)) + d*((b1*c1)+(b*c1)+(b1*c)+(b*c)) ) ; 

      (*Displace_subVoxel).z = ( d1*(DeformFld[nk][ni][nj].z*(b1*c1) + DeformFld[nk][niP1][nj].z*(b*c1) + DeformFld[nk][ni][njP1].z*(b1*c) + DeformFld[nk][niP1][njP1].z*(b*c)) + d*(DeformFld[nkP1][ni][nj].z*(b1*c1) + DeformFld[nkP1][niP1][nj].z*(b*c1) + DeformFld[nkP1][ni][njP1].z*(b1*c) + DeformFld[nkP1][niP1][njP1].z*(b*c)) )/( d1*((b1*c1)+(b*c1)+(b1*c)+(b*c)) + d*((b1*c1)+(b*c1)+(b1*c)+(b*c)) ) ; 
    }
  
  if(ni==image_size-1 && nj>=0 && nj<image_size-1 && nk>=0 && nk<z_size-1 || ni>=0 && ni<image_size-1 && nj==image_size-1 && nk>=0 && nk<z_size-1  || ni>=0 && ni<image_size-1 && nj>=0 && nj<image_size-1 && nk==z_size-1)
    {
      (*Displace_subVoxel).x = DeformFld[nk][ni][nj].x ;
      (*Displace_subVoxel).y = DeformFld[nk][ni][nj].y ;
      (*Displace_subVoxel).z = DeformFld[nk][ni][nj].z ;
    }
}


void RAVENSmap_CleverWay( int SampleNum, unsigned char ***ObjOriginalImg, Fvector3d ***DeformFld, int Intensity, int image_size, int z_size, char filename[180] ) 
{
  int s, i, j, k, x, y, z ;
  FILE  *fp;
  unsigned char ***resImg ;
  float         ***Mass, max, min ;
  float  ii, jj, kk, interval ;
  int    temp, GreyValue, MappedV ;
  Fvector3d  Mdl_subVoxel, Displace_subVoxel ;
  /* asymmetry */
  float  mapping, EstiLeft, EstiRight, EstiRatio, VoxelSize ;
  int    size ;
  char   *tempText ;
  FILE    *fp_Percent;
  double total =0 ;

  interval = 1.0/(SampleNum*2+1) ;
  printf("interval=%f SampleNum=%d\n", interval, SampleNum) ;

  resImg = UCalloc3d(image_size,image_size,z_size); 
  Mass   = Falloc3d(image_size,image_size,z_size);
  for(k=0; k<z_size; k++)
    for(i=0; i<image_size; i++)
      for(j=0; j<image_size; j++)
	Mass[k][i][j] = 0 ;

  for(k=0; k<z_size; k++)
    {
      printf("z=%d\n", k) ;
      for(i=0; i<image_size; i++)
	for(j=0; j<image_size; j++)
	  {
	    if( ObjOriginalImg[k][i][j]==Intensity )
	      for(z=-SampleNum; z<=SampleNum; z++)
		for(x=-SampleNum; x<=SampleNum; x++)
		  for(y=-SampleNum; y<=SampleNum; y++)
		    {
		      Mdl_subVoxel.x = x*interval + i ;
		      Mdl_subVoxel.y = y*interval + j ;
		      Mdl_subVoxel.z = z*interval + k ;
		      
                      /* deleted this greyvalue interpolation on Sept. 4, 2001 */
                      /*
		      GreyValue = InterpolatedIntensity(Mdl_subVoxel.x, Mdl_subVoxel.y, Mdl_subVoxel.z, ObjOriginalImg, image_size, z_size) ; 
		      MappedV = BG ; 
		      if( GreyValue>=VN/2   && GreyValue<=(VN+GM)/2 ) 
			MappedV = VN ; 
		      if( GreyValue>(VN+GM)/2 && GreyValue<=(GM+WM)/2 ) 
			MappedV = GM ; ; 
		      if( GreyValue>(GM+WM)/2) 
			MappedV = WM ; 
		      */
		      MappedV=Intensity ;

		      if( MappedV==Intensity ) 
			{
			  InterpolatedDisplacement(&Displace_subVoxel, Mdl_subVoxel.x, Mdl_subVoxel.y, Mdl_subVoxel.z, DeformFld, image_size, z_size) ;
			  
			  ii = Mdl_subVoxel.x + Displace_subVoxel.x ;
			  jj = Mdl_subVoxel.y + Displace_subVoxel.y ;
			  kk = Mdl_subVoxel.z + Displace_subVoxel.z ;
			  
			  if( SoftlyDistributeVolume==YYES )
			    DistributingVolume(8.0/pow((SampleNum*2+1)*2,3.0), ii, jj, kk, Mass, image_size, z_size) ;
			  else
			    DistributingVolume_HardCase(8.0/pow((SampleNum*2+1)*2,3.0), ii, jj, kk, Mass, image_size, z_size) ;
			}
		    }
	  }
    }


  /* compare tissue (i.e. ventricle) asymmetry, and save it */
  if( CompareAxis>0 )
    {
      VoxelSize = 0.9375*0.9375*1.5*1/Zscale ;
      EstiLeft=0;
      EstiRight=0;

      for(k=0;k<z_size;k++)
	for(i=0;i<image_size;i++)
	  for(j=0;j<image_size;j++)
	    {
	      mapping = 1.0*(j-CompareAxis) ;
	      
	      /* try to separate the subject image into left and right parts and save them */
	      if( mapping>0 )  /* right */
		EstiRight += Mass[k][i][j]*VoxelSize ;
	      else             /* left */
		EstiLeft  += Mass[k][i][j]*VoxelSize ;
	    }

      /* parameters */
      EstiRatio = (EstiRight-EstiLeft)/(EstiRight+EstiLeft) ;
      printf("Estimated (Left, Right), ratio= (%f, %f -> %f) %f\n", EstiLeft, EstiRight, EstiLeft+EstiRight, EstiRatio ) ;
      
      /* save the result */
      /* read content in the existing file */
      size = -1000 ;
      if((fp_Percent=fopen("RavensMap_Results.txt","rb"))!=NULL)
	{
	  fseek(fp_Percent,0,SEEK_END);
	  size = ftell(fp_Percent) ;
	  tempText = Calloc1d(size) ;
	  rewind(fp_Percent);
	  fread(tempText,1,size,fp_Percent);
	  fclose(fp_Percent);
	}
      /* save it */
      fp_Percent=myopen("RavensMap_Results.txt","wb") ;
       if(size>0) fprintf(fp_Percent, "%s\n", tempText) ; 
       fprintf(fp_Percent, "\n\n%s:\n", filename) ;
       fprintf(fp_Percent, "Estimated (Left, Right), ratio= (%f, %f) %f\n", EstiLeft, EstiRight, EstiRatio ) ;
      fclose(fp_Percent);
    }


  /* find max */
  max = 0 ;
  min = 10000.0 ;
  for(k=0; k<z_size; k++)
    for(i=0; i<image_size; i++)
      for(j=0; j<image_size; j++)
	{
	  if( Mass[k][i][j]>max )
	    max = Mass[k][i][j] ;
	  if( Mass[k][i][j]<min )
	    min = Mass[k][i][j] ;
	}
  printf("max=%f  min=%f\n", max, min) ;
  /* normalize */
  for(k=0; k<z_size; k++)
    for(i=0; i<image_size; i++)
      for(j=0; j<image_size; j++)
	{
	  temp = (int)(Mass[k][i][j]*Factor4Scaling + 0.5) ; /* Factor4Scaling=20, before May 1 2003 */
	  if( temp>255 ) temp = 255 ;
	  resImg[k][i][j]= temp ;
	}
  /*  k=101; i=142; j=128; printf("byte:   resImg[101][142][128]=%d\n", resImg[k][i][j]) ;*/

  /* save */
  /*WriteImg(filename, resImg, image_size, z_size) ;*/
  if( OutputShortImg==YYES ) 
    OutPutRAVENSmapByShort(Mass, image_size, z_size, filename) ;
  else
    WriteImg(filename, resImg, image_size, z_size) ;


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

void OutPutRAVENSmapByShort(float ***Mass, int image_size, int z_size, char filename[180])
{
  int   i, j, k;
  short ***resImg ;  /* -????? ~ +?????*/
  int temp, MaxValueofShort ;

  
  MaxValueofShort = 65536/2 -1 ;  printf("MaxValueofShort=%d\n", MaxValueofShort) ;

  resImg = Salloc3d(image_size,image_size,z_size); 
  /* sscaling */
  for(k=0; k<z_size; k++)
    for(i=0; i<image_size; i++)
      for(j=0; j<image_size; j++)
	{
	  temp = (int)(Mass[k][i][j]*Factor4Scaling + 0.5) ; /* Factor4Scaling=20, before May 1 2003 */

	  if( temp> MaxValueofShort )  temp =  MaxValueofShort ;
	  if( temp<-MaxValueofShort )  temp = -MaxValueofShort ;

	  resImg[k][i][j]= temp ;
	}
  /*  k=101; i=142; j=128; printf("Short: resImg[101][142][128]=%d\n", resImg[k][i][j]) ;*/

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

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



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



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


  num=4;

  if(argc<num)
    show_usage_SHEN() ;


  /* input image size */
  image_size= 256;
  z_size    = 124 ;
  HeadInField = NNO ;
  SampleNum = 1 ;
  CompareAxis = -1000.0 ; /* negative means no effect */
  SoftlyDistributeVolume = YYES ;
  Factor4Scaling = 20 ;
  OutputShortImg = NNO ;

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

	case 'h':
	  HeadInField = YYES ;
	  break ;

	case 's':
	  sscanf(optarg, "%d", &SampleNum) ; /* for upsampling vector domain */
	  break ;

	case 'c':
	  CompareAxis = atof(optarg) ;
	  break ;

	case 'H':
	  SoftlyDistributeVolume = NNO ;
	  break ;

	case 'F':
	  Factor4Scaling = atoi(optarg) ;
	  break ;

	case 'S':
	  OutputShortImg = YYES ;
	  break ;

	case 'X':
	  image_size=atoi(optarg);
	  break;


	default:
	  break;
	}
    }
  printf("CompareAxis=%f\n", CompareAxis) ;
  printf("Factor4Scaling=%d\n", Factor4Scaling) ;
  printf("OutputShortImg=%d\n", OutputShortImg) ;

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


  /* vector field */ 
  DeformFld = Fvector3dalloc3d(image_size,image_size,z_size);
  OpenDeformationField(argv[1], DeformFld, image_size, z_size);

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


void show_usage_SHEN()
{
    char fullVersion[] = "$Rev: 83 $"; //This line will be automatically 
                                       //populated by svn and should be
                                       //of the form "$Rev: 83 $" else it could
                                       //cause a segmentation fault
    char *shortVersion = strchr(fullVersion,' ')+1; //Eliminate te word "$Rev: " 
    char *secondSpaceInVersion = strchr(shortVersion,' '); //Find the space after the number
    *secondSpaceInVersion ='\0';//Eliminate anything after the number, by zeroing the space
    printf("\t \n\ravens version: 1.0.%s \n\ ",shortVersion);
  printf("USAGE: ravens <VectorField_subj2Mdl> <Subj img> <output img>\n\
\t -v <int>             : labels to focus (WM 250, GM 150, CSF 10, VN 50) \n\
\t -h                   : there is head information in vector field \n\
\t -s <int>             : resample rate (i.e. 1,2,) \n\
\t -H                   : get RAVENS map by hard distribution of subvolumes \n\
\t -F <int>             : factor to scale the RAVENS value (default: 20) \n\
\t -S                   : output RAVENS map with 2 bytes \n\
\t -X                   : image size at x and y dimensions (should be same) \n\
");
/*\t -c <float>           : compare asymmetry on the input axis (y axis) \n\*/
  exit(1);
}

