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

/**
 * nucleus.c
 * 
 * Based on AdaptiveBrainMappingUsingGirdmesh_V1.5
 * 
 * This program is what is consider the heart of the HAMMER algorithm.
 *
 * It takes two segmented images in Analyze format following the Radiology 
 * Convention, and creates a deformation field that can be used to warp one into
 * the other. Additionally these images must follow the SBIA segmented images
 * convention described in the documentation, that in particular MUST have the
 * same XY dimmensions.
 * 
 * This program does minimal or no checking about the inputs, so other programs 
 * must be used to perform robust prior checks.
 * 
 * In particular, non-developer users should be encouraged (strongly) not to use
 * this program directly, instead they should use the HAMMER Command Line Interface
 * HAMMER CLI.
 * 
 * To see a definition of the parameters it expects and their description see
 * showUsage()
 * 
 * The wrapper method is shenMain(); this is the one that defines the logic of
 * the program. See the method notes to get an overview of what it does.
 * 
 */




/* get  MdlVer, FixedMdlVer, MdlVer_Fea  why?*/

/* Note: The variables below are unsafe, trying to get rid of them to avoid
 its globality breaks the algorithm, that means that they are referenced in a
 way that they are not suppossed to. In particular there are "duplicate" variables
 with different case and used "aparently" interchangeably.*/

Fvector3d  *globalMdlVer;       /* focused edge points from MODEL image */
Ivector3d  *globalFixedMdlVer;  /* focused edge points from MODEL image */
Ivector3d  *globalObjVer;       /* focused edge points from OBJECT image */
ImgAttribute *mdlVerFea;
ImgAttribute *objVerFea;


void shenMain(int argc, char *argv[]) ;
void showUsage() ;

void showUsage() {
    printf("USAGE: nucleus <model.segmented.image> <obj.segmented.image> <deformationfieldname>\n\
    \t -R <int>               : deformation rate, i.e. 0.05\n\
    \t -l <int>               : last interation order\n\
    \t -r <int>               : search resolution\n\
    \t -i <int>               : # of initial iterations(18)\n\
    \t -s <float>             : sampling rate(default 1 )\n\
    \t -a <float>             : degree for expressing local affine transformation(0.2)\n\
    \t -b <float>             : statistical factor(0.1)\n\
    \t -m <float>             : Matching degree between model and image edge(0.1)\n\
    \t -v <int>               : max neighbor size to describe the local connection of the current point(8)\n\
    \t -t                     : add the method of stopping surface constraction\n\
    \t -e                     : add estimation procedure for estimating scaling and shift of 3D model from image data\n\
    \t -d                     : add statistics information to constrain deformation procedure\n\
    \t -c <float>             : smooth factor(i.e. 0.01)\n\
    \t -F                     : read deformation field from file in the local directory\n\
    \t -O <string>            : file name for orignal object image\n\
    \t -S <string>            : model surface file name(dir/MDL.ver.norm.nbr).\n\
    \t -h <int>               : number of maximum neighborhood(13)\n\
    \t -E <int>               : Edge preserving smoothing times(default: 0) \n\
    \t -N                     : new force from object\n\
    \t -U                     : update initial VerMdl by the result from the last resolution\n\
    \t -X <int>               : size of XY slice image; default:256\n\
    \t -Y                     : Young Brain(less CSF)\n\
    \t -C <float>             : Confidence on deformation field calculated in the last resolution(0~1.0); default: 0.5\n\
    \t -n                     : no Global linear constraint since the very beginning stage of registration\n\
    ");
    /*\t -E <int>               : Edge preserving smoothing times, i.e. 1,2 \n\*/
    exit(1);
}

/* for model */
void select3DEdgePointsInModelTo1D(int *MdlVer_Num, ImgAttribute ***MdlImg, Fvector3d ***DeformFld, int image_size, int z_size, Threshold mdlThreshold) {
    int   i, j, k, c ;
    int   Geom_DownsUp ;
    unsigned char ***FeaPoints ;
    FILE  *fp;
    
    
    /* To view which points are selected as landmarks. */
    FeaPoints = UCalloc3d(image_size, image_size, z_size);
    
    Geom_DownsUp = mdlThreshold.Geom_DN/1.1 ; /*1.5*/
    
    (*MdlVer_Num) = 0 ;
    for(k=0;k<z_size;k++)
        for(i=0;i<image_size;i++)
            for(j=0;j<image_size;j++)
                if( MdlImg[k][i][j].Edge!=0 && (MdlImg[k][i][j].Geom>mdlThreshold.Geom_UP || (MdlImg[k][i][j].Geom<mdlThreshold.Geom_DN && MdlImg[k][i][j].Geom>Geom_DownsUp) || MdlImg[k][i][j].VNvlm>mdlThreshold.VNvlm_UP || MdlImg[k][i][j].CSFBG>mdlThreshold.CSFBG_UP) )
                    (*MdlVer_Num)+=1 ;
    //TODO add next line as vervose = 2
    //printf("There are %d focused edge points in model image!\n", (*MdlVer_Num));
    
    globalMdlVer     = Fvector3dalloc1d(*MdlVer_Num);
    globalFixedMdlVer= Ivector3dalloc1d(*MdlVer_Num);  /* for connection */
    mdlVerFea = ImgAttributealloc1d(*MdlVer_Num) ; /* attributes for each selected point */
    
    c = 0 ;
    for(k=0;k<z_size;k++)
        for(i=0;i<image_size;i++)
            for(j=0;j<image_size;j++)
                if( MdlImg[k][i][j].Edge!=0 && (MdlImg[k][i][j].Geom>mdlThreshold.Geom_UP || (MdlImg[k][i][j].Geom<mdlThreshold.Geom_DN && MdlImg[k][i][j].Geom>Geom_DownsUp) || MdlImg[k][i][j].VNvlm>mdlThreshold.VNvlm_UP || MdlImg[k][i][j].CSFBG>mdlThreshold.CSFBG_UP) ) {
                    /* position */
                    globalMdlVer[c].x = i + DeformFld[k][i][j].x ;
                    globalMdlVer[c].y = j + DeformFld[k][i][j].y ;
                    globalMdlVer[c].z = k + DeformFld[k][i][j].z ;
                    globalFixedMdlVer[c].x = i ;
                    globalFixedMdlVer[c].y = j ;
                    globalFixedMdlVer[c].z = k ;
                    
                    /* attributes  */
                    mdlVerFea[c].Edge  = MdlImg[k][i][j].Edge;   /* edge */
                    mdlVerFea[c].Tiss  = MdlImg[k][i][j].Tiss;   /* intensity */
                    mdlVerFea[c].Geom  = MdlImg[k][i][j].Geom;   /* geometric */
                    mdlVerFea[c].VNvlm = MdlImg[k][i][j].VNvlm;  /* VN volume */
                    mdlVerFea[c].CSFBG = MdlImg[k][i][j].CSFBG;  /* CSF/BG volume */
                    
                    c ++ ;
                    
                    FeaPoints[k][i][j] = 255 ;
                }
                else
                    FeaPoints[k][i][j] = 0 ;
    
    /* saving ... */
//    fp=myopen("MdlImg_FocuedEdges", "wb");
//    for(k=0;k<z_size;k++)
//        for(i=0;i<image_size;i++)
//            fwrite(FeaPoints[k][i], 1, image_size, fp);
//    fclose(fp);
    
    /* free */
    UCfree3d(FeaPoints, z_size, image_size) ;
}

/* for object */
void select3DEdgePointsInObjectTo1D(int *ObjVer_Num, ImgAttribute ***ObjImg, int image_size, int z_size) {
    int   i, j, k, c ;
    float alpha, beta;
    int   Geom_DownsUp  ;
    unsigned char ***FeaPoints ;
    FILE  *fp;
    Threshold  objThreshold;
    
    /* To view which points are selected as landmarks. */
    FeaPoints = UCalloc3d(image_size, image_size, z_size);
    
    alpha = 0.9 ; /* */
    beta  = 0.4;
    
    objThreshold.Geom_UP = alpha*255 ;
    objThreshold.Geom_DN = beta*255 ;
    Geom_DownsUp = objThreshold.Geom_DN/1.1 ; /*1.5*/
    objThreshold.VNvlm_UP = 255/4 ;
    objThreshold.CSFBG_UP = 255/4 ;
    
    
    (*ObjVer_Num) = 0 ;
    for(k=0;k<z_size;k++)
        for(i=0;i<image_size;i++)
            for(j=0;j<image_size;j++)
                if( ObjImg[k][i][j].Edge!=0 && (ObjImg[k][i][j].Geom>objThreshold.Geom_UP || (ObjImg[k][i][j].Geom<objThreshold.Geom_DN && ObjImg[k][i][j].Geom>Geom_DownsUp) || ObjImg[k][i][j].VNvlm>objThreshold.VNvlm_UP || ObjImg[k][i][j].CSFBG>objThreshold.CSFBG_UP) )
                    (*ObjVer_Num)+=1 ;
    //TODO add next line as vervose = 2                
    //printf("There are %d focued edge points in the object image!\n", *ObjVer_Num);
    
    globalObjVer     = Ivector3dalloc1d(*ObjVer_Num);
    objVerFea = ImgAttributealloc1d(*ObjVer_Num) ; /* attributes for each selected point */
    
    c = 0 ;
    for(k=0;k<z_size;k++)
        for(i=0;i<image_size;i++)
            for(j=0;j<image_size;j++)
                if( ObjImg[k][i][j].Edge!=0 && (ObjImg[k][i][j].Geom>objThreshold.Geom_UP || (ObjImg[k][i][j].Geom<objThreshold.Geom_DN && ObjImg[k][i][j].Geom>Geom_DownsUp) || ObjImg[k][i][j].VNvlm>objThreshold.VNvlm_UP || ObjImg[k][i][j].CSFBG>objThreshold.CSFBG_UP) ) {
                    /* position */
                    globalObjVer[c].x = i ;
                    globalObjVer[c].y = j ;
                    globalObjVer[c].z = k ;
                    
                    /* attributes  */
                    objVerFea[c].Edge  = ObjImg[k][i][j].Edge;   /* edge */
                    objVerFea[c].Tiss  = ObjImg[k][i][j].Tiss;   /* intensity */
                    objVerFea[c].Geom  = ObjImg[k][i][j].Geom;   /* geometric */
                    objVerFea[c].VNvlm = ObjImg[k][i][j].VNvlm;  /* VN volume */
                    objVerFea[c].CSFBG = ObjImg[k][i][j].CSFBG;  /* CSF/BG volume */
                    
                    c ++ ;
                    
                    FeaPoints[k][i][j] = 255 ;
                }
                else
                    FeaPoints[k][i][j] = 0 ;
    
    /* saving ... */
//    fp=myopen("ObjImg_FocuedEdges", "wb");
//    for(k=0;k<z_size;k++)
//        for(i=0;i<image_size;i++)
//            fwrite(FeaPoints[k][i], 1, image_size, fp);
//    fclose(fp);
    
    /* free */
    UCfree3d(FeaPoints, z_size, image_size) ;
}

void affineDeform3D(Fvector3d *ModelVer, Ivector3d *FixedMdlVer, int MdlVer_Num, ImgAttribute *MdlVer_Fea, Fvector3d ***DeformFld, ImgAttribute ***MdlImg, Ivector3d *ObjVer, int ObjVer_Num, ImgAttribute *ObjVer_Fea, ImgAttribute ***ObjImg, int image_size, int z_size, int resolution, Fvector3d    *forceByFocusedPntInObj, Matrix *transform, Ivector3d *myHood, int *multiple, int smoothingTimes, int maxNbr, float ratioIteration, float deformRate, float statisticalFactor, float smoothFactor, float matchingDegreeOfMdlOnImgEdge, int statisticsEmployedOrNot, int objectForce, int globalConstraint, int youngBrain, char mdlDirectory[]) {
    int   i, j, s, k, n, count, max, t, SHIFT, incre, SelectVerNum, vv, RealVV ;
    float weight, CurrentDegree, OverlapDegree, MaxDegree, DistSeg, MinDist ;
    Fvector3d Search, Crnt, Tmp, dfm, globl;
    float Delta, level ;
    int   x, y, z, sx, sy, sz ;
    
    Ivector3d  *nb_pnt, Grid, StudiedPoint;
    int      nb_size ;
    
    /* for estimating the overlapping degree */
    int real_size ;
    
    /* for local geometric features of 3D shape */
    float     CenterRate, LocalRatio ;
    Fvector3d *verModelLast, nbr_tot ;
    int       numTEMP, Last_searched_point, temp_num ;
    
    /* June 5, 2001 */
    Fvector3d  ***DeformFld_Last ;
    DeformFld_Last=Fvector3dalloc3d(image_size, image_size, z_size);
    
    int stepNbr=12 ;        /* 6 (May2001), 8 10*/
    static int   startSearchPoint=0 ;
    
    float   smrThr=0.8;  /* 0.8 */
    
    /* global constraints  */
    verModelLast      = Fvector3dalloc1d(MdlVer_Num); /* used as model */
    /* remember */
    for(numTEMP=0; numTEMP<MdlVer_Num; numTEMP++) {
        verModelLast[numTEMP].x=ModelVer[numTEMP].x; verModelLast[numTEMP].y=ModelVer[numTEMP].y; verModelLast[numTEMP].z=ModelVer[numTEMP].z;
    }
    for(k=0;k<z_size;k++)
        for(i=0;i<image_size;i++)
            for(j=0;j<image_size;j++) {
                DeformFld_Last[k][i][j].x=DeformFld[k][i][j].x ;
                DeformFld_Last[k][i][j].y=DeformFld[k][i][j].y ;
                DeformFld_Last[k][i][j].z=DeformFld[k][i][j].z ;
            }
    
    
    /* shift assigned */
    SHIFT = resolution/2 ;  if(SHIFT<1) SHIFT=1;  //TODO add next line as vervose = 2 printf("Shift=%d\n", SHIFT) ;
    
    
    /* get the forces from the landmarks */
    smrThr = 0.8*(1-ratioIteration)+0.001 ; /* 0.8 */ //TODO add next line as vervose = 2 printf("SMR_THR=%f\n", smrThr) ;
    //TODO add next line as vervose = 2 printf("before ObjVer_Num=%d\n", ObjVer_Num) ;
    if(ratioIteration<=ITER_THRD && objectForce==YYES )
        searchTheNearestVertexForEachLandmark(ObjVer, ObjVer_Num, ObjVer_Fea, ObjImg, ModelVer, MdlVer_Num, MdlVer_Fea, FixedMdlVer, MdlImg, DeformFld,  image_size, z_size,  SHIFT+1, forceByFocusedPntInObj, myHood, multiple, smrThr, matchingDegreeOfMdlOnImgEdge, youngBrain) ;
    /*SearchTheNearestVertexForEachLandmark( ObjVer, ObjVer_Num, ObjVer_Fea, MdlVer, MdlVer_Num, MdlVer_Fea, image_size, z_size ) ; used before March 2003*/
    
    
    incre = 2*SHIFT/6 /*8, 20, May 2001*/ ; if(incre<1) incre=1 ;
    max = SHIFT+1 ;
    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.) ;
    //TODO add next line as vervose = 2
    //printf("nbr num:%d with rad %d\n", nb_size, count) ;
    t = 0;   /* 3 8 */ /* 0 ~ 9*/
    t = resolution/10 ;if(t>2) t=2 ;
    SelectVerNum = MdlVer_Num ;
    //TODO add next line as vervose = 2
    //printf("incre=%d  max=%d   searched Num=%d    t=%d\n", incre, max, SelectVerNum, t ) ;
    
    
    RealVV = 0 ;
    for(vv=0; vv<MdlVer_Num; vv+=STEPPNT)  /*for(s=0; s<MdlVer_Num; s+=STEPPNT)*/ {
        s = (vv+startSearchPoint)%MdlVer_Num ;
        
        if( ( RealVV<SelectVerNum) || (Fvector3dmag(forceByFocusedPntInObj[s])!=0) ) {
            RealVV ++ ;
            Last_searched_point = s ;
            if( s%100==0 ) { //printf("\ns=%d  Last_searched_point= %d", s, Last_searched_point) ;
                //printf("percent finished = %f\n", (float)RealVV/SelectVerNum) ;
            }
            
            /* May 24, 2001 */
            stepNbr = count/20 ;  if(stepNbr<1) stepNbr=1 ;
            
            if( Fvector3dmag(forceByFocusedPntInObj[s])==0 || ratioIteration>ITER_THRD) {
                /* begin to search by greedy algorithm: external part */
                dfm.x = 0 ; dfm.y = 0 ; dfm.z = 0 ;
                MaxDegree = 0 ;
                MinDist = 100000.0 ;
                /*printf("VerNumInTheSameRegionOfCurrentVer[%d]=%d\n", s,VerNumInTheSameRegionOfCurrentVer[s]) ;*/
                sx = FixedMdlVer[s].x ;
                sy = FixedMdlVer[s].y ;
                sz = FixedMdlVer[s].z ;
                if( sx>=image_size || sy>=image_size || sz>=z_size || sx<0 || sy<0 || sz<0 ) { printf("out\n"); exit(0);}
                for(Search.x=-SHIFT; Search.x<=SHIFT; Search.x+=incre)
                    for(Search.y=-SHIFT; Search.y<=SHIFT; Search.y+=incre)
                        for(Search.z=-SHIFT; Search.z<=SHIFT; Search.z+=incre) {
                            /*		  x = (int)(MdlVer[s].x+Search.x) ;
                             y = (int)(MdlVer[s].y+Search.y) ;
                             z = (int)(MdlVer[s].z+Search.z) ;
                             */
                            /* MdlVer[s]'s current position */
                            x = (int)(DeformFld[sz][sx][sy].x + sx + Search.x + 0.5) ;
                            y = (int)(DeformFld[sz][sx][sy].y + sy + Search.y + 0.5) ;
                            z = (int)(DeformFld[sz][sx][sy].z + sz + Search.z + 0.5) ;
                            
                            /*printf("1:(%d) xyz=%d,%d,%d   %f,%f,%f\n", SHIFT, x,y,z, Search.x, Search.y, Search.z) ;*/
                            
                            if( x<image_size && y<image_size && z<z_size && x>=0 && y>=0 && z>=0 && similarityBetweenFeaturesInImgAndFeaturesInMdl(ObjImg, x, y, z, MdlVer_Fea, s)>smrThr ) {
                                /*printf("2: xyz=%d,%d,%d\n", x,y,z) ;*/
                                
                                OverlapDegree = 0 ;
                                DistSeg = 0 ;
                                real_size = 0 ;
                                for(n=0; n<count; n+=stepNbr) {
                                    /* Coordinate In Template */
                                    Grid.x = sx+nb_pnt[n].x ;
                                    Grid.y = sy+nb_pnt[n].y ;
                                    Grid.z = sz+nb_pnt[n].z ;
                                    
                                    /*printf("3: Gxyz=%d,%d,%d\n", Grid.x,Grid.y,Grid.z) ;*/
                                    
                                    if( Grid.x<image_size && Grid.y<image_size && Grid.z<z_size && Grid.x>=0 && Grid.y>=0 && Grid.z>=0 ) /* true */ {
                                        /*printf("4: Gxyz=%d,%d,%d\n", Grid.x,Grid.y,Grid.z) ;*/
                                        
                                        /*real_size ++ ;	Nov 15, 2001 */
                                        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 ;
                                        /*level = sqrt( nb_pnt[n].x*nb_pnt[n].x + nb_pnt[n].y*nb_pnt[n].y + nb_pnt[n].z*nb_pnt[n].z ) ; */ /* tried before June 18, 2001 */
                                        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) ;
                                        
                                        /*printf("5: xyz=%d,%d,%d\n", x,y,z) ;*/
                                        
                                        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 ) /* June 6, 2001*/ {
                                                CurrentDegree *= 1.2 ;
                                                real_size += (1.2-1.0) ;
                                            }
                                        }
                                        else
                                            CurrentDegree = 0 ;
                                        
                                        /*if( MdlImg[Grid.z][Grid.x][Grid.y].Edge>0 )
                                         {
                                         CurrentDegree *= 1.2 ;
                                         real_size += (1.2-1.0) ;
                                         } Nov 15, 2001 */
                                        
                                        DistSeg += (1-CurrentDegree)/(count/stepNbr);
                                        if( DistSeg>MinDist ) break ; /* no need to continue on this selected deformation */
                                        OverlapDegree += CurrentDegree ;
                                    }
                                }
                                if(real_size>0)
                                    OverlapDegree = OverlapDegree/real_size ;
                                
                                if( OverlapDegree>MaxDegree ) {
                                    MinDist = DistSeg ;
                                    MaxDegree = OverlapDegree ;
                                    dfm.x = Search.x ;
                                    dfm.y = Search.y ;
                                    dfm.z = Search.z ;
                                }
                            }
                            /*printf("1: end\n") ;*/
                        }
            }
            else {
                MaxDegree = 10000.0 ;
                dfm.x = forceByFocusedPntInObj[s].x*(1.0+(1/*+ratio_iteration Mar 2003*/))/2. ;  /*5.*/
                dfm.y = forceByFocusedPntInObj[s].y*(1.0+(1/*+ratio_iteration Mar 2003*/))/2. ;
                dfm.z = forceByFocusedPntInObj[s].z*(1.0+(1/*+ratio_iteration Mar 2003*/))/2. ; /* May 2001 */
                /*printf("dfm=(%f,%f,%f)\n", dfm.x, dfm.y, dfm.z) ;*/
            }
            /*printf("\ns=%d  MaxDegree=%f  (%f,%f,%f)\n", s, MaxDegree, dfm.x, dfm.y, dfm.z) ;*/
            
            
            //if( s%100==0 )      printf("s=%d  MaxDegree=%f\n", s, MaxDegree) ;
            if( MaxDegree>matchingDegreeOfMdlOnImgEdge ) /*&& (dfm.x!=0 || dfm.y!=0 || dfm.z!=0) )*/   /* deform it*/
                /* MatchingDegreeOfMdlOnImgEdge=0.1 */
            {
                if( s%100==0 ) {
                    //printf("%d->   OL(%d)=%f: (%f,%f,%f)\n", SelectVerNum, s, MaxDegree, dfm.x, dfm.y, dfm.z) ;
                }
                
                for(n=0; n<count; n++) {
                    StudiedPoint.x = FixedMdlVer[s].x+nb_pnt[n].x ;
                    StudiedPoint.y = FixedMdlVer[s].y+nb_pnt[n].y ;
                    StudiedPoint.z = FixedMdlVer[s].z+nb_pnt[n].z ;
                    
                    if( StudiedPoint.x<image_size && StudiedPoint.y<image_size && StudiedPoint.z<z_size && StudiedPoint.x>=0 && StudiedPoint.y>=0 && StudiedPoint.z>=0 ) {
                        /* 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 ;
                        /*level = sqrt( nb_pnt[n].x*nb_pnt[n].x + nb_pnt[n].y*nb_pnt[n].y + nb_pnt[n].z*nb_pnt[n].z ) ;*/ /* tried before June 18, 2001 */
                        weight = exp(-level*level/(2.*nb_size*nb_size)) ;
                        
                        /* converge to its center */
                        nbr_tot.x = 0 ;  nbr_tot.y = 0 ;  nbr_tot.z = 0 ;
                        temp_num = 0 ;
                        for(i=1;i<27;i++) /* 26 neighbor */ {
                            Grid.x = StudiedPoint.x+nb_pnt[i].x ;
                            Grid.y = StudiedPoint.y+nb_pnt[i].y ;
                            Grid.z = StudiedPoint.z+nb_pnt[i].z ;
                            
                            if( Grid.x<image_size && Grid.y<image_size && Grid.z<z_size && Grid.x>=0 && Grid.y>=0 && Grid.z>=0 ) {
                                nbr_tot.x += DeformFld[Grid.z][Grid.x][Grid.y].x;
                                nbr_tot.y += DeformFld[Grid.z][Grid.x][Grid.y].y;
                                nbr_tot.z += DeformFld[Grid.z][Grid.x][Grid.y].z;
                                
                                temp_num++ ;
                            }
                        }
                        if(temp_num>0) {
                            nbr_tot.x /= temp_num ;
                            nbr_tot.y /= temp_num ;
                            nbr_tot.z /= temp_num ;
                        }
                        //TODO add next line as vervose = 2
                        //else
                            //printf("(%d,%d,%d) temp_num=%d (total 8)\n", StudiedPoint.x, StudiedPoint.y, StudiedPoint.z, temp_num) ;
                        
                        CenterRate = smoothFactor ; /*0.05 */
                        nbr_tot.x =  (nbr_tot.x - DeformFld[StudiedPoint.z][StudiedPoint.x][StudiedPoint.y].x)*CenterRate ;
                        nbr_tot.y =  (nbr_tot.y - DeformFld[StudiedPoint.z][StudiedPoint.x][StudiedPoint.y].y)*CenterRate ;
                        nbr_tot.z =  (nbr_tot.z - DeformFld[StudiedPoint.z][StudiedPoint.x][StudiedPoint.y].z)*CenterRate ;
                        
                        Delta=0.005*ratioIteration ; /* June 6, 2001*/
                        /*printf("weight=%f (%d)", weight, level[n]) ; getchar() ;*/
                        DeformFld[StudiedPoint.z][StudiedPoint.x][StudiedPoint.y].x += (dfm.x*weight + nbr_tot.x)*(deformRate+Delta) ;
                        DeformFld[StudiedPoint.z][StudiedPoint.x][StudiedPoint.y].y += (dfm.y*weight + nbr_tot.y)*(deformRate+Delta) ;
                        DeformFld[StudiedPoint.z][StudiedPoint.x][StudiedPoint.y].z += (dfm.z*weight + nbr_tot.z)*(deformRate+Delta) ;
                    }
                }
            }
            /*printf("good!\n") ;*/
        }
    }
    
    
    startSearchPoint = Last_searched_point ;
    
    /* get update MdlVer's position from the DeformFld, for estimating global transformation*/
    for(s=0; s<MdlVer_Num; s++) {
        x = FixedMdlVer[s].x ;
        y = FixedMdlVer[s].y ;
        z = FixedMdlVer[s].z ;
        
        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 ;
    }
    
    /*0.3*//*0.3*/
    /*0.4*//*0.4*/
    /*0.6*//*0.4*/
    /*0.5*//*0.5*/   /* probably for EN** et al */
    LocalRatio = 0.25 + 0.65*exp(-(ratioIteration-1.)*(ratioIteration-1.)/2./.25/.25) ; /* (0.3,0.3),  directly 0.88 */
    if( globalConstraint==NNO ) LocalRatio=1.0 ;
    /* deformation modified by global affine-transformation plus local deformation */
    globalAffineUpdatePlusLocalDeformation(ModelVer, verModelLast, MdlVer_Num, transform ) ;
    Mat_Print(transform) ;
    /* update deformation matrix */
    for(k=0;k<z_size;k++)
        for(i=0;i<image_size;i++)
            for(j=0;j<image_size;j++) {
                Crnt.x = DeformFld_Last[k][i][j].x + i ;
                Crnt.y = DeformFld_Last[k][i][j].y + j ;
                Crnt.z = DeformFld_Last[k][i][j].z + k ;
                
                globl.x = transform->data[0][0]*Crnt.x + transform->data[0][1]*Crnt.y + transform->data[0][2]*Crnt.z + transform->data[0][3] -i ;
                globl.y = transform->data[1][0]*Crnt.x + transform->data[1][1]*Crnt.y + transform->data[1][2]*Crnt.z + transform->data[1][3] -j ;
                globl.z = transform->data[2][0]*Crnt.x + transform->data[2][1]*Crnt.y + transform->data[2][2]*Crnt.z + transform->data[2][3] -k ;
                
                DeformFld[k][i][j].x = globl.x + (DeformFld[k][i][j].x-globl.x)*LocalRatio/1.2;
                DeformFld[k][i][j].y = globl.y + (DeformFld[k][i][j].y-globl.y)*LocalRatio/1.2;
                DeformFld[k][i][j].z = globl.z + (DeformFld[k][i][j].z-globl.z)*LocalRatio/1.2;
                
                /* 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 ;
            }
    smoothDeformationField(DeformFld, image_size, z_size) ;
    if(smoothingTimes>0 && ratioIteration<=0.8)
        smoothDeformationFieldWithEdgePreserving( FixedMdlVer, MdlVer_Num, DeformFld, MdlImg, image_size, z_size, ratioIteration, smoothingTimes ) ; /* opened: June 1, 2004*/
    /* focused edge points in the model */
    for(s=0; s<MdlVer_Num; s++) {
        x = FixedMdlVer[s].x ;
        y = FixedMdlVer[s].y ;
        z = FixedMdlVer[s].z ;
        
        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 ;
    }
    
    /* statistical correcting */
    if(statisticsEmployedOrNot==ADD) {
        statisticalMapping(ModelVer, MdlVer_Num, statisticalFactor, maxNbr, mdlDirectory) ; /* Setp. 3, 1999 */
        //TODO add next line as vervose = 2 printf("statistics information added!\n\n\n") ;
    }
    //TODO add next line as vervose = 2
    //else
        //printf("statistics information not added!\n\n\n") ;
    
    
    /* free */
    free(nb_pnt) ;
    
    /* geometric */
    free(verModelLast) ;
    Fvector3dfree3d(DeformFld_Last, z_size, image_size) ;
}






/**
* The main method
* Really there is no need to yet wrap again the main method, so indeed this 
* needs to change. TODO: Move shenMain to main.
*/
int main(int argc, char *argv[]) {
    shenMain(argc, argv);
    return 0;
}


/**
* This is the wrapper method for the warping procedure.
* 
* The follwoing is an overview of what it does:
*
* 0) Declares the variables, that are to be considered with global scope.
* 1) Obtains the values via getopt from the command line arguments
* 2) Loads the model
* 3) Loads the subject
* 4) Computes some geometric atributes
* 5) Optionally the so call original image can be loaded. TO DO: This feauture 
* is confusing
* 6) Loads the deformation field if it exists already (for med and high resolutions)
* 7) Creates the transformation
* 8) Applys the transformation
* 9) Smooths
* 10) Writes the deformation field
*/
void shenMain(int argc, char *argv[]) {
    unsigned char ***Img;
    int           Img_XY, Img_Z ;
    ImgAttribute  ***MdlImg ;
    ImgAttribute  ***ObjImg ;
    unsigned char ***ObjOriginalImg ;
    Fvector3d     ***DeformFld ;
    int           i, j, k, iter, c, num, resolution, OpenDeformationFieldFromLastScale;
    FILE          *fp;
    extern char   *optarg;
    char          filename[180] ;
    char          defFilename[180] ;
    int           scale ;
    char          ObjOriginalImgFile[180], SurfVerFileName[180] ;
    int           InputOriginalObjImg, InputSurfVer ;
    int           s, sx, sy, sz, InitialUpdate ;
    int           zSize;
    Fvector3d    *forceByFocusedPntInObj;
    
    Threshold mdlThreshold;
    Matrix *transform;
    Ivector3d    *myHood;
    
    int          *multiple;
    
    int    smoothingTimes ; /* for edge preserving smoothing */
    int        mdlVerNum ;   /* total number of the focused edge points */
    int        objVerNum ;   /* The number of focused edge points */
    
    int    maxNbr=13 ;
    int    imageSize;
    int          myHoodSize;
    int    pixelNumInMyHood;
    
    int interpolate=YYES;
    
    float  xyzRes;
    
    static float ratioIteration=0;
    static int   searchResolution;
    static int   lastOrder;
    static float deformRate=0.04;
    static float affineDegree=0.2;
    static float statisticalFactor=0.1;
    static float smoothFactor;
    static float matchingDegreeOfMdlOnImgEdge=0.1;
    static float confidenceOnLastDeformation=0.5 ;
    static int   addMthdOfAvoidingContraction=NOTADD;
    static int   estimateTransformOrNot=NOTADD;
    static int   statisticsEmployedOrNot=NOTADD;
    static int   objectForce=YYES;
    static int   globalConstraint=YYES;
    int   verbose;
    static int   youngBrain=NNO;
    
    char   mdlDirectory[180] ;
    
    char fullVersion[] = "$Rev: 177 $"; //This line will be automatically 
                                       //populated by svn and should be
                                       //of the form "$Rev: 177 $" 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
    if(verbose == 1) printf("\n\nHammer version 1.0.1.%s!\n\n",shortVersion) ;
    
    num=3;
    iter=25;
    xyzRes = 1. ;
    searchResolution= 6 ;
    lastOrder       = 0 ;
    deformRate      = 0.025 ;
    affineDegree    = 0.2 ;
    statisticalFactor = 0.1 ;
    matchingDegreeOfMdlOnImgEdge = 0.1 ;
    addMthdOfAvoidingContraction = NOTADD;
    estimateTransformOrNot       = NOTADD ;
    statisticsEmployedOrNot      = NOTADD ;
    smoothFactor = 0.01 ;
    OpenDeformationFieldFromLastScale=NNO;
    sprintf(mdlDirectory, "./");
    InputOriginalObjImg = YYES ; //This chaged from NNO to YYES as it seems like -O was always used
    InputSurfVer = NNO ;
    smoothingTimes=0 ;
    objectForce  = YYES ;
    InitialUpdate = NNO ;
    Img_XY = 256 ;
    youngBrain = NNO ;   /*March 31 2003*/
    confidenceOnLastDeformation = 0.5 ; /* March 2005 */
    globalConstraint = YYES; /* April 2004 */
    verbose = 1;
    
    if(argc<num){
        showUsage() ;
    }
    
    // <editor-fold defaultstate="collapsed" desc=" name processing the arguments via getopt ">
    while((c=getopt(argc-3, argv+3, "R:l:r:i:s:a:b:m:tedc:FO:S:h:E:NUX:YC:nv")) != -1) {
        switch(c) {
            case 'R':
                deformRate=atof(optarg);
                break;
            case 'l':
                lastOrder=atoi(optarg);
                break;
            case 'r':
                searchResolution=atoi(optarg);
                break;
            case 'i':
                iter=atoi(optarg);
                break;
            case 's':
                xyzRes=atof(optarg);
                break;
            case 'a':
                affineDegree=atof(optarg);
                break;
            case 'b':
                statisticalFactor=atof(optarg);
                break;
            case 'm':
                matchingDegreeOfMdlOnImgEdge=atof(optarg) ;
                break ;
            case 't':
                addMthdOfAvoidingContraction=ADD ;
                break ;
            case 'e':
                estimateTransformOrNot=ADD ;
                break ;
            case 'd':
                statisticsEmployedOrNot=ADD ;
                break ;
            case 'c':
                smoothFactor =atof(optarg);
                break ;
            //case 'D':
            //    sscanf(optarg, "%s", mdlDirectory) ;
            //   break ;
            case 'F':
                OpenDeformationFieldFromLastScale=YYES ;
                break ;
            case 'O':
                sscanf(optarg, "%s", ObjOriginalImgFile) ;
                InputOriginalObjImg = YYES ;
                break ;
            case 'S':
                sscanf(optarg, "%s", SurfVerFileName) ;
                InputSurfVer = YYES ;
                break ;
            case 'h':
                maxNbr=atoi(optarg);
                break;
            case 'E':
                smoothingTimes=atoi(optarg);
                break;
                
            case 'N':
                objectForce = NNO ;
                break;
            case 'U':
                InitialUpdate = YYES ;
                break;
            case 'X':
                Img_XY=atoi(optarg);
                break;
                
            case 'Y':
                youngBrain = YYES ;   /*March 31 2003*/
                break;
                
            case 'C':
                confidenceOnLastDeformation=atof(optarg) ;
                break ;
            case 'n':
                globalConstraint = NNO ;   /*March 31 2003*/
                break;
            case 'v':
                verbose = 2 ;   /*March 31 2003*/
                break;
                
            default:
                break;
        }
    }
    //</editor-fold>
    
    if(verbose == 1) printf("\n\n search_resolution=%d\n iter=%d\n Deform_RATE=%f\n Affine_Degree=%f\n Statistical_factor=%f\n MatchingDegreeOfMdlOnImgEdge=%f\n AddMthdOfAvoidingContraction=%d\n", searchResolution, iter, deformRate, affineDegree, statisticalFactor, matchingDegreeOfMdlOnImgEdge, addMthdOfAvoidingContraction) ;
    if(verbose == 1) printf(" smoothFactor=%f\n", smoothFactor) ;
    if(verbose == 1) printf(" Model directory: %s\n", mdlDirectory) ;
    if(verbose == 1) printf(" Sampling rate: %f\n", xyzRes) ;
    if(verbose == 1) printf(" InputOriginalObjImg=%d\n\n", InputOriginalObjImg) ;
    if( InputOriginalObjImg==YYES ) if(verbose == 1) printf(" Warping result on orignal image will be saved!\n") ;
    if(verbose == 1) printf("\n\n SmoothingTimes=%d\n", smoothingTimes) ;
    if(verbose == 1) printf("ObjectForce=%d\n", objectForce) ;
    if(verbose == 1) printf("Img_XY=%d\n", Img_XY) ;
    if(  youngBrain==YYES )  if(verbose == 1) printf("\n\n ... YoungBrain!\n\n") ; /*March 31 2003*/
    if(verbose == 1) printf(" ConfidenceOnLastDeformation=%f\n", confidenceOnLastDeformation) ;
    if(verbose == 1) printf(" GlobalConstraint=%d\n", globalConstraint) ;
    
    //debug
    if( OpenDeformationFieldFromLastScale==YYES ) {
        if(verbose == 1) printf("I will be reading existing defomation field! \n");
    }
    else {
        if(verbose == 1) printf("I WONT be reading existing defomation field! \n");
    }
    
    /***** Model image, segmented *****/
    /*Img_XY = 256 ;*/
    //sprintf(filename, "%s%s", mdlDirectory, argv[1]);    printf("\n\nmodel image : %s\n", filename) ;
    sprintf(filename, "%s", argv[1]);    if(verbose == 1) printf("\n\nmodel image : %s\n", filename) ;
    
    //This fragment was still kept to be able to reuse the function loadImage
    //but it should really be changed, it is too expensive to read the whole
    //image to determine its size properties, we really need  to start reading
    //headers.
    fp=myopen(filename, "rb");
    fseek(fp, 0, SEEK_END);
    Img_Z=ftell(fp)/(Img_XY*Img_XY);
    rewind(fp);
    imageSize = Img_XY/xyzRes ;
    zSize = Img_Z/xyzRes ;
    fclose(fp);
    //initialize variables
    MdlImg = ImgAttributealloc3d(imageSize, imageSize, zSize);
    DeformFld = Fvector3dalloc3d(imageSize, imageSize, zSize);
    //load images
    loadImage(filename, MdlImg, Img_XY, zSize, imageSize, xyzRes);
    initializeDeformationField(DeformFld, zSize, imageSize);
    
    /* get its edge map, geometric features */
    getWMEdgeGMVnEdgeFromSegmentedImg(MdlImg, zSize, imageSize) ;
    scale = 7*(1./xyzRes) ; if(scale<3) scale=3; if(verbose == 1) printf("scale=%d\n", scale) ;
    computeGeometricFeatures(MdlImg, scale, 1., 1., 1.5, zSize, imageSize) ;
    
    writeImgAttribute("MdlImg", MdlImg, imageSize, zSize) ;
    
    /* Select edge points in the model image to be focused */
    mdlThreshold.Geom_UP = 0.9*255 ;
    mdlThreshold.Geom_DN = 0.4*255 ;
    mdlThreshold.VNvlm_UP = 255/12 ;
    mdlThreshold.CSFBG_UP = 255/4 ;
    select3DEdgePointsInModelTo1D( &mdlVerNum, MdlImg, DeformFld, imageSize, zSize, mdlThreshold) ; /* get  MdlVer, FixedMdlVer, MdlVer_Fea*/
    
    
    
    /***** Object image, segmented  *****/
    /*Img_XY = 256 ;	 */
    //sprintf(filename, "%s%s", mdlDirectory, argv[2]);    printf("\n\nsubject image : %s\n", filename) ;
    sprintf(filename, "%s", argv[2]);    if(verbose == 1) printf("\n\nsubject image : %s\n", filename) ;    
    //This part needs to be reused, there are acouple of times
    //when an image is loaded and allocated, and even further,
    //the image is loaded, allocated and their features are computed
    //there does not seem to exist a real reason to replicate the code
    ObjImg = ImgAttributealloc3d(imageSize, imageSize, zSize);
    loadImage(filename, ObjImg, Img_XY, zSize, imageSize, xyzRes);
    
    
    /* get its edge map, geometric features */
    getWMEdgeGMVnEdgeFromSegmentedImg(ObjImg, zSize, imageSize) ;
    scale = 7*(1./xyzRes) ; if(scale<3) scale=3; if(verbose == 1) printf("scale=%d\n", scale) ;
    computeGeometricFeatures(ObjImg, scale, 1., 1., 1.5, zSize, imageSize) ;
    
    writeImgAttribute("ObjImg", ObjImg, imageSize, zSize) ;
    
    /* Select edge points in the object image to be used as landmarks */
    select3DEdgePointsInObjectTo1D(&objVerNum, ObjImg, imageSize, zSize) ; /* got ObjVer, ObjVer_Fea*/
    forceByFocusedPntInObj = Fvector3dalloc1d(mdlVerNum);
    multiple         = Ialloc1d(mdlVerNum);
    myHoodSize       = searchResolution/2*2+12+1;  /*20+1 ;*/
    myHood           = Ivector3dalloc1d(myHoodSize*myHoodSize*myHoodSize) ;
    calculateHoodByIncreasingRadius(myHood, myHoodSize, &pixelNumInMyHood, 1., 1., 1.) ;
    
    
    
    
    /***** Check here whether to input original Object image for warping it *****/
    //sprintf(filename, "%s%s", mdlDirectory, ObjOriginalImgFile);    printf("\n\nOriginal image : %s\n", filename) ;
    //sprintf(filename, "%s", ObjOriginalImgFile);    printf("\n\nOriginal image : %s\n", filename) ;
    ///sprintf(filename, "%s", argv[4]);    if(verbose == 1) printf("\n\nOriginal image : %s\n", filename) ;
    //ObjOriginalImg = UCalloc3d(imageSize, imageSize, zSize);
    //loadImage(ObjOriginalImgFile, ObjOriginalImg, Img_XY, zSize, imageSize, xyzRes);
    //begin reuse 1
    if( InputOriginalObjImg==YYES ) {
        /*Img_XY = 256 ;  */
        ///fp=myopen(filename, "rb");
        fp=myopen(ObjOriginalImgFile,"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);
        if(verbose == 1) printf("\n\n^^^^  Original object image input!\n") ;
        /*image_size = Img_XY/XYZres ; z_size = Img_Z/XYZres ; */
        
        ObjOriginalImg = UCalloc3d(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++)
                        ObjOriginalImg[k][i][j]=Img[(int)(k*xyzRes)][(int)(i*xyzRes)][(int)(j*xyzRes)] ;
            else
                for(i=0; i<imageSize; i++)
                    for(j=0; j<imageSize; j++)
                        ObjOriginalImg[k][i][j]=0;
        }
        UCfree3d(Img, Img_Z, Img_XY) ;
    }
    //end reuse 1
    /* Inout model's surface model for deforming and carving it into subject's space */
    if( InputSurfVer==YYES && xyzRes==1.0 ) {
        if(verbose == 1) printf("\n\n^^^^  Surface model input!\n") ;
        readModelSurfVer(SurfVerFileName, &InputSurfVer, maxNbr) ;
    }
    
    
    
    /* estimate the global shift and scaling */
    if( estimateTransformOrNot==ADD ) {
        estimatingScalingAndShiftOf3DSurfaceFromImg(globalMdlVer, globalFixedMdlVer, mdlVerFea, mdlVerNum, DeformFld, ObjImg, imageSize, zSize ) ;
        saveCurrentWarpingResultOnSegmentedObj( ObjImg, DeformFld, imageSize, zSize, (int)xyzRes, 999, interpolate) ;
        
        if( InputOriginalObjImg==YYES )
            saveCurrentWarpingResultOnOriginalObjImg( ObjOriginalImg, DeformFld, imageSize, zSize, (int)xyzRes, 999, interpolate) ;
        
        if( InputSurfVer==YYES && xyzRes==1.0 ) /* only on sample rate 1 */
            saveDeformedModelSurfVer(DeformFld, imageSize, zSize, xyzRes, 999, maxNbr) ;
    }
    
    if( OpenDeformationFieldFromLastScale==YYES ) {
        
        //openDeformationField("DeformationField.float.img", DeformFld, imageSize, zSize, confidenceOnLastDeformation) ;
        sprintf(defFilename, "%s", argv[3]);
        if(verbose == 1) printf("\n\nDeformation field : %s\n", defFilename) ;
        openDeformationField(defFilename, DeformFld, imageSize, zSize, confidenceOnLastDeformation) ;
        saveCurrentWarpingResultOnSegmentedObj( ObjImg, DeformFld, imageSize, zSize, (int)xyzRes, 999, interpolate ) ;
        
        if( InputOriginalObjImg==YYES )
            saveCurrentWarpingResultOnOriginalObjImg( ObjOriginalImg, DeformFld, imageSize, zSize, (int)xyzRes, 999, interpolate) ;
        
        if( InputSurfVer==YYES && xyzRes==1.0 ) /* only on sample rate 1 */
            saveDeformedModelSurfVer(DeformFld, imageSize, zSize, xyzRes, 999, maxNbr) ;
        
        /* update the initial deformations in MdlVer; Added on Oct 23, 2002  */
        /* Theoretically, we need to update MdlVer by initial input. But it is removed, for robustness issues */
        if( InitialUpdate==YYES ) {
            if(verbose == 1) printf("Update VerMdl by the result from the last resolution.") ;
            for(s=0; s<mdlVerNum; s++) {
                sx = globalFixedMdlVer[s].x ;
                sy = globalFixedMdlVer[s].y ;
                sz = globalFixedMdlVer[s].z ;
                
                globalMdlVer[s].x = DeformFld[sz][sx][sy].x + sx ;
                globalMdlVer[s].y = DeformFld[sz][sx][sy].y + sy ;
                globalMdlVer[s].z = DeformFld[sz][sx][sy].z + sz ;
            }
        }
        else
            if(verbose == 1) printf("No initial update for VerMdl!\n") ;
        
    }
    
    
    /* global transformation matrix */
    CreateMatrix(&transform,    4, 4);
    
    /* deform */
    ratioIteration=0;
    for(i=0;i<iter;i++) {
        if(ratioIteration>0.5) /*0.5*/ {
            if(verbose == 1) printf("focus change!\n") ;
            /* focuse on more edge points */
            free(globalMdlVer) ;
            free(globalFixedMdlVer) ;
            free(mdlVerFea) ;
            free(forceByFocusedPntInObj) ;
            free(multiple) ;
            
            mdlThreshold.Geom_UP = 0.9*(1-ratioIteration)*255 ;
            mdlThreshold.Geom_DN = 0.4*(2.*ratioIteration)*255 ;
            mdlThreshold.VNvlm_UP = 255/4 ;
            mdlThreshold.CSFBG_UP = 255/4 ;
            select3DEdgePointsInModelTo1D( &mdlVerNum, MdlImg, DeformFld, imageSize, zSize, mdlThreshold) ; /* get  MdlVer, FixedMdlVer, MdlVer_Fea*/
            
            forceByFocusedPntInObj = Fvector3dalloc1d(mdlVerNum);
            multiple               = Ialloc1d(mdlVerNum);
        }
        
        ratioIteration = (float)i/(float)iter ;
        resolution = searchResolution*exp(-ratioIteration*ratioIteration/2./0.36) ;  if(resolution<1) resolution=1 ;
        //
        if(verbose == 1) printf("\n\n\n****************i=%d:   resolution=%d  ratio_iteration=%f************\n", i, resolution, ratioIteration) ;
        if(verbose == 1) printf("Iteration %d out of %d       ", i+1, iter);

        affineDeform3D(globalMdlVer, globalFixedMdlVer, mdlVerNum, mdlVerFea, DeformFld, MdlImg, globalObjVer, objVerNum, objVerFea, ObjImg, imageSize, zSize, resolution, forceByFocusedPntInObj, transform, myHood, multiple, smoothingTimes, maxNbr, ratioIteration, deformRate, statisticalFactor, smoothFactor, matchingDegreeOfMdlOnImgEdge, statisticsEmployedOrNot, objectForce, globalConstraint, youngBrain, mdlDirectory);
        
        /* save temporary results */
        saveCurrentWarpingResultOnSegmentedObj( ObjImg, DeformFld, imageSize, zSize, (int)xyzRes, i, interpolate) ;
        if( InputOriginalObjImg==YYES )
            saveCurrentWarpingResultOnOriginalObjImg( ObjOriginalImg, DeformFld, imageSize, zSize, (int)xyzRes, i, interpolate) ;
        if( InputSurfVer==YYES && xyzRes==1.0 ) /* only on sample rate 1 */
            saveDeformedModelSurfVer(DeformFld, imageSize, zSize, xyzRes, i, maxNbr) ;
        
        /*EvaluationOfRegistration( MdlImg, ObjImg, DeformFld, image_size, z_size, (int)XYZres, i )  ;*/
        /* June 28, 2001: for movie */
        /*sprintf(filename, "DeformationField.float.img%d.%d", (int)XYZres, i);
         if( XYZres>1.0 ) WriteDeformationField(filename, DeformFld, image_size, z_size) ;*/
    }
    
    /* smooth */
    smoothDeformationField(DeformFld, imageSize, zSize) ;
    
    if( xyzRes>=1.0 ){
        //debugWriteDeformationField("DeformationField.float.img", DeformFld, imageSize, zSize) ;
        //writeDeformationField("DeformationField.float.img", DeformFld, imageSize, zSize) ;
        writeDeformationField(argv[3], DeformFld, imageSize, zSize) ;
        
    }
    
    if(verbose == 1) printf("Deformation step on scale %d has been finished!\n", (int)xyzRes) ;
}



