/*
 * File:   adaptivebrain.c
 * Author: andresmf
 *
 * Created on March 26, 2007, 9:38 AM
 */
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <strings.h>
#include "mvcd.h"
#include "cres.h"
#include "matrixSHEN.h"  /*by SHEN*/
#include "adaptivebrain.h"

void getWMEdgeGMVnEdgeFromSegmentedImg(ImgAttribute ***Img, int zSize, int imageSize) {
    int i, j, k ;
    int flag, strength ;
    int flag_VN, flag_GM ;
    
    //TODO add next line as vervose = 2
    printf("\nfinding WM edges and GM+VN edges ...\n") ;
    
    
    strength = 1 ;
    
    /* reset edge map */
    for(k=0;k<zSize;k++)
        for(i=0;i<imageSize;i++)
            for(j=0;j<imageSize;j++)
                Img[k][i][j].Edge=0 ;
    
    /* WM edge map */
    for(k=1;k<zSize-1;k++)
        for(i=1;i<imageSize-1;i++)
            for(j=1;j<imageSize-1;j++)
                if(Img[k][i][j].Tiss==WM) {
                    flag_GM = 0 ;
                    if(Img[k+1][i][j].Tiss==GM) flag_GM++;
                    if(Img[k-1][i][j].Tiss==GM) flag_GM++;
                    if(Img[k][i+1][j].Tiss==GM) flag_GM++;
                    if(Img[k][i-1][j].Tiss==GM) flag_GM++;
                    if(Img[k][i][j+1].Tiss==GM) flag_GM++;
                    if(Img[k][i][j-1].Tiss==GM) flag_GM++;
                    
                    flag_VN = 0 ;
                    if(Img[k+1][i][j].Tiss==VN) flag_VN++;
                    if(Img[k-1][i][j].Tiss==VN) flag_VN++;
                    if(Img[k][i+1][j].Tiss==VN) flag_VN++;
                    if(Img[k][i-1][j].Tiss==VN) flag_VN++;
                    if(Img[k][i][j+1].Tiss==VN) flag_VN++;
                    if(Img[k][i][j-1].Tiss==VN) flag_VN++;
                    
                    /* determine, whether edge? If edge, which type */
                    if( flag_GM>flag_VN )
                        Img[k][i][j].Edge = WMGM_EDGE ;
                    if( flag_GM<=flag_VN && flag_VN!=0 )
                        Img[k][i][j].Edge = WMVN_EDGE ;
                }
    
    /* June 4 2001: edge between GM and CSF/BG */
    for(k=1;k<zSize-1;k++)
        for(i=1;i<imageSize-1;i++)
            for(j=1;j<imageSize-1;j++)
                if(Img[k][i][j].Tiss==GM) {
                    flag = 0 ;
                    if(Img[k+1][i][j].Tiss<=CSF) flag++;
                    if(Img[k-1][i][j].Tiss<=CSF) flag++;
                    if(Img[k][i+1][j].Tiss<=CSF) flag++;
                    if(Img[k][i-1][j].Tiss<=CSF) flag++;
                    if(Img[k][i][j+1].Tiss<=CSF) flag++;
                    if(Img[k][i][j-1].Tiss<=CSF) flag++;
                    if( flag>=strength )
                        Img[k][i][j].Edge = GMCSFBG_EDGE ;
                }
    
    /* edge between GM and VN */
    for(k=1;k<zSize-1;k++)
        for(i=1;i<imageSize-1;i++)
            for(j=1;j<imageSize-1;j++)
                if(Img[k][i][j].Tiss==GM) {
                    flag = 0 ;
                    if(Img[k+1][i][j].Tiss==VN) flag++;
                    if(Img[k-1][i][j].Tiss==VN) flag++;
                    if(Img[k][i+1][j].Tiss==VN) flag++;
                    if(Img[k][i-1][j].Tiss==VN) flag++;
                    if(Img[k][i][j+1].Tiss==VN) flag++;
                    if(Img[k][i][j-1].Tiss==VN) flag++;
                    if( flag>=strength )
                        Img[k][i][j].Edge = GMVN_EDGE ;
                }
    
}

void calculateHoodSpecial(Fvector3d *bubble, int bubble_size, int *pixelNumInBubble, float xres, float yres, float zres) {
    int i, j, k, t, rad_sqrd, radius;
    
    t=0;
    radius=bubble_size+BASIC_R;
    rad_sqrd=radius*radius;
    for(i=-radius;i<=radius;i++)
        for(j=-radius;j<=radius;j++)
            for(k=-radius;k<=radius;k++)
                if((i*i+j*j+k*k)<=rad_sqrd) {
                    bubble[t].x=((float) i)/xres;
                    bubble[t].y=((float) j)/yres;
                    bubble[t].z=((float) k)/zres;
                    t++;
                }
    (*pixelNumInBubble) = t;
}

void computeGeometricFeatures(ImgAttribute ***Img, int scale, float Xres, float Yres, float Zres, int zSize, int imageSize) {
    int i, j, k, t, l, m, n ;
    float degree ;
    int   NumDown, value, VN_value, CSFBG_value, XYscale, Zscale ;
    
    int       bubble_size, pixelNumInBubble;
    Fvector3d *bubble ;
    
    //TODO add next line as vervose = 2
    printf("Compute geometric features for each edge voxel.\n") ;
    
    bubble = Fvector3dalloc1d(10000);
    bubble_size = scale ;
    calculateHoodSpecial(bubble, bubble_size, &pixelNumInBubble, Xres, Yres, Zres);
    //TODO add next line as vervose = 2
    printf("bubble_size=%d, pixelNumInBubble=%d\n", bubble_size, pixelNumInBubble) ;
    
    Zscale  = scale/Zres ;
    XYscale = scale/Xres ;
    
    /* action */
    for(k=Zscale;k<zSize-Zscale;k++)
        for(i=XYscale;i<imageSize-XYscale;i++)
            for(j=XYscale;j<imageSize-XYscale;j++) {
                if(Img[k][i][j].Edge==0) {
                    Img[k][i][j].Geom  = 0 ;
                    Img[k][i][j].VNvlm = 0 ;
                    Img[k][i][j].CSFBG = 0 ;
                }
                
                /* limit computation candidates */
                if(Img[k][i][j].Edge!=0 ) {
                    NumDown = 0 ;
                    VN_value = 0 ;
                    CSFBG_value = 0 ;
                    for(t=0; t<pixelNumInBubble; t++) {
                        l = (int)bubble[t].x ;
                        m = (int)bubble[t].y ;
                        n = (int)bubble[t].z ;
                        
                        if( Img[k+n][i+l][j+m].Tiss!=WM )  NumDown++ ;
                        if( Img[k+n][i+l][j+m].Tiss==VN )  VN_value++ ;
                        if( Img[k+n][i+l][j+m].Tiss<=CSF)  CSFBG_value++ ;
                    }
                    degree = ((float)NumDown/(float)pixelNumInBubble) ;
                    value = degree*255*1.2 ; /*degree*degree*255*/
                    if( value>255 ) value = 255 ;
                    Img[k][i][j].Geom = (unsigned char)value ;
                    /*printf("(%d,%d,%d)=%d\n", k, i, j, NumDown) ;*/
                    
                    /* VN volume */
                    VN_value = VN_value*255*1.2/pixelNumInBubble ;
                    if( VN_value>255 ) VN_value=255 ;
                    Img[k][i][j].VNvlm = (unsigned char)VN_value ;
                    
                    /* CSF/BG volume */
                    CSFBG_value = CSFBG_value*255*1.2/pixelNumInBubble ;
                    if( CSFBG_value>255 ) CSFBG_value=255 ;
                    Img[k][i][j].CSFBG = (unsigned char)CSFBG_value ;
                }
            }
    
    /* free */
    free(bubble);
}

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 writeImgAttribute(char filename[180], ImgAttribute ***Img, int image_size, int z_size) {
    int   i, j, k ;
    unsigned char ***TempImg;
    char fn[180] ;
    
    TempImg = UCalloc3d(image_size, image_size, z_size);
    
    /* Tissue */
    for(k=0;k<z_size;k++)
        for(i=0;i<image_size;i++)
            for(j=0;j<image_size;j++)
                TempImg[k][i][j] = Img[k][i][j].Tiss ;
//    sprintf(fn, "%s_Tissue", filename);
//    writeImg(fn,   TempImg,   image_size, z_size) ;
    
    /* Edge */
    for(k=0;k<z_size;k++)
        for(i=0;i<image_size;i++)
            for(j=0;j<image_size;j++)
                TempImg[k][i][j] = Img[k][i][j].Edge ;
    //sprintf(fn, "%s_Edge", filename);
    //writeImg(fn,   TempImg,   image_size, z_size) ;
    
    /* Geometric */
    for(k=0;k<z_size;k++)
        for(i=0;i<image_size;i++)
            for(j=0;j<image_size;j++)
                TempImg[k][i][j] = Img[k][i][j].Geom ;
//    sprintf(fn, "%s_Geometric", filename);
//    writeImg(fn,   TempImg,   image_size, z_size) ;
    
    /* VN volume */
    for(k=0;k<z_size;k++)
        for(i=0;i<image_size;i++)
            for(j=0;j<image_size;j++)
                TempImg[k][i][j] = Img[k][i][j].VNvlm ;
//    sprintf(fn, "%s_VNvolume", filename);
//    writeImg(fn,   TempImg,   image_size, z_size) ;
    
    /* CSF/BG volume */
    for(k=0;k<z_size;k++)
        for(i=0;i<image_size;i++)
            for(j=0;j<image_size;j++)
                TempImg[k][i][j] = Img[k][i][j].CSFBG ;
    //sprintf(fn, "%s_CSFBG", filename);
    //writeImg(fn,   TempImg,   image_size, z_size) ;
    
    UCfree3d(TempImg, z_size, image_size) ;
}

void calculateHoodByIncreasingRadius(Ivector3d *Hood, int HoodSize, int *pixelNumInHood, float xres, float yres, float zres) {
    int        x, y, z, t, r  ;
    int        half ;
    
    
    t = 0 ;  Hood[0].x = 0 ;  Hood[0].y = 0 ;  Hood[0].z = 0 ;
    t++ ;
    half = HoodSize/2;
    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[t].x = x/xres ;
                    Hood[t].y = y/yres ;
                    Hood[t].z = z/zres ;
                    
                    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[t].x = x/xres ;
                    Hood[t].y = y/yres ;
                    Hood[t].z = z/zres ;
                    
                    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[t].x = x/xres ;
                    Hood[t].y = y/yres ;
                    Hood[t].z = z/zres ;
                    
                    t++ ;
                    /*printf("t=%d: (%d,%d,%d)\n", t,x,y,z) ;*/
                }
    }
    (*pixelNumInHood) = t ;
    //TODO add next line as vervose = 2
    printf("total %d in the search area!\n", t) ;
}

void readModelSurfVer(char SurfVerFileName[180], int *InputSurfVer, int maxNbr) {
    int i ;
    FILE  *fp;
    Fvector3d  *surfVer;
    Fvector3d  *surfNorms;
    int        **surfNbr;
    int        surfVerNum;
    
    
    /***** allocate memory for surface and initialize arrays *****/
    //TODO add next line as vervose = 2
    printf("%s: max_nbr=%d\n\n\n", SurfVerFileName, maxNbr) ; /* a file name likes "MDL.ver.norm.nbr" */
    
    if( (fp=fopen(SurfVerFileName, "rb"))==NULL ) {
        printf("\n\nThis is invalidate surface model. \nProgram will not output any deformation result on the surface model.\n\n");
        (*InputSurfVer) = NNO ;
    }
    else {
        fseek(fp, 0, SEEK_END);
        surfVerNum = ftell(fp)/(6*4+(maxNbr+1)*4); /*48*/
        rewind(fp);
        
        surfVer   = Fvector3dalloc1d(surfVerNum);
        surfNorms = Fvector3dalloc1d(surfVerNum);
        surfNbr   = Ialloc2d(surfVerNum, maxNbr+1); /* 6+1 */
        
        fread(surfVer,  sizeof(Fvector3d), surfVerNum, fp);
        fread(surfNorms, sizeof(Fvector3d), surfVerNum, fp);
        for(i=0;i<surfVerNum;i++)
            fread(surfNbr[i], sizeof(int), maxNbr+1, fp);
        fclose(fp);
    }
}

void calculateHood(Fvector3d **hood, int *nbr_size, float xres, float yres, float zres) {
    int i, j, k, s, t, rad_sqrd, radius;
    
    /*  printf("xres=%f yres=%f zres=%f\n",xres,yres,zres); */
    for(s=0;s<10;s++) {
        t=0;
        radius=s+BSIC_RAD;
        rad_sqrd=radius*radius;
        for (i=-radius;i<=radius;i++)
            for(j=-radius;j<=radius;j++)
                for(k=-radius;k<=radius;k++) {
                    if((i*i+j*j+k*k)<=rad_sqrd) {
                        hood[s][t].x=((float) i)/xres;
                        hood[s][t].y=((float) j)/yres;
                        hood[s][t].z=((float) k)/zres;
                        t++;
                    }
                }
        nbr_size[s]=t;
    }
    
}

/*scaling and shifting*/
void estimatingScalingAndShiftOf3DSurfaceFromImg(Fvector3d *ModelVer, Ivector3d *FixedMdlVer, ImgAttribute *MdlVer_Fea, int MdlVer_Num, Fvector3d ***DeformFld, ImgAttribute ***ObjImg, int image_size, int z_size ) {
    int   i, j, k, s, t, real_size, max_shift ;
    float CurrentDegree, OverlapDegree, MaxDegree ;
    int x, y, z ;
    Fvector3d Crnt, center, shift, estimated_shift ;
    Fvector3d scaling, estimated_scaling;
    
    /* for estimating the overlapping degree */
    int bubble_size[BUBBNUM] ;
    Fvector3d **MultiBubble, cBubble ;
    
    MultiBubble = Fvector3dalloc2d(BUBBNUM, 10000);
    /* prepare for overlapping estimation */
    calculateHood(MultiBubble, bubble_size, 1.0, 1.0, 1.0);
    
    //TODO add next line as vervose = 2
    printf("Estimating the scaling and the shifting\n") ;
    
    /* calculate center of the surface */
    center.x=center.y=center.z=0 ;
    for(s=0; s<MdlVer_Num; s++) {
        center.x += ModelVer[s].x/MdlVer_Num ;
        center.y += ModelVer[s].y/MdlVer_Num ;
        center.z += ModelVer[s].z/MdlVer_Num ;
    }
    
    
    t=2; /*0, 3 */ MaxDegree=-1.0 ;
    max_shift = 0.0 ; /* 0, 6.0 */
    for(scaling.x=0.85;  scaling.x<1.35;  scaling.x+=0.05)
        for(scaling.y=0.85;  scaling.y<1.35;  scaling.y+=0.05)
            for(scaling.z=0.85; scaling.z<1.35; scaling.z+=0.05) {
                //TODO add next line as vervose = 2
                printf("scaling.x=%f  scaling.y=%f  scaling.z=%f\n", scaling.x, scaling.y, scaling.z) ;
                for(shift.x=-max_shift; shift.x<=max_shift; shift.x+=2.0)
                    for(shift.y=-max_shift; shift.y<=max_shift; shift.y+=2.0)
                        for(shift.z=-max_shift; shift.z<=max_shift; shift.z+=2.0) {
                            /*printf("shift.x=%f shift.y=%f shift.z=%f\n", shift.x, shift.y, shift.z) ;*/
                            
                            /* on surface */
                            OverlapDegree = 0 ;
                            for(s=0; s<MdlVer_Num; s+=3) /*3, 30*/ {
                                Crnt.x = scaling.x*(ModelVer[s].x-center.x) + center.x + shift.x ;
                                Crnt.y = scaling.y*(ModelVer[s].y-center.y) + center.y + shift.y ;
                                Crnt.z = scaling.z*(ModelVer[s].z-center.z) + center.z + shift.z ;
                                
                                real_size = 0 ;  CurrentDegree = 0 ;
                                cBubble.x=cBubble.y=cBubble.z=0;
                                for(j=0;j<bubble_size[t];j++) {
                                    x = Crnt.x+MultiBubble[t][j].x ;
                                    y = Crnt.y+MultiBubble[t][j].y ;
                                    z = Crnt.z+MultiBubble[t][j].z ;
                                    
                                    if( x<image_size && y<image_size && z<z_size && x>=0 && y>=0 && z>=0 && ObjImg[z][x][y].Edge==MdlVer_Fea[s].Edge ) {
                                        real_size ++ ;
                                        
                                        cBubble.x += (MultiBubble[t][j].x) ;
                                        cBubble.y += (MultiBubble[t][j].y) ;
                                        cBubble.z += (MultiBubble[t][j].z) ;
                                    }
                                }
                                if( real_size>0 ) {
                                    cBubble.x /= (real_size*(t+3)) ;
                                    cBubble.y /= (real_size*(t+3)) ;
                                    cBubble.z /= (real_size*(t+3)) ;
                                    
                                    CurrentDegree = 1.0 - sqrt(cBubble.x*cBubble.x + cBubble.y*cBubble.y + cBubble.z*cBubble.z) ;
                                }
                                else
                                    CurrentDegree = 0 ;
                                
                                if( CurrentDegree>0.1 )  CurrentDegree=1.0 ;
                                else                     CurrentDegree=0.  ;
                                
                                OverlapDegree += CurrentDegree/MdlVer_Num ;
                            }
                            
                            if( OverlapDegree>MaxDegree ) {
                                MaxDegree = OverlapDegree ;
                                estimated_shift.x  = shift.x ;
                                estimated_shift.y  = shift.y ;
                                estimated_shift.z  = shift.z ;
                                estimated_scaling.x = scaling.x ;
                                estimated_scaling.y = scaling.y ;
                                estimated_scaling.z = scaling.z ;
                            }
                        }
                //printf("      MaxDegree=%f\n", MaxDegree) ;
            }
    //TODO add next line as vervose = 2
    printf(" estimated_shift.x=%f\n estimated_shift.y=%f\n estimated_shift.z=%f\n estimated_scaling.x=%f\n estimated_scaling.y=%f\n estimated_scaling.z=%f\n", estimated_shift.x, estimated_shift.y, estimated_shift.z, estimated_scaling.x, estimated_scaling.y, estimated_scaling.z) ;
    
    /* update by estimation */
    for(k=0;k<z_size;k++)
        for(i=0;i<image_size;i++)
            for(j=0;j<image_size;j++) {
                Crnt.x = DeformFld[k][i][j].x + i ;
                Crnt.y = DeformFld[k][i][j].y + j ;
                Crnt.z = DeformFld[k][i][j].z + k ;
                
                DeformFld[k][i][j].x = estimated_scaling.x*(Crnt.x-center.x) + center.x + estimated_shift.x -i ;
                DeformFld[k][i][j].y = estimated_scaling.y*(Crnt.y-center.y) + center.y + estimated_shift.y -j ;
                DeformFld[k][i][j].z = estimated_scaling.z*(Crnt.z-center.z) + center.z + estimated_shift.z -k ;
                
                /* boundary constraints */
                if(DeformFld[k][i][j].x+i<0) DeformFld[k][i][j].x=-i ;
                if(DeformFld[k][i][j].y+j<0) DeformFld[k][i][j].y=-j ;
                if(DeformFld[k][i][j].z+k<0) DeformFld[k][i][j].z=-k ;
                
                if(DeformFld[k][i][j].x+i>=image_size) DeformFld[k][i][j].x=image_size-i ;
                if(DeformFld[k][i][j].y+j>=image_size) DeformFld[k][i][j].y=image_size-j ;
                if(DeformFld[k][i][j].z+k>=z_size)     DeformFld[k][i][j].z=z_size-k ;
            }
    //TODO add next line as vervose = 2
    printf("Estimation finished!\n") ;
    
    for(s=0; s<MdlVer_Num; s++) {
        x = FixedMdlVer[s].x ;
        y = FixedMdlVer[s].y ;
        z = FixedMdlVer[s].z ;
        
        if( x<image_size && y<image_size && z<z_size && x>=0 && y>=0 && z>=0 ) {
            ModelVer[s].x = DeformFld[z][x][y].x + x ;
            ModelVer[s].y = DeformFld[z][x][y].y + y ;
            ModelVer[s].z = DeformFld[z][x][y].z + z ;
        }
        //TODO add next line as vervose = 2
        else
            printf("see here: (%d,%d,%d)\n", x, y, z) ;
    }
    //TODO add next line as vervose = 2
    printf("Estimation finished!\n") ;
    
    /* free */
    Fvector3dfree2d(MultiBubble, BUBBNUM) ;
}


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 saveCurrentWarpingResultOnSegmentedObj( ImgAttribute ***ObjImg, Fvector3d ***DeformFld, int image_size, int z_size, int XYZres, int fileOrder, int interpolate) {
    int i, j, k, x, y, z ;
    char  filename[180] ;
    unsigned char ***resImg, ***Img ;
    float  ii, jj, kk ;
    unsigned char GreyValue, MappedV ;
    
    
    Img    = UCalloc3d(image_size, image_size, z_size);
    resImg = UCalloc3d(image_size, image_size, z_size);
    
    /* get intensity image in ObjImg */
    for(k=0; k<z_size; k++)
        for(i=0; i<image_size; i++)
            for(j=0; j<image_size; j++)
                Img[k][i][j] = ObjImg[k][i][j].Tiss ;
    
    
    for(k=0; k<z_size; k++)
        for(i=0; i<image_size; i++)
            for(j=0; j<image_size; j++) {
                if(interpolate==YYES) {
                    ii = i + DeformFld[k][i][j].x ;
                    jj = j + DeformFld[k][i][j].y ;
                    kk = k + DeformFld[k][i][j].z ;
                    
                    GreyValue = interpolatedIntensity(ii, jj, kk, Img, image_size, z_size) ;
                    
                    if( GreyValue>=BG          && GreyValue<=(BG+CSF)/2 )
                        MappedV = BG ;
                    if( GreyValue>=(BG+CSF)/2  && GreyValue<=(CSF+VN)/2 )
                        MappedV = CSF ;
                    if( GreyValue>=(CSF+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 ;
                        
                        resImg[k][i][j] = MappedV ;
                }
                else {
                    x = i + DeformFld[k][i][j].x + 0.5 ;
                    y = j + DeformFld[k][i][j].y + 0.5 ;
                    z = k + DeformFld[k][i][j].z + 0.5 ;
                    
                    if( x<image_size && y<image_size && z<z_size && x>=0 && y>=0 && z>=0 )
                        resImg[k][i][j] = Img[z][x][y] ;
                    else
                        resImg[k][i][j] = 0 ;
                }
            }
    
    /* save */
    sprintf(filename, "Res.img.sample%d.%d", XYZres, fileOrder);
    writeImg(filename, resImg, image_size, z_size) ;
    
    /* free */
    UCfree3d(resImg, z_size, image_size) ;
    UCfree3d(Img,    z_size, image_size) ;
}

void saveCurrentWarpingResultOnOriginalObjImg( unsigned char ***ObjOriginalImg, Fvector3d ***DeformFld, int image_size, int z_size, int XYZres, int fileOrder, int interpolate) {
    int i, j, k, x, y, z ;
    char  filename[180] ;
    unsigned char ***resImg ;
    float  ii, jj, kk ;
    
    resImg = UCalloc3d(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++) {
                if(interpolate==YYES) {
                    ii = i + DeformFld[k][i][j].x ;
                    jj = j + DeformFld[k][i][j].y ;
                    kk = k + DeformFld[k][i][j].z ;
                    
                    resImg[k][i][j] = interpolatedIntensity(ii, jj, kk, ObjOriginalImg, image_size, z_size) ;
                }
                else {
                    x = i + DeformFld[k][i][j].x + 0.5 ;
                    y = j + DeformFld[k][i][j].y + 0.5 ;
                    z = k + DeformFld[k][i][j].z + 0.5 ;
                    
                    if( x<image_size && y<image_size && z<z_size && x>=0 && y>=0 && z>=0 )
                        resImg[k][i][j] = ObjOriginalImg[z][x][y] ;
                    else
                        resImg[k][i][j] = 0 ;
                }
            }
    
    /* save */
    sprintf(filename, "Warped.OriginalImg.sample%d.%d", XYZres, fileOrder);
    writeImg(filename, resImg, image_size, z_size) ;
    
    /* free */
    UCfree3d(resImg, z_size, image_size) ;
}

void saveDeformedModelSurfVer(Fvector3d ***DeformFld, int image_size, int z_size, int XYZres, int fileOrder, int maxNbr) {
    
    int        s, x, y, z, i ;
    Fvector3d  *TempVer ;
    FILE  *fp;
    char  filename[180] ;
    Fvector3d  *surfVer;
    Fvector3d  *surfNorms;
    int        **surfNbr;
    int        surfVerNum;
    
    /*space*/
    TempVer = Fvector3dalloc1d(surfVerNum);
    
    for(s=0; s<surfVerNum; s++) {
        x = surfVer[s].x + 0.5 ;
        y = surfVer[s].y + 0.5 ;
        z = surfVer[s].z + 0.5 ;
        
        if( x<image_size && y<image_size && z<z_size && x>=0 && y>=0 && z>=0 ) {
            TempVer[s].x = surfVer[s].x + DeformFld[z][x][y].x ;
            TempVer[s].y = surfVer[s].y + DeformFld[z][x][y].y ;
            TempVer[s].z = surfVer[s].z + DeformFld[z][x][y].z ;
        }
        else {
            TempVer[s].x = surfVer[s].x ;
            TempVer[s].y = surfVer[s].y ;
            TempVer[s].z = surfVer[s].z ;
        }
    }
    
    /* save */
    sprintf(filename, "SurfaceMdl.Deformed.ver.norm.nbr.sample%d.%d", XYZres, fileOrder);
    fp=myopen(filename, "wb");
    fwrite(TempVer, sizeof(Fvector3d), surfVerNum, fp);
    fwrite(surfNorms, sizeof(Fvector3d), surfVerNum, fp);
    for(i=0;i<surfVerNum;i++)
        fwrite(surfNbr[i], sizeof(int), maxNbr+1, fp);
    fclose(fp);
    
    /* free */
    free(TempVer) ;
}


void openDeformationField(char filename[180], Fvector3d ***DeformFld, int image_size, int z_size, float confidenceOnLastDeformation) {
    FILE  *fp;
    Fvector3d ***Last_DeformFld ;
    int Last_image_size, Last_z_size ;
    float XYZ_ratio, deform_ratio, dx, dy, dz ;
    /* for linear interpolation */
    float ii, jj, kk, b, c, d, b1, c1, d1;
    int   i, j, k, ni, nj, nk, niP1, njP1, nkP1;
    float alpha ;
    
    /* open deformation field in the last resolution */
    fp=myopen(filename, "rb");
    fread(&Last_image_size, sizeof(int), 1, fp); //TODO add next line as vervose = 2 
    printf("Last_image_size=%d ", Last_image_size) ;
    fread(&Last_z_size, sizeof(int), 1, fp);     //TODO add next line as vervose = 2 
    printf("Last_z_size=%d\n", Last_z_size) ;
    
    Last_DeformFld = Fvector3dalloc3d(Last_image_size, Last_image_size, Last_z_size);
    
    for(k=0;k<Last_z_size;k++)
        for(i=0;i<Last_image_size;i++)
            fread(Last_DeformFld[k][i], sizeof(Fvector3d), Last_image_size, fp);
    fclose(fp);
    
    
    /* interpolation */
    XYZ_ratio = (float)Last_z_size/(float)z_size ;
    
    deform_ratio = 1.0/XYZ_ratio ;
    alpha        = 1.0-confidenceOnLastDeformation ;  /*0.5 ; before Mar 2005 */  /* it was directly 1.0 at June 18 2001 ~ Marc 25 2003; theoretically it should be 0. */
    deform_ratio = ( 1.0*alpha + deform_ratio*(1.0-alpha) ) ;  /* changed back June 18, 2001 */
    for(k=0; k<z_size; k++)
        for(i=0; i<image_size; i++)
            for(j=0; j<image_size; j++) {
                ii = i*XYZ_ratio ;
                jj = j*XYZ_ratio ;
                kk = k*XYZ_ratio ;
                
                ni = (int)ii ;
                nj = (int)jj ;
                nk = (int)kk ;
                
                niP1 = ni+1 ;
                njP1 = nj+1 ;
                nkP1 = nk+1 ;
                
                if(ni>=0 && ni<Last_image_size-1  &&  nj>=0 && nj<Last_image_size-1  &&  nk>=0 && nk<Last_z_size-1 ) {
                    b = ii-ni ;        b1 = 1.-b ;
                    c = jj-nj ;        c1 = 1.-c ;
                    d = kk-nk ;        d1 = 1.-d ;
                    
                    dx = ( d1*(Last_DeformFld[nk][ni][nj].x*(b1*c1) + Last_DeformFld[nk][niP1][nj].x*(b*c1) + Last_DeformFld[nk][ni][njP1].x*(b1*c) + Last_DeformFld[nk][niP1][njP1].x*(b*c)) + d*(Last_DeformFld[nkP1][ni][nj].x*(b1*c1) + Last_DeformFld[nkP1][niP1][nj].x*(b*c1) + Last_DeformFld[nkP1][ni][njP1].x*(b1*c) + Last_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)) ) ;
                    dy = ( d1*(Last_DeformFld[nk][ni][nj].y*(b1*c1) + Last_DeformFld[nk][niP1][nj].y*(b*c1) + Last_DeformFld[nk][ni][njP1].y*(b1*c) + Last_DeformFld[nk][niP1][njP1].y*(b*c)) + d*(Last_DeformFld[nkP1][ni][nj].y*(b1*c1) + Last_DeformFld[nkP1][niP1][nj].y*(b*c1) + Last_DeformFld[nkP1][ni][njP1].y*(b1*c) + Last_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)) ) ;
                    dz = ( d1*(Last_DeformFld[nk][ni][nj].z*(b1*c1) + Last_DeformFld[nk][niP1][nj].z*(b*c1) + Last_DeformFld[nk][ni][njP1].z*(b1*c) + Last_DeformFld[nk][niP1][njP1].z*(b*c)) + d*(Last_DeformFld[nkP1][ni][nj].z*(b1*c1) + Last_DeformFld[nkP1][niP1][nj].z*(b*c1) + Last_DeformFld[nkP1][ni][njP1].z*(b1*c) + Last_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)) ) ;
                    
                    DeformFld[k][i][j].x = dx*deform_ratio ; /* *deform_ratio, June 14, 2001*/
                    DeformFld[k][i][j].y = dy*deform_ratio ;
                    DeformFld[k][i][j].z = dz*deform_ratio ;
                }
                
                if(((ni==Last_image_size-1) && (nj>=0) && (nj<Last_image_size-1) && (nk>=0) && (nk<Last_z_size-1)) || ((ni>=0) && (ni<Last_image_size-1) && (nj==Last_image_size-1) && (nk>=0) && (nk<Last_z_size-1))  || ((ni>=0) && (ni<Last_image_size-1) && (nj>=0) && (nj<Last_image_size-1) && (nk==Last_z_size-1))) {
                    DeformFld[k][i][j].x = Last_DeformFld[nk][ni][nj].x*deform_ratio ; /* *deform_ratio, June 14, 2001*/
                    DeformFld[k][i][j].y = Last_DeformFld[nk][ni][nj].y*deform_ratio ;
                    DeformFld[k][i][j].z = Last_DeformFld[nk][ni][nj].z*deform_ratio ;
                }
                
                /* boundary constraints */
                if(DeformFld[k][i][j].x+i<0) DeformFld[k][i][j].x=-i ;
                if(DeformFld[k][i][j].y+j<0) DeformFld[k][i][j].y=-j ;
                if(DeformFld[k][i][j].z+k<0) DeformFld[k][i][j].z=-k ;
                
                if(DeformFld[k][i][j].x+i>=(image_size-1)) DeformFld[k][i][j].x=(image_size-1)-i ;
                if(DeformFld[k][i][j].y+j>=(image_size-1)) DeformFld[k][i][j].y=(image_size-1)-j ;
                if(DeformFld[k][i][j].z+k>=(z_size-1))     DeformFld[k][i][j].z=(z_size-1)-k ;
            }
    
    /* free */
    Fvector3dfree3d(Last_DeformFld, Last_z_size, Last_image_size) ;
}

float similarityBetweenFocusedPointsInMdlObjImgs( ImgAttribute *ObjVer_Fea, int l, ImgAttribute *MdlVer_Fea, int s) {
    float Degree ;
    
    Degree = 0 ;
    if( ObjVer_Fea[l].Edge==MdlVer_Fea[s].Edge ) {
        Degree  =  1.0-fabs(MdlVer_Fea[s].Geom -ObjVer_Fea[l].Geom)/255.0 ;
        Degree *= (1.0-fabs(MdlVer_Fea[s].VNvlm-ObjVer_Fea[l].VNvlm)/255.0) ;
        Degree *= (1.0-fabs(MdlVer_Fea[s].CSFBG-ObjVer_Fea[l].CSFBG)/255.0) ;
        Degree *= (1.0-fabs(MdlVer_Fea[s].Tiss -ObjVer_Fea[l].Tiss)/255.0) ;
    }
    
    return Degree ;
}

/* To speed up the algorithm */
void shiftByGuassianWeight(Fvector3d In, Fvector3d *Tmp, Fvector3d Search, float level, int max) {
    float weight ;
    
    weight = exp(-level*level/(2.*max*max)) ;
    
    (*Tmp).x = In.x + Search.x*weight ;
    (*Tmp).y = In.y + Search.y*weight ;
    (*Tmp).z = In.z + Search.z*weight ;
}

float similarityBetweenFeaturesInMdlAndObjImgs( ImgAttribute ***ObjImg, int x, int y, int z, ImgAttribute ***MdlImg, int i, int j, int k ) {
    float Degree ;
    
    Degree = 0 ;
    if( ObjImg[z][x][y].Edge==MdlImg[k][i][j].Edge ) {
        Degree  =  1.0-fabs(MdlImg[k][i][j].Geom -ObjImg[z][x][y].Geom)/255.0 ;
        Degree *= (1.0-fabs(MdlImg[k][i][j].VNvlm-ObjImg[z][x][y].VNvlm)/255.0) ;
        Degree *= (1.0-fabs(MdlImg[k][i][j].CSFBG-ObjImg[z][x][y].CSFBG)/255.0) ;
        Degree *= (1.0-fabs(MdlImg[k][i][j].Tiss -ObjImg[z][x][y].Tiss)/255.0) ;
    }
    
    return Degree ;
}

float similarityBetweenFeaturesInImgAndFeaturesInMdl( ImgAttribute ***ObjImg, int x, int y, int z, ImgAttribute *MdlVer_Fea, int s) {
    float Degree ;
    
    Degree = 0 ;
    if( ObjImg[z][x][y].Edge==MdlVer_Fea[s].Edge ) {
        Degree  =  1.0-fabs(MdlVer_Fea[s].Geom -ObjImg[z][x][y].Geom)/255.0 ;
        Degree *= (1.0-fabs(MdlVer_Fea[s].VNvlm-ObjImg[z][x][y].VNvlm)/255.0) ;
        Degree *= (1.0-fabs(MdlVer_Fea[s].CSFBG-ObjImg[z][x][y].CSFBG)/255.0) ;
        Degree *= (1.0-fabs(MdlVer_Fea[s].Tiss -ObjImg[z][x][y].Tiss)/255.0) ;
    }
    
    return Degree ;
}

void globalAffineUpdatePlusLocalDeformation( Fvector3d *verObject, Fvector3d *verModel, int MdlVer_Num, Matrix *transform) {
    int i, j, k ;
    Matrix *ModelMatrix, *ObjectMatrix, *FittingMatrix, *TmpM1, *TmpM2, *InversedMtrx/*, *Transform*/ ;
    
    
    
    /* create  metrices*/
    CreateMatrix(&ModelMatrix,  4, MdlVer_Num);
    CreateMatrix(&ObjectMatrix, 4, MdlVer_Num);
    CreateMatrix(&FittingMatrix, 4, MdlVer_Num);
    CreateMatrix(&TmpM1,        4, 4);
    CreateMatrix(&TmpM2,        4, 4);
    CreateMatrix(&InversedMtrx, 4, 4);
    /*   CreateMatrix(&Transform,    4, 4);*/
    
    
    /* get ModelMatrix->data[4][MdlVer_Num] */
    for(j=0; j<MdlVer_Num; j++) {
        ModelMatrix->data[0][j] = verModel[j].x ;
        ModelMatrix->data[1][j] = verModel[j].y ;
        ModelMatrix->data[2][j] = verModel[j].z ;
        ModelMatrix->data[3][j] = 1 ;
    }
    
    /* get ObjectMatrix->data[4][MdlVer_Num] */
    for(j=0; j<MdlVer_Num; j++) {
        ObjectMatrix->data[0][j] = verObject[j].x ;
        ObjectMatrix->data[1][j] = verObject[j].y ;
        ObjectMatrix->data[2][j] = verObject[j].z ;
        ObjectMatrix->data[3][j] = 1 ;
    }
    
    
    /*get matrix 4X4 TmpM1*/
    for(i=0; i<4; i++)
        for(j=0; j<4; j++) {
            TmpM1->data[i][j] = 0 ;
            
            for(k=0; k<MdlVer_Num; k++)
                TmpM1->data[i][j] += (ModelMatrix->data[i][k]*ModelMatrix->data[j][k]) ;
        }
    
    
    /*get matrix 4X4 TmpM2 */
    for(i=0; i<4; i++)
        for(j=0; j<4; j++) {
            TmpM2->data[i][j] = 0 ;
            
            for(k=0; k<MdlVer_Num; k++)
                TmpM2->data[i][j] += (ObjectMatrix->data[i][k]*ModelMatrix->data[j][k]) ;
        }
    
    
    Mat_Inverse( TmpM1, InversedMtrx ) ;
    
    /* get affine transformation */
    Mat_A_equal_BxC(transform, TmpM2, InversedMtrx) ;             /*A=BxC*/
    
    
    /* use model to best fit the object */
    Mat_A_equal_BxC(FittingMatrix, transform, ModelMatrix) ;
    
    
    /* back to model variables */
    for(j=0; j<MdlVer_Num; j++) {
        verModel[j].x = FittingMatrix->data[0][j] ;
        verModel[j].y = FittingMatrix->data[1][j] ;
        verModel[j].z = FittingMatrix->data[2][j] ;
    }
    
    
    /* free */
    FreeMatrix(ModelMatrix) ;
    FreeMatrix(ObjectMatrix) ;
    FreeMatrix(FittingMatrix) ;
    FreeMatrix(TmpM1) ;
    FreeMatrix(TmpM2) ;
    FreeMatrix(InversedMtrx) ;
    /*   FreeMatrix(Transform) ;*/
}

void smoothDeformationField(Fvector3d ***DeformFld, int image_size, int z_size) {
    int i, j, k, n ;
    int x, y, z ;
    Ivector3d  *nb_pnt ;
    int        nb_size, count, half, temp_num ;
    Fvector3d  nbr_tot ;
    float      LocalRatio ;
    //TODO add next line as vervose = 2
    printf("smoothing ...\n") ;
    nb_size = 3 ;
    nb_pnt= Ivector3dalloc1d(nb_size*nb_size*nb_size) ;
    calculateHoodByIncreasingRadius(nb_pnt, nb_size, &count, 1., 1., 1.) ;
    
    half=nb_size/2;
    LocalRatio = 0.5 ;
    for(k=0;k<z_size;k++)
        for(i=0;i<image_size;i++)
            for(j=0;j<image_size;j++) {
                nbr_tot.x=0; nbr_tot.y=0; nbr_tot.z=0; temp_num=0;
                for(n=1;n<27;n++) /* 26 neighbor */ {
                    x = i + nb_pnt[n].x ;
                    y = j + nb_pnt[n].y ;
                    z = k + nb_pnt[n].z ;
                    
                    if( x<image_size && y<image_size && z<z_size && x>=0 && y>=0 && z>=0 ) {
                        nbr_tot.x += DeformFld[z][x][y].x;
                        nbr_tot.y += DeformFld[z][x][y].y;
                        nbr_tot.z += DeformFld[z][x][y].z;
                        
                        temp_num++ ;
                    }
                }
                if(temp_num>0) {
                    nbr_tot.x /= temp_num ;
                    nbr_tot.y /= temp_num ;
                    nbr_tot.z /= temp_num ;
                }
                
                DeformFld[k][i][j].x += (nbr_tot.x - DeformFld[k][i][j].x)*LocalRatio ;
                DeformFld[k][i][j].y += (nbr_tot.y - DeformFld[k][i][j].y)*LocalRatio ;
                DeformFld[k][i][j].z += (nbr_tot.z - DeformFld[k][i][j].z)*LocalRatio ;
                
                /* boundary constraints */
                if(DeformFld[k][i][j].x+i<0) DeformFld[k][i][j].x=-i ;
                if(DeformFld[k][i][j].y+j<0) DeformFld[k][i][j].y=-j ;
                if(DeformFld[k][i][j].z+k<0) DeformFld[k][i][j].z=-k ;
                
                if(DeformFld[k][i][j].x+i>=(image_size-1)) DeformFld[k][i][j].x=(image_size-1)-i ;
                if(DeformFld[k][i][j].y+j>=(image_size-1)) DeformFld[k][i][j].y=(image_size-1)-j ;
                if(DeformFld[k][i][j].z+k>=(z_size-1))     DeformFld[k][i][j].z=(z_size-1)-k ;
            }
    //TODO add next line as vervose = 2
    printf("End\n") ;
    
    /* free */
    free(nb_pnt);
}

void smoothDeformationFieldWithEdgePreserving( Ivector3d *FixedMdlVer, int MdlVer_Num, Fvector3d ***DeformFld, ImgAttribute ***MdlImg, int image_size, int z_size, float ratio_iteration, int SmoothingTimes ) {
    int i, j, k, n, t, s ;
    int x, y, z, sx, sy, sz ;
    Ivector3d  *nb_pnt ;
    int        nb_size, count, temp_num ;
    Fvector3d  nbr_tot ;
    float      LocalRatio ;
    unsigned char ***FixedImg;
    
    //TODO add next line as vervose = 2
    printf("\nEdge preserving smoothing ...\n") ;
    nb_size = 3 ;
    nb_pnt= Ivector3dalloc1d(nb_size*nb_size*nb_size) ;
    calculateHoodByIncreasingRadius(nb_pnt, nb_size, &count, 1., 1., 1.) ; /* 0.5, 0.5, 0.5, Feb 28, 2002 */
    
    
    /* insert the MdelVer poistions into an image, for the computation convenience. */
    FixedImg = UCalloc3d(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++)
                FixedImg[k][i][j]=0 ;
    for(s=0; s<MdlVer_Num; s++) {
        sx = FixedMdlVer[s].x ;
        sy = FixedMdlVer[s].y ;
        sz = FixedMdlVer[s].z ;
        
        FixedImg[sz][sx][sy]=255 ;
    }
    
    /* smoothing */
    LocalRatio = 0.75-0.25*ratio_iteration*ratio_iteration ;   /* 0.75, 0.25 */
    //TODO add next line as vervose = 2
    printf("Hierarchical ratio in edge-preserving smoothing : %f\n", LocalRatio) ;
    for( t=0; t<SmoothingTimes; t++) {
        //TODO add next line as vervose = 2
        printf("The %dth time ...\n", t) ;
        for(k=0;k<z_size;k++)
            for(i=0;i<image_size;i++)
                for(j=0;j<image_size;j++)
                    if( /*FixedImg[k][i][j]<255 &&*/ MdlImg[k][i][j].Tiss>0 ) /* for saving time, smooth on brain region only, no background. Feb 26, 2002 */ {
                        nbr_tot.x=0; nbr_tot.y=0; nbr_tot.z=0; temp_num=0;
                        for(n=1;n<27;n++) /* 26 neighbor */ {
                            x = i + nb_pnt[n].x ;
                            y = j + nb_pnt[n].y ;
                            z = k + nb_pnt[n].z ;
                            
                            if( x<image_size && y<image_size && z<z_size && x>=0 && y>=0 && z>=0 ) {
                                nbr_tot.x += DeformFld[z][x][y].x;
                                nbr_tot.y += DeformFld[z][x][y].y;
                                nbr_tot.z += DeformFld[z][x][y].z;
                                
                                temp_num++ ;
                            }
                        }
                        if(temp_num>0) {
                            nbr_tot.x /= temp_num ;
                            nbr_tot.y /= temp_num ;
                            nbr_tot.z /= temp_num ;
                        }
                        
                        if( FixedImg[k][i][j]<255 ) {
                            DeformFld[k][i][j].x += (nbr_tot.x - DeformFld[k][i][j].x)*LocalRatio ;
                            DeformFld[k][i][j].y += (nbr_tot.y - DeformFld[k][i][j].y)*LocalRatio ;
                            DeformFld[k][i][j].z += (nbr_tot.z - DeformFld[k][i][j].z)*LocalRatio ;
                        }
                        else {
                            DeformFld[k][i][j].x += (nbr_tot.x - DeformFld[k][i][j].x)*LocalRatio ; /*/2.0 ; */
                            DeformFld[k][i][j].y += (nbr_tot.y - DeformFld[k][i][j].y)*LocalRatio ; /*/2.0 ; */
                            DeformFld[k][i][j].z += (nbr_tot.z - DeformFld[k][i][j].z)*LocalRatio ; /*/2.0 ; */
                        }
                        
                        /* boundary constraints */
                        if(DeformFld[k][i][j].x+i<0) DeformFld[k][i][j].x=-i ;
                        if(DeformFld[k][i][j].y+j<0) DeformFld[k][i][j].y=-j ;
                        if(DeformFld[k][i][j].z+k<0) DeformFld[k][i][j].z=-k ;
                        
                        if(DeformFld[k][i][j].x+i>=(image_size-1)) DeformFld[k][i][j].x=(image_size-1)-i ;
                        if(DeformFld[k][i][j].y+j>=(image_size-1)) DeformFld[k][i][j].y=(image_size-1)-j ;
                        if(DeformFld[k][i][j].z+k>=(z_size-1))     DeformFld[k][i][j].z=(z_size-1)-k ;
                    }
    }
    //TODO add next line as vervose = 2
    printf("End\n") ;
    
    /* free */
    free(nb_pnt);
    UCfree3d(FixedImg, z_size, image_size) ;
}

void openTrainingResults( Matrix *T1, Matrix *T2, float *T3, int SizeOfT3, char mdlDirectory[]) {
    FILE *fp;
    int i, j ;
    double *TempT1, *TempT2 ;
    int T1m, T1n, T2m, T2n ;
    char filename[180] ;
    
    T1m = T1->height ;
    T1n = T1->width ;
    T2m = T2->height ;
    T2n = T2->width ;
    
    /* allocate space */
    TempT1 = (double *)calloc( T1m*T1n, sizeof(double) );
    TempT2 = (double *)calloc( T2m*T2n, sizeof(double) );
    
    
    
    sprintf(filename, "%sTraningResults.3D", mdlDirectory);    //TODO add next line as vervose = 2 
    printf("training resulted file: %s\n", filename) ;
    if((fp=fopen(filename, "rb"))==NULL) {
        printf("cannot open file\n");
        return ;
    }
    
    fseek(fp, 0L, SEEK_SET);
    
    fread( TempT1,  sizeof(double), T1m*T1n,  fp ) ;
    fread( TempT2,  sizeof(double), T2m*T2n,  fp ) ;
    fread( T3,      sizeof(float),  SizeOfT3, fp ) ;
    
    fclose(fp);
    
    
    /* for T1 */
    for(i=0; i<T1m; i++)
        for(j=0; j<T1n; j++)
            T1->data[i][j] = TempT1[i*T1n+j] ;
    /* for T2 */
    for(i=0; i<T2m; i++)
        for(j=0; j<T2n; j++)
            T2->data[i][j] = TempT2[i*T2n+j] ;
    
    
    /* free */
    free(TempT1) ;
    free(TempT2) ;
}

void alignmentObjectToModel( Fvector3d *verMdl, Fvector3d *verObj, Matrix *tMdl2Obj, int MdlVer_Num ) {
    int i, j, k ;
    Matrix *ObjMatrix, *MdlMatrix, *FittingMatrix, *TmpM1, *TmpM2, *InversedMtrx, *Transform ;
    
    
    
    /* create  metrices*/
    CreateMatrix(&ObjMatrix,    4, MdlVer_Num);
    CreateMatrix(&MdlMatrix,    4, MdlVer_Num);
    CreateMatrix(&FittingMatrix, 4, MdlVer_Num);
    CreateMatrix(&TmpM1,        4, 4);
    CreateMatrix(&TmpM2,        4, 4);
    CreateMatrix(&InversedMtrx, 4, 4);
    CreateMatrix(&Transform,    4, 4);
    
    
    /* get ObjMatrix->data[4][MdlVer_Num] */
    for(j=0; j<MdlVer_Num; j++) {
        ObjMatrix->data[0][j] = verObj[j].x ;
        ObjMatrix->data[1][j] = verObj[j].y ;
        ObjMatrix->data[2][j] = verObj[j].z ;
        ObjMatrix->data[3][j] = 1 ;
    }
    
    /* get MdlMatrix->data[4][MdlVer_Num] */
    for(j=0; j<MdlVer_Num; j++) {
        MdlMatrix->data[0][j] = verMdl[j].x ;
        MdlMatrix->data[1][j] = verMdl[j].y ;
        MdlMatrix->data[2][j] = verMdl[j].z ;
        MdlMatrix->data[3][j] = 1 ;
    }
    
    
    /*get matrix 4X4 TmpM1*/
    for(i=0; i<4; i++)
        for(j=0; j<4; j++) {
            TmpM1->data[i][j] = 0 ;
            
            for(k=0; k<MdlVer_Num; k++)
                TmpM1->data[i][j] += (ObjMatrix->data[i][k]*ObjMatrix->data[j][k]) ;
        }
    
    
    /*get matrix 4X4 TmpM2 */
    for(i=0; i<4; i++)
        for(j=0; j<4; j++) {
            TmpM2->data[i][j] = 0 ;
            
            for(k=0; k<MdlVer_Num; k++)
                TmpM2->data[i][j] += (MdlMatrix->data[i][k]*ObjMatrix->data[j][k]) ;
        }
    
    
    Mat_Inverse( TmpM1, InversedMtrx ) ;
    
    /* get affine transformation */
    Mat_A_equal_BxC(Transform, TmpM2, InversedMtrx) ;             /*A=BxC*/
    
    
    /* use model to best fit the object */
    Mat_A_equal_BxC(FittingMatrix, Transform, ObjMatrix) ;
    
    
    /* back to model variables */
    for(j=0; j<MdlVer_Num; j++) {
        verObj[j].x = FittingMatrix->data[0][j] ;
        verObj[j].y = FittingMatrix->data[1][j] ;
        verObj[j].z = FittingMatrix->data[2][j] ;
    }
    
    /* To return the transform from model to object */
    Mat_Inverse( Transform, tMdl2Obj ) ;
    
    
    /* free */
    FreeMatrix(ObjMatrix) ;
    FreeMatrix(MdlMatrix) ;
    FreeMatrix(FittingMatrix) ;
    FreeMatrix(TmpM1) ;
    FreeMatrix(TmpM2) ;
    FreeMatrix(InversedMtrx) ;
    FreeMatrix(Transform) ;
}

void correct3DSurfaceByT1T2T3(Fvector3d *ver, int MdlVer_Num, Matrix *T1, Matrix *T2, float  *T3, float alpha) {
    float  *Pout, *Pin, *TmpP ;
    int    i, k ;
    int    PntNUMx3=3*(int)(MdlVer_Num) ;
    
    /* to apply for vectors */
    Pout = vectorSHEN(0, PntNUMx3-1) ;
    Pin  = vectorSHEN(0, PntNUMx3-1) ;
    TmpP = vectorSHEN(0, PntNUMx3-1) ; /* for temporary use */
    
    
    for(i=0; i<(int)(MdlVer_Num); i++) {
        Pin[i*3]   = (float)ver[i].x;
        Pin[i*3+1] = (float)ver[i].y;
        Pin[i*3+2] = (float)ver[i].z;
    }
    
    /* correcting */
    Mat_times_Vector(TmpP, T2, Pin) ;
    Mat_times_Vector(Pout, T1, TmpP) ;
    for(k=0; k<PntNUMx3; k++)
        Pout[k] += T3[k] ;
    
    for(i=0; i<(int)(MdlVer_Num); i++) {
        ver[i].x = (alpha*Pout[i*3]  + (1-alpha)*Pin[i*3] );
        ver[i].y = (alpha*Pout[i*3+1]+ (1-alpha)*Pin[i*3+1]);
        ver[i].z = (alpha*Pout[i*3+2]+ (1-alpha)*Pin[i*3+2]);
    }
    
    /* free */
    free_vectorSHEN(Pout, 0, PntNUMx3-1) ;
    free_vectorSHEN(Pin,  0, PntNUMx3-1) ;
    free_vectorSHEN(TmpP, 0, PntNUMx3-1) ;
}

void affineTransform3DSurface( Fvector3d *ver, Matrix *tMdl2Obj,  int  MdlVer_Num ) {
    int j ;
    Matrix *verMatrix, *resMatrix ;
    
    
    /* create  metrices*/
    CreateMatrix(&verMatrix,    4, MdlVer_Num);
    CreateMatrix(&resMatrix,    4, MdlVer_Num);
    
    
    /* get verMatrix->data[4][MdlVer_Num] */
    for(j=0; j<MdlVer_Num; j++) {
        verMatrix->data[0][j] = ver[j].x ;
        verMatrix->data[1][j] = ver[j].y ;
        verMatrix->data[2][j] = ver[j].z ;
        verMatrix->data[3][j] = 1 ;
    }
    
    
    /* affine transformation */
    Mat_A_equal_BxC(resMatrix, tMdl2Obj, verMatrix) ;             /*A=BxC*/
    
    /* back to model variables */
    for(j=0; j<MdlVer_Num; j++) {
        ver[j].x = resMatrix->data[0][j] ;
        ver[j].y = resMatrix->data[1][j] ;
        ver[j].z = resMatrix->data[2][j] ;
    }
    
    
    /* free */
    FreeMatrix(verMatrix) ;
    FreeMatrix(resMatrix) ;
}


/*int max_nbr=1 ;*/
void statisticalMapping(Fvector3d *ver,  int  MdlVer_Num, float alpha, int maxNbr, char mdlDirectory[]) {
    /* for statistcs*/
    Matrix *T1, *T2 ;
    float  *T3 ;
    int    s ;
    
    /* for object */
    char   filename[180];
    FILE  *fp;
    Fvector3d  *verMdl, *verObj ;
    /* sizes: */
    int PntNUMx3 ;
    
    Matrix *tMdl2Obj ;
    /* create  metrices*/
    CreateMatrix(&tMdl2Obj,  4, 4);
    
    
    /* To get model */
    sprintf(filename, "%sMDL.ver.norm.nbr", mdlDirectory);    //TODO add next line as vervose = 2 
    printf("model file name: %s\n", filename) ;
    fp=myopen(filename, "rb");
    fseek(fp, 0, SEEK_END);
    MdlVer_Num = ftell(fp)/(6*4+(maxNbr+1)*4); /*48*/
    fclose(fp);
    
    verMdl = Fvector3dalloc1d(MdlVer_Num);
    
    fp=myopen(filename, "rb");
    fread(verMdl, sizeof(Fvector3d), MdlVer_Num, fp);
    fclose(fp);
    
    
    /* detecmine the sizes */
    PntNUMx3 = 3*(int)(MdlVer_Num) ;
    
    
    /* apply for mem */
    CreateMatrix(&T1,   PntNUMx3,     CHNEIGENNUM);
    CreateMatrix(&T2,   CHNEIGENNUM, PntNUMx3);
    T3 = vectorSHEN(0, PntNUMx3-1) ;
    
    /* Get the trained data */
    openTrainingResults( T1, T2, T3, PntNUMx3, mdlDirectory);
    
    
    
    /* copy imput object, April 27, 2000 */
    verObj = Fvector3dalloc1d(MdlVer_Num);
    for(s=0; s<MdlVer_Num; s++) {
        verObj[s].x = ver[s].x ;  verObj[s].y = ver[s].y ;  verObj[s].z = ver[s].z ;
    }
    
    /* correcting */
    alignmentObjectToModel( verMdl, ver, tMdl2Obj, MdlVer_Num ) ;
    correct3DSurfaceByT1T2T3(ver, MdlVer_Num, T1, T2, T3, alpha) ;
    affineTransform3DSurface( ver, tMdl2Obj,  MdlVer_Num ) ;
    
    /* This part is added on April 27, 2000, trying to fixing energy deduction problem in shape statistics method */
    alignmentObjectToModel( verObj, ver, tMdl2Obj, MdlVer_Num ) ;
    
    
    
    /* statistics free */
    free(verMdl) ;
    
    FreeMatrix(T1) ;
    FreeMatrix(T2) ;
    free_vectorSHEN(T3, 0, PntNUMx3-1) ;
    
    FreeMatrix(tMdl2Obj) ;
}

void searchTheNearestVertexForEachLandmark(Ivector3d *ObjVer, int ObjVer_Num, ImgAttribute *ObjVer_Fea, ImgAttribute ***ObjImg, \
Fvector3d *ModelVer, int MdlVer_Num, ImgAttribute *MdlVer_Fea, Ivector3d *FixedMdlVer, ImgAttribute ***MdlImg, Fvector3d ***DeformFld,   int image_size, int z_size,  int max, Fvector3d *forceByFocusedPntInObj, Ivector3d    *myHood, int *multiple, float smrThr, float matchingDegreeOfMdlOnImgEdge, int youngBrain) {
    int        i, j, k, l ;
    int        ***VerticesImg, x, y, z, px, py, pz, vx, vy, vz, s ;
    int        TotalMatchingPoints, TotalMdlPntMatched, TotalSamples ;
    float      degree, StepDesign, Pointer ;
    
    /* for subvolume matching*/
    float level, CurrentDegree, OverlapDegree, MaxDegree ;
    Fvector3d Crnt, Tmp, Search, SubjNb, dfm;
    int   sx, sy, sz, real_size, SmplStep, incre, dfm_s ;
    
    /* subvolume */
    Ivector3d  *nb_pnt, Grid;
    int        nb_size, n, count ;
    
    
    
    nb_size = max*2+1; /*nb_size should be odd number */
    nb_pnt= Ivector3dalloc1d(nb_size*nb_size*nb_size) ;
    calculateHoodByIncreasingRadius(nb_pnt, nb_size, &count, 1., 1., 1.) ;
    SmplStep = count/20 ;  if(SmplStep<1) SmplStep=1 ;
    //TODO add next line as vervose = 2 
    printf("   nbr num:%d with rad %d <- sampling step=%d\n", count, nb_size, SmplStep) ;
    
    
    
    /* For saving searching time */
    VerticesImg = Ialloc3d(image_size, image_size, z_size);
    //TODO add next line as vervose = 2
    if (!VerticesImg) printf("   memeoy application in VerticesImg\n") ;
    /* transfering surface's vertices */
    for(k=0; k<z_size; k++)
        for(i=0; i<image_size; i++)
            for(j=0; j<image_size; j++)
                VerticesImg[k][i][j] = -1 ;
    for( i=0; i<MdlVer_Num; i++) {
        x = (int)(ModelVer[i].x) ;
        y = (int)(ModelVer[i].y) ;
        z = (int)(ModelVer[i].z) ;
        
        if( x<image_size && y<image_size && z<z_size && x>=0 && y>=0 && z>=0 )
            VerticesImg[z][x][y] = i ;
    }
    //TODO add next line as vervose = 2
    printf("   image setting\n") ;
    
    /* reset */
    for( i=0; i<MdlVer_Num; i++) {
        forceByFocusedPntInObj[i].x = 0 ;
        forceByFocusedPntInObj[i].y = 0 ;
        forceByFocusedPntInObj[i].z = 0 ;
        multiple[i] = 0 ;
    }
    
    
    TotalSamples = MdlVer_Num/2;
    if(TotalSamples>ObjVer_Num) TotalSamples = ObjVer_Num ;
    if(TotalSamples>20000)      TotalSamples = 20000 ;
    StepDesign = (float)ObjVer_Num/(float)TotalSamples ; //TODO add next line as vervose = 2 
    printf("   StepDesign=%f\n", StepDesign) ;
    incre = 2*max/6 ; if(incre<1) incre=1 ;              //TODO add next line as vervose = 2 
    printf("   incre=%d\n", incre);
    TotalMatchingPoints = 0;
    for(Pointer=0, l=0; l<ObjVer_Num; Pointer += StepDesign, l=(int)Pointer) {
        px = ObjVer[l].x ;
        py = ObjVer[l].y ;
        pz = ObjVer[l].z ;
        
        dfm.x = 0 ; dfm.y = 0 ; dfm.z = 0 ;
        MaxDegree = 0 ;
        for(SubjNb.x=-max; SubjNb.x<=max; SubjNb.x+=incre)
            for(SubjNb.y=-max; SubjNb.y<=max; SubjNb.y+=incre)
                for(SubjNb.z=-max; SubjNb.z<=max; SubjNb.z+=incre)
                    /*for(t=0; t<pixelNumIn_MyHood && Status==FFAIL; t++)*/
                {
                    vx = px + SubjNb.x ; /*+ MyHood[t].x ;*/
                    vy = py + SubjNb.y ; /*+ MyHood[t].y ;*/
                    vz = pz + SubjNb.z ; /*+ MyHood[t].z ;*/
                    
                    if( vx<image_size && vy<image_size && vz<z_size && vx>=0 && vy>=0 && vz>=0) {
                        s = VerticesImg[vz][vx][vy] ;
                        
                        if( s>=0 && px<image_size && py<image_size && pz<z_size && px>=0 && py>=0 && pz>=0) {
                            degree = similarityBetweenFocusedPointsInMdlObjImgs(ObjVer_Fea, l, MdlVer_Fea, s) ;
                            
                            if( degree>smrThr ) {
                                /* get subvolume similarity */
                                /*****************************************/
                                sx = FixedMdlVer[s].x ;
                                sy = FixedMdlVer[s].y ;
                                sz = FixedMdlVer[s].z ;
                                
                                Search.x = -SubjNb.x ;  /*+ MyHood[t].x ;*/
                                Search.y = -SubjNb.y ;  /*+ MyHood[t].y ;*/
                                Search.z = -SubjNb.z ;  /*+ MyHood[t].z ;*/
                                
                                OverlapDegree = 0 ;
                                real_size = 0 ;
                                for(n=0; n<count; n+=SmplStep) {
                                    /* Coordinate In Template */
                                    Grid.x = sx+nb_pnt[n].x ;
                                    Grid.y = sy+nb_pnt[n].y ;
                                    Grid.z = sz+nb_pnt[n].z ;
                                    
                                    if( Grid.x<image_size && Grid.y<image_size && Grid.z<z_size && Grid.x>=0 && Grid.y>=0 && Grid.z>=0 ) /* true */ {
                                        Crnt.x = DeformFld[Grid.z][Grid.x][Grid.y].x + Grid.x ;
                                        Crnt.y = DeformFld[Grid.z][Grid.x][Grid.y].y + Grid.y ;
                                        Crnt.z = DeformFld[Grid.z][Grid.x][Grid.y].z + Grid.z ;
                                        
                                        /* shift for every vertex in this set of neighborhood */
                                        level = (fabs(nb_pnt[n].x)+fabs(nb_pnt[n].y)+fabs(nb_pnt[n].z))/3.0 ;
                                        shiftByGuassianWeight(Crnt, &Tmp, Search, level, nb_size) ;
                                        
                                        x = (int)(Tmp.x + 0.5) ;
                                        y = (int)(Tmp.y + 0.5) ;
                                        z = (int)(Tmp.z + 0.5) ;
                                        
                                        if( x<image_size && y<image_size && z<z_size && x>=0 && y>=0 && z>=0 ) {
                                            CurrentDegree = similarityBetweenFeaturesInMdlAndObjImgs( ObjImg, x, y, z, MdlImg, Grid.x, Grid.y, Grid.z ) ;
                                            
                                            /* March 27, 2003: This is added for considering   */
                                            /* (1) binary tissue matching, (2) BG effect.      */  /*good for yng brain*/
                                            if( youngBrain==YYES &&  MdlImg[Grid.z][Grid.x][Grid.y].Edge==0 && CurrentDegree<0.6 )
                                                CurrentDegree=0;
                                            /*if(  MdlImg[Grid.z][Grid.x][Grid.y].Tiss==BG && CurrentDegree>0.6 )
                                             {
                                             CurrentDegree=0;
                                             real_size -- ;
                                             }*/
                                            /* will not count current one. Notable size will be increased one nest*/
                                            
                                            real_size ++ ;
                                            if( MdlImg[Grid.z][Grid.x][Grid.y].Edge>0 ) {
                                                CurrentDegree *= 1.2 ;
                                                real_size += (1.2-1.0) ;
                                            }
                                        }
                                        else
                                            CurrentDegree = 0 ;
                                        
                                        OverlapDegree += CurrentDegree ;
                                    }
                                }
                                if(real_size>0)
                                    OverlapDegree = OverlapDegree/real_size ;
                                /*****************************************/
                                
                                if( OverlapDegree>MaxDegree ) {
                                    MaxDegree = OverlapDegree ;
                                    dfm.x = Search.x ;
                                    dfm.y = Search.y ;
                                    dfm.z = Search.z ;
                                    dfm_s = s ;
                                }
                            }
                        }
                    }
                }
        
        /* get a maximal one */
        if( MaxDegree>matchingDegreeOfMdlOnImgEdge ) {
            TotalMatchingPoints ++;
            forceByFocusedPntInObj[dfm_s].x += dfm.x ;
            forceByFocusedPntInObj[dfm_s].y += dfm.y ;
            forceByFocusedPntInObj[dfm_s].z += dfm.z ;
            multiple[dfm_s] += 1 ;
        }
    }
    //TODO add next line as vervose = 2
    printf("   TotalMatchingPoints=%d (%f)\n", TotalMatchingPoints, (float)TotalMatchingPoints/(float)ObjVer_Num*100.0) ;
    
    
    TotalMdlPntMatched = 0 ;
    for( i=0; i<MdlVer_Num; i++)
        if( multiple[i]>0 ) {
            /*printf("i=%d num=%d\n", i,multiple[i]) ;*/
            forceByFocusedPntInObj[i].x /= multiple[i] ;
            forceByFocusedPntInObj[i].y /= multiple[i] ;
            forceByFocusedPntInObj[i].z /= multiple[i] ;
            TotalMdlPntMatched ++ ;
        }
    //TODO add next line as vervose = 2
    printf("   TotalMdlPntMatched=%d (%f)\n", TotalMdlPntMatched, (float)TotalMdlPntMatched/(float)MdlVer_Num*100.0) ;
    
    /* free */
    free(nb_pnt);
    Ifree3d(VerticesImg, z_size, image_size) ;
}


void writeDeformationField(char filename[180], Fvector3d ***DeformFld, int image_size, int z_size) {
    FILE  *fp;
    int   i, k ;
    
    /* write the smoothed image */
    fp=myopen(filename, "wb");
    fwrite(&image_size, sizeof(int), 1, fp);
    fwrite(&z_size, sizeof(int), 1, fp);
    //begin debug
    //TODO add next line as vervose = 2
    //printf("The size of Fvector3d is: %d", sizeof(Fvector3d));
    //printf("The size of int is: %d", sizeof(int));
    //printf("The size of image is: %d", &image_size);
    //printf("The size of z is: %d", &z_size);
    //end debug
    for(k=0;k<z_size;k++)
        for(i=0;i<image_size;i++)
            fwrite(DeformFld[k][i], sizeof(Fvector3d), image_size, fp);
    fclose(fp);
}

void debugWriteDeformationField(char filename[180], Fvector3d ***DeformFld, int image_size, int z_size) {
    int   i, k, j;
    //TODO add next line as vervose = 2
    //printf("The size of Fvector3d is: %d\n", sizeof(Fvector3d));
    //printf("The size of int is: %d\n", sizeof(int));
    //printf("The size of image is: %d\n", image_size);
    //printf("The size of z is: %d\n", z_size);
    for(k=0;k<z_size;k++)
        for(i=0;i<image_size;i++)
            for(j=0;j<image_size;j++) {
                    printf("Hello (%d,%d,%d):(%f,%f,%f)\n",i,j,k, DeformFld[k][i][j].x, DeformFld[k][i][j].y, DeformFld[k][i][j].z);                   
                }
}

void loadImage(char filename[], ImgAttribute  ***ObjImg, int Img_XY, int zSize, int imageSize, float xyzRes){
    unsigned char ***Img;
    FILE          *fp;
    int Img_Z;
    int i, j, k;
    
    fp=myopen(filename, "rb");
    fseek(fp, 0, SEEK_END);
    Img_Z=ftell(fp)/(Img_XY*Img_XY);
    rewind(fp);
    
    Img = UCalloc3d(Img_XY, Img_XY, Img_Z);
    
    for(k=0;k<Img_Z;k++)
        for(i=0;i<Img_XY;i++)
            fread(Img[k][i], 1, Img_XY, fp);
    fclose(fp);
    //TODO add next line as vervose = 2
    printf("\n\n^^^^ Segmented object image input!\n") ;
    
    /*image_size = Img_XY/XYZres ;
     z_size = Img_Z/XYZres ; */
    
    //ObjImg = ImgAttributealloc3d(imageSize, imageSize, zSize);
    
    for(k=0; k<zSize; k++) {
        if(k<Img_Z/xyzRes)
            for(i=0; i<imageSize; i++)
                for(j=0; j<imageSize; j++)
                    ObjImg[k][i][j].Tiss=Img[(int)(k*xyzRes)][(int)(i*xyzRes)][(int)(j*xyzRes)] ;
        else
            for(i=0; i<imageSize; i++)
                for(j=0; j<imageSize; j++)
                    ObjImg[k][i][j].Tiss=0;
    }
    UCfree3d(Img, Img_Z, Img_XY) ;
}

void initializeDeformationField(Fvector3d ***DeformFld, int zSize, int imageSize){
    int i, j, k;
    for(k=0; k<zSize; k++)
        for(i=0; i<imageSize; i++)
            for(j=0; j<imageSize; j++) {
                DeformFld[k][i][j].x = 0 ;
                DeformFld[k][i][j].y = 0 ;
                DeformFld[k][i][j].z = 0 ;
            }
}
