/**
 * @file  fslmat2mymat.cxx
 * @brief Originally part of HAMMER.
 *
 * Copyright (c) 2001, 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 <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <string.h>
#include <getopt.h>

#include "mvcd.h"
#include "cres.h"
#include "matrixSHEN.h"

#include "basis.h"

#define YYES    1
#define NNO     0


// acceptable in .cxx file
using namespace sbia;


// ===========================================================================
// help
// ===========================================================================

// ---------------------------------------------------------------------------
void show_usage()
{
    basis::print_version("fslmat2mymat", "", "");
    printf("\n");
    printf("USAGE: fslmat2mymat FSL-matrix My-matrix\n");
    printf("\n");
    printf("Optional arguments:\n");
    printf("  -r <float>,<float>,<float>   Reference image voxel size. (default: 0.9375,0.9375,1.5)\n");
    printf("  -R <float>,<float>,<float>   Float image voxel size. (default: 0.9375,0.9375,1.5)\n");
}

// ===========================================================================
// auxiliary functions
// ===========================================================================

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) ;
}

// ===========================================================================
// main
// ===========================================================================

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();
    exit(1);
  }

  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 ;
     }

  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;
}
