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


#define YYES    1
#define NNO     0


int  HeadInField, WriteHeadInField ;
void show_usage_SHEN() ;


void Open_Transformation( Matrix *Transform, Fvector3d *center1, Fvector3d *center2) {
    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) );
    
    
    if((fp=fopen("GlobalTransformation.Affine", "rb"))==NULL) {
        printf("cannot open file\n");
        return ;
    }
    
    fseek(fp, 0L, SEEK_SET);
    
    fread( Temp_transf,  sizeof(double), Tm*Tn,  fp ) ;
    fread( center1,      sizeof(Fvector3d),  1,  fp ) ;
    fread( center2,      sizeof(Fvector3d),  1,  fp ) ;
    
    fclose(fp);
    
    
    /* for T1 */
    for(i=0; i<Tm; i++)
        for(j=0; j<Tn; j++)
            Transform->data[i][j] = Temp_transf[i*Tn+j] ;
    
    /* free */
    free(Temp_transf) ;
}


void CompareVectorFields( Fvector3d ***DeformFld1, Fvector3d ***DeformFld2, int image_size, int z_size, char filename[80]) {
    Matrix *Transform ;
    Fvector3d center1, center2 ;
    float  *current_position, *transformed_position;
    FILE  *fp;
    int   i, j, k ;
    float difference;
    
    
    CreateMatrix(&Transform, 3, 3);
    current_position= vectorSHEN(0, 3-1) ;
    transformed_position= vectorSHEN(0, 3-1) ;
    
    Open_Transformation( Transform, &center1, &center2) ;
    Mat_Print(Transform);
    printf("center1=(%f, %f, %f)\n", (center1).x, (center1).y, (center1).z) ;
    printf("center2=(%f, %f, %f)\n", (center2).x, (center2).y, (center2).z) ;
    
    
    /* transform it */
    difference = 0.0;
    for(k=0;k<z_size;k++)
        for(i=0;i<image_size;i++)
            for(j=0;j<image_size;j++) {
                current_position[0] = (DeformFld1[k][i][j].x+i) - center2.x ;
                current_position[1] = (DeformFld1[k][i][j].y+j) - center2.y ;
                current_position[2] = (DeformFld1[k][i][j].z+k) - center2.z ;
                if((abs(DeformFld1[k][i][j].x - DeformFld2[k][i][j].x) > .01)|| (abs(DeformFld1[k][i][j].z - DeformFld2[k][i][j].z) > .01) || (abs(DeformFld1[k][i][j].z - DeformFld2[k][i][j].z) > .01)) {
                    printf("1:(%f,%f,%f)\n", DeformFld1[k][i][j].x, DeformFld1[k][i][j].y, DeformFld1[k][i][j].z);
                    printf("2:(%f,%f,%f)\n", DeformFld2[k][i][j].x, DeformFld2[k][i][j].y, DeformFld2[k][i][j].z);
                    difference += abs(DeformFld1[k][i][j].x - DeformFld2[k][i][j].x) + abs(DeformFld1[k][i][j].y - DeformFld2[k][i][j].y) + abs(DeformFld1[k][i][j].z - DeformFld2[k][i][j].z);
                }
                Mat_times_Vector( transformed_position, Transform, current_position) ;
                
                DeformFld1[k][i][j].x = (transformed_position[0]+center1.x)-i  ;
                DeformFld1[k][i][j].y = (transformed_position[1]+center1.y)-j  ;
                DeformFld1[k][i][j].z = (transformed_position[2]+center1.z)-k  ;
            }
    
    printf("difference=%f \n", difference);
    /* save it */
    fp=myopen(filename, "wb");
    if( HeadInField==YYES || WriteHeadInField==YYES ) {
        fwrite(&image_size, sizeof(int), 1, fp);
        fwrite(&z_size, sizeof(int), 1, fp);
    }
    for(k=0;k<z_size;k++)
        for(i=0;i<image_size;i++)
            fwrite(DeformFld1[k][i], sizeof(Fvector3d), image_size, fp);
    fclose(fp);
    
    /* free */
    //FreeMatrix(Transform);
    free_vectorSHEN(current_position, 0, 3-1) ;
    free_vectorSHEN(transformed_position, 0, 3-1) ;
}



void InterpolatedDisplacement(Fvector3d *Displace_subVoxel, float ii, float jj, float kk, Fvector3d ***DeformFld, int image_size, int z_size) {
    float b, c, d, b1, c1, d1;
    int   ni, nj, nk, niP1, njP1, nkP1;
    
    
    ni = (int)ii ;
    nj = (int)jj ;
    nk = (int)kk ;
    
    niP1 = ni+1 ;
    njP1 = nj+1 ;
    nkP1 = nk+1 ;
    
    (*Displace_subVoxel).x = 0 ;
    (*Displace_subVoxel).y = 0 ;
    (*Displace_subVoxel).z = 0 ;
    
    if(ni>=0 && ni<image_size-1  &&  nj>=0 && nj<image_size-1  &&  nk>=0 && nk<z_size-1 ) {
        b = ii-ni ;        b1 = 1.-b ;
        c = jj-nj ;        c1 = 1.-c ;
        d = kk-nk ;        d1 = 1.-d ;
        
        (*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 ;
    }
}





int main(int argc, char *argv[]) {
    int           image_size, z_size;
    Fvector3d     ***DeformFld1 ;
    Fvector3d     ***DeformFld2 ;
    int           i, k, c, num;
    FILE          *fp;
    extern char   *optarg;
    int           InverTransform ;
    
    
    num=3;
    
    if(argc<num)
        show_usage_SHEN() ;
    
    
    /* input image size */
    image_size= 256;
    z_size    = 124 ;
    HeadInField = YYES ;
    InverTransform = NNO ;
    WriteHeadInField = NNO ;
    
    while((c=getopt(argc-2, argv+2, "v:IH")) != -1) {
        switch(c) {
            case 'v':
                sscanf(optarg, "%d", &image_size) ; /* for vector field */
                HeadInField = NNO ;
                break ;
                
            case 'I':
                InverTransform = YYES ;
                break;
                
            case 'H':
                WriteHeadInField = YYES ;
                break;
                
            default:
                break;
        }
    }
    if(HeadInField==YYES) printf("There is size information in vector field file!\n") ;
    else                  printf("No head in vector field file!\n") ;
    
    
    /* open deformation field in the last resolution */
    fp=myopen(argv[1], "rb");
    if( HeadInField==YYES ) {
        fread(&image_size, sizeof(int), 1, fp); printf("image_size=%d ", image_size) ;
        fread(&z_size, sizeof(int), 1, fp);     printf("z_size=%d\n", z_size) ;
    }
    else {
        fseek(fp, 0, SEEK_END);
        z_size=ftell(fp)/(image_size*image_size*12);
        rewind(fp);
    }
    printf("vector field 1 size: image_size=%d  z_size=%d\n", image_size, z_size) ;
    
    DeformFld1 = Fvector3dalloc3d(image_size, image_size, z_size);
    
    for(k=0;k<z_size;k++)
        for(i=0;i<image_size;i++) {
            fread(DeformFld1[k][i], sizeof(Fvector3d), image_size, fp);
        }
    fclose(fp);
    /* open deformation field in the last resolution */
    fp=myopen(argv[2], "rb");
    if( HeadInField==YYES ) {
        fread(&image_size, sizeof(int), 1, fp); printf("image_size=%d ", image_size) ;
        fread(&z_size, sizeof(int), 1, fp);     printf("z_size=%d\n", z_size) ;
    }
    else {
        fseek(fp, 0, SEEK_END);
        z_size=ftell(fp)/(image_size*image_size*12);
        rewind(fp);
    }
    printf("vector field 2 size: image_size=%d  z_size=%d\n", image_size, z_size) ;
    
    DeformFld2 = Fvector3dalloc3d(image_size, image_size, z_size);
    
    for(k=0;k<z_size;k++)
        for(i=0;i<image_size;i++) {
            fread(DeformFld2[k][i], sizeof(Fvector3d), image_size, fp);
        }
    fclose(fp);
    printf("Vector fields reading finished!\n") ;
    
    /*debug
  for(k=0;k<z_size;k++)
    for(i=0;i<image_size;i++)
        for(j=0;j<image_size;j++)
            printf("The %d %d %d coordinate value is %f \n", i, j, k, DeformFld[i][j][k]);
     */
    /* perform transformation */
    if( InverTransform==YYES )
        //InverseTransformVectorFieldAndSaveIt( DeformFld, image_size, z_size, argv[2] ) ;
        CompareVectorFields( DeformFld1, DeformFld2, image_size, z_size, argv[3]) ;
    //else
    //TransformVectorFieldAndSaveIt( DeformFld1, image_size, z_size, 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\comparevectorfields version: 1.0.%s \n\
    USAGE: comparevectorfields <VectorField1> <VectorField2>\n\
    \t \n\
    \t The current deformation field is from model to the affine transformed subject. The goal of this program is to make the deformation field pointing from the model to the original subject, using transformation parameters saved under GlobalTransformation.Affine in the currect directory \n\
    \t \n\
    \t -v <int>             : xy_size(i.e. 256 ) in vector field for the case that there is no head info in the file\n\
    \t -I                   : The current one, the deformation is from the rigid transformed model to a certain subject. If we have file GlobalTransformation.Affine that links the rigid transformed model to its original version, then we can make the deformation field directly from the original model to that certain subject. \n\
    \t -H                   : write the sizes of fields(image_size, z_size) into the file\n\
    ",shortVersion);//
    exit(1);
}

