/******************************************************************
* calculate deconvolution filter 1/(s[w]+water_level).
* the water-level is c*auto-correlation(s[t]). A low-pass gaussian filter
* exp(-w^2/(4 sigma^2))
* is also applied.
******************************************************************/
void water(
complex *a, /* IN/OUT, spectrum of s(t) */
int nft, /* number of pts of s(w), 2^N*/
float dt, /* sampling interval in sec */
float c, /* water-level in %*/
float gauss /* sigma in Hz */
)
{
int j;
float w, delw;
double *d2, agg, water, u;
u = 1+c;
d2 = (double *) malloc ((nft+1)*sizeof(double));
delw = PI/(dt*nft);
d2[0] = a[0].x*a[0].x;
d2[nft] = a[0].y*a[0].y;
water = d2[0]+d2[nft];
for (j=1; j<nft; j++) {
d2[j] = a[j].x*a[j].x+a[j].y*a[j].y;
water += d2[j];
}
water = c*water/nft; /* in terms of auto-correlation */
a[0].x *= u/(d2[0]+water);
for (w=delw,j=1; j<nft; j++,w+=delw) {
agg = 0.5*w/gauss;
a[j] = dmltp(u*exp(-agg*agg)/(d2[j]+water),conjg(a[j]));
}
agg = 0.5*w/gauss;
a[0].y *= u*exp(-agg*agg)/(d2[j]+water);
free(d2);
}
void prepsychfft(int n,int mode,int *nexp,Complex *w)
{
int i,k,nt,nexp1;
float s;
Complex c1,c2;
nexp1=1;nt=1;
do{
nt=1;
for(i=1;i<=nexp1;i++)
nt=2*nt;
if(nt>=n)
break;
nexp1=nexp1+1;
}while(1);
if(nt==n)
{
s=8*atan(1.0)/(float)(nt);
c1=set(cos(s),-sin(s));
if(mode!=0) c1=conjg(c1);
c2=set(1.,0);
for(k=1;k<=nt;k++)
{
w[k]=c2;
c2=cmulc(c2,c1);
}
}
else
{
nexp1=-1;
}
*nexp=nexp1;
}
void fdmig( complex **cp, int nx, int nw, float *v,float fw,float
dw,float dz,float dx,float dt,int dip)
{
int iw,ix,step=1;
float *s1,*s2,w,coefa[5],coefb[5],v1,vn,trick=0.1;
complex cp2,cp3,cpnm1,cpnm2;
complex a1,a2,b1,b2;
complex endl,endr;
complex *data,*d,*a,*b,*c;
s1=alloc1float(nx);
s2=alloc1float(nx);
data=alloc1complex(nx);
d=alloc1complex(nx);
a=alloc1complex(nx);
b=alloc1complex(nx);
c=alloc1complex(nx);
if(dip==45){
coefa[0]=0.5;coefb[0]=0.25;
step=1;
}
if(dip==65){
coefa[0]=0.478242060;coefb[0]=0.376369527;
step=1;
}
if(dip==79){
coefa[0]=coefb[0]=0.4575;
step=1;
}
if(dip==80){
coefa[1]=0.040315157;coefb[1]=0.873981642;
coefa[0]=0.457289566;coefb[0]=0.222691983;
step=2;
}
if(dip==87){
coefa[2]=0.00421042;coefb[2]=0.972926132;
coefa[1]=0.081312882;coefb[1]=0.744418059;
coefa[0]=0.414236605;coefb[0]=0.150843924;
step=3;
}
if(dip==89){
coefa[3]=0.000523275;coefb[3]=0.994065088;
coefa[2]=0.014853510;coefb[2]=0.919432661;
coefa[1]=0.117592008;coefb[1]=0.614520676;
coefa[0]=0.367013245;coefb[0]=0.105756624;
step=4;
}
if(dip==90){
coefa[4]=0.000153427;coefb[4]=0.997370236;
coefa[3]=0.004172967;coefb[3]=0.964827992;
coefa[2]=0.033860918;coefb[2]=0.824918565;
coefa[1]=0.143798076;coefb[1]=0.483340757;
coefa[0]=0.318013812;coefb[0]=0.073588213;
step=5;
}
v1=v[0];vn=v[nx-1];
do {
step--;
for(iw=0,w=fw;iw<nw;iw++,w+=dw){
if(fabs(w)<=1.0e-10)w=1.0e-10/dt;
for(ix=0;ix<nx;ix++){
s1[ix]=(v[ix]*v[ix])*coefb[step]/(dx*dx*w*w)+trick;
s2[ix]=-v[ix]*dz*coefa[step]/(w*dx*dx)*0.5;
}
for(ix=0;ix<nx;ix++){
data[ix]=cp[iw][ix];
}
cp2=data[1];
cp3=data[2];
cpnm1=data[nx-2];
cpnm2=data[nx-3];
a1=crmul(cmul(cp2,conjg(cp3)),2.0);
b1=cadd(cmul(cp2,conjg(cp2)),cmul(cp3,conjg(cp3)));
if(b1.r==0.0 && b1.i==0.0)
a1=cwp_cexp(cmplx(0.0,-w*dx*0.5/v1));
else
a1=cdiv(a1,b1);
if(a1.i>0.0)a1=cwp_cexp(cmplx(0.0,-w*dx*0.5/v1));
a2=crmul(cmul(cpnm1,conjg(cpnm2)),2.0);
b2=cadd(cmul(cpnm1,conjg(cpnm1)),cmul(cpnm2,conjg(cpnm2)));
if(b2.r==0.0 && b2.i==0.0)
a2=cwp_cexp(cmplx(0.0,-w*dx*0.5/vn));
else
a2=cdiv(a2,b2);
if(a2.i>0.0)a2=cwp_cexp(cmplx(0.0,-w*dx*0.5/vn));
for(ix=0;ix<nx;ix++){
a[ix]=cmplx(s1[ix],s2[ix]);
b[ix]=cmplx(1.0-2.0*s1[ix],-2.0*s2[ix]);
}
for(ix=1;ix<nx-1;ix++){
d[ix]=cadd(cadd(cmul(data[ix+1],a[ix+1]),cmul(data[ix-1],a[ix-1])),
cmul(data[ix],b[ix]));
}
d[0]=cadd(cmul(cadd(b[0],cmul(a[0],a1)),data[0]),cmul(data[1],a[1]));
d[nx-1]=cadd(cmul(cadd(b[nx-1],cmul(a[nx-1],a2)),data[nx-1]),
cmul(data[nx-2],a[nx-2]));
for(ix=0;ix<nx;ix++){
data[ix]=cmplx(s1[ix],-s2[ix]);
b[ix]=cmplx(1.0-2.0*s1[ix],2.0*s2[ix]);
}
endl=cadd(b[0],cmul(data[0],a1));
endr=cadd(b[nx-1],cmul(data[nx-1],a2));
for(ix=1;ix<nx-1;ix++){
a[ix]=data[ix+1];
c[ix]=data[ix-1];
}
a[0]=data[1];
c[nx-1]=data[nx-2];
retris(data,a,c,b,endl,endr,nx,d);
for(ix=0;ix<nx;ix++){
cp[iw][ix]=data[ix];
}
}
}while(step);
free1complex(data);
free1complex(d);
free1complex(b);
free1complex(c);
free1complex(a);
free1float(s1);
free1float(s2);
return;
}
int main(void)
{
/* *************************************************************************
Create Window
********************************************************************** */
wwB = 1000;
wid = pageb(100, 0, 900, wwB, "B-mode");
start();
Bx_ini = 100; By_ini = 100;
/**************************************************************************
Input Serial No.
********************************************************************** */
//printf("input beginning and end of #serial\n");
//scanf("%d %d", &iseq1);
iseq1 = 101;
printf("iseq1 = %d \n", iseq1);
printf("*******************************\n");
printf("* start analysing *\n");
printf("*******************************\n");
/* *************************************************************************
Reading Parameters from File
********************************************************************** */
start:;
if ((fin = fopen("./ssd6500.coe", "r")) == NULL){
fprintf(stderr, "cannot open dpman1M.coe\n");
exit(1);
}
icfile = 0;
while (icfile == 0){
fscanf(fin, "%d %s %d %d %s %d %f %d %d %d %d",
&islct00, name, &iseq, &nfhead, fname, &iage, &rryy, &icnt, &ip_s, &ip_d,
&anabt);
if (iseq == iseq1) icfile = 1;
if (islct00 == 9){ fclose(fin); exit(0); }
}
fclose(fin);
rmaxdh = 1e-6*icnt;
printf("islct00 = %d\n", islct00);
printf("Name of subject = %s\n", name);
printf("Serial number = %d\n", iseq);
printf("Age of subject = %d years old\n", iage);
printf("Maximal velocity range = %f m/s\n", rryy);
printf("Systolic & Diastolic blood pressure = %d, %d mmHg\n", ip_s, ip_d);
/* *************************************************************************
Read Raw Data from File
********************************************************************** */
sprintf(fall, "%s%s.rfa", fhead[nfhead], fname);
printf("File name = %s\n", fall);
if ((fin = fopen(fall, "rb")) == NULL){
printf("cannot open file: %s\n", fall);
exit(1);
}
fseek(fin, 480L, SEEK_SET);
//fread(buf,sizeof(char),480,fin);
fread(&fdat, sizeof(float), 1, fin); /* 送信周波数 [MHz] */
printf("f0 = %f Hz\n", f0 = 1e6*fdat);
fread(&fdat, sizeof(float), 1, fin); /* Burst波数 */
fread(&usdat, sizeof(unsigned short), 1, fin);
npb = usdat;
fread(&fdat, sizeof(float), 1, fin); /* PRF [kHz] */
printf("PRF = %f Hz\n", 1e3*fdat);
fread(&fdat, sizeof(float), 1, fin); /* 受信サンプリング周波数 [MHz] */
printf("fs = %f Hz\n", fs = 1e6*fdat);
fread(&fdat, sizeof(float), 1, fin); /* ビーム間隔 [mm] */
printf("beam interval = %f mm\n", elp = fdat);
fread(&usdat, sizeof(unsigned short), 1, fin); /* ビーム数 */
printf("number of beams = %d\n", nposi = usdat);
fread(&usdat, sizeof(unsigned short), 1, fin); /* ROI上端深さ(point)=0 */
printf("record offset = %d\n", usdat);
fread(&usdat, sizeof(unsigned short), 1, fin); /* ビーム数当たりサンプル数 */
printf("number of samples per line = %d\n", ncount = usdat);
fread(&usdat, sizeof(unsigned short), 1, fin); /* 開始ビーム番号 */
fread(&usdat, sizeof(unsigned short), 1, fin); /* 終了ビーム番号 */
fread(&usdat, sizeof(unsigned short), 1, fin); /* フォーカス段数 */
for (i = 0; i<7; ++i) fread(&fdat, sizeof(float), 1, fin); /* フォーカス位置設定 */
for (i = 0; i<4; ++i)
fread(&usdat, sizeof(unsigned short), 1, fin); /* フォーカス音圧設定 */
fread(&fdat, sizeof(float), 1, fin); /* ステア角 [degree] */
printf("steering angle: %f\n", fdat);
fread(&fdat, sizeof(float), 1, fin); /* フレームレート [Hz] */
printf("frame rate = %f Hz\n", PRF2 = fdat);
fread(&fdat, sizeof(float), 1, fin); /* スケールファクター */
fread(&uldat, sizeof(unsigned long), 1, fin); /* フレーム数 */
printf("number of frames = %d\n", nframe = uldat);
fread(&usdat, sizeof(unsigned short), 1, fin); /* データ情報 */
fread(&fdat, sizeof(float), 1, fin); /* 受信周波数 [MHz] */
//printf("receiving frequency: %f MHz\n",fdat);
for (i = 0; i<14; ++i) fread(&cdat, sizeof(char), 1, fin); /* 未使用 */
for (i = 0; i<4; ++i) fread(&usdat, sizeof(unsigned short), 1, fin); /* 諸設定 */
for (i = 0; i<16; ++i) fread(&cdat, sizeof(char), 1, fin); /* Sweepファイル名 */
fread(&usdat, sizeof(unsigned short), 1, fin); /* bit resolution */
fread(&usdat, sizeof(unsigned short), 1, fin); /* 波形種別 */
fread(&usdat, sizeof(unsigned short), 1, fin); /* 波形関数 */
fread(&fdat, sizeof(float), 1, fin); /* 振幅補正 */
fread(&usdat, sizeof(unsigned short), 1, fin); /* データ数 */
fread(&fdat, sizeof(float), 1, fin); /* 比帯域 */
//fread(&fdat,sizeof(float),1,fin); /* Flat比率 */
//fread(&fdat,sizeof(float),1,fin); /* 送信波形 台 */
for (i = 0; i<8; ++i) fread(&cdat, sizeof(char), 1, fin); /* スペア */
fread(&fdat, sizeof(float), 1, fin); /* 送信サンプル周波数 [MHz] */
fread(&usdat, sizeof(unsigned short), 1, fin); /* Ex-PHDのon/off */
fread(&usdat, sizeof(unsigned short), 1, fin); /* B FE-Gain上限 */
fread(&usdat, sizeof(unsigned short), 1, fin); /* B FE-Gainの下限 */
fread(&usdat, sizeof(unsigned short), 1, fin); /* B-Gain */
fread(&usdat, sizeof(unsigned short), 1, fin); /* 送信回数 */
printf("number of transmissions per line: %d\n", usdat);
fread(&usdat, sizeof(unsigned short), 1, fin); /* アライメント */
fread(&usdat, sizeof(unsigned short), 1, fin); /* プレーン識別番号 */
fread(&usdat, sizeof(unsigned short), 1, fin); /* 空間コンパウンド識別番号 */
printf("spatial compound: %d\n", usdat);
fread(&fdat, sizeof(float), 1, fin); /* 音響パワー */
fread(&usdat, sizeof(unsigned short), 1, fin); /* Interm Frame */
fread(&fdat, sizeof(float), 1, fin); /* Interm Inverval */
fread(&usdat, sizeof(unsigned short), 1, fin); /* エラスト識別番号 */
fread(&usdat, sizeof(unsigned short), 1, fin); /* RFビット位置 */
for (i = 0; i<4; ++i) fread(&cdat, sizeof(char), 1, fin);
//fread(&uldat,sizeof(unsigned long),1,fin);
/*printf("number of data = %d bytes\n",uldat);*/
/*printf("%d bytes\n",4*4+4*ncount*nposi*nframe);*/
fseek(fin, 996L, SEEK_SET); //932
// loading RF data
tcnt = (4 + ncount)*nposi*nframe;
raw_dat = (int *)malloc((size_t)(4 * tcnt));
fread(raw_dat, sizeof(int), tcnt, fin);
printf("raw size = %f\n", sizeof(raw_dat) / sizeof(int));
// loading physio data (ECG)
fread(&idcnt, sizeof(int), 1, fin);
icnt = (idcnt & 0x3FFFC00) >> 6;
printf("icnt = %d\n", icnt);
for (i = 0; i<3; ++i) fread(&idcnt, sizeof(int), 1, fin);
for (i = 0; i<icnt; ++i){
fread(&emin, sizeof(unsigned short), 1, fin);
fread(&emax, sizeof(unsigned short), 1, fin);
fread(&pmin, sizeof(unsigned short), 1, fin);
fread(&pmax, sizeof(unsigned short), 1, fin);
fread(&plmin, sizeof(unsigned short), 1, fin);
fread(&plmax, sizeof(unsigned short), 1, fin);
for (j = 0; j<18; ++j) fread(&idcnt, sizeof(unsigned short), 1, fin);
idcnt = PRF2*i / 1000.;
ecg[i] = emax;
pcg[i] = plmax;
}
fclose(fin);
fin = fopen("./ecg.dat", "w");
for (i = 0; i<icnt; ++i) fprintf(fin, "%f\n", ecg[i]);
fclose(fin);
/*fin=fopen("./pcg.dat","w");
for(i=0;i<icnt;++i) fprintf(fin,"%f\n",pcg[i]);
fclose(fin);*/
for (i = 0; i<nframe; ++i){
for (j = 0; j<nposi; ++j){
ihof[j][i] = (4 + ncount)*j + (4 + ncount)*nposi*i;
}
}
nfr_low = nframe;
printf("interval of beams = %f mm\n", elp);
dadd = 4 * 2 * elp / 0.15;
/* Post ECG */
for (i = 0; i<nframe; ++i){
MpF = (int)(1000 / PRF2*i);
ecgmax = ecg[MpF];
ecg[i] = ecgmax;
}
fin = fopen("./ecg2.dat", "w");
for (i = 0; i<nframe; ++i) fprintf(fin, "%f\n", ecg[i]);
fclose(fin);
/* *************************************************************************
Setting Parameters (Temporal)
********************************************************************** */
if (aux_flg == 0) iaux = 5;
if (aux_flg == 1) iaux = 6;
f0i = f0;
ifs = (int)(fs / 1e6);
nspc = 1;
if (SPAv == 1) nspc = 1;
/* *************************************************************************
Setting Parameters
********************************************************************** */
heartrate = 100;
p_s = (float)ip_s;
p_d = (float)ip_d;
icgain = 4000000;
idltx = 10; /* nwv wavelength (default:4x) */
idltx2 = 10;
deltad = c0 / (fs*2.0);
lp2s = (int)(2.0 / f0*fs);
lp2q = (int)(2.0 / f0*fs); /* cut off frequency in quadrature demodulation */
//f0=5.5*1e+6;
/* *************************************************************************
Memory Allocation
********************************************************************** */
if (ncount >= nframe){
dmy = (float *)malloc((size_t)(8 * (ncount + 1)));
sgn = (float *)malloc((size_t)(8 * (ncount + 1)));
}
if (nframe>ncount){
dmy = (float *)malloc((size_t)(8 * (nframe + 1)));
sgn = (float *)malloc((size_t)(8 * (nframe + 1)));
}
/* *************************************************************************
Read ECG, PCG, AUX
********************************************************************** */
ecgbias = 0.0; icnt = 0;
for (i = 0; i<nframe / 2; ++i){
ecgbias = ecgbias + ecg[i];
++icnt;
}
for (i = 0; i<nframe; ++i) ecg[i] = ecg[i] - ecgbias / (float)icnt;
ecgmax = 0.0;
for (i = 0; i<nframe / 2; ++i){
if (fabs(ecg[i])>ecgmax) ecgmax = fabs(ecg[i]);
}
pcgbias = 0.0; icnt = 0;
for (i = 0; i<nframe / 2; ++i){
pcgbias = pcgbias + pcg[i];
++icnt;
}
for (i = 0; i<nframe; ++i) pcg[i] = pcg[i] - pcgbias / (float)icnt;
pcgmax = 0.0;
for (i = 0; i<nframe / 2; ++i){
if (fabs(pcg[i])>pcgmax) pcgmax = fabs(pcg[i]);
}
/* ****************************************************************************
Detect Timing of R-wave
************************************************************************* */
nbeat = 0; beat = 0.0;
ilevel = 0.6*ecgmax;
period = 1.0 / (heartrate / 60.0);
for (j = 0; j<maxfrm; ++j){
if (j == 0) ii1 = 0;
if (j >= 1) ii1 = track0[j - 1] + (int)(PRF2*period);
if (ii1>nframe) break;
for (i = ii1 + 1; i<nframe; ++i){
if (ecg[i] >= ilevel){
track0[j] = i;
nbeat = nbeat + 1;
break;
}
if (i == nframe - 1) break;
}
if (track0[j]<0){
nbeat = nbeat - 1;
break;
}
if (track0[j] == 0) break;
for (i = 0; i <= j - 1; ++i){
beat = beat + (float)(track0[i + 1] - track0[i]);
}
beat = beat / (float)(j + 1);
}
tbeat = beat / fs * 1000;
/*for(i=0;i<nbeat;++i){
rr=ecg[track0[i]];
for(j>track0[i]-(int)(0.01*PRF2);j<track0[i]+(int)(0.01*PRF2);++j){
if(j>=0 && j<nframe && fabs(ecg[j])>rr){
rr=fabs(ecg[j]);
icnt=j;
}
}
track0[i]=j;
}*/
for (i = 0; i<nbeat; ++i){
printf("track0(%d) = %d\n", i, track0[i]);
}
//nbeat=2;
//track0[0]=0; track0[1]=nframe;
/* *************************************************************************
Setting Parameters
********************************************************************** */
ifrm1 = 0;
ifrm2 = nframe;
if (nbeat == 0){
ifrm1 = 0;
ifrm2 = nframe;
}
if (nbeat>0){
ifrm1 = track0[0] - (int)(0.2*PRF2);
if (ifrm1<0) ifrm1 = 0;
ifrm2 = nframe;
}
printf("ifrm1, ifrm2 = %d, %d\n", ifrm1, ifrm2);
if (ifrm1<0 || ifrm2>nframe || ifrm1>ifrm2) exit(0);
/* *************************************************************************
Display B-mode Image
********************************************************************** */
rmaxz = 0.0;
for (i = 0;i < nposi;++i){
for (j = 0; j < ncount; ++j) {
dmy[j] = pow((float)raw_dat[ihof[i][track0[0]] + j + 4], 2);
}
wint2s(wary, lp2s);
ndmy = ncount;
lpfs(wary, dmy, sgn, lp2s, ndmy);
for (j = 0; j<ncount; ++j){
sgn[j] = sqrt(sgn[j]);
if (fabs(sgn[j])>rmaxz) rmaxz = fabs(sgn[j]);
}
}
rwb = dadd / 8.;
nwb = (int)rwb;
/* 直交検波 */
for (i = 0; i<nposi; ++i){
for (j = 0; j<ncount; ++j){
dmy[j] = pow((float)raw_dat[ihof[i][track0[0]] + j + 1], 2);
}
wint2s(wary, lp2s);
ndmy = ncount;
lpfs(wary, dmy, sgn, lp2s, ndmy);
for (j = 0; j<ncount; ++j) sgn[j] = sqrt(sgn[j]);
for (j = 0; j<ncount; ++j){
ic = 49 + (int)(100.*(20.*log10((sgn[j] + 1e-4) / rmaxz) + B_gain) / B_gain);
if (ic<49) ic = 49;
if (ic>148) ic = 148;
ic = (ic - 49) * 255 / 99;
gscolor = gscol256(ic, ic, ic);
for (k = 0;k < (int)(izoom_B / (mpp / deltad) + 1);++k) {
gsline(Bx_ini + (int)(izoom_B*rwb*i),
By_ini + (int)(izoom_B*j*deltad / mpp) + k,
Bx_ini + (int)(izoom_B*rwb*(i + 1)),
By_ini + (int)(izoom_B*j*deltad / mpp) + k,
gscolor);
}
}
}
gscolor = gscol256(0, 0, 0);
gsline(Bx_ini - 15, By_ini, Bx_ini - 15,
By_ini + (int)(izoom_B*ncount*deltad / mpp), gscolor);
for (i = 0;i<ncount;i = i + (int)(5 * mpp / deltad)) {
gsline(Bx_ini - 15, By_ini + (int)(izoom_B*i*deltad / mpp),
Bx_ini - 10, By_ini + (int)(izoom_B*i*deltad / mpp), gscolor);
}
for (i = 0;i<ncount / 5;i = i + (int)(5 * mpp / deltad)) {
gsline(Bx_ini - 15, By_ini + (int)(5.*izoom_B*i*deltad / mpp),
Bx_ini - 5, By_ini + (int)(5.*izoom_B*i*deltad / mpp), gscolor);
}
gsline(Bx_ini, By_ini - 15, Bx_ini + (int)(izoom_B*rwb*nposi), By_ini - 15, gscolor);
for (i = 0;i<(int)(rwb*nposi / 10.) + 1;++i) {
gsline(Bx_ini + izoom_B * 10 * i, By_ini - 15,
Bx_ini + izoom_B * 10 * i, By_ini - 5, gscolor);
}
sprintf(buf, "%4d %56s", iseq, fall);
ptext(10, By_ini - 65, strlen(buf), buf);
sprintf(buf, "age:%2d BP: %3d/%3d", iage, ip_s, ip_d);
ptext(10, By_ini - 50, strlen(buf), buf);
sprintf(buf, "mkdirhier ./images/%s", fname);
//system(buf);
/*sprintf(buf,"import -window 0x%x -crop %dx%d+%d+%d -silent ./images/%s/Bmode.gif",
wid,(int)(izoom_B*rwb*nposi),
(int)(izoom_B*ncount*deltad/mpp),
Bx_ini,wwB-By_ini+60,fname);*/
sprintf(buf, "import -window 0x%x -silent ./images/%s/Bmode.gif", wid, fname);
//system(buf);
/* *************************************************************************
Setting Tracking Position by Tracing B-mode (itrace=1)
********************************************************************** */
if (itrace == 1){
gscolor = gscol256(0,0,255);
for (i = 0; i<nposi; ++i) ipeakflg[i] = 0;
cursorpos(0, 0);
trackingpoint_top();
cursorpos(0, 0);
for (i = 0; i<nposi0; ++i) ipeakflg[i] = 0;
trackingpoint_bot();
///* Save Peak Position */
//fin = fopen("./tmp/peaks", "w");
//for (i = 0; i<nposi; ++i)
// fprintf(fin, "%d %d %d %d\n", ipeak11[i], ipeak22[i], ipeak33[i], ipeak44[i]);
//fclose(fin);
}
if (itrace == 0){
sprintf(buf, "./tmp/peaks.%d", iseq1);
fin = fopen(buf, "r");
for (i = 0; i<nposi; ++i){
fscanf(fin, "%d %d %d %d", &ip1, &ip2, &ip3, &ip4);
ipeak11[i] = ip1; ipeak22[i] = ip2; ipeak33[i] = ip3; ipeak44[i] = ip4;
}
fclose(fin);
}
if (DispTr == 1){
gscolor = gscol256(255,0,0);
for (i = 0; i<nposi; ++i){
gscolor = gscol256(0,0,0);
for (k = 0; k<(int)(izoom_B / (mpp / deltad) + 1); ++k)
gsline(Bx_ini + (int)(izoom_B*rwb*i),
By_ini - (int)(izoom_B*ipeak33[i] * deltad / mpp) - k,
Bx_ini + (int)(izoom_B*rwb*(i + 1)),
By_ini - (int)(izoom_B*ipeak33[i] * deltad / mpp) - k, gscolor);
}
}
for (i = 0; i<nposi; ++i){
if (ipeak44[i] - ipeak33[i]<4 * idltx2) ipeak44[i] = ipeak33[i] + 4 * idltx2;
}
Sleep(1);
gscolor = gscol256(0,0,0);
if (ipause == 1){
printf("Input 0 if ready to go next.\n");
scanf("%d", &iflg);
}
/* *************************************************************************
Clear Window
********************************************************************** */
eraseg();
/* *************************************************************************
Estimation of Displacements
********************************************************************** */
ifrm1 = track0[anabt]; ifrm2 = track0[anabt + 2]; dsh = 1;
for (i = 0; i<nDFT; ++i) han_w[i] = 0.5 - 0.5*cos(rad*i / (float)nDFT);
sprintf(buf, "mkdirhier ./images/%s/Mmode", fname);
//system(buf);
//sprintf(buf, "./ddmax_%s.dat", fname);
sprintf(buf, "./ddmax.dat", fname);
fin = fopen(buf, "w");
for (iiposi = 0; iiposi<nposi; ++iiposi){
ddmax = 0.0;
eraseg();
sprintf(buf, "%4d %56s", iseq, fall);
ptext(100, wwB + 10, strlen(buf), buf);
sprintf(buf, "age:%2d, BP: %3d/%3d, vel. max: %5.1f mm/s, dd max: %6.1f um, beam position: %2d",
iage, ip_s, ip_d, 1e3*rryy, 1e6*rmaxdh, iiposi + 1);
ptext(100, wwB + 22, strlen(buf), buf);
if (aux_flg == 0)
sprintf(buf, "max. & bias of ECG: %7.1f, %7.1f, max. PCG: %7.1f",
ecgmax, ecgbias / PRF2, pcgmax);
if (aux_flg == 1)
sprintf(buf, "max & bias of ECG: %7.1f, %7.1f, max. PULSE: %7.1f",
ecgmax, ecgbias / PRF2, pcgmax);
ptext(100, wwB + 34, strlen(buf), buf);
/* *************************************************************************
Setting Tracking Position and Number of Tracking Points
********************************************************************** */
ndep[iiposi] = 2;
/* *************************************************************************
Display M-mode Image
********************************************************************** */
lp2s = (int)(2.0 / f0*fs);
Mx_ini = 50; My_ini = 800;
ry0 = nframe / 200.;
iy0 = (int)ry0;
if (iy0<1) iy0 = 1;
for (i = 0; i<200; ++i){
for (j = 0; j<ncount; ++j)
dmy[j] = pow((float)raw_dat[ihof[iiposi][(int)(ry0*i)] + j], 2);
wint2s(wary, lp2s);
ndmy = ncount;
lpfs(wary, dmy, sgn, lp2s, ndmy);
for (j = 0; j<ncount; ++j)
sgn[j] = sqrt(sgn[j]);
for (j = 0; j<ncount; j = j + (int)(mpp / deltad / izoom_M)){
ic = 49 + (int)(100.*((20.*log10(sgn[j] / rmaxz) + B_gain) / B_gain));
if (ic<49) ic = 49;
if (ic>148) ic = 148;
ic = (ic - 49) * 255 / 99;
gscolor = gscol256(ic, ic, ic);
for (k = -1; k <= 1; ++k)
gsline(Mx_ini + (int)(izoom_M*j*deltad / mpp), My_ini - 3 * i - k,
Mx_ini + (int)(izoom_M*j*deltad / mpp), My_ini - 3 * (i + 1) - k, gscolor);
}
}
gscolor = gscol256(0, 0, 0);
gsline(Mx_ini, My_ini + 15,
Mx_ini + (int)(izoom_M*ncount*deltad / mpp), My_ini + 15, gscolor);
for (i = 0; i < ncount; i = i + (int)(5 * mpp / deltad)) {
gsline(Mx_ini + (int)(izoom_M*i*deltad / mpp), My_ini + 15,
Mx_ini + (int)(izoom_M*i*deltad / mpp), My_ini + 10, gscolor);
}
for (i = 0; i < ncount / 5; i = i + (int)(5 * mpp / deltad)) {
gsline(Mx_ini + (int)(5.*izoom_M*i*deltad / mpp), My_ini + 15,
Mx_ini + (int)(5.*izoom_M*i*deltad / mpp), My_ini + 5, gscolor);
}
gsline(Mx_ini + (int)(izoom_M*ncount*deltad / mpp) + 10, My_ini,
Mx_ini + (int)(izoom_M*ncount*deltad / mpp) + 10, My_ini - 600, gscolor);
for (i = 0; i < (int)(2.*nframe / PRF2) + 1; ++i) {
gsline(Mx_ini + (int)(izoom_M*ncount*deltad / mpp) + 10,
My_ini - (int)(300.*PRF2 / nframe*i),
Mx_ini + (int)(izoom_M*ncount*deltad / mpp) + 5,
My_ini - (int)(300.*PRF2 / nframe*i), gscolor);
}
/* *************************************************************************
Frame for Displaying Signals
********************************************************************** */
for (i = 0; i<4; ++i){
gsrect(Mx_ini + (int)(izoom_M*ncount*deltad / mpp) + 20 + 100 * i,
My_ini + 15,
Mx_ini + (int)(izoom_M*ncount*deltad / mpp) + 110 + 100 * i,
My_ini - 615, RGB(255, 255, 255), RGB(0, 0, 0));
}
for (i = 0; i<4; ++i){
for (j = 0; j<2; ++j){
for (k = 0; k<3; ++k){
gsline(Mx_ini + (int)(izoom_M*ncount*deltad / mpp) + 30 + 35 * k + 100 * i,
My_ini + 15 - 622 * j,
Mx_ini + (int)(izoom_M*ncount*deltad / mpp) + 30 + 35 * k + 100 * i,
My_ini + 15 - 8 - 622 * j, gscolor);
}
}
}
for (i = 0; i<4; ++i){
gsline_d(Mx_ini + (int)(izoom_M*ncount*deltad / mpp) + 20 + 45 + 100 * i, My_ini,
Mx_ini + (int)(izoom_M*ncount*deltad / mpp) + 20 + 45 + 100 * i,
My_ini - 600, gscolor);
}
for (i = 0; i<4; ++i){
for (j = 0; j<2; ++j){
for (k = 0; k<(int)(2.*(ifrm2 - ifrm1) / PRF2) + 1; ++k){
gsline(Mx_ini + (int)(izoom_M*ncount*deltad / mpp) + 20 + 100 * i + 82 * j,
My_ini - (int)(300.*PRF2 / (ifrm2 - ifrm1)*k),
Mx_ini + (int)(izoom_M*ncount*deltad / mpp) + 20 + 8 + 100 * i + 82 * j,
My_ini - (int)(300.*PRF2 / (ifrm2 - ifrm1)*k), gscolor);
}
}
}
/* *************************************************************************
Display ECG
********************************************************************** */
ry0 = 600. / nframe; rx0 = 35. / ecgmax;
for (i = 0; i<nframe - 2; ++i){
nx0 = (int)(izoom_M*ncount*deltad / mpp) + 20 + 45;
gsline(Mx_ini + nx0 - (int)(rx0*ecg[i]),
My_ini - (int)(ry0*i),
Mx_ini + nx0 - (int)(rx0*ecg[i + 1]),
My_ini - (int)(ry0*(i + 1)), gscolor);
}
/* *************************************************************************
Display Timing of R-wave
********************************************************************** */
if (nbeat != 0){
nbeat2 = nbeat;
for (i = 0; i<nbeat; ++i){
if (track0[i]>0){
nbeat1 = i;
break;
}
}
for (i = 0; i<nbeat; ++i){
if (track0[i]>nframe){
nbeat2 = i - 1;
break;
}
}
if (nbeat1 == 0) nbeat1 = 0;
if (nbeat2 == 0) nbeat2 = 0;
gscolor = gscol256(255, 0, 0);
nx0 = (int)(izoom_M*ncount*deltad / mpp);
for (i = nbeat1; i<nbeat2; ++i){
gsline_d(Mx_ini, My_ini - (int)(ry0*track0[i]),
Mx_ini + nx0, My_ini - (int)(ry0*track0[i]), gscolor);
}
nx0 = (int)(izoom_M*ncount*deltad / mpp) + 30;
for (i = 0; i<4; ++i){
for (j = nbeat1; j<nbeat2; ++j){
gsline_d(Mx_ini + nx0 + 100 * i, My_ini - (int)(ry0*track0[j]),
Mx_ini + nx0 + 70 + 100 * i, My_ini - (int)(ry0*track0[j]), gscolor);
}
}
gscolor = gscol256(0, 0, 0);
}
/* *************************************************************************
Display PCG
********************************************************************** */
/*ry0=600./nframe; rx0=35./pcgmax;
for(i=0;i<nframe-1;++i){
nx0=(int)(izoom_M*ncount*deltad/mpp)+20+145;
gsline(Mx_ini+nx0-(int)(rx0*pcg[i]),
My_ini+(int)(ry0*i),
Mx_ini+nx0-(int)(rx0*pcg[i+1]),
My_ini+(int)(ry0*(i+1)));
}*/
/* *************************************************************************
Start of Loop for Frame for Displacement Estimation
********************************************************************** */
for (idcnt = 0; idcnt<ndep[iiposi]; ++idcnt){
disp[idcnt][ifrm1] = 0.0;
dd[ifrm1] = 0.0;
}
ip1 = 0; ip2 = ncount;
for (ifrm = ifrm1; ifrm<ifrm2; ++ifrm){
/* Quadrature Demodulation */
czeros = cmplx(0.0, 0.0);
for (i = -1; i<2; ++i){
if (iiposi + i>-1 && iiposi + i<nposi){
/* For Previous Frame */
if (ifrm == ifrm1){
for (j = ip1; j<ip2; ++j){
qdr_p[j][i + 1] = 0.0; qdi_p[j][i + 1] = 0.0; icnt = 0;
for (k = -nDFT / 2; k < nDFT / 2; ++k) {
if (j + k >= 0 && j + k<ncount){
qdr_p[j][i + 1] = qdr_p[j][i + 1] + han_w[k + nDFT / 2] *
cos(nfrq*rad*(k + nDFT / 2) / (float)nDFT)*
(float)raw_dat[ihof[iiposi + i][ifrm] + j + k];
qdi_p[j][i + 1] = qdi_p[j][i + 1] - han_w[k + nDFT / 2] *
sin(nfrq*rad*(k + nDFT / 2) / (float)nDFT)*
(float)raw_dat[ihof[iiposi + i][ifrm] + j + k];
++icnt;
}
}
qdr_p[j][i + 1] = qdr_p[j][i + 1] / (float)icnt;
qdi_p[j][i + 1] = qdi_p[j][i + 1] / (float)icnt;
}
}
/* For Post-Frame */
for (j = ip1; j<ip2; ++j){
qdr_f[j][i + 1] = 0.0; qdi_f[j][i + 1] = 0.0; icnt = 0;
for (k = -nDFT / 2; k<nDFT / 2; ++k){
if (j + k >= 0 && j + k<ncount){
qdr_f[j][i + 1] = qdr_f[j][i + 1] + han_w[k + nDFT / 2] *
cos(nfrq*rad*(k + nDFT / 2) / (float)nDFT)*
(float)raw_dat[ihof[iiposi + i][ifrm + 1] + j + k];
qdi_f[j][i + 1] = qdi_f[j][i + 1] - han_w[k + nDFT / 2] *
sin(nfrq*rad*(k + nDFT / 2) / (float)nDFT)*
(float)raw_dat[ihof[iiposi + i][ifrm + 1] + j + k];
++icnt;
}
}
qdr_f[j][i + 1] = qdr_f[j][i + 1] / (float)icnt;
qdi_f[j][i + 1] = qdi_f[j][i + 1] / (float)icnt;
}
}
}
/* Start of Loop of Depth for Displacement Estimation */
for (idcnt = 0; idcnt<ndep[iiposi]; ++idcnt){
if (ifrm == ifrm1){
if (idcnt == 0) peak0[idcnt] = ipeak11[iiposi];
if (idcnt == 1) peak0[idcnt] = ipeak22[iiposi];
}
xpeak0[idcnt] = (float)peak0[idcnt];
// velocity estimation
crr0 = czeros; crr1 = czeros;
for (ispc = -nspc; ispc<nspc + 1; ++ispc){
if (iiposi + ispc >= 0 && iiposi + ispc<nposi){
for (i = 0; i <= 2 * idltx; ++i){
if (i + peak0[idcnt] >= 0 && i + peak0[idcnt]<ncount){
rr = qdr_p[i + peak0[idcnt]][ispc + 1];
ri = qdi_p[i + peak0[idcnt]][ispc + 1];
cdmyp = cmplx(rr, ri);
rr = qdr_f[i + peak0[idcnt]][ispc + 1];
ri = qdi_f[i + peak0[idcnt]][ispc + 1];
cdmy = cmplx(rr, ri);
ctemp = conjg(cdmyp);
crr0 = cadd(crr0, cmul(cdmy, ctemp));
}
}
}
}
vel_p[idcnt] = vel[idcnt];
if (ifrm == ifrm1) vel_p[idcnt] = 0.0;
vel[idcnt] = -0.5*c0*atan2(crr0.i, crr0.r) / rad / f0;
disp[idcnt][ifrm + 1] = disp[idcnt][ifrm] + vel[idcnt];
if (idcnt == 0){
xpeak0[idcnt] = ipeak11[iiposi] + disp[idcnt][ifrm + 1] / deltad;
}
if (idcnt == 1){
xpeak0[idcnt] = ipeak22[iiposi] + disp[idcnt][ifrm + 1] / deltad;
}
peak0[idcnt] = (int)xpeak0[idcnt];
}
dd[ifrm + 1] = disp[1][ifrm + 1] - disp[0][ifrm + 1];
ry0 = 600. / nframe;
for (idcnt = 0; idcnt<ndep[iiposi]; ++idcnt){
/* Display Tracked Position */
gscolor = gscol256(255, 0, 0);
if (ifrm >= ifrm1 && ifrm<ifrm2)
gsline(Mx_ini + (int)(izoom_M*peak0[idcnt] * deltad / mpp),
My_ini - (int)(ry0*ifrm),
Mx_ini + (int)(izoom_M*peak0[idcnt] * deltad / mpp),
My_ini - (int)(ry0*(ifrm + 1)), gscolor);
gscolor = gscol256(255, 255, 255);
gsline(Mx_ini + 1 + (int)(izoom_M*peak0[idcnt] * deltad / mpp),
My_ini - (int)(ry0*ifrm),
Mx_ini + 1 + (int)(izoom_M*peak0[idcnt] * deltad / mpp),
My_ini - (int)(ry0*(ifrm + 1)), gscolor);
gscolor = gscol256(0, 0, 0);
/* Display Velocity */
rx0 = 35. / rryy;
if (idcnt == 0) nx0 = (int)(izoom_M*ncount*deltad / mpp) + 20 + 145;
if (idcnt == 1) nx0 = (int)(izoom_M*ncount*deltad / mpp) + 20 + 245;
if (ifrm > ifrm1 && ifrm < ifrm2 - 1) {
gsline(Mx_ini + nx0 + (int)(rx0*vel_p[idcnt] * PRF2),
My_ini - (int)(ry0*ifrm),
Mx_ini + nx0 + (int)(rx0*vel[idcnt] * PRF2),
My_ini - (int)(ry0*(ifrm + 1)), gscolor);
}
}
/* Display Change in Diameter */
rx0 = 35. / rmaxdh;
nx0 = (int)(izoom_M*ncount*deltad / mpp) + 20 + 345;
if (ifrm >= ifrm1 && ifrm<ifrm2 - 1){
gsline(Mx_ini + nx0 - (int)(rx0*dd[ifrm]),
My_ini - (int)(ry0*ifrm),
Mx_ini + nx0 - (int)(rx0*dd[ifrm + 1]),
My_ini - (int)(ry0*(ifrm + 1)), gscolor);
}
for (i = 0; i<3; ++i){
for (j = 0; j<ncount; ++j){
qdr_p[j][i] = qdr_f[j][i]; qdi_p[j][i] = qdi_f[j][i];
}
}
if (ddmax>dd[ifrm + 1])
ddmax = dd[ifrm + 1];
} // end of loop of frame
sprintf(buf, "import -window 0x%x -silent ./images/%s/Mmode/pos%d.gif", wid, fname, iiposi);
//system(buf);
fprintf(fin, "%d %f\n", iiposi, ddmax*1e+6);
} // end of loop of beam position
fclose(fin);
/* *************************************************************************
Terminate Program
********************************************************************** */
free(raw_dat); free(sgn); free(dmy);
end();
printf("INPUT 0 for termination of program.\n");
scanf("%d", &iflg);
printf("*******************************\n");
printf("* normally terminated *\n");
printf("*******************************\n");
}
/******************************** process_frame() ***********************************/
void process_frame(void)
{
int k, m;
int io_ptr0;
float cabsInput;
float cabs2Input;
float *tmpM;
float tmpMinNoise;
float tmpResult;
//static float noiseRatio;
//static float prevNoiseRatio;
complex tmpMin3Frames;
/* work out fraction of available CPU time used by algorithm */
cpuFrac = ((float) (io_ptr & (FRAMEINC - 1)))/FRAMEINC;
/* wait until io_ptr is at the start of the current frame */
while((io_ptr/FRAMEINC) != frame_ptr);
/* then increment the framecount (wrapping if required) */
if (++frame_ptr >= (CIRCBUFFLEN/FRAMEINC)) frame_ptr=0;
/* save a pointer to the position in the I/O buffers (inBuffer/outBuffer) where the
data should be read (inBuffer) and saved (outBuffer) for the purpose of processing */
io_ptr0=frame_ptr * FRAMEINC;
/* copy input data from inBuffer into inFrame (starting from the pointer position) */
m=io_ptr0; //start from current io_ptr
for (k=0;k<FFTLEN;k++)
{
inFrame[k] = inBuffer[m] * inWin[k]; //windows the input samples
if (++m >= CIRCBUFFLEN) m=0; /* wrap if required */
}
/************************* DO PROCESSING OF FRAME HERE **************************/
/* please add your code, at the moment the code simply copies the input to the
ouptut with no processing */
for (k=0;k<FFTLEN;k++){ //copy inFrame to working set for FFT
fft_input[k].r = inFrame[k];
fft_input[k].i = 0;
}
fft(FFTLEN,fft_input);
//************** shuffle noise buffers *******************//
if(++numFrames >= FRAMESLIMIT){
numFrames = 0;
tmpM = M4;
M4 = M3;
M3 = M2;
M2 = M1;
M1 = tmpM;
}
//*************** processing *****************************//
for(k=0; k<NFREQ; k++){
cabsInput = cabs(fft_input[k]); //|X(w)|
cabs2Input = cabsInput*cabsInput; //|X(w)|**2
//************** ENHANCEMENT 1/2 *****************//
if (enhancement1or2 == 1)
lowPass[k] = (1-inputLPF_k)*cabsInput +inputLPF_k*lowPass[k]; //lowpass formula from notes
else if (enhancement1or2 == 2)
lowPass[k] = sqrt((1-inputLPF_k)*cabs2Input+ inputLPF_k*lowPass[k]*lowPass[k]); //lowpass on power
else
lowPass[k] = cabsInput; //doesnt LPF at all
if(numFrames == 0){ // if first frame after swapping noise buffer pointers
M1[k] = lowPass[k]; //then youre *hapless laughter* so youre filling the entire M1 with the whole input frame. lowPass[k] is either the whole input frame OR lowpassed version based off the enhancement case
//min noise is just the frame
tmpMinNoise = M4[k]; //this finds the min noise out of the the buffers M4-M2 + a tiny bit M1 cos its still doing it?!! for the particular frequency k
if(tmpMinNoise > M3[k])
tmpMinNoise = M3[k];
if(tmpMinNoise > M2[k])
tmpMinNoise = M2[k];
if(tmpMinNoise > M1[k])
tmpMinNoise = M1[k];
}
else if (lowPass[k] < M1[k]){ //else if not the first frame then and the the current noise is smaller than whats in M1
M1[k] = lowPass[k]; //replace with the smaller one
if(minM[k] > M1[k])
tmpMinNoise = M1[k]; //so if this is smaller than the current minimum, then udpate
else
tmpMinNoise = minM[k]; //tmpMinNoise is for enhancement 3 - single minimum(for this k because of the for loop but is declared as a float)
}
else
tmpMinNoise = minM[k];
//************* ENHANCEMENT 3 *************//
if(enhancement3 == 1) //low pass filter noise estimate
minM[k] = (1- noiseLPF_k)*tmpMinNoise + noiseLPF_k*minM[k]; //lowpass formula from notes
else
minM[k] = tmpMinNoise;
//************* ENHANCEMENT 6 *************//
if(enhancement6 == 1)
/*
if (pureNoiseRatio < 0.1)
alpha = 1;
else if (pureNoiseRatio > 0.562)
alpha = 5;
else
alpha = 8.658*pureNoiseRatio + 0.134;*/
if (pureNoiseRatio < 0.1)
alpha = 1;
else if (pureNoiseRatio > 1.78)
alpha = 5;
else
alpha = 3*log10f(pureNoiseRatio) +4;
prevNoiseRatio = noiseRatio;
pureNoiseRatio = (minM[k]/cabsInput) ;
noiseRatio = (alpha*pureNoiseRatio); //|N(w)|/|X(w)|
//************* ENHANCEMENT 4 *************//
if (enhancement4or5 == 4) {
switch(enhancement4){
case 1:
fft_gain[k] = 1.0 - noiseRatio;
minNR = MIN_NR*(alpha*(minM[k])/cabsInput);
break;
case 2:
pOverX = (lowPass[k]/cabsInput);
fft_gain[k] = 1.0 - noiseRatio;
minNR = MIN_NR*pOverX;
break;
case 3:
fft_gain[k] = 1.0 - (alpha*(minM[k])/lowPass[k]);
minNR = MIN_NR*(alpha*(minM[k])/lowPass[k]);
break;
case 4:
fft_gain[k] = 1.0 - (alpha*(minM[k])/lowPass[k]);
minNR = MIN_NR;
break;
default:
fft_gain[k] = 1.0 - (noiseRatio);
minNR = MIN_NR;
break;
}
}
else if (enhancement4or5 == 5){
switch(enhancement5){
case 1:
// tmpResult = ((alpha*alpha*(minM[k])*(minM[k]))/cabs2Input);
tmpResult = pureNoiseRatio*pureNoiseRatio;
fft_gain[k] = sqrt(1.0 - tmpResult);
minNR = MIN_NR*tmpResult;
break;
case 2:
//fft_gain[k] = sqrt(1.0 - ((alpha*alpha*(minM[k])*(minM[k]))/cabs2Input));
fft_gain[k] = sqrt(1.0 - ((lowPass[k]/cabsInput)*(lowPass[k]/cabsInput)) );
minNR = MIN_NR*(lowPass[k]/cabsInput);
break;
case 3:
tmpResult = ((minM[k]*minM[k])/(lowPass[k]*lowPass[k]));
fft_gain[k] = sqrt(1.0 - tmpResult);
minNR = MIN_NR*tmpResult;
break;
case 4:
fft_gain[k] = sqrt(1.0 - ((minM[k]*minM[k])/(lowPass[k]*lowPass[k])));
minNR = MIN_NR;
break;
default:
fft_gain[k] = sqrt(1.0 - (minM[k]*minM[k])/cabs2Input);
minNR = MIN_NR;
break;
}
}
else {
fft_gain[k] = 1.0 - noiseRatio;
minNR = MIN_NR;
}
//vanilla g(w) which is the gain for each frequency
fft_gain[k] = maxOfFloats(minNR,fft_gain[k]); //where minNR is lambda in note,
//************* ENHANCEMENT 8 ***************//
if (enhancement8==1) {
autoRecent3Frames[k].s[2] = autoRecent3Frames[k].s[1];
autoRecent3Frames[k].s[1] = autoRecent3Frames[k].s[0];
autoRecent3Frames[k].s[0] = rmul(fft_gain[k], fft_input[k]);
if ( prevNoiseRatio > noiseRatioThresh) {
tmpMin3Frames = autoRecent3Frames[k].s[0];
if (cabs(tmpMin3Frames) > cabs(autoRecent3Frames[k].s[1]))
tmpMin3Frames = autoRecent3Frames[k].s[1];
if (cabs(tmpMin3Frames) > cabs(autoRecent3Frames[k].s[2]))
tmpMin3Frames = autoRecent3Frames[k].s[2];
fft_input[k] = tmpMin3Frames;
}
else {
fft_input[k] = autoRecent3Frames[k].s[1];
}
}
//enhancement 8 off
else{
fft_input[k] = rmul(fft_gain[k], fft_input[k]);
}
if (k!=0 || k!=NFREQ-1)
fft_input[FFTLEN-k] = conjg(fft_input[k]); //symmetric copy
}//end of giantfor loop iterating over frequencies
ifft(FFTLEN,fft_input); //TODO ask about the fft and why its half, and what do we do with the other empty array size.
if (original == 1) {
for (k=0;k<FFTLEN;k++) //loop over the current frame
outFrame[k] = inFrame[k];// copy input straight into output
}
else{
for (k=0;k<FFTLEN;k++){ //loop over the current frame
outFrame[k] = (fft_input[k].r);// copy input straight into output
}
}
/********************************************************************************/
/* multiply outFrame by output window and overlap-add into output buffer */
m=io_ptr0;//start from current io_ptr
for (k=0;k<(FFTLEN-FRAMEINC);k++)
{ /* this loop adds into outBuffer */
outBuffer[m] = outBuffer[m]+outFrame[k]*outWin[k];
if (++m >= CIRCBUFFLEN) m=0; /* wrap if required */
}
for (;k<FFTLEN;k++)
{
outBuffer[m] = outFrame[k]*outWin[k]; /* this loop over-writes outBuffer */
m++;
}
}
complex cinvs(complex a) {
complex dmltp(float, complex);
complex conjg(complex a);
return(dmltp(1./(a.x*a.x+a.y*a.y), conjg(a)));
}
void do_minphdec(float *tr,int nt, float *filter,int fnl,int fnr,float prw)
{
float *rtr;
float *rtx;
complex *f;
complex *w;
complex a;
int iamp;
float amp;
float ampm=-1.0e+20;
float amps;
float *am;
float *ph;
float mean=0.0;
float sum=0.0;
int nfftc;
int nf;
int i,j; /* counter */
float snfftc;
/* Set up pfa fft */
nfftc = npfao(nt,LOOKFAC*nt);
if (nfftc >= SU_NFLTS || nfftc >= PFA_MAX)
err("Padded nt=%d--too big", nfftc);
nf = nfftc/2 + 1;
snfftc=1.0/nfftc;
rtr = ealloc1float(nfftc);
rtx = ealloc1float(nf);
f = ealloc1complex(nfftc);
w = ealloc1complex(nfftc);
am = ealloc1float(nf);
ph = ealloc1float(nf);
/* clean the arrays */
memset( (void *) w, (int) '\0', nfftc*sizeof(complex));
memset( (void *) rtr, (int) '\0', nfftc*FSIZE);
/* Cross correlation */
xcor(nt,0,tr,nt,0,tr,nf,0,rtr);
/* FFT */
pfarc(1, nfftc,rtr,w);
/* stabilize */
for(i=0;i<nf;i++) {
am[i] += am[i]*prw;
}
/* Normalize */
for(i=0;i<nf;i++) {
a=w[i];
am[i]= sqrt(a.r*a.r+a.i*a.i);
sum += am[i];
if(am[i]!=0) ph[i] = atan2(a.i,a.r);
else ph[i]=0;
}
sum *= 1.0/nf;
sum = 1.0/sum;
sscal(nf,sum,am,1);
/* Smooth the apmlitude spectra */
if(fnl!=0) conv (fnl+fnr+1,-fnl,filter,nf,0,am,nf,0,am);
fprintf(stderr," %f\n",sum);
for(i=0;i<nf;i++) {
w[i].r = am[i]*cos(ph[i]);
w[i].i = am[i]*sin(ph[i]);
}
for(i=nf,j=nf-1;i<nfftc;i++,j--) {
w[i].r = am[j]*cos(ph[j]);
w[i].i = am[j]*sin(ph[j]);
}
/* log spectra */
for (i = 0; i < nfftc; ++i) w[i] =
crmul(clog(cmul(w[i],conjg(w[i]))),0.5);
/* Hilbert transform */
pfacc(-1,nfftc,w);
for (i=0; i<nfftc; ++i) {
w[i].r *=snfftc;
w[i].i *=snfftc;
}
for(i=1;i<nfftc/2;i++) w[i] = cadd(w[i],w[i]);
for(i=nfftc/2;i<nfftc;i++) w[i] = cmplx(0,0);
pfacc(1,nfftc,w);
/* end of Hilbert transform */
/* exponentiate */
for(i=0;i<nfftc;i++) w[i] = cexp(w[i]);
/* inverse filter */
for(i=0;i<nfftc;i++) f[i] = cdiv(cmplx(1.0,0),w[i]);
/* Load trace into tr (zero-padded) */
memset( (void *) w, (int) '\0',nfftc*sizeof(complex));
for(i=0;i<nt;i++) w[i].r = tr[i];
/* Trace to frequency domain */
pfacc(1,nfftc,w);
/* apply filter */
for(i=0;i<nfftc;i++) w[i] = cmul(w[i],f[i]);
/* Time domain */
pfacr(-1, nfftc,w,rtr);
for(i=0;i<nt;i++) rtr[i] *=snfftc;
memcpy( (void *) tr, (const void *) rtr, nt*FSIZE);
free1float(rtr);
free1float(am);
free1float(ph);
free1complex(f);
free1complex(w);
}
void fdmig( complex **cp, int nx, int nw, float *v,float fw,float
dw,float dz,float dx,float dt,float vc,int dip)
{
int iw,ix;
float *p,*s1,*s2,w,coefa,coefb,v1,vn,trick=0.1;
complex cp2,cp3,cpnm1,cpnm2;
complex a1,a2,b1,b2;
complex endl,endr;
complex *data,*d,*a,*b,*c;
p=alloc1float(nx);
s1=alloc1float(nx);
s2=alloc1float(nx);
data=alloc1complex(nx);
d=alloc1complex(nx);
a=alloc1complex(nx);
b=alloc1complex(nx);
c=alloc1complex(nx);
for(ix=0;ix<nx;ix++){
p[ix]=vc/v[ix];
p[ix]=(p[ix]*p[ix]+p[ix]+1.0);
}
if(dip!=65){
coefa=0.5;coefb=0.25;
} else {
coefa=0.4784689;
coefb=0.37607656;
}
v1=v[0];
vn=v[nx-1];
for(iw=0,w=fw;iw<nw;iw++,w+=dw){
if(fabs(w)<=1.0e-10)w=1.0e-10/dt;
for(ix=0;ix<nx;ix++){
s1[ix]=(v[ix]*v[ix])*p[ix]*coefb/(dx*dx*w*w)+trick;
s2[ix]=-(1-vc/v[ix])*v[ix]*dz*coefa/(w*dx*dx)*0.5;
}
for(ix=0;ix<nx;ix++){
data[ix]=cp[iw][ix];
}
cp2=data[1];
cp3=data[2];
cpnm1=data[nx-2];
cpnm2=data[nx-3];
a1=crmul(cmul(cp2,conjg(cp3)),2.0);
b1=cadd(cmul(cp2,conjg(cp2)),cmul(cp3,conjg(cp3)));
if(b1.r==0.0 && b1.i==0.0)
a1=cwp_cexp(cmplx(0.0,-w*dx*0.5/v1));
else
a1=cdiv(a1,b1);
if(a1.i>0.0)
a1=cwp_cexp(cmplx(0.0,-w*dx*0.5/v1));
a2=crmul(cmul(cpnm1,conjg(cpnm2)),2.0);
b2=cadd(cmul(cpnm1,conjg(cpnm1)),cmul(cpnm2,conjg(cpnm2)));
if(b2.r==0.0 && b2.i==0.0)
a2=cwp_cexp(cmplx(0.0,-w*dx*0.5/vn));
else
a2=cdiv(a2,b2);
if(a2.i>0.0)
a2=cwp_cexp(cmplx(0.0,-w*dx*0.5/vn));
for(ix=0;ix<nx;ix++){
a[ix]=cmplx(s1[ix],s2[ix]);
b[ix]=cmplx(1.0-2.0*s1[ix],-2.0*s2[ix]);
}
for(ix=1;ix<nx-1;ix++){
d[ix]=cadd(cadd(cmul(data[ix+1],a[ix+1]),
cmul(data[ix-1],a[ix-1])),
cmul(data[ix],b[ix]));
}
d[0]=cadd(cmul(cadd(b[0],cmul(a[0],a1)),
data[0]),cmul(data[1],a[1]));
d[nx-1]=cadd(cmul(cadd(b[nx-1],
cmul(a[nx-1],a2)),data[nx-1]),
cmul(data[nx-2],a[nx-2]));
for(ix=0;ix<nx;ix++){
data[ix]=cmplx(s1[ix],-s2[ix]);
b[ix]=cmplx(1.0-2.0*s1[ix],2.0*s2[ix]);
}
endl=cadd(b[0],cmul(data[0],a1));
endr=cadd(b[nx-1],cmul(data[nx-1],a2));
for(ix=1;ix<nx-1;ix++){
a[ix]=data[ix+1];
c[ix]=data[ix-1];
}
a[0]=data[1];
c[nx-1]=data[nx-2];
retris(data,a,c,b,endl,endr,nx,d);
for(ix=0;ix<nx;ix++){
cp[iw][ix]=data[ix];
}
}
free1complex(data);
free1float(p);
free1complex(d);
free1complex(b);
free1complex(c);
free1complex(a);
free1float(s1);
free1float(s2);
return;
}
