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


#define YYES    1
#define NNO     0


void show_usage_SHEN() ;


void Write_Transformation( Matrix *Transform, Fvector3d center1, Fvector3d center2, char filename[80])
{
  FILE *fp;
  int i, j ;

  double *Temp_transf ;
  int Tm, Tn ;

  Tm = Transform->height ;
  Tn = Transform->width ;

  /* allocate space */
  Temp_transf = (double *)calloc( Tm*Tn, sizeof(double) );


  /* for T1 */
  for(i=0; i<Tm; i++)
   for(j=0; j<Tn; j++)
    Temp_transf[i*Tn+j] = Transform->data[i][j] ; 


  /* begin to write*/
  if((fp=fopen(filename,"wb"))==NULL)
  {
   printf("cannot open file\n");
   exit(1);
  }

  fseek(fp,0L,SEEK_SET);

  fwrite( Temp_transf,  sizeof(double), Tm*Tn,  fp ) ; 
  fwrite( &center1,      sizeof(Fvector3d),  1,  fp ) ;
  fwrite( &center2,      sizeof(Fvector3d),  1,  fp ) ;
  
  fclose(fp);

  /* free */
  free(Temp_transf) ;
}




void Tranform2myformat( Matrix *Transform, Matrix *MyTransform, Fvector3d *Mycenter1, Fvector3d *Mycenter2)
{
  Matrix *Transform3D ;
  float  *current_position, *transformed_position;
  int    image_size, z_size ;
  int    i,j ;

  current_position= vectorSHEN(0, 3-1) ;
  transformed_position= vectorSHEN(0, 3-1) ;


  /* Get 3D transform */
  for(i=0; i<3; i++)
    for(j=0; j<3; j++)
      MyTransform->data[i][j] = Transform->data[i][j] ;

  image_size =256 ; z_size=124 ;
  /* first center */
  (*Mycenter1).x = image_size/2 ;
  (*Mycenter1).y = image_size/2 ;
  (*Mycenter1).z = z_size/2 ;
   /* get 2nd center */
    current_position[0] = (*Mycenter1).x ;
    current_position[1] = (*Mycenter1).y ;
    current_position[2] = (*Mycenter1).z ;
     Mat_times_Vector( transformed_position, MyTransform, current_position) ;
    (*Mycenter2).x = transformed_position[0] + Transform->data[0][3] ;
    (*Mycenter2).y = transformed_position[1] + Transform->data[1][3] ;
    (*Mycenter2).z = transformed_position[2] + Transform->data[2][3] ;

    Mat_Inverse( MyTransform, MyTransform ) ;

  /* free */
  free_vectorSHEN(current_position, 0, 3-1) ;
  free_vectorSHEN(transformed_position, 0, 3-1) ;
}





int main(int argc,char *argv[])
{
  int           i,j,c,num;
  FILE          *fp;
  extern char   *optarg;
  char          filename[80] ;
  Matrix        *Transform, *TransformCP, *MyTransform ;
  Fvector3d     Mycenter1, Mycenter2;
  float         temp ;
  float         Rx, Ry, Rz,   rx, ry, rz ;

  unsigned char ***src, ***outp ;
  int           x,y,z, k ;
  float         fx,fy,fz ;  

  num=3;

  if(argc<num)
    show_usage_SHEN() ;

  Rx = 0.9375 ; Ry = 0.9375 ; Rz = 1.5;
  rx = 0.9375 ; ry = 0.9375 ; rz = 1.5;
  while((c=getopt(argc-2,argv+2,"r:R:")) != -1)
    {
      switch(c)
	{
	case 'R':
	  sscanf(optarg, "%f,%f,%f", &Rx, &Ry, &Rz) ;
	  break;

	case 'r':
	  sscanf(optarg, "%f,%f,%f", &rx, &ry, &rz) ;
	  break;

	default:
	  break;
	}
    }
  printf("Reference: (%f,%f,%f); float: (%f,%f,%f)\n", Rx, Ry, Rz, rx, ry, rz ) ;


  /* apply for memory */
  CreateMatrix(&Transform,   4, 4);
  CreateMatrix(&TransformCP, 4, 4);
  CreateMatrix(&MyTransform, 3, 3);


  /* read transformation */ 
  if((fp=fopen(argv[1],"rb"))==NULL)
  {
    printf("cannot open file\n");
    exit(1) ;
  }
  fseek(fp,0L,SEEK_SET);
  for(i=0; i<4; i++)
    for(j=0; j<4; j++)
      {
	fscanf( fp,  "%f", &temp ) ;  
	Transform->data[i][j] = temp ;
      }
  fclose(fp);
  /*Mat_Print(Transform);*/
  Mat_Inverse( Transform, Transform ) ;
  /*Mat_Print(Transform);*/


  /* change their x,y to our x,y */
   /* copy */
   for(i=0; i<4; i++)
     for(j=0; j<4; j++)
       TransformCP->data[i][j] = Transform->data[i][j] ;
   /* change */
   for(i=0; i<2; i++)
     for(j=0; j<2; j++)
       Transform->data[i][j] = TransformCP->data[1-i][1-j] ;
   for(i=0; i<2; i++)
     for(j=2; j<4; j++)
       Transform->data[i][j] = TransformCP->data[1-i][j] ;  
   for(j=0; j<2; j++)
     Transform->data[2][j] = TransformCP->data[2][1-j] ;  
   printf("Matrix before transforming to my format:") ;
   Mat_Print(Transform);


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


   /* testing */
  /* apply for memory */ 
  /*src  = UCalloc3d(256, 256, 124);
  outp = UCalloc3d(256, 256, 124); 

  fp=myopen("DF02_cbq.img","r"); 
  fseek(fp,0,SEEK_END); 
  rewind(fp); 
  for(k=0;k<124;k++) 
    for(i=0;i<256;i++) 
      fread(src[k][i],1,256,fp); 
  fclose(fp); 
  for(k=0;k<124;k++) 
    for(i=0;i<256;i++)
      for(j=0;j<256;j++) 
	{ 
	  fx = Transform->data[0][0]*i + Transform->data[0][1]*j + Transform->data[0][2]*k + Transform->data[0][3] ;
	  fy = Transform->data[1][0]*i + Transform->data[1][1]*j + Transform->data[1][2]*k + Transform->data[1][3] ; 
	  fz = Transform->data[2][0]*i + Transform->data[2][1]*j + Transform->data[2][2]*k + Transform->data[2][3] ; 

	  if( fx>=0 && fx<256 && fy>=0 && fy<256 && fz>=0 && fz<124 )
	    {
	      x = fx ;
	      y = fy ;
	      z = fz ;
	      outp[k][i][j] = src[z][x][y] ;
	    }
	}
  printf("Write sampled file!\n") ;
  fp=myopen("out.img","w");
  for(k=0;k<124;k++)
    for(i=0;i<256;i++)
      fwrite(outp[k][i],1,256,fp);
      fclose(fp);*/



  Mat_Inverse( Transform, Transform ) ; /* from float to ref */ 
  Tranform2myformat( Transform, MyTransform, &Mycenter1, &Mycenter2) ;
  printf("Matrix at my format:") ;
  Mat_Print(MyTransform);
  Write_Transformation( MyTransform, Mycenter1, Mycenter2, argv[2]) ;
  
  return 0;
}


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\fslmat2mymat version: 1.0.%s \n\ ",shortVersion);
  printf("USAGE: fslmat2mymat FSL-matrix My-matrix\n\
\t \n\
\t -r <float,float,float>   : reference image voxel size,   default: -r0.9375,0.9375,1.5\n\
\t -R <float,float,float>   : float image voxel size,       default: -R0.9375,0.9375,1.5\n\
");
  exit(1);
}
