//Source code for EPI DTI geometric distortion correction
//Reference: Chen, B., Guo, H., and Song, A. W., 
//		   Correction for direction-dependent distortion in DTI 
//		   using match magnetic field maps (NeuroImage,2005).
//The input units are defined in readme.txt file
//
//Developed under Microsoft Visual Studio .Net 2003
//
//Bin Chen, BIAC, Duke University, 2005


#include <iostream>
#include <string>
#include "stdafx.h"
#include "matlab.h"


/***********  Function Prototypes  ****************/
double* compTE(double);
mxArray* readseq(char []);
mxArray* readginx(char []);
mxArray* recon_ginx(char []);

/* --------compute field maps---------- */
mxArray* dB0_map(mxArray*, double []);
mxArray* unwrap1D_itoh(mxArray *);
mxArray* epcsirecf_5(mxArray**,mxArray* );
mxArray* epcsi2D(mxArray**,mxArray* );


/*------eddy current computation----------*/
mxArray* eddycurrent_correction_u(mxArray* , int []);
void findFittingROI(int [], mxArray*);
	/* eddycurrent computation utility functions*/
int mxArray2int(const mxArray *);
void mxArray2char(unsigned char i[], const mxArray *);
int otsu (unsigned char *, int , int , int , int , int , int , int);
mxArray* Otsu_thresh(mxArray *);
/*------END of eddy current computation-----------*/

/*-------image distortion correction --------*/
mxArray* corr_eddy(mxArray* ,mxArray* ,mxArray* ,mxArray*);
mxArray* dBCorrection(mxArray **, mxArray *, mxArray *);
mxArray * ShearFT(mxArray* , double );
double mxArray2double(const mxArray *);
/*-------End of image distortion correction --------*/

/* File operation */
	/* writefile(data, datatype(1=int16,2=double), endian(little,big) */
void writefile(mxArray*, char[], int , int );
/* Usage info */
void usage();



/* Global Variables */
	/*these are constants, write here for debugging only*/
	int dimx=128,dimy=128,dimz=1,dimt=32,DW=7,trans=24;
	/* if FOVx==FOVy, then FOV can be defined */
	double FOV=0.24, FOVx=0.24, FOVy=0.24; 
	int offset = 39984;  /* offset bytes of phaseepi file */
	int ginxoffset = 7904; /*offset bytes of ginx file */
	double te = 140; /*unit: ms*/
	double dt = 0.000008; /*8 mu_second*/
	double gyrobar = 42580000; 

	char dirname[255] ; /*= "D:\\data\\P_Files\\BIAC_4T\\20050726_45012\\45102\\007\\"*/

int _tmain(int argc, _TCHAR* argv[])
{	
	char *pfname, *pfdir;
	//char tmpdir[256];
	double *TE;
	int ii,jj,zz, *s;
	int dirlen;
	int corners[4]={0,0,0,0};
	mxArray *rawdata = NULL;
	mxArray *phase_recon=NULL;
	mxArray *magnitude_recon=NULL;
	mxArray *dB0map = NULL;
	mxArray *dB0map4D=NULL;
	mxArray *phase_recon_bi = NULL;
	mxArray *magnitude_recon_b0=NULL;
	mxArray *dGxdGy_mean = NULL;
	mxArray *dB0map_1dw=NULL;
	mxArray *correctedDWIs=NULL;
	mxArray *dGx_mean=NULL;
	mxArray *dGy_mean=NULL;
	mxArray *epiDWIs_human=NULL;
	mxArray *dB0map_1slice=NULL;
	mxArray *dGxdGy_mean_allslices=NULL;
	mxArray *correctedDWIs_allslices=NULL;

	mlfEnterNewContext(0,0);
	/*--------------------*/

	if(argc < 10 ) 	{usage(); exit(1);}
	
	/* assign input parameters to variables*/
	pfdir = argv[2]; jj=0;
	dirlen = int(strlen(argv[2]));
    for(ii=0;ii<dirlen;ii++)
	{ 	if(pfdir[ii] == '\\')
		{  dirname[jj]=pfdir[ii]; jj++;   dirname[jj] = pfdir[ii]; jj++;
		}
		else
		{  dirname[jj] = pfdir[ii];jj++;
		}
	}

	dirname[jj] = '\0';
	if(pfdir[dirlen-1] != '\\')
	{  dirname[jj] = '\\'; jj++; dirname[jj] = '\\';jj++; dirname[jj] = '\0';
	}
	//printf("argv[2]=%d=%s \n",dirlen, dirname);

	pfname = argv[1];
	dimx = atoi(argv[3]); dimy = atoi(argv[4]); dimz = atoi(argv[5]);
	dimt = atoi(argv[6]);  DW = atoi(argv[7]); 
	FOV = atof(argv[8])/100.0; FOVx = FOV; FOVy = FOV; /* FOV in cm */
	te = atof(argv[9]);
	if(argc == 11) { dt = 1.0/1000.0/atof(argv[10]);} /* sweep rate in KHz */
	if(argc ==12) { offset = atoi(argv[11]);} /* header size of phaseEPI file */
	if(argc == 13) {offset = atoi(argv[11]); ginxoffset = atoi(argv[12]);} /* DWI file header size */

	/* step1: read in file. PASS TEST*/
	printf("\n==== Reading and reconstructing phase EPI images ...... ");
	mlfAssign(&rawdata, readseq(pfname));
	printf(" Done.\n\n");



	/* step2: compute multiecho TEs */
	TE = compTE(te);
	/* debug: display TE
	for(ii=0;ii<dimt;ii++){printf("outside TE[%d] = %f;  ",ii,TE[ii]);}
	*/

	/* step3: reconstruct image and return image phases. TEST PASSED*/
	printf("==== Computing field variation maps ...... ");
	mlfAssign(&phase_recon,epcsirecf_5(&magnitude_recon,rawdata));
/*					s = (int *) mxGetDimensions(phase_recon); 
			printf("dims of phase_recon = [%d, %d, %d, %d, %d]. \n", s[0],s[1],s[2],s[3],s[4]);
						s = (int *) mxGetDimensions(magnitude_recon); 
			printf("dims of magnitude_recon = [%d, %d, %d, %d, %d]. \n", s[0],s[1],s[2],s[3],s[4]);
*/
				/* save phase and magnitude. Checkpoint: phase and mag is the same as matlab gen.*/
			/*mlfSave(mxCreateString("tmpSaved_recon"),"w","phase_recon",phase_recon,"magnitude_recon",magnitude_recon,NULL);*/

	/*step 4: compute dB0map. TEST PASSED*/
	/* reconstructed phase: phase_recon(dimx,dimy,dimt,DW,dimz).
		when dimz=1 (one slice), phase_recon(dimx,dimy,dimt,DW)*/
	if (dimz==1)
	{	/* b0 case: ii=1 */
		mlfAssign(&phase_recon_bi,mlfSqueeze(mlfIndexRef(phase_recon,"(?,?,?,?)",mlfCreateColonIndex(),mlfCreateColonIndex(),mlfCreateColonIndex(),mlfScalar(1))));
		mlfAssign(&dB0map, dB0_map(phase_recon_bi, TE));
		
		for (ii=2;ii<=DW;ii++)
		{	printf(".");
			mlfAssign(&phase_recon_bi,mlfSqueeze(mlfIndexRef(phase_recon,"(?,?,?,?)",mlfCreateColonIndex(),mlfCreateColonIndex(),mlfCreateColonIndex(),mlfScalar(ii))));
			/* dB0 of b0 image between C and matlab are compared. difference is ~1e-15 level. TEST PASSED*/
			mlfAssign(&dB0map_1dw, dB0_map(phase_recon_bi, TE));
			mlfAssign(&dB0map, mlfCat(mlfScalar(3), dB0map, dB0map_1dw, NULL));
			//printf(" dB0map(32,32,:)=\n");
			//mlfPrintMatrix(mlfIndexRef(dB0map,"(?,?,?)", mlfScalar(32),mlfScalar(32),mlfCreateColonIndex()));
		}

/*---- this section allocates dB0map space first, then populates dB0map with dB0map_1dw. I don't know why the later assignment zeros the former assignments.
		for (ii=1;ii<=DW;ii++)
		{
		mlfAssign(&dB0map, mlfZeros(mlfScalar(dimx),mlfScalar(dimy),mlfScalar(DW),NULL));
		mlfAssign(&phase_recon_bi,mlfSqueeze(mlfIndexRef(phase_recon,"(?,?,?,?)",mlfCreateColonIndex(),mlfCreateColonIndex(),mlfCreateColonIndex(),mlfScalar(ii))));
			// dB0 of b0 image between C and matlab are compared. difference is ~1e-15 level. TEST PASSED
		mlfAssign(&dB0map_1dw, dB0_map(phase_recon_bi, TE));
		mlfIndexAssign(&dB0map,"(?,?,?)", mlfCreateColonIndex(),mlfCreateColonIndex(), mlfScalar(ii),dB0map_1dw);
		}
*/
	}

	/* if index assignment works fine, the code can be much shorter. Performance might be the same though. */
	if(dimz>1)
	{ 
	  for(jj=1;jj<=dimz;jj++)
	  {	
		  mlfAssign(&phase_recon_bi,mlfSqueeze(mlfIndexRef(phase_recon,"(?,?,?,?,?)",mlfCreateColonIndex(),mlfCreateColonIndex(),mlfCreateColonIndex(),mlfScalar(1),mlfScalar(jj))));
		  mlfAssign(&dB0map,dB0_map(phase_recon_bi, TE));
		for (ii=2;ii<=DW;ii++)
		{   printf(".");
			mlfAssign(&phase_recon_bi,mlfSqueeze(mlfIndexRef(phase_recon,"(?,?,?,?,?)",mlfCreateColonIndex(),mlfCreateColonIndex(),mlfCreateColonIndex(),mlfScalar(ii),mlfScalar(jj))));
			mlfAssign(&dB0map_1dw, dB0_map(phase_recon_bi, TE));
			mlfAssign(&dB0map, mlfCat(mlfScalar(3), dB0map, dB0map_1dw, NULL));
		}

		if(jj==1)
		{	mlfAssign(&dB0map4D, dB0map);
			mxDestroyArray(dB0map); dB0map=NULL;
		}
		else
		{	mlfAssign(&dB0map4D, mlfCat(mlfScalar(4), dB0map4D, dB0map, NULL));
			mxDestroyArray(dB0map); dB0map=NULL;
		}

	  }

	} /* end of if(dimz>1) */

	mlfAssign(&dB0map, mlfFlipdim(dB0map, mlfScalar(2))); /* flip along y to match the same shape as matlab code*/
	printf(" Done.\n"); //dB0map done.

	/* save dB0map*/
		//mlfSave(mxCreateString("tmpSaved_dB0map"),"w","dB0map",dB0map,NULL);
		/*write dB0map as double, little endian*/
		writefile(dB0map, "c_dBmap",2,1);

		
		//==== Field variation map saved.\n\n");

/* Input epi DWI images here. */
    /* The following line is for 64x64 with Duke EPI acquistion only */
	/*mlfLoad(mxCreateString("epidw.mat"),"epidw", &epiDWIs_human, NULL);*/
	printf("\n==== Reading and reconstructing EPI images ...... ");
		mlfAssign(&epiDWIs_human, recon_ginx(dirname)); /*readin distorted DW-EPI images*/
		mlfAssign(&epiDWIs_human, mlfFlipdim(epiDWIs_human, mlfScalar(1))); /*flip x dir to match the same shape as dB0map*/
			/* save DW EPI images. Check the Read-In functions*/
		//mlfSave(mxCreateString("tmpSaved_epiDWI007"),"w","epiDWI007",epiDWIs_human,NULL);
printf(" Done. \n"); // save EPI images
		/* save distorted image as int16, little endian*/
		writefile(epiDWIs_human, "c_epiDWI",1,1);
	
/*
		s = (int *) mxGetDimensions(epiDWIs_human); 
			printf("dims of phase_recon = [%d, %d, %d, %d, %d]. \n", s[0],s[1],s[2],s[3],s[4]);
*/
	printf("\n==== Correcting the image distortions ...... ");
	if(dimz==1)
	{
		/* By looking at the corners values(defines the fitting ROI). This function works fine on ball, cylinder -shaped objects. */
		mlfAssign(&magnitude_recon_b0,mlfIndexRef(magnitude_recon,"(?,?,?,?)",mlfCreateColonIndex(),mlfCreateColonIndex(),mlfScalar(dimt/2),mlfScalar(1)));
		findFittingROI(corners, mlfSqueeze(magnitude_recon_b0));
//		printf(" Corners of ROI: [x1,y1] = [%d,%d], [x2,y2] = [%d,%d]. \n", corners[0],corners[1],corners[2],corners[3]);

	    mlfAssign(&dGxdGy_mean, eddycurrent_correction_u(dB0map,corners));
//		printf("dGxdGy_mean from b0-b6 [1e-5 T/m]: \n");
//		mlfPrintMatrix(mlfTimes(dGxdGy_mean,mlfScalar(100000)));


		/* step6 distortion correction. */

		/* convert eddy current gradient to pixel shearing, scaling. */
		mlfAssign(&dGx_mean, mlfIndexRef(dGxdGy_mean,"(?)",mlfColon(mlfScalar(1),mlfScalar(DW),NULL)));
		mlfAssign(&dGy_mean, mlfIndexRef(dGxdGy_mean,"(?)",mlfColon(mlfScalar(DW+1),mlfScalar(2*DW),NULL)));

		mlfAssign(&correctedDWIs, corr_eddy(epiDWIs_human,dB0map,dGx_mean,dGy_mean));
		/* save corrected DWI images*/
		//mlfSave(mxCreateString("tmpSavedData"),"w","correctedDWIs",correctedDWIs,NULL);
				/*write corrected data to disk.*/
		printf(" Done. \n");
		writefile(correctedDWIs, "cDWI",1,1);
	}

	if(dimz>1)
	{	/* dGxdGy_mean_allslices saves eddy current gradient values. Size of dGxdGy_mean = [2*DW*dimz]. 
			   Order: dimz number of units. each unit = [ DW number of dGx + DW number of dGy]; 
			   It can be re-organized into [DW,DW,dimz]. First DW is for dGx, Second DW for dGy*/
			for(zz=1;zz<=dimz;zz++)
			{
				mlfAssign(&magnitude_recon_b0,mlfIndexRef(magnitude_recon,"(?,?,?,?,?)",mlfCreateColonIndex(),mlfCreateColonIndex(),
					mlfScalar(dimt/2),mlfScalar(1),mlfScalar(zz)));

				findFittingROI(corners, mlfSqueeze(magnitude_recon_b0));
				mlfAssign(&dB0map_1slice, mlfIndexRef(dB0map4D, "(?,?,?,?)", mlfCreateColonIndex(),mlfCreateColonIndex(),mlfCreateColonIndex(),mlfScalar(zz)));
			    mlfAssign(&dGxdGy_mean, eddycurrent_correction_u(dB0map_1slice,corners));
				if(zz==1)
				{ mlfAssign(&dGxdGy_mean_allslices,dGxdGy_mean);}
				else
				{ mlfAssign(&dGxdGy_mean_allslices, mlfCat(mlfScalar(2),dGxdGy_mean_allslices,dGxdGy_mean,NULL));}
			}

				/*--------------------------*/
		
		for (zz=1;zz<=dimz; zz++)
		{
			/* convert eddy current gradient to pixel shearing, scaling. */
			mlfAssign(&dGx_mean, mlfIndexRef(dGxdGy_mean_allslices,"(?)",mlfColon(mlfScalar(1+(zz-1)*2*DW),mlfScalar(DW+(zz-1)*2*DW),NULL)));
			mlfAssign(&dGy_mean, mlfIndexRef(dGxdGy_mean_allslices,"(?)",mlfColon(mlfScalar(DW+1+(zz-1)*2*DW),mlfScalar(zz*2*DW),NULL)));

			mlfAssign(&correctedDWIs, corr_eddy(epiDWIs_human,dB0map,dGx_mean,dGy_mean));
			if(zz==1)
			{ mlfAssign(&correctedDWIs_allslices,correctedDWIs);}
			else
			{mlfAssign(&correctedDWIs_allslices, mlfCat(mlfScalar(3),correctedDWIs_allslices, correctedDWIs,NULL));}
		}
		/* save corrected DWI images*/
		//mlfSave(mxCreateString("tmpSavedData"),"w","correctedDWIs",correctedDWIs_allslices,NULL);
		printf(" Done. \n");
		/*write corrected data to disk.*/
		writefile(correctedDWIs_allslices, "cDWI",1,1);
	}




	/*-------------------------*/
	mxDestroyArray(rawdata);
	mxDestroyArray(phase_recon);
	mxDestroyArray(dB0map);
	mxDestroyArray(phase_recon_bi);
	mxDestroyArray(magnitude_recon);
	mxDestroyArray(magnitude_recon_b0);
	mxDestroyArray(dGxdGy_mean);
	mxDestroyArray(dB0map_1dw);
	mxDestroyArray(correctedDWIs);
	mxDestroyArray(dB0map4D);
	mxDestroyArray(dGx_mean);
	mxDestroyArray(dGy_mean);
	mxDestroyArray(epiDWIs_human);
	mxDestroyArray(dB0map_1slice);
	mxDestroyArray(dGxdGy_mean_allslices);
	mxDestroyArray(correctedDWIs_allslices);

	mlfRestorePreviousContext(0,0);
	return 0;
}




/*imc -- returned corrected 2d image;
  im -- input, the distorted, original image;
  dBmap -- field map to correct im. unit: Tesla

  Note: gyrobar and dt are hard coded.
        dt is sequence and scanner dependent
*/
mxArray* dBCorrection(mxArray *im, mxArray *dBmap)
{
	double dt_blip, Tacq;
	int d1,d2,*s,ii;

	//mxArray *m=NULL; 
	//mxArray *n=NULL;

	/* temp variables */
	mxArray *dr_phase = NULL;
	mxArray *IntLoc = NULL;
	mxArray *Yline = NULL;
	mxArray *Yloc = NULL;
	mxArray *imc = NULL;

//printf("\n -- Enter dBCorrection --\n");

	/*-------------------------------------*/
	mlfEnterNewContext(0,2,im,dBmap);

	s = (int *) mxGetDimensions(im);
	d1=s[0]; d2=s[1];

//mlfSize(mlfVarargout(&m,&n,NULL),im,NULL);
//	dimx = (double*) mxGetPr(m);
//	dimy = (double*) mxGetPr(n);
//	d1 = (int) (*dimx); d2 = (int) (*dimy);
//	printf(" d1 = %d, d2 = %d. \n", d1,d2);

	dt_blip = dt * trans;
	Tacq = d2 * (dt_blip + dt*d1)-dt_blip;
	Tacq = gyrobar * Tacq;
	mlfAssign(&dr_phase, mlfTimes(dBmap,mlfScalar(Tacq)));
	mlfAssign(&IntLoc, mlfColon(mlfScalar(1), mlfScalar(d2), NULL));
	mlfAssign(&imc, mlfZeros(mlfScalar(d1), mlfScalar(d2),NULL));
	
	for(ii = 1; ii <= d1;ii++)
	{
		mlfAssign(&Yline, mlfIndexRef(im, "(?,?)", mlfScalar(ii), mlfCreateColonIndex()));
		mlfAssign(&Yloc, mlfPlus(IntLoc, mlfIndexRef(dr_phase, "(?,?)", mlfScalar(ii), mlfCreateColonIndex())));	
		mlfIndexAssign(&imc,"(?,?)", mlfScalar(ii), mlfCreateColonIndex(), mlfInterp1(IntLoc,Yline,Yloc,NULL));
/*		mlfPrintMatrix(Yline);
		mlfPrintMatrix(Yloc);
		mlfPrintMatrix(imc);	
*/
	}


	//mxDestroyArray(m);mxDestroyArray(n);
	mxDestroyArray(dr_phase);
	mxDestroyArray(IntLoc);
	mxDestroyArray(Yloc);
	mxDestroyArray(Yline);

	mlfRestorePreviousContext(0,2,im,dBmap);
	return mlfReturnValue(imc);	


}
/* 1D phase unwrapping */
mxArray* unwrap1D_itoh(mxArray *p)
{
	int ii,m;

	mxArray *dp = NULL;
	mxArray *dp_delta=NULL;
	mxArray *tmp = NULL;
	mxArray *q=NULL;

//-------------------------------------
	mlfEnterNewContext(0,1,p);

	m = mxGetNumberOfElements(p);
	mlfAssign(&dp, mlfDiff(p,NULL,NULL));
	
	mlfAssign(&dp_delta, mlfAtan(mlfRdivide(mlfSin(dp),mlfCos(dp))));
	//printf("dp_delta=\n");mlfPrintMatrix(dp_delta);
	mlfAssign(&q, mlfZeros(mlfScalar(m),mlfScalar(1),NULL));
	mlfIndexAssign(&q,"(?)", mlfScalar(1),mlfIndexRef(p,"(?)",mlfScalar(1)));

	for(ii=2;ii<=m; ii++)
	{	mlfAssign(&tmp, mlfPlus(mlfIndexRef(q, "(?)", mlfScalar(ii-1)), mlfIndexRef(dp_delta, "(?)", mlfScalar(ii-1))));
		mlfIndexAssign(&q,"(?)",mlfScalar(ii), tmp);
	}

	mxDestroyArray(dp);
	mxDestroyArray(tmp);
	mxDestroyArray(dp_delta);

	mlfRestorePreviousContext(0,1,p);

	return mlfReturnValue(q);	

}
/* dB0_map compute field map
   Input: wrapped angles imAng from (-pi, pi)
			imAng: dimension (X,Y,T);
			TEs:	col vector, unit: msec
   Output: field map in Tesla. dBmap in (X,Y) dimension
*/
mxArray* dB0_map(mxArray *imAng, double TE0[])

{	/*static double gyrobar = 42580000; //make it global in the end*/
	static double pi = 3.14159265;
	double gyro,*TE0odd,*TE0even,*evenIndex0,*oddIndex0;
	int ii,jj,count1=0,count2=0,sx,sy,st,*s;

	mxArray *dB=NULL;
	mxArray *TE=NULL;
	mxArray *te1=NULL;
	mxArray *te2=NULL;
	mxArray *p=NULL;
	mxArray *uw1d_odd=NULL;
	mxArray *uw1d_even=NULL;
	mxArray *oddIndex=NULL;
	mxArray *evenIndex=NULL;
	mxArray *coef1=NULL;
	mxArray *coef2=NULL;
	mxArray *tmp1=NULL;
	mxArray *tmp2=NULL;
	mxArray *tmp3 = NULL;
	mxArray *mu = NULL;
	mxArray *tmpA = NULL;
	mxArray *tmpB = NULL;
	
	//printf(" == Enter dB0_map == \n");

	s = (int *) mxGetDimensions(imAng);
	sx=s[0]; sy=s[1]; st=s[2];
	/*printf("phase angle matrix dim = %d,%d,%d. \n", sx,sy,st);*/

		TE0odd =(double*) malloc(st/2*sizeof(double));
		if(!TE0odd){printf("Out of memory. Hard to believe it. \n"); exit(1);}
		TE0even =(double*) malloc(st/2*sizeof(double));
		if(!TE0even){printf("Out of memory. Hard to believe it. \n"); exit(1);}
		evenIndex0 =(double*) malloc(st*sizeof(double));
		if(!evenIndex0){printf("Out of memory. Hard to believe it. \n"); exit(1);}
		oddIndex0 =(double*) malloc(st*sizeof(double));
		if(!oddIndex0){printf("Out of memory. Hard to believe it. \n"); exit(1);}

	//convert TE0 from msec to sec
	count1=0;count2=0;
	for (ii=0;ii<st;ii++)
	{	TE0[ii]=TE0[ii];
		//printf("TE0[%d]= %f",ii,TE0[ii]);
		
		if((ii/2.0) > ((double)(ii/2)))
		{ TE0even[count1] = TE0[ii];
		  evenIndex0[ii]=1; oddIndex0[ii] = 0;
		//printf("TE0even[%d]= %f",count1,TE0even[count1]);
		count1++;
		}
		else
		{TE0odd[count2] = TE0[ii];
		evenIndex0[ii]=0; oddIndex0[ii] = 1;
		//printf("TE0odd[%d]= %f",count2,TE0odd[count2]);
		count2++;
		}
		
	//printf("TE0[%d] = %f;  ",ii,TE0[ii]);
	}

/*------------------------------*/
mlfEnterNewContext(0,1,imAng);
	

	//mlfAssign(&oddIndex, mlfGt(mlfRdivide(mlfColon(mlfScalar(1),mlfScalar(2), mlfScalar(st)),mlfScalar(2)),mlfScalar(0)));
	//mlfAssign(&evenIndex,mlfGt(mlfRdivide(mlfColon(mlfScalar(2),mlfScalar(2), mlfScalar(st)),mlfScalar(2)),mlfScalar(0)));
	//printf("oddIndex: \n"); mlfPrintMatrix(oddIndex);
	//printf("evenIndex: \n"); mlfPrintMatrix(evenIndex);

	// odd and even TEs
	mlfAssign(&TE, mlfDoubleMatrix(st,1,TE0,NULL));
	mlfAssign(&te1, mlfDoubleMatrix(st/2,1,TE0odd,NULL));
	mlfAssign(&te2, mlfDoubleMatrix(st/2,1,TE0even,NULL));
	mlfAssign(&oddIndex, mlfGt(mlfDoubleMatrix(st,1,oddIndex0,NULL),mlfScalar(0)));
	mlfAssign(&evenIndex, mlfGt(mlfDoubleMatrix(st,1,evenIndex0,NULL),mlfScalar(0)));

	//printf("TE=\n"); mlfPrintMatrix(TE);
	//printf("oddIndex=\n"); mlfPrintMatrix(oddIndex);
	//printf("evenIndex=\n"); mlfPrintMatrix(evenIndex);
	//printf("te1: \n"); mlfPrintMatrix(te1);
	//printf("te2: \n"); mlfPrintMatrix(te2);

	//assign memory space for returning var dB
	mlfAssign(&dB, mxCreateNumericMatrix(sx,sy,mxDOUBLE_CLASS,mxREAL));
	mlfAssign(&p, mlfZeros(mlfScalar(st),mlfScalar(1),NULL));
	for (ii=1;ii<=sx;ii++)
		for(jj=1;jj<=sy;jj++)
		{
		mlfAssign(&p,mlfIndexRef(imAng,"(?,?,?)",mlfScalar(ii),mlfScalar(jj), mlfCreateColonIndex()));
		mlfAssign(&p,mlfIndexRef(p, "(?)", mlfCreateColonIndex()));
		mlfAssign(&uw1d_odd,unwrap1D_itoh(mlfIndexRef(p,"(?)", oddIndex)));
		/*check point: data passing to unwrap1D_itoh, and returned unwrapped phase from unwrap1D_itoh are correct.*/

		mlfAssign(&coef1, mlfNPolyfit(2,&mu,NULL,te1,uw1d_odd,mlfScalar(1)));
		mlfAssign(&uw1d_even,unwrap1D_itoh(mlfIndexRef(p,"(?)", evenIndex)));
		mlfAssign(&coef2, mlfNPolyfit(2, &mu, NULL,te2,uw1d_even,mlfScalar(1)));
		mlfAssign(&tmp1, mlfIndexRef(coef1,"(?)",mlfScalar(1)));
		mlfAssign(&tmp2, mlfIndexRef(coef2,"(?)",mlfScalar(1)));
		mlfIndexAssign(&dB,"(?,?)", mlfScalar(ii), mlfScalar(jj),mlfRdivide(mlfPlus(tmp1,tmp2),mlfScalar(2)));
		}

		gyro = 2*pi*gyrobar;
		mlfAssign(&dB, mlfRdivide(dB,mlfScalar(gyro)));
/*--------------------------------*/
		mxDestroyArray(TE);
		mxDestroyArray(te1);
		mxDestroyArray(te2);
		mxDestroyArray(p);
		mxDestroyArray(uw1d_odd);
		mxDestroyArray(uw1d_even);
		mxDestroyArray(oddIndex);
		mxDestroyArray(evenIndex);
		mxDestroyArray(coef1);
		mxDestroyArray(coef2);
		mxDestroyArray(tmp1);
		mxDestroyArray(tmp2);
		mxDestroyArray(mu);

mlfRestorePreviousContext(0,1,imAng);
return mlfReturnValue(dB);	
}

/*epcsi2D is a utility function called by epcsiref_4.c
Input: raw data line of one image from scanner P file
Output: phase matrix*/
mxArray * epcsi2D(mxArray** imc_mag, mxArray* d)
{
	int i,j,szrow,itmp0,itmp1;

	mxArray* imgr=NULL;
	mxArray* ndx_dest=NULL;
	mxArray* temp = NULL;
	mxArray* imgraw=NULL;
	mxArray* tmpA = NULL;
	mxArray* tmpB = NULL;
	mxArray* tmpC = NULL;
	mxArray* tmpD = NULL;
	mxArray* imgraw1=NULL;
	mxArray* final= NULL;
	mxArray* finalswap= NULL;
	mxArray* imcomplex= NULL;
	mxArray* imc_re= NULL;
	mxArray* imc_im= NULL;
	mxArray* imc_ph = NULL;


	//printf(" Enter epcsi2D. \n");
	//printf(".");
/*------------------------------*/
mlfEnterNewContext(1,1,imc_mag,d);

	szrow = 2*((dimx+trans)*dimt - trans);
	itmp0 = szrow+2*trans;
		//imgr = zeros(szrow+2*trans,dimy);
	mlfAssign(&imgr, mlfZeros(mlfScalar(itmp0), mlfScalar(dimy),NULL));

	for (i=1;i<=dimy;i++)
	{//imgr(1:szrow,i) = d((i-1)*szrow+1 : i*szrow);
		itmp0 = (i-1)*szrow+1;
		itmp1 = i*szrow;
		mlfAssign(&ndx_dest, mlfColon(mlfScalar(1),mlfScalar(szrow),NULL));
		mlfIndexAssign(&imgr,"(?,?)", ndx_dest, mlfScalar(i),mlfIndexRef(d,"(?)",mlfColon(mlfScalar(itmp0), mlfScalar(itmp1), NULL)));
	}


	/* allocate memory for temp variable imgraw1 */
	mlfAssign(&imgraw1, mlfZeros(mlfScalar(dimx), mlfScalar(dimt),NULL));

	for (j=1;j<=dimy;j++)
	{
		mlfAssign(&temp, mlfIndexRef(imgr,"(?,?)",mlfCreateColonIndex(), mlfScalar(j)));
		itmp0 = (dimx+trans)*2;
		mlfAssign(&imgraw, mlfReshape(temp, mlfScalar(itmp0),mlfScalar(dimt),NULL));
		/* this c file did not empty imgraw1 */
		for(i=1;i<=dimt;i++)
		{
			mlfAssign(&tmpA,mlfIndexRef(imgraw,"(?,?)", mlfColon(mlfScalar(1),mlfScalar(2),mlfScalar(2*dimx)),mlfScalar(i)));
			mlfAssign(&tmpB,mlfIndexRef(imgraw,"(?,?)", mlfColon(mlfScalar(2),mlfScalar(2),mlfScalar(2*dimx)),mlfScalar(i)));
			mlfAssign(&tmpC,mlfTimes(mlfComplexScalar(0,1),tmpB));
			mlfIndexAssign(&imgraw1,"(?,?)", mlfCreateColonIndex(),mlfScalar(i),mlfPlus(tmpA,tmpC));
		}
		for (i=2;i<=dimt; i=i+2)
		{	
			mlfIndexAssign(&imgraw1,"(?,?)",mlfCreateColonIndex(),mlfScalar(i),mlfFlipud(mlfIndexRef(imgraw1,"(?,?)",mlfCreateColonIndex(),mlfScalar(i))));
		}
		/* mlfCtranspose does A', matrix transposed PLUS conj the elemements. a+bi ==> a-bi.
		   mlfTranspose does A.', transpose the matrix only
		mlfIndexAssign(&final,"(?,?,?)", mlfCreateColonIndex(),mlfCreateColonIndex(),mlfScalar(j),mlfCtranspose(imgraw1));*/
		mlfIndexAssign(&final,"(?,?,?)", mlfCreateColonIndex(),mlfCreateColonIndex(),mlfScalar(j),mlfTranspose(imgraw1));
	}
	
		/* imgraw1 is correct. 
		mlfSave(mxCreateString("tmpSavedData4"),"w","epcsi2D_final_C",final,NULL);
		*/
		for (i=1;i<=dimt;i++)
		{
			mlfIndexAssign(&finalswap,"(?,?,?)", mlfCreateColonIndex(),mlfCreateColonIndex(),mlfScalar(i), mlfIndexRef(final,"(?,?,?)",mlfScalar(i),mlfCreateColonIndex(),mlfCreateColonIndex()));
		}

		

		for (i=1;i<=dimt;i++)
		{
			mlfAssign(&tmpD,mlfIndexRef(finalswap,"(?,?,?)", mlfCreateColonIndex(),mlfCreateColonIndex(),mlfScalar(i)));
			mlfIndexAssign(&imcomplex,"(?,?,?)", mlfCreateColonIndex(),mlfCreateColonIndex(),mlfScalar(i), mlfFftshift(mlfFft2(tmpD,NULL,NULL),mlfScalar(1)));
		}
		/* Note: imcomplex is the same as the imcomplex genby epcsirecf_4.m when using mlfCtranspose
		         imcomplex should be the same as the imcomplex genby epcsirecf_5.m when using mlfTranspose
		
		mlfSave(mxCreateString("tmpSavedData"),"w","epcsi2D_imcomplex_C",imcomplex,NULL);
		*/
		

		mlfAssign(&imc_re, mlfReal(imcomplex));
		mlfAssign(&imc_im, mlfImag(imcomplex));

		mxDestroyArray(tmpC); tmpC=NULL;
		/* mclAssignAns cannot be changed to mlfAssign */
		mclAssignAns(&tmpC, mlfNWarning(0, NULL, mxCreateString("off"), mxCreateString("all"), NULL));

		mlfAssign(&imc_ph, mlfAtan(mlfRdivide(imc_im,imc_re)));
		mlfAssign(imc_mag, mlfAbs(imcomplex));
/*------------------------------*/
		mxDestroyArray(imgr);
		mxDestroyArray(ndx_dest);
		mxDestroyArray(temp);
		mxDestroyArray(imgraw);
		mxDestroyArray(tmpA);
		mxDestroyArray(tmpB);
		mxDestroyArray(tmpC);
		mxDestroyArray(tmpD);
		mxDestroyArray(imgraw1);
		mxDestroyArray(final);
		mxDestroyArray(finalswap);
		mxDestroyArray(imcomplex);
		mxDestroyArray(imc_re);
		mxDestroyArray(imc_im);

mlfRestorePreviousContext(1,1,imc_mag,d);
return mlfReturnValue(imc_ph);

}

/* epcsi reconstruction function.
Input: a long line of data out of readseq
Output: reconstructed phase and magnitude of whole DTI volume
Dependency: epcsi2D
*/
mxArray* epcsirecf_5(mxArray** recon_mag, mxArray* d1)
{
	/*static int dimx=64,dimy=64,dimz=1,dimt=64,trans=24;*/
	int ii,jj,szrow,offsetA;

	mxArray* end_index = NULL;
	mxArray* end_index1 = NULL;
	mxArray* d = NULL;
	mxArray* dnext = NULL;
	mxArray* outang = NULL;
	mxArray* recon_ph = NULL;
	mxArray* imc_ph=NULL; /*temp var, delete it after debugging*/
	mxArray* imc_mag=NULL;
	mxArray* outmag = NULL;

	//printf("== Enter epcsirecf_5 == \n");

	/*------------------------------*/

	mlfEnterNewContext(1,1,recon_mag,d1);

	szrow = 2*((dimx+trans)*dimt - trans);

	mlfAssign(&end_index, mlfColon(mlfScalar(szrow+1), mlfEnd(d1,mlfScalar(1),mlfScalar(1)),NULL));
	mlfAssign(&d, mlfIndexRef(d1, "(?)", end_index));
	mlfAssign(&dnext,d);

	mlfAssign(&outang, mlfZeros( mlfScalar(dimx),mlfScalar(dimy),mlfScalar(dimt),mlfScalar(DW),mlfScalar(dimz),NULL));
	mlfAssign(&outmag, mlfZeros( mlfScalar(dimx),mlfScalar(dimy),mlfScalar(dimt),mlfScalar(DW),mlfScalar(dimz),NULL));

	for (jj=1;jj<=dimz;jj++)
		for(ii=1;ii<=DW;ii++)
		{
			mlfAssign(&imc_ph, epcsi2D(&imc_mag,dnext));
			mlfIndexAssign(&outang,"(?,?,?,?,?)",mlfCreateColonIndex(),mlfCreateColonIndex(),mlfCreateColonIndex(),mlfScalar(ii), mlfScalar(jj),imc_ph);
			mlfIndexAssign(&outmag,"(?,?,?,?,?)",mlfCreateColonIndex(),mlfCreateColonIndex(),mlfCreateColonIndex(),mlfScalar(ii), mlfScalar(jj),imc_mag);

			/*-debug: check the output of epcsi2D--
			if(ii==1)
			{	mlfSave(mxCreateString("tmpSavedData1"),"w","epcsi2D_b0_C",imc_ph,NULL);
				
			}
			*/

			/*clear dnext*/
			mxDestroyArray(dnext); dnext = NULL;
			offsetA = szrow*dimy*ii + szrow*dimy*DW*(jj-1);
			mlfAssign(&end_index1, mlfColon(mlfScalar(offsetA+1), mlfEnd(d,mlfScalar(1),mlfScalar(1)), NULL));
			mlfAssign(&dnext, mlfIndexRef(d, "(?)", end_index1));
		}

	 /*mlfAssign(&tmpA, mlfFlipdim(outang, mlfScalar(2)));*/
	if (dimz==1) /* squeeze last dimension (dimz) */
	{ mlfAssign(&recon_ph, mlfIndexRef(mlfFlipdim(outang, mlfScalar(2)),"(?,?,?,?,?)",mlfCreateColonIndex(),mlfCreateColonIndex(),mlfCreateColonIndex(),mlfCreateColonIndex(),mlfScalar(1)));
	  mlfAssign(recon_mag, mlfIndexRef(mlfFlipdim(outmag, mlfScalar(2)),"(?,?,?,?,?)",mlfCreateColonIndex(),mlfCreateColonIndex(),mlfCreateColonIndex(),mlfCreateColonIndex(),mlfScalar(1)));
		mlfAssign(&recon_ph, mlfSqueeze(recon_ph));
		mlfAssign(recon_mag, mlfSqueeze(*recon_mag));
	}
	else
	{	mlfAssign(&recon_ph, mlfFlipdim(outang, mlfScalar(2)));
		mlfAssign(recon_mag, mlfFlipdim(outmag, mlfScalar(2)));
	}
	

	/*--------------------------------*/
		mxDestroyArray(end_index);
		mxDestroyArray(end_index1);
		mxDestroyArray(d);
		mxDestroyArray(dnext);
		mxDestroyArray(outang);
		mxDestroyArray(outmag);
		mxDestroyArray(imc_ph);
		mxDestroyArray(imc_mag);

mlfRestorePreviousContext(1,1,recon_mag,d1);
return mlfReturnValue(recon_ph);

}


/*Shear a whole image with parameter sh
Input: im -- image needs to be sheared
		sh -- shearing (in pixel) between 2 adjcent lines on Y direction
Output: imsh  - sheared image
*/
mxArray * ShearFT(mxArray* im, double sh)
{	static double pi = 3.14159265;
	int m,n,*s;


		mxArray* raw = NULL;
		mxArray* tmpA = NULL;
		mxArray* tmpB = NULL;
		mxArray* ramp2 = NULL;
		mxArray* phase = NULL;
		mxArray* null_matrix = NULL;
		mxArray* raw1 = NULL;
		mxArray* imsh = NULL;
	/*------------------------------*/
	mlfEnterNewContext(0,1,im);

	s = (int *) mxGetDimensions(im);
	m=s[0]; n=s[1];
	sh = sh*n;

	    mlfAssign(&raw,mlfFftshift(mlfFftn(mlfFftshift(im,NULL),NULL),NULL));
		mlfAssign(&tmpA,mlfMinus(mlfColon(mlfScalar(1),mlfScalar(n),NULL),mlfScalar(n/2)));
		mlfAssign(&tmpB,mlfMinus(mlfColon(mlfScalar(1),mlfScalar(m),NULL),mlfScalar(m/2)));
		mlfAssign(&ramp2,mlfRdivide(mlfMtimes(mlfTranspose(tmpA),tmpB),mlfScalar(m*n)));
		mlfAssign(&phase,mlfTimes(mlfTranspose(ramp2),mlfScalar(sh)));

		/*ifft(raw,[],1)*/
		mlfAssign(&null_matrix, mlfZeros(mlfScalar(0),mlfScalar(0),NULL));
		mlfAssign(&raw1, mlfFftshift(mlfIfft(raw,null_matrix,mlfScalar(1)),mlfScalar(1)));
		/*reuse tmpA and tmpB */
		mxDestroyArray(tmpA);mxDestroyArray(tmpB); tmpA=NULL; tmpB=NULL;
		/* raw1.*exp(-i*2*pi*phase) = tmpA */
		mlfAssign(&tmpA,mlfTimes(raw1,mlfExp(mlfTimes(mlfTimes(mlfComplexScalar(0,1),mlfScalar(-2*pi)),phase))));
		mlfAssign(&tmpB,mlfFftshift(mlfIfft(tmpA,null_matrix,mlfScalar(2)),mlfScalar(2)));
		mlfAssign(&imsh,mlfAbs(tmpB));

		/*--------------------------------*/
		mxDestroyArray(raw);
		mxDestroyArray(tmpA);
		mxDestroyArray(tmpB);
		mxDestroyArray(ramp2);
		mxDestroyArray(phase);
		mxDestroyArray(null_matrix);
		mxDestroyArray(raw1);
	
mlfRestorePreviousContext(0,1,im);
return mlfReturnValue(imsh);
}
/*read in the file.*/
mxArray* readseq(char filename[])
{	
	mxArray *fid=NULL;
	mxArray *status=NULL;
	mxArray *a = NULL;

	/*------------------------------*/
	mlfEnterNewContext(0,0);


	//printf("read sequence: Input phase EPI file= %s  \n",filename);
	
	mlfAssign(&fid, mlfFopen(NULL, NULL, mxCreateString(filename), mxCreateString("r"), mxCreateString("b")));
	mclAssignAns(&status, mlfFseek(fid, mlfScalar(offset), mxCreateString("bof")));
	mlfAssign(&a, mlfFread(NULL, fid, mxCreateString("int16"), NULL, NULL));

	/*------------------------------*/
		mxDestroyArray(fid);
		mxDestroyArray(status);

	mlfRestorePreviousContext(0,0);
	return mlfReturnValue(a);
}
mxArray* readginx(char filename[])
{
	mxArray *fid=NULL;
	mxArray *status=NULL;
	mxArray *a = NULL;
	/*------------------------------*/
	mlfEnterNewContext(0,0);
	//printf("Enter readginx: Inputfile = %s  \n",filename);
	
	mlfAssign(&fid, mlfFopen(NULL, NULL, mxCreateString(filename), mxCreateString("r"), mxCreateString("b")));
	mclAssignAns(&status, mlfFseek(fid, mlfScalar(ginxoffset), mxCreateString("bof")));
	mlfAssign(&a, mlfFread(NULL, fid, mxCreateString("int16"), NULL, NULL));

	/*------------------------------*/
		mxDestroyArray(fid);
		mxDestroyArray(status);

	mlfRestorePreviousContext(0,0);
	return mlfReturnValue(a);
}

mxArray* recon_ginx(char dirname[])
{
	int ii;
	mxArray *im=NULL;
	mxArray *tmp=NULL;
	char fname[256];
	char tmpchar[2]={'0','\0'};

	/*------------------------------*/
	mlfEnterNewContext(0,0);

	//printf("Enter recon_ginx \n");

    fname[0] = '\0'; /*init fname */
	mlfAssign(&im, mlfZeros(mlfScalar(dimx),mlfScalar(dimy),mlfScalar(DW),NULL));

  for (ii=1;ii<=DW;ii++)
  {
        strcat(fname,dirname);
		strcat(fname,"I.00");
		tmpchar[0] = ii+'0'; 
		strcat(fname,tmpchar);
    	//printf("recon_ginx: Read DW-EPI File = %s  \n",fname);

		mlfAssign(&tmp, mlfReshape(readginx(fname), mlfScalar(dimx), mlfScalar(dimy),NULL));
		mlfIndexAssign(&im,"(?,?,?)", mlfCreateColonIndex(), mlfCreateColonIndex(), mlfScalar(ii), tmp);
		fname[0] = '\0';/*init fname */
  }

  	/*------------------------------*/
		mxDestroyArray(tmp);

	mlfRestorePreviousContext(0,0);
	return mlfReturnValue(im);
}
double* compTE(double T_TE)
{		double *TE;
		double TE1; 
		double gaptime; 
		/*double TE[dimx];*/ 
		int ii;

		/*
		 double *TE = new double[dimt];
		*/
		TE =(double*) malloc(dimt*sizeof(double));
		if(!TE){printf("Out of memory. Hard to believe it. \n"); exit(1);}

		gaptime = dt*(dimx+trans); 
		TE1 = T_TE - gaptime*(dimt/2 -1);
		
		for (ii=0;ii<dimt;ii++)
		{TE[ii] = TE1+ gaptime*(ii-1);}
		return TE;
}

void findFittingROI(int corners[], mxArray* im)
{	int x1,y1,x2,y2,x1roi,y1roi,x2roi,y2roi;
	mxArray* norm_im=NULL;
	mxArray* thresh_im=NULL;
	mxArray* xval=NULL;
	mxArray* yval=NULL;
	mxArray* x_index=NULL;
	mxArray* y_index=NULL;
	mxArray* tmpA=NULL;

/*------------------------------*/
	mlfEnterNewContext(0,1,im);

	mlfAssign(&thresh_im, Otsu_thresh(im));
	mlfAssign(&xval, mlfSum(thresh_im,mlfScalar(1)));
	mlfAssign(&yval, mlfSum(thresh_im,mlfScalar(2)));
	mlfAssign(&x_index, mlfFind(NULL,NULL,mlfGt(xval,mlfScalar(0))));
	mlfAssign(&y_index, mlfFind(NULL,NULL,mlfGt(yval,mlfScalar(0))));

	x1 = mxArray2int(mlfMin(NULL,x_index,NULL,NULL));
	y1 = mxArray2int(mlfMin(NULL,y_index,NULL,NULL));
	x2 = mxArray2int(mlfMax(NULL,x_index,NULL,NULL));
	y2 = mxArray2int(mlfMax(NULL,y_index,NULL,NULL));
     
	x1roi = (int) (((x1+x2)/2.0 + x1)/2.0);
    y1roi = (int) (((y1+y2)/2.0 + y1)/2.0);
        
    x2roi = (int) (((x1+x2)/2.0 + x2)/2.0);
    y2roi = (int)(((y1+y2)/2.0 + y2)/2.0);
	
	corners[0] = x1roi; corners[1] = y1roi; corners[2] = x2roi; corners[3] = y2roi;

		/*------------------------------*/
	mxDestroyArray(norm_im);
	mxDestroyArray(thresh_im);
	mxDestroyArray(xval);
	mxDestroyArray(yval);
	mxDestroyArray(x_index);
	mxDestroyArray(y_index);

	mlfRestorePreviousContext(0,1,im);
}


int mxArray2int(const mxArray *in)
{
int size;
double* temp = (double*) mxGetPr(in);
if (temp==0) printf("mxArray2int: Pointer to data is NULL \n");
size = mxGetNumberOfElements(in);
if (size!=1) printf("mxArray2int: Size of data is not equal to one \n");
return (int) (*temp);
}
double mxArray2double(const mxArray *in)
{
int size;
double* temp = (double*) mxGetPr(in);
if (temp==0) printf("mxArray2double: Pointer to data is NULL \n");
size = mxGetNumberOfElements(in);
if (size!=1) printf("mxArray2double: Size of data is not equal to one \n");
return (*temp);
}
/* Otsu_thresh: compute optimized thresholding of a 2D image.
	Input: 2D image in a mxArray
	Output: the thresholded image, blackwhite only
*/
mxArray* Otsu_thresh(mxArray *im)
{	unsigned char *imChar;
	int *s, m,n,threshold;
	mxArray* norm_im =NULL;

/*------------------------------*/
	mlfEnterNewContext(0,1,im);

	mlfAssign(&norm_im, mlfRdivide(im,mlfMax(NULL,mlfIndexRef(im,"(?)", mlfCreateColonIndex()),NULL,NULL)));
	mlfAssign(&norm_im, mlfTimes(norm_im,mlfScalar(255)));

	s = (int *) mxGetDimensions(norm_im);
	m=s[0]; n=s[1];

	imChar = (unsigned char*) malloc(m*n*sizeof(unsigned char));
	mxArray2char(imChar, norm_im);
	threshold = otsu(imChar,m,n,0,0,m,n,0);
	mlfAssign(&norm_im, mlfGt(norm_im,mlfScalar(threshold)));
	/*------------------------------*/

	mlfRestorePreviousContext(0,1,im);
	return mlfReturnValue(norm_im);
}

void mxArray2char(unsigned char imChar[], const mxArray *in)
{
int size,ii;
double* temp = (double*) mxGetPr(in);
if (temp==0) printf("mxArray2char: Pointer to data is NULL \n");

size = mxGetNumberOfElements(in);

for (ii=0;ii<size;ii++)
{
	imChar[ii] = (unsigned char) temp[ii];
}

}

/*======================================================================*/
/* OTSU global thresholding routine                                     */
/*   takes a 2D unsigned char array pointer, number of rows, and        */
/*   number of cols in the array. returns the value of the threshold    */
/* Original: Joerg.Schulenburg@physik.uni-magdeburg.de
	Modified by bc15, 06/28/2005										*/
/*======================================================================*/
int otsu (unsigned char *image, int rows, int cols, int x0, int y0, int dx, int dy, int vvv)
{
  unsigned char *np;    // pointer to position in the image we are working with
  int thresholdValue=1; // value we will threshold at
  int ihist[256];       // image histogram
  int i, j, k;          // various counters
  int n, n1, n2, gmin, gmax;
  double m1, m2, sum, csum, fmax, sb;
  // zero out histogram ...
  memset(ihist, 0, sizeof(ihist));
  gmin=255; gmax=0;
  // generate the histogram
  for (i = y0 + 1; i < y0 + dy - 1; i++) {
    np = &image[i*cols+x0+1];
    for (j = x0 + 1; j < x0 + dx - 1; j++) {
      ihist[*np]++;
      if(*np > gmax) gmax=*np;
      if(*np < gmin) gmin=*np;
      np++; /* next pixel */
    }
  }
  // set up everything
  sum = csum = 0.0;
  n = 0;
  for (k = 0; k <= 255; k++) {
    sum += (double) k * (double) ihist[k];  /* x*f(x) mass moment */
    n   += ihist[k];                        /*  f(x)    mass      */
  }
  if (!n) {
    // if n has no value we have problems...
    fprintf (stderr, "NOT NORMAL thresholdValue = 160\n");
    return (160);
  }
  // do the otsu global thresholding method
  fmax = -1.0;
  n1 = 0;
  for (k = 0; k < 255; k++) {
    n1 += ihist[k];
    if (!n1) { continue; }
    n2 = n - n1;
    if (n2 == 0) { break; }
    csum += (double) k *ihist[k];
    m1 = csum / n1;
    m2 = (sum - csum) / n2;
    sb = (double) n1 *(double) n2 *(m1 - m2) * (m1 - m2);
    /* bbg: note: can be optimized. */
    if (sb > fmax) {
      fmax = sb;
      thresholdValue = k;
    }
  }
  // at this point we have our thresholding value
  // debug code to display thresholding values
  if ( vvv & 1 )
  fprintf(stderr,"# OTSU: thresholdValue = %d gmin=%d gmax=%d\n",
     thresholdValue, gmin, gmax);
  return(thresholdValue);
}

/* eddycurrent_correction_u returns the eddycurrent induced scaling and shearing in pixels between two adjecent lines 
	the returned variable dGxdGy_mean has the length of 2*DW. First half is dGx_mean, 2nd half = dGy_mean. 
	
	The matlab version returns DW-1 elements which ignores the b0 situation.

	Input: dB -- fieldmap of one slice [dimx,dimy,DW]. DW=1 ==> b0 image.
			corners -- fitting ROI [x1,y1,x2,y2]
	Output: dGxdGy_mean -- 1st half: DW number of dGx_mean, 2nd half: DW number of dGy_mean
				unit: T/m

	corners[] has 4 elements. order: [x1,y1] [x2,y2]. two points of a ROI rectangular corner.
*/

mxArray* eddycurrent_correction_u(mxArray* dB, int corners[])
{
	int ii,gg;
	int RxUp,RyUp,RxDn,RyDn;
	//int *s;
	mxArray* dB0=NULL;
	mxArray* dBeddy=NULL;
	mxArray* dBeddyROI = NULL;
	mxArray* dBeddyROI_x=NULL;
	mxArray* dBeddyROI_y=NULL;
	mxArray* xval=NULL;
	mxArray* tmp=NULL;
	mxArray* tmp0=NULL;
	mxArray* dGx_mean=NULL;
	mxArray* yval=NULL;
	mxArray* tmpy=NULL;
	mxArray* tmp0y=NULL;
	mxArray* dGy_mean=NULL;
	mxArray* dGxdGy_mean=NULL;

	//printf(" == Enter eddycurrent_correction_u == \n");

	RxUp=corners[0];
	RyUp=corners[1];
	RxDn=corners[2];
	RyDn=corners[3];
/*------------------------------*/
mlfEnterNewContext(0,1,dB);


    mlfAssign(&dB0, mlfSqueeze(mlfIndexRef(dB, "(?,?,?)", mlfCreateColonIndex(), mlfCreateColonIndex(), mlfScalar(1))));

	mlfAssign(&dBeddy, mlfZeros(mlfScalar(dimx),mlfScalar(dimy),mlfScalar(DW-1),NULL));


	for(ii=2;ii<=DW;ii++)
	{
     	mlfIndexAssign(&dBeddy, "(?,?,?)", mlfCreateColonIndex(), mlfCreateColonIndex(), mlfScalar(ii-1), mclMinus(mlfSqueeze(mlfIndexRef(dB, "(?,?,?)", mlfCreateColonIndex(), mlfCreateColonIndex(), mlfScalar(ii))), dB0));
	}

   /* dBeddyROI = dBeddy(RxUp:RxDn,RyUp:RyDn,:);*/
    mlfAssign(&dBeddyROI, mlfIndexRef(dBeddy, "(?,?,?)", mlfColon(mlfScalar(RxUp), mlfScalar(RxDn), NULL), 
		mlfColon(mlfScalar(RyUp), mlfScalar(RyDn), NULL), mlfCreateColonIndex()));


     /* dBeddyROI_x = mean(dBeddyROI,2); */
    mlfAssign(&dBeddyROI_x, mlfSqueeze(mlfMean(dBeddyROI, mlfScalar(2))));
		//printf("dBeddyROI_x= \n"); mlfPrintMatrix(dBeddyROI_x);

    mlfAssign(&dBeddyROI_y, mlfSqueeze(mlfMean(dBeddyROI, mlfScalar(1))));
		//printf("dBeddyROI_y= \n"); mlfPrintMatrix(dBeddyROI_x);

	/* compute dGx_mean */
		mlfAssign(&xval, mlfZeros(mlfScalar(RxDn-RxUp+1),mlfScalar(1),NULL));
    for(ii=RxUp;ii<=RxDn;ii++)
	{	mlfIndexAssign(&xval,"(?)", mlfScalar(ii-RxUp+1), mlfScalar(FOV/dimx*(ii-RxUp+1)));
	}

		//printf("xval = \n"); mlfPrintMatrix(xval);

	mlfAssign(&dGx_mean, mlfZeros(mlfScalar(DW),mlfScalar(1),NULL));
	for(gg=1;gg<=DW-1;gg++)
	{
		/* CHECK: tmp should have the same dims as xval */
	       mlfAssign(&tmp, mlfIndexRef(dBeddyROI_x, "(?,?)", mlfCreateColonIndex(), mlfScalar(gg)));
			//printf("tmp_x = \n"); mlfPrintMatrix(tmp);

           mlfAssign(&tmp0, mlfNPolyfit(1, NULL, NULL, xval, tmp, mlfScalar(1)));
		   //printf(" fitted slope at X: \n"); mlfPrintMatrix(tmp0);
           mlfIndexAssign(&dGx_mean, "(?)", mlfScalar(gg+1), mlfIndexRef(tmp0,"(?)", mlfScalar(1)));
	}

/* compute dGy_mean */
	mlfAssign(&yval, mlfZeros(mlfScalar(RyDn-RyUp+1),mlfScalar(1),NULL));
    for(ii=RyUp;ii<=RyDn;ii++)
	{	mlfIndexAssign(&yval,"(?)", mlfScalar(ii-RyUp+1), mlfScalar(FOV/dimy*(ii-RyUp+1)));
	}
			//   s = (int *) mxGetDimensions(yval); 
			//printf("dims of yval = [%d, %d, %d, %d, %d]. \n", s[0],s[1],s[2],s[3],s[4]);

	mlfAssign(&dGy_mean, mlfZeros(mlfScalar(DW),mlfScalar(1),NULL));
	for(gg=1;gg<=DW-1;gg++)
	{
		/* CHECK: tmpy should have the same dims as xval */
	       mlfAssign(&tmpy, mlfIndexRef(dBeddyROI_y, "(?,?)", mlfCreateColonIndex(), mlfScalar(gg)));
		 //  		   s = (int *) mxGetDimensions(tmpy); 
			//printf("dims of tmp_y = [%d, %d, %d, %d, %d]. \n", s[0],s[1],s[2],s[3],s[4]);

           mlfAssign(&tmp0y, mlfNPolyfit(1, NULL, NULL, yval, tmpy, mlfScalar(1)));
           mlfIndexAssign(&dGy_mean, "(?)", mlfScalar(gg+1), mlfIndexRef(tmp0y,"(?)", mlfScalar(1)));
	}

	/* return dGx_mean and dGy_mean in one array */
	mlfAssign(&dGxdGy_mean, mlfZeros(mlfScalar(2*DW), mlfScalar(1),NULL));
	mlfIndexAssign(&dGxdGy_mean,"(?)", mlfColon(mlfScalar(1), mlfScalar(DW),NULL),dGx_mean);
	mlfIndexAssign(&dGxdGy_mean,"(?)", mlfColon(mlfScalar(DW+1), mlfScalar(2*DW),NULL),dGy_mean);


/*----------------------------*/
	mxDestroyArray(dB0);
	mxDestroyArray(dBeddy);
	mxDestroyArray(dBeddyROI);
	mxDestroyArray(dBeddyROI_x);
	mxDestroyArray(dBeddyROI_y);
	mxDestroyArray(xval);
	mxDestroyArray(tmp);
	mxDestroyArray(tmp0);
	mxDestroyArray(yval);
	mxDestroyArray(tmpy);
	mxDestroyArray(tmp0y);
	mxDestroyArray(dGx_mean);
	mxDestroyArray(dGy_mean);

mlfRestorePreviousContext(0,1,dB);
return mlfReturnValue(dGxdGy_mean);
}
/* Image correction.
   Input: epiDW -- diffusion weighted EPI images [dimx,dimy,DW]. One slice only
			dB0map -- static field map [dimx,dimy,DW] in T/m
			dGx_mean -- eddycurrent gradient in x dir. causes shearing. T/m
			dGypix_mean -- eddycurrent gradient in y dir. causes scaling. T/m
	Output: corrected images.
*/
mxArray* corr_eddy(mxArray* epiDW,mxArray* dB0map,mxArray* dGx_mean,mxArray*  dGy_mean)
{	int gg;
	int sx,sy,sd, *s;
	mxArray* dB=NULL;
	mxArray* tmpA=NULL;
	mxArray* yramp=NULL;
	mxArray* yramp2d=NULL;
	mxArray* tmpB=NULL;
	mxArray* tmpC=NULL;
	mxArray* tmpD=NULL;
	mxArray* epiDWcorr=NULL;
	mxArray* sh=NULL;
	mxArray* pixShear=NULL;
	mxArray* dGypix_mean=NULL; /* dGy_mean has unit T/m, dGypix_mean has unit T/pixel */
	mxArray* dB0map_bi=NULL;

	//printf(" == Enter corr_eddy == \n");

/*------------------------------*/
mlfEnterNewContext(0,4,epiDW,dB0map, dGx_mean,dGy_mean);

/* convert shearing in pixel between two adjcent lines:
	 Gx=1/(gyro_bar*dt*FOVx);unit:T/m
		pixShear = dGx_mean/Gx*dimx; OK, but not very accurate
		pixShear = (dimx+trans)*dGx_mean/(Gx*dimx) * dimx;
		pixShear = (dimx+trans)*dGx_mean/Gx;

		pixShear = (dimx+trans)*dGx_mean*(gyro_bar*dt*FOVx);
		Unit: dGx_mean -- T/m; All ISO units
*/
			//	printf(" dGx_mean and dGy_mean=\n");
			//mlfPrintMatrix(dGx_mean);mlfPrintMatrix(dGy_mean);

	mlfAssign(&pixShear, mlfTimes(dGx_mean, mlfScalar((dimx+trans)*(gyrobar*dt*FOVx))));
	mlfAssign(&dGypix_mean, mlfTimes(dGy_mean, mlfScalar(FOVy/dimy)));
	/*printf("pixShear=\n"); mlfPrintMatrix(pixShear);
	printf("dGypix_mean=\n"); mlfPrintMatrix(dGypix_mean);
	*/
	s = (int *) mxGetDimensions(epiDW);
	sx=s[0]; sy=s[1]; sd=s[2];
	//printf("one slice epi DWI: dim = %d,%d,%d,%d,%d. \n", sx,sy,sd,s[3],s[4]);

    mlfAssign(&dB, mlfZeros(mlfScalar(sx),mlfScalar(sy),mlfScalar(sd), NULL));

	/* temp constants */
	mlfAssign(&tmpA, mlfMinus(mlfColon(mlfScalar(1), mlfScalar(sy),NULL),mlfScalar(sy/2)));
		//printf("tempA = \n"); mlfPrintMatrix(tmpA);
	for(gg=1;gg<=sd;gg++)
	{
            /* yramp = ((1:sy)-floor(sy/2))*dGypix_mean(gg); % should be 1 x sy matrix */
		mlfAssign(&yramp, mlfTimes(tmpA,mlfIndexRef(dGypix_mean,"(?)", mlfScalar(gg))));
			//mlfPrintMatrix(mlfTimes(yramp, mlfScalar(100000000)));

            /* yramp2d = repmat(yramp, [sx,1]); % becomes [sx,sy] matrix */
        mlfAssign(&yramp2d, mlfRepmat(yramp, mlfScalar(sx), mlfScalar(1)));
		//printf("yramp2d=\n"); mlfPrintMatrix(mlfTimes(yramp2d,mlfScalar(100000000))); 

		if(mxGetNumberOfElements(dB0map) > (dimx*dimy))
		{

            /* dB(:,:,gg) = dB0map + yramp2d; */
			mlfAssign(&dB0map_bi, mlfIndexRef(dB0map, "(?,?,?)", mlfCreateColonIndex(), mlfCreateColonIndex(), mlfScalar(gg)));
            //mlfIndexAssign(&dB, "(?,?,?)", mlfCreateColonIndex(), mlfCreateColonIndex(), mlfScalar(gg), mlfPlus(dB0map_bi,yramp2d));

	/***** for debug only. ignore dGypix_mean. No eddy current correction ****/
			mlfIndexAssign(&dB, "(?,?,?)", mlfCreateColonIndex(), mlfCreateColonIndex(), mlfScalar(gg), dB0map_bi);
	
				//printf(" dB[32,32,:]=\n");
				//mlfPrintMatrix(mlfIndexRef(dB,"(?,?,?)", mlfScalar(32),mlfScalar(32),mlfCreateColonIndex()));

            /* epiDWcorr(:,:,gg) = dBCorrection(squeeze(epiDW(:,:,gg)), squeeze(dB(:,:,gg)));*/
		mlfAssign(&tmpB, mlfSqueeze(mlfIndexRef(epiDW, "(?,?,?)", mlfCreateColonIndex(), mlfCreateColonIndex(), mlfScalar(gg))));
		mlfAssign(&tmpC, mlfSqueeze(mlfIndexRef(dB, "(?,?,?)", mlfCreateColonIndex(), mlfCreateColonIndex(), mlfScalar(gg))));
        mlfIndexAssign(&epiDWcorr, "(?,?,?)", mlfCreateColonIndex(), mlfCreateColonIndex(), mlfScalar(gg), dBCorrection(tmpB,tmpC));
		}
		if(mxGetNumberOfElements(dB0map) == (dimx*dimy))/* dB0map of b0 only */
		{	mlfAssign(&tmpC, mlfPlus(dB0map,yramp2d));
			mlfAssign(&tmpB, mlfSqueeze(mlfIndexRef(epiDW, "(?,?,?)", mlfCreateColonIndex(), mlfCreateColonIndex(), mlfScalar(gg))));
			mlfIndexAssign(&epiDWcorr, "(?,?,?)", mlfCreateColonIndex(), mlfCreateColonIndex(), mlfScalar(gg), dBCorrection(tmpB,tmpC));
		}
	}


    /*epiDWcorr(isnan(epiDWcorr))=0;*/
    mlfIndexAssign(&epiDWcorr, "(?)", mlfIsnan(epiDWcorr), mlfScalar(0));


    mlfAssign(&sh, mlfUminus(pixShear));
	//mlfAssign(&sh, pixShear);

	for( gg=2;gg<=sd;gg++)
	{
 		mlfAssign(&tmpD, mlfSqueeze(mlfIndexRef(epiDWcorr, "(?,?,?)", mlfCreateColonIndex(), mlfCreateColonIndex(), mlfScalar(gg))));
       
		 /*epiDWcorr(:,:,gg) = ShearFT(squeeze(epiDWcorr(:,:,gg)),sh(gg-1));*/
        mlfIndexAssign(&epiDWcorr, "(?,?,?)", mlfCreateColonIndex(), mlfCreateColonIndex(), mlfScalar(gg), 
			ShearFT(tmpD,mxArray2double(mlfIndexRef(sh, "(?)", mlfScalar(gg-1)))));
 	}


/*--------------------------*/
	mxDestroyArray(dB);
	mxDestroyArray(tmpA);
	mxDestroyArray(yramp);
	mxDestroyArray(yramp2d);
	mxDestroyArray(tmpB);
	mxDestroyArray(tmpC);
	mxDestroyArray(tmpD);
	mxDestroyArray(sh);
	mxDestroyArray(dGypix_mean);
	mxDestroyArray(dB0map_bi);

mlfRestorePreviousContext(0,4,epiDW,dB0map, pixShear,dGy_mean);
return mlfReturnValue(epiDWcorr);

}

/* Write matrix into file.
	Input: data -- matrix array
			datatype -- 1 = int16; 2 = double.
			endian -- 1 = little endian, 2 = big endian.
	Output: void
*/
void writefile(mxArray* data, char filename[], int format, int endian)
{
	mxArray *datatype=NULL;
	mxArray *fd=NULL;
	mxArray *tmp=NULL;
	char s1[] = "int16";
	char s2[] = "double";
	char s3[] = "little endian";
	char s4[] = "big endian";
	char s5[] = "system default";
	char *s12, *s34;
	/*------------------------------*/
	mlfEnterNewContext(0,0);

    if (format==1) {
        mlfAssign(&datatype, mxCreateString("int16"));
		s12 = s1;
    }
	else if(format==2){
        mlfAssign(&datatype, mxCreateString("double"));
		s12 = s2;
    }
	else
	{	printf("writefile: DataType not recognized. Write data as Int16. \n");
		mlfAssign(&datatype, mxCreateString("int16"));
		s12 = s1;
	}
		
	if(endian==1)
	{
		mlfAssign(&fd, mlfFopen(NULL, NULL, mxCreateString(filename), mxCreateString("w"), mxCreateString("l")));
		s34 = s3;
	}
	else if(endian==2)
	{    mlfAssign(&fd, mlfFopen(NULL, NULL, mxCreateString(filename), mxCreateString("w"), mxCreateString("b")));
		s34 = s4;
	}
	else
	{   printf("writefile: Endian type not recognized. Write data as system default. \n");
	mlfAssign(&fd, mlfFopen(NULL, NULL, mxCreateString(filename), mxCreateString("w"), mxCreateString("n")));
		s34 = s5;
	}

    mlfAssign(&tmp, mlfFwrite(fd, data, datatype, NULL));
	printf("        File saved. Filename=%s; DataType=%s; Endian=%s. \n", filename,s12,s34);
    mlfAssign(&tmp, mlfFclose(fd));

	/*------------------------------*/
		mxDestroyArray(fd);
		mxDestroyArray(datatype);
		mxDestroyArray(tmp);

	mlfRestorePreviousContext(0,0);
}

void usage()
{
	printf("Usage:	EPI_C PI DWI d1 d2 d3 dt dw FOV TE [SW] \n\n");
	printf("Input: \n");
	printf("	PI :		phase step EPI during the calibration stage. \n");
	printf("	DWI :		DW-EPI image. \n");
	printf("	d1,d2,d3 :	resolution in frequency, phase and slice directions. \n");
	printf("	dt :		number of echoes in phase step EPI. \n");
	printf("	dw :		number of total diffusion weighting image volumes. \n");
	printf("			For 1 base image and 6 diffusion weighting directions, dw = 7.\n");
	printf("	FOV:		field of view. Unit: centimeter. \n");
	printf("	TE :		time of echo. Unit: millisecond.\n");
	printf("	SW :		optional.sweep frequency in KHz. Default = 125KHz. \n");
	printf("Output:\n");
	printf("	cDWI:		Corrected DW images saved as little endian 16-bit integer \n");
	printf("			in the order of d1,d2,d3,dw. \n\n");

}