/*	
 *	RF Beamforming Simulation of Ultrasound Scanner based on Delta-Sigma A/D or Multi-Bit A/D or Raw Data
 *
 * Program Flow : Data Bank(N*N) => Given Parameters =>
 *						For each Beam,{ Delay Evaluation(Delay Controler/Delay Table) => 
 * 					Apodization Coef. Evaluation => Received Signal Synthesis(N, tx delay ok) =>
 *						Quantization(Delta-Sigma A/D,Mult-Bit A/D,Raw Data) => 
 *						Received Delaying(N, rx delay ok) => Beamsum }
 *		
 *																											Edited by JJH
 *																											Commented by mlLee 2001/2/5
 */

#include <math.h>
#include <iostream.h>
#include <fcntl.h>
#include <sys\stat.h>
#include <io.h>
#include <memory.h>

#pragma hdrstop

int main(int argc, char **argv)
{

/*  
 *	--------------------------- Given Parameters ----------------------------------
 */
    bool IsApodization=false;					// Apodization Flag
    bool IsDelayControl=false;				// Delay Control Flag

    enum {rf, multibit, delsig} a2d;		// enumeration for the choice of A/Ds,raw data,mult-bit,delta-sigma
    a2d = rf;

    enum {rep, divide, zero, hold, alt} method;         // enumeration for the choice 
    method = divide;												  // of Delay Compensation with delta-sigma A/D

    const int Nbit=10;                     // Number of Bits for multi-bit A/D
    const int Napodiz=7;                   // Number of Apodization ,applied when IsApodization is true;

    const float normal=200000;             // used to normalize data obtained from acuson17c.dat

    const float my_pi=3.14159265358979;	// value definition of pi

    // Command Arguments Check
    if(argc!=3){
        cout<<"You must give I/O file names!"<<endl;
        cout<<"Usage: exe_filename input_file output_file"<<endl;
        return -1;
    }

    const char* infile=argv[1];		// input filename, NChan*NChan*Nsample 
    const char* beamfile=argv[2];	// output filename, Nbeam*Nsample

    const int Nchan=128;				// Total Channel Number of Sector Array
    const float fad=13.8889;			// A/D sampling rate (in MHz)
    const float f0=3.5;					// Transducer center frequency (in MHz)
    const float timeoffset=29.448;	// A/D time offset(or A/D received delay) of received signal (in us)
    const int Nsample=2048;			// Number of samples
    const float pitch=0.22;			// array pitch (in mm)
    const float soundv=1.48;			// sound velocity (in mm/us)
    const int upsamp=8;					// upsampling factor
    const float tfnumber=2;			// tx F number
    const float rfnumber=1.5; 		// rx F number
    const float tfocus=60.0;        // tx focal point (in mm)                         
    																								//geabr.dat has done atten!!!!!
    //const float atten=0.5;        // attenuation coef. (in db/Mhz/cm)    //we do analog atten here!?

    const float tapersize=tfocus/tfnumber/2;			// tx aperture size (in mm)
    const float drange=soundv/fad/2/upsamp;      	// distance between two sample points after upsampling (in mm)
    const float rangeoffset=timeoffset*soundv/2;	// Range offset (from A/D time offset)
    const float apersize=Nchan*pitch;					// array aperture size (in mm)
    const float lambda=soundv/f0;						// sound wavelength (in mm)
    const float dsin=lambda/apersize/2;		 		// interval between two beams,¡µsin£c=£f/4a
    const int Nbeam=INT((sqrt(2)/dsin)+0.5);			// Number of beams (interger)
    const int UNsample=upsamp*Nsample;					// Number of samples after upsampling
	 const int garbage=-99999;                   	// must bigger than UNsample
    
    // interpolation filter coef. for upsampling 8 times
    const float Interp[]={0,-0.0024,-0.0046,-0.0061,-0.0068,-0.0065,-0.0052,-0.0029,0,0.0136,0.0258,0.0349,
                          0.0395,0.0384,0.0312,0.0181,0,-0.045,-0.0877,-0.1222,-0.1427,-0.144,-0.122,-0.0743,0,
                          0.1370,0.291,0.4522,0.6098,0.753,0.8713,0.956,1,0.956,0.8713,0.753,0.6098,0.4522,0.291,0.137,0,-0.0743,
                          -0.122,-0.144,-0.1427,-0.1222,-0.0877,-0.045,0,0.0181,0.0312,0.0384,0.0395,0.0349,0.0258,0.0136,0,
                          -0.0029,-0.0052,-0.0065,-0.0068,-0.0061,-0.0046,-0.0024,0,0,0,0,0,0,0,0};
	
    float** apodiz=new float*[Nchan];			// apodization coef.
    for(int i=0;i<Nchan;i++){	
        apodiz[i]=new float[UNsample];
        for(int j=0;j<UNsample;j++)
            apodiz[i][j]=0;						// memory initialization => 0
    }

    short ***datafile=new short**[Nchan];		// Raw Data in the format Nchan*Ncha*Nsample
    for(int i=0;i<Nchan;i++){
        datafile[i]=new short*[Nchan];
        for(int j=0;j<Nchan;j++){
            datafile[i][j]=new short[Nsample];
            for(int k=0;k<Nsample;k++)
                datafile[i][j][k]=0;			// memory initialization => 0
        }
    }

    int *tdelay=new int[Nchan];					// tx delay for one beam, Nchan
    for (int i=0;i<Nchan;i++)
	    tdelay[i]=0;

    int **rdelay=new int*[Nchan];				// rx delay for one beam, Nchan*Nsample
    for (int i=0;i<Nchan;i++){
        rdelay[i]=new int[UNsample];	
    	for (int j=0;j<UNsample;j++)
	    rdelay[i][j]=0;
    }

    float **gotdata=new float*[Nchan];			// received signal for one beam (tx delay ok), Nchan*Nsample
    for (int i=0;i<Nchan;i++){
    	gotdata[i]=new float[UNsample];
    	for (int j=0;j<UNsample;j++)
    	    gotdata[i][j]=0;
    }

    float **finaldata=new float*[Nchan];		// signal for one beam after rx delay processing, Nchan*Nsample 
    for (int i=0;i<Nchan;i++){
    	finaldata[i]=new float[UNsample];
    	for (int j=0;j<UNsample;j++)
    	    finaldata[i][j]=0;
    }

    short *buff=new short[Nsample+8];			// data buffer used in the processing of received signal, Nsample
    for (int i=0;i<Nsample+8;i++)				//, Nsample ????? +8 why? for FIR interpolation.
        buff[i]=0;

    float *buff2=new float[UNsample+2];		// data buffer used in upsampling, UNsample
    for (int i=0;i<UNsample+2;i++)				// ????? +2 why? for interpolation
        buff2[i]=0;

    float big=0;										// the largest value in one beam
    float g_big=0;									// the largest value in total beams used to normalize 

	
	 // u1,u2,y,v1,v2 used in Quantization of delta-sigma A/D
    float* u1=new float[UNsample];			
    for (int i=0;i<UNsample;i++)
        u1[i]=0;

    float* u2=new float[UNsample];
    for (int i=0;i<UNsample;i++)
        u2[i]=0;

    int* y=new int[UNsample+1];
    for (int i=0;i<UNsample+1;i++)
        y[i]=0;

    float* v1=new float[UNsample+1];
    for (int i=0;i<UNsample+1;i++)
        v1[i]=0;

    float* v2=new float[UNsample+1];
    for (int i=0;i<UNsample+1;i++)
        v2[i]=0;

    float* undeci_beamsum=new float[UNsample];	// data of one beam after beamsum
    for(int i=0;i<UNsample;i++)
        undeci_beamsum[i]=0;


/*
 * ---------------- Load Data from Input File ------------------------------------------
 */
    cout<<"read datafile"<<endl;

    int fid=open(infile, O_RDONLY | O_BINARY);

    for (int i=0;i<Nchan;i++)
 		for(int j=0;j<Nchan;j++)
            read (fid,datafile[i][j],Nsample*2);

    close(fid);

/*
 *	-------------- Open and Creat Output File -----------------------------------
 */
    int fidBE=open(beamfile, O_CREAT|O_TRUNC|O_BINARY|O_RDWR,S_IREAD|S_IWRITE);


/*
 *	-------------- Beamforming Loop Begin-----------------------------------
 */
 	 // ---------- Beam Loop (outer loop) ---------------
    for(int beam=0;beam<Nbeam;beam++){
        cout<<endl;
//------------------------------------------------------------------------------------------
//--------------------------------------compute delay---------------------------------------
        cout<<beam<<"  compute delay"<<endl;

        float x,ax,sint,cost2,cost,rangeon,range;

        for (int i=0;i<Nchan;i++){
            x=(i+1-(float)(Nchan+1)/2)*pitch;
            ax=fabs(x);
            sint =dsin*(beam+1-(float)(Nbeam+1)/2);
            cost2=1-sint*sint;
            cost =sqrt(cost2);

            if (ax <=tapersize)
       		tdelay[i]=floor(fad*upsamp*((x*x*cost2/2/tfocus)-(x*sint))/soundv+0.5);
            else
       	        tdelay[i]=garbage;

            if (IsDelayControl){			//refer to the appendix B of jjh's thesis
       	        //parameters for rdelay quick computing
       	      int indexoffset=(int)(timeoffset*soundv/2/drange+0.5);
	        		int indexon=(int)(rfnumber*2*ax*cost/drange);

	        		int n0=(indexoffset>indexon)?indexoffset:indexon;
	        		int k0=(int)(x*x*cost2*fad*fad*upsamp*upsamp/soundv/soundv/n0-x*sint*fad*upsamp/soundv+0.5);
	        		int r0=(int)(x*x*cost2*fad*fad*upsamp*upsamp/soundv/soundv/n0);
	        		int tau=1-r0;

               int A=n0+tau;
	        		int n=n0+1;
               int B=A+n+2;

	        		rdelay[i][n0-indexoffset]=k0;

	        		for(int j=n0-indexoffset;j<UNsample-1;j++)
	            	if(A<=0){
	               	A=B+tau*2;
	               	tau=tau+1;
	               	n=n+1;
	               	rdelay[i][j+1]=rdelay[i][j]-1;
	            	}
	            	else{
	               	B=A+n+2;
	               	A=A+tau;
	               	n=n+1;
	               	rdelay[i][j+1]=rdelay[i][j];
	            	}

	        			for(int j=0;j<n0-indexoffset;j++)
	            		rdelay[i][j]=garbage;
            }
            else{
                rangeon=rfnumber*2*ax*cost;
                for (int j=0;j<UNsample;j++){
                    range=rangeoffset+j*drange;
                    if (range>=rangeon)
                        rdelay[i][j]=floor(fad*upsamp*((x*x*cost2/2/range)-(x*sint))/soundv+0.5);
                    else
                        rdelay[i][j]=garbage;
                }
            }
        }

//------------------------------------------------------------------------------------------
//------------------------------------apodization coeff-------------------------------------
        if(IsApodization){
            cout<<beam<<"  compute apodization coefficient"<<endl;

            float sint, cost2, cost, range;
            int Nopen;

            for(int j=0;j<UNsample;j++){
                sint =dsin*(beam+1-(float)(Nbeam+1)/2);
                cost2=1-sint*sint;
                cost =sqrt(cost2);

                range=rangeoffset+j*drange;
                Nopen=2*floor(range/rfnumber/cost/pitch/2);     //number of channel opened, must be even for symmetry
                if(Nopen>Nchan)
                    Nopen=Nchan;

                for(int i=0;i<Nchan;i++){
                    int n=(i+1)-((Nchan/2)-(Nopen/2)+1);
                    if(n>=0 && n<Nopen){
                        apodiz[i][j]=0.54-0.46*cos(2*my_pi*n/(Nopen-1));
                        apodiz[i][j]=floor(apodiz[i][j]*pow(2.0, Napodiz-1)+0.5);
                    }
                    else
                        apodiz[i][j]=0;
                }
            }
        }
        else{
            for(int i=0;i<Nchan;i++)
                for(int j=0;j<UNsample;j++)
                    apodiz[i][j]=1;
        }

//------------------------------------------------------------------------------------------
//--------------------------------------receive signal--------------------------------------
        cout<<beam<<"  receive dataset"<<endl;

        for (int i=0;i<Nchan;i++)
    	    for (int j=0;j<UNsample;j++)
                gotdata[i][j]=0;                                //reset to 0

        for (int i=0;i<Nchan;i++){
	    for(int j=0;j<Nchan;j++){
                //if(i==j)    continue;
                memset(buff,0,2*(Nsample+8));
                for(int k=0;k<Nsample;k++)
                    buff[k+4]=datafile[i][j][k];

                int mm;
                int nn;
                for (int m=0;m<UNsample;m++){
                    mm=m/upsamp;
                    nn=m%upsamp;
                    buff2[m]=buff[mm]*Interp[nn+64]+buff[mm+1]*Interp[nn+56]+buff[mm+2]*Interp[nn+48]+buff[mm+3]*Interp[nn+40]
                        +buff[mm+4]*Interp[nn+32]+buff[mm+5]*Interp[nn+24]+buff[mm+6]*Interp[nn+16]+buff[mm+7]*Interp[nn+8]
                        +buff[mm+8]*Interp[nn];
                }

	        buff2[UNsample]=0;
	        buff2[UNsample+1]=0;

	        for(int k=0;k<UNsample;k++){
       		    int index=k+tdelay[i];	// fixed tx focusing
       		    // int index=k+rdelay[i][k]; // dynamic tx focusing
	            if(!(index>=0 && (index+1)<UNsample))
                        index=UNsample;
       		    gotdata[j][k]+=buff2[index];                //channel received signal
	        }
            }
    	}

        big=0;

        for(int i=0;i<Nchan;i++)
            for(int j=0;j<UNsample;j++){
                //gotdata[i][j]*=pow(10,(2*j*drange*0.1*f0*atten/20));
                gotdata[i][j]/=normal;
                if(fabs(gotdata[i][j])>big){
	            big=fabs(gotdata[i][j]);
                }
            }

        cout<<beam<<"  big  = "<<big<<endl;
        if(big>g_big)
            g_big=big;

//------------------------------------------------------------------------------------------
//---------------------------------------quantization---------------------------------------
        if(a2d==delsig){
            cout<<beam<<"  delta sigma"<<endl;

            for(int i=0;i<Nchan;i++){
                y[0]=1;
                v1[0]=0;
                v2[0]=0;
	        float* rf=gotdata[i];

	        for(int j=0;j<UNsample;j++){
                    u1[j]=rf[j]-y[j];
                    v1[j+1]=u1[j]+v1[j];
                    u2[j]=v1[j+1]-y[j];
                    v2[j+1]=u2[j]+v2[j];
                    y[j+1]=(v2[j+1]>=0)+(v2[j+1]<0)*-1;
     	        }

                for(int j=0;j<UNsample;j++)                               //output logic level +1 -1
            	    gotdata[i][j]=y[j+1]; 				  //+1?
            }
        }
        else if(a2d==multibit){
            cout<<beam<<"  multi bit: "<<Nbit<<" bits"<<endl;

            float temp=0;
            for(int i=0;i<Nchan;i++)
                for(int j=0;j<UNsample;j++){
                    temp=floor(gotdata[i][j]*pow(2.0, Nbit-1)+0.5);       //Nbit-1 because of negative part
                    gotdata[i][j]=temp;
                }
        }

//------------------------------------------------------------------------------------------
//---------------------------------------rdelay---------------------------------------------
        cout<<beam<<"  rdelay"<<endl;
        //int fidzz=open("zz2.dat", O_CREAT|O_TRUNC|O_BINARY|O_RDWR,S_IREAD|S_IWRITE);
        for (int i=0;i<Nchan;i++)
            for (int j=0;j<UNsample;j++)
                finaldata[i][j]=0;                                //reset to 0

        for(int i=0;i<Nchan;i++){
            int pre_index=0;
            int toggle_state=(i%2)?1:-1;
    	    for(int j=0;j<UNsample;j++){
		int rindex=j+rdelay[i][j];	// dynamic rx focusing
		//int rindex=j+tdelay[i]; // fixed rx focusing

                if(a2d==delsig && pre_index==rindex)
                    switch(method){
                        case rep :
                            finaldata[i][j]=finaldata[i][j-1];
                            break;
                        case divide :
                            finaldata[i][j]=0.5*finaldata[i][j-1];
                            break;
                        case zero:
                            finaldata[i][j]=0;
                            break;
                        case hold :
                            finaldata[i][j]=toggle_state;
                            break;
                        case alt :
                            finaldata[i][j]=toggle_state;
                            toggle_state=-toggle_state;
                            break;
                        default :
                            cout<<"Wrong Method !"<<endl;
                    }
                else if(!(rindex>0 && (rindex+1)<UNsample))           //important!!!!!
                    finaldata[i][j]=0;
                else
                    finaldata[i][j]=gotdata[i][rindex];

                pre_index=rindex;
            }
            //write(fidzz,finaldata[i],UNsample*sizeof(float));
        }
        //close(fidzz);
//------------------------------------------------------------------------------------------
//---------------------------------------beamform-------------------------------------------
        cout<<beam<<"  beamform"<<endl;
        memset(undeci_beamsum,0,sizeof(float)*UNsample);

        for(int i=0;i<Nchan;i++)
            for(int j=0;j<UNsample;j++)
                undeci_beamsum[j]+=finaldata[i][j]*apodiz[i][j];

        write(fidBE,undeci_beamsum,UNsample*sizeof(float));
    } // End of Beam Loop
    close(fidBE);

    cout<<endl<<"g_big = "<<g_big<<endl;
//------------------------------------------------------------------------------------------
//---------------------------------release memory-------------------------------------------
    delete [] buff;
    delete [] buff2;

    delete [] tdelay;

    for (int i=0;i<Nchan;i++){
	delete [] rdelay[i];
	delete [] gotdata[i];
	delete [] finaldata[i];
        delete [] apodiz[i];
    }
    delete [] rdelay;
    delete [] gotdata;
    delete [] finaldata;
    delete [] apodiz;

    delete [] u1;
    delete [] u2;
    delete [] y;
    delete [] v1;
    delete [] v2;

    return 0;
}