void fftfilt_free(struct fftfilt *s)
{
if (s) {
fft_free(s->fft);
fft_free(s->ift);
fft_free(s->tmpfft);
g_free(s->ovlbuf);
g_free(s->filter);
g_free(s);
}
}
static void fft_linop_free(const linop_data_t* _data)
{
const struct fft_linop_s* data = CAST_DOWN(fft_linop_s, _data);
fft_free(data->frw);
fft_free(data->adj);
free(data->dims);
free(data->strs);
free((void*)data);
}
void end_ncurses(void)
{
int index = 0;
for(index=0; index<window_history_n; index++)
{
free(slow_history[index]);
free(fast_history[index]);
}
free(slow_history);
free(fast_history);
if (w_stats)
{
delwin(w_stats);
delwin(w_line1);
delwin(w_slow);
delwin(w_line2);
delwin(w_fast);
}
endwin();
free(history);
free(history_set);
#ifdef FW
fft_free();
fft_stop();
#endif
free(history_temp);
free(history_fft_phase);
free(history_fft_magn);
}
static void create_filter(struct eq_data *eq, unsigned size_log2,
struct eq_gain *gains, unsigned num_gains, double beta)
{
int i;
int half_block_size = eq->block_size >> 1;
double window_mod = 1.0 / kaiser_window(0.0, beta);
fft_t *fft = fft_new(size_log2);
float *time_filter = (float*)calloc(eq->block_size * 2, sizeof(*time_filter));
if (!fft || !time_filter)
goto end;
// Make sure bands are in correct order.
qsort(gains, num_gains, sizeof(*gains), gains_cmp);
// Compute desired filter response.
generate_response(eq->filter, gains, num_gains, half_block_size);
// Get equivalent time-domain filter.
fft_process_inverse(fft, time_filter, eq->filter, 1);
// ifftshift() to create the correct linear phase filter.
// The filter response was designed with zero phase, which won't work unless we compensate
// for the repeating property of the FFT here by flipping left and right blocks.
for (i = 0; i < half_block_size; i++)
{
float tmp = time_filter[i + half_block_size];
time_filter[i + half_block_size] = time_filter[i];
time_filter[i] = tmp;
}
// Apply a window to smooth out the frequency repsonse.
for (i = 0; i < (int)eq->block_size; i++)
{
// Kaiser window.
double phase = (double)i / (eq->block_size - 1);
phase = 2.0 * (phase - 0.5);
time_filter[i] *= window_mod * kaiser_window(phase, beta);
}
// Debugging.
#if 0
FILE *file = fopen("/tmp/test.txt", "w");
if (file)
{
for (i = 0; i < (int)eq->block_size; i++)
fprintf(file, "%.6f\n", time_filter[i]);
fclose(file);
}
#endif
// Padded FFT to create our FFT filter.
fft_process_forward(eq->fft, eq->filter, time_filter, 1);
end:
fft_free(fft);
free(time_filter);
}
static void eq_free(void *data)
{
struct eq_data *eq = (struct eq_data*)data;
if (!eq)
return;
fft_free(eq->fft);
free(eq->save);
free(eq->block);
free(eq->fftblock);
free(eq->filter);
free(eq);
}
static void dominoex_free(struct dominoex *s)
{
if (s) {
fft_free(s->fft);
sfft_free(s->sfft);
filter_free(s->hilbert);
g_free(s->pipe);
filter_free(s->filt);
g_free(s);
}
}
void fwiener( TrformStr *TrPtr, Image *nf, Image *psf, double gamma , double frame)
{
int dims[2], i, k, dim, prog=0, delta = 100/27;
double **d1 = NULL, **d2 = NULL, **d3 = NULL, **d4 = NULL; // double arrays
char percent[25];
dims[0] = TrPtr->src->width;
dims[1] = TrPtr->src->height;
dim = TrPtr->src->width * TrPtr->src->height;
Progress( _initProgress, "Wiener Filter" );
d1 = (double**)mymalloc( dim * sizeof( double ) );
d2 = (double**)mymalloc( dim * sizeof( double ) );
d3 = (double**)mymalloc( dim * sizeof( double ) );
d4 = (double**)mymalloc( dim * sizeof( double ) );
if( d1 == NULL || d2 == NULL || d3 == NULL || d4 == NULL )
{
PrintError("Not enough memory");
TrPtr->success = 0;
goto _fwiener_exit;
}
for( i=0; i<3; i++ )
{
register double x,y,si,sn,rden,a,b;
UPDATE_PROGRESS_WIENER
if( makeDoubleImage ( nf, *d1, *d2, i, TrPtr->gamma ) != 0 )
{
PrintError("Could not make Real-Version of image");
TrPtr->success = 0;
goto _fwiener_exit;
}
fftn (2, dims, *d1, *d2, 1, 1.0 ); // Noise filtered image
UPDATE_PROGRESS_WIENER
makeDoubleDiffImage ( TrPtr->src, nf, *d3, *d4, i );
fftn (2, dims, *d3, *d4, 1, 1.0 ); // Noise in image
UPDATE_PROGRESS_WIENER
for( k= 0; k<dim; k++)
{
x = (*d1)[k]; y = (*d2)[k];
(*d1)[k] = x * x + y * y; // S_ii
x = (*d3)[k]; y = (*d4)[k];
(*d2)[k] = x * x + y * y; // S_nn
}
UPDATE_PROGRESS_WIENER
makePSF( TrPtr->src->width, TrPtr->src->height, psf, *d3, *d4, i, -1 );
fftn (2, dims, *d3, *d4, 1, 1.0 ); // H(w1,w2)
UPDATE_PROGRESS_WIENER
for( k= 0; k<dim; k++)
{
x = (*d3)[k]; y = (*d4)[k];
si = (*d1)[k];
sn = (*d2)[k];
if( si == 0.0 )
{
(*d1)[k] = 0.0;
(*d2)[k] = 0.0;
}
else
{
rden = x*x +y*y + gamma * sn/si;
(*d1)[k] = x / rden ;
(*d2)[k] = y / rden ;
}
}
UPDATE_PROGRESS_WIENER
if( makeDoubleImage ( TrPtr->src, *d3, *d4, i, TrPtr->gamma ) != 0 )
{
PrintError("Could not make Real-Version of image");
TrPtr->success = 0;
goto _fwiener_exit;
}
if( frame > 0.0)
windowFunction( *d3, TrPtr->src->width, TrPtr->src->height, frame );
fftn (2, dims, *d3, *d4, 1, 1.0 ); // v(w1,w2)
UPDATE_PROGRESS_WIENER
for( k= 0; k<dim; k++)
{
x = (*d3)[k]; y = (*d4)[k];
a = (*d1)[k]; b = (*d2)[k];
(*d1)[k] = x*a - y*b;
(*d2)[k] = x*b + y*a;
}
UPDATE_PROGRESS_WIENER
fftn (2, dims, *d1, *d2, -1, -1.0 ); // backward transform;
// invWindowFunction( *d1, TrPtr->src->width, TrPtr->src->height, 25.0);
UPDATE_PROGRESS_WIENER
makeUcharImage ( TrPtr->dest, *d1, i );
}
TrPtr->success = 1;
_fwiener_exit:
Progress( _disposeProgress, percent );
fft_free();
if( d1 != NULL ) myfree( (void**)d1 );
if( d2 != NULL ) myfree( (void**)d2 );
if( d3 != NULL ) myfree( (void**)d3 );
if( d4 != NULL ) myfree( (void**)d4 );
}
/*
* singleton's mixed radix routine
*
* could move allocation out to fftn(), but leave it here so that it's
* possible to make this a standalone function
*/
static int
FFTRADIX (REAL Re [],
REAL Im [],
unsigned int nTotal,
unsigned int nPass,
unsigned int nSpan,
int iSign,
unsigned int maxFactors,
unsigned int maxPerm)
{
int ii, nFactor, kspan, ispan, inc;
int j, jc, jf, jj, k, k1, k3, kk, kt, nn, ns, nt;
REAL radf;
REAL c1, c2, c3, cd;
REAL s1, s2, s3, sd;
REAL * Rtmp = NULL; /* temp space for real part*/
REAL * Itmp = NULL; /* temp space for imaginary part */
REAL * Cos = NULL; /* Cosine values */
REAL * Sin = NULL; /* Sine values */
#ifndef FFT_RADIX4
REAL s60 = SIN60; /* sin(60 deg) */
REAL s72 = SIN72; /* sin(72 deg) */
REAL c72 = COS72; /* cos(72 deg) */
#endif
REAL pi2 = M_PI; /* use PI first, 2 PI later */
/* gcc complains about k3 being uninitialized, but I can't find out where
* or why ... it looks okay to me.
*
* initialize to make gcc happy
*/
k3 = 0;
/* gcc complains about c2, c3, s2,s3 being uninitialized, but they're
* only used for the radix 4 case and only AFTER the (s1 == 0.0) pass
* through the loop at which point they will have been calculated.
*
* initialize to make gcc happy
*/
c2 = c3 = s2 = s3 = 0.0;
/* Parameter adjustments, was fortran so fix zero-offset */
Re--;
Im--;
if (nPass < 2)
return 0;
/* allocate storage */
if (SpaceAlloced < maxFactors * sizeof (REAL))
{
#ifdef SUN_BROKEN_REALLOC
if (!SpaceAlloced) /* first time */
{
SpaceAlloced = maxFactors * sizeof (REAL);
Tmp0 = malloc (SpaceAlloced);
Tmp1 = malloc (SpaceAlloced);
Tmp2 = malloc (SpaceAlloced);
Tmp3 = malloc (SpaceAlloced);
}
else
{
#endif
SpaceAlloced = maxFactors * sizeof (REAL);
Tmp0 = realloc (Tmp0, SpaceAlloced);
Tmp1 = realloc (Tmp1, SpaceAlloced);
Tmp2 = realloc (Tmp2, SpaceAlloced);
Tmp3 = realloc (Tmp3, SpaceAlloced);
#ifdef SUN_BROKEN_REALLOC
}
#endif
}
else
{
/* allow full use of alloc'd space */
maxFactors = SpaceAlloced / sizeof (REAL);
}
if (MaxPermAlloced < (size_t)maxPerm)
{
#ifdef SUN_BROKEN_REALLOC
if (!MaxPermAlloced) /* first time */
Perm = malloc (maxPerm * sizeof(int));
else
#endif
Perm = realloc (Perm, maxPerm * sizeof(int));
MaxPermAlloced = maxPerm;
}
else
{
/* allow full use of alloc'd space */
maxPerm = MaxPermAlloced;
}
if (!Tmp0 || !Tmp1 || !Tmp2 || !Tmp3 || !Perm) goto Memory_Error;
/* assign pointers */
Rtmp = (REAL *) Tmp0;
Itmp = (REAL *) Tmp1;
Cos = (REAL *) Tmp2;
Sin = (REAL *) Tmp3;
/*
* Function Body
*/
inc = iSign;
if (iSign < 0)
{
#ifndef FFT_RADIX4
s60 = -s60;
s72 = -s72;
#endif
pi2 = -pi2;
inc = -inc; /* absolute value */
}
/* adjust for strange increments */
nt = inc * nTotal;
ns = inc * nSpan;
kspan = ns;
nn = nt - inc;
jc = ns / nPass;
radf = pi2 * (double) jc;
pi2 *= 2.0; /* use 2 PI from here on */
ii = 0;
jf = 0;
/* determine the factors of n */
nFactor = factorize (nPass, &kt);
/* test that nFactors is in range */
if (nFactor > NFACTOR)
{
fprintf (stderr, "Error: " FFTRADIXS "() - exceeded number of factors\n");
goto Memory_Error;
}
/* compute fourier transform */
for (;;) {
sd = radf / (double) kspan;
cd = sin (sd);
cd = 2.0 * cd * cd;
sd = sin (sd + sd);
kk = 1;
ii++;
switch (factor [ii - 1]) {
case 2:
/* transform for factor of 2 (including rotation factor) */
kspan /= 2;
k1 = kspan + 2;
do {
do {
REAL tmpr;
REAL tmpi;
int k2;
k2 = kk + kspan;
tmpr = Re_Data (k2);
tmpi = Im_Data (k2);
Re_Data (k2) = Re_Data (kk) - tmpr;
Im_Data (k2) = Im_Data (kk) - tmpi;
Re_Data (kk) += tmpr;
Im_Data (kk) += tmpi;
kk = k2 + kspan;
} while (kk <= nn);
kk -= nn;
} while (kk <= jc);
if (kk > kspan)
goto Permute_Results; /* exit infinite loop */
do {
int k2;
c1 = 1.0 - cd;
s1 = sd;
do {
REAL tmp;
do {
do {
REAL tmpr;
REAL tmpi;
k2 = kk + kspan;
tmpr = Re_Data (kk) - Re_Data (k2);
tmpi = Im_Data (kk) - Im_Data (k2);
Re_Data (kk) += Re_Data (k2);
Im_Data (kk) += Im_Data (k2);
Re_Data (k2) = c1 * tmpr - s1 * tmpi;
Im_Data (k2) = s1 * tmpr + c1 * tmpi;
kk = k2 + kspan;
} while (kk < nt);
k2 = kk - nt;
c1 = -c1;
kk = k1 - k2;
} while (kk > k2);
tmp = c1 - (cd * c1 + sd * s1);
s1 = sd * c1 - cd * s1 + s1;
c1 = 2.0 - (tmp * tmp + s1 * s1);
s1 *= c1;
c1 *= tmp;
kk += jc;
} while (kk < k2);
k1 += (inc + inc);
kk = (k1 - kspan) / 2 + jc;
} while (kk <= jc + jc);
break;
case 4: /* transform for factor of 4 */
ispan = kspan;
kspan /= 4;
do {
c1 = 1.0;
s1 = 0.0;
do {
do {
REAL ajm, ajp, akm, akp;
REAL bjm, bjp, bkm, bkp;
int k2;
k1 = kk + kspan;
k2 = k1 + kspan;
k3 = k2 + kspan;
akp = Re_Data (kk) + Re_Data (k2);
akm = Re_Data (kk) - Re_Data (k2);
ajp = Re_Data (k1) + Re_Data (k3);
ajm = Re_Data (k1) - Re_Data (k3);
bkp = Im_Data (kk) + Im_Data (k2);
bkm = Im_Data (kk) - Im_Data (k2);
bjp = Im_Data (k1) + Im_Data (k3);
bjm = Im_Data (k1) - Im_Data (k3);
Re_Data (kk) = akp + ajp;
Im_Data (kk) = bkp + bjp;
ajp = akp - ajp;
bjp = bkp - bjp;
if (iSign < 0)
{
akp = akm + bjm;
bkp = bkm - ajm;
akm -= bjm;
bkm += ajm;
}
else
{
akp = akm - bjm;
bkp = bkm + ajm;
akm += bjm;
bkm -= ajm;
}
/* avoid useless multiplies */
if (s1 == 0.0)
{
Re_Data (k1) = akp;
Re_Data (k2) = ajp;
Re_Data (k3) = akm;
Im_Data (k1) = bkp;
Im_Data (k2) = bjp;
Im_Data (k3) = bkm;
}
else
{
Re_Data (k1) = akp * c1 - bkp * s1;
Re_Data (k2) = ajp * c2 - bjp * s2;
Re_Data (k3) = akm * c3 - bkm * s3;
Im_Data (k1) = akp * s1 + bkp * c1;
Im_Data (k2) = ajp * s2 + bjp * c2;
Im_Data (k3) = akm * s3 + bkm * c3;
}
kk = k3 + kspan;
} while (kk <= nt);
c2 = c1 - (cd * c1 + sd * s1);
s1 = sd * c1 - cd * s1 + s1;
c1 = 2.0 - (c2 * c2 + s1 * s1);
s1 *= c1;
c1 *= c2;
/* values of c2, c3, s2, s3 that will get used next time */
c2 = c1 * c1 - s1 * s1;
s2 = 2.0 * c1 * s1;
c3 = c2 * c1 - s2 * s1;
s3 = c2 * s1 + s2 * c1;
kk = kk - nt + jc;
} while (kk <= kspan);
kk = kk - kspan + inc;
} while (kk <= jc);
if (kspan == jc)
goto Permute_Results; /* exit infinite loop */
break;
default:
/* transform for odd factors */
#ifdef FFT_RADIX4
fprintf (stderr, "Error: " FFTRADIXS "(): compiled for radix 2/4 only\n");
fft_free (); /* free-up memory */
return -1;
break;
#else /* FFT_RADIX4 */
ispan = kspan;
k = factor [ii - 1];
kspan /= factor [ii - 1];
switch (factor [ii - 1]) {
case 3: /* transform for factor of 3 (optional code) */
do {
do {
REAL aj, tmpr;
REAL bj, tmpi;
int k2;
k1 = kk + kspan;
k2 = k1 + kspan;
tmpr = Re_Data (kk);
tmpi = Im_Data (kk);
aj = Re_Data (k1) + Re_Data (k2);
bj = Im_Data (k1) + Im_Data (k2);
Re_Data (kk) = tmpr + aj;
Im_Data (kk) = tmpi + bj;
tmpr -= 0.5 * aj;
tmpi -= 0.5 * bj;
aj = (Re_Data (k1) - Re_Data (k2)) * s60;
bj = (Im_Data (k1) - Im_Data (k2)) * s60;
Re_Data (k1) = tmpr - bj;
Re_Data (k2) = tmpr + bj;
Im_Data (k1) = tmpi + aj;
Im_Data (k2) = tmpi - aj;
kk = k2 + kspan;
} while (kk < nn);
kk -= nn;
} while (kk <= kspan);
break;
case 5: /* transform for factor of 5 (optional code) */
c2 = c72 * c72 - s72 * s72;
s2 = 2.0 * c72 * s72;
do {
do {
REAL aa, aj, ak, ajm, ajp, akm, akp;
REAL bb, bj, bk, bjm, bjp, bkm, bkp;
int k2, k4;
k1 = kk + kspan;
k2 = k1 + kspan;
k3 = k2 + kspan;
k4 = k3 + kspan;
akp = Re_Data (k1) + Re_Data (k4);
akm = Re_Data (k1) - Re_Data (k4);
bkp = Im_Data (k1) + Im_Data (k4);
bkm = Im_Data (k1) - Im_Data (k4);
ajp = Re_Data (k2) + Re_Data (k3);
ajm = Re_Data (k2) - Re_Data (k3);
bjp = Im_Data (k2) + Im_Data (k3);
bjm = Im_Data (k2) - Im_Data (k3);
aa = Re_Data (kk);
bb = Im_Data (kk);
Re_Data (kk) = aa + akp + ajp;
Im_Data (kk) = bb + bkp + bjp;
ak = akp * c72 + ajp * c2 + aa;
bk = bkp * c72 + bjp * c2 + bb;
aj = akm * s72 + ajm * s2;
bj = bkm * s72 + bjm * s2;
Re_Data (k1) = ak - bj;
Re_Data (k4) = ak + bj;
Im_Data (k1) = bk + aj;
Im_Data (k4) = bk - aj;
ak = akp * c2 + ajp * c72 + aa;
bk = bkp * c2 + bjp * c72 + bb;
aj = akm * s2 - ajm * s72;
bj = bkm * s2 - bjm * s72;
Re_Data (k2) = ak - bj;
Re_Data (k3) = ak + bj;
Im_Data (k2) = bk + aj;
Im_Data (k3) = bk - aj;
kk = k4 + kspan;
} while (kk < nn);
kk -= nn;
} while (kk <= kspan);
break;
default:
k = factor [ii - 1];
if (jf != k)
{
jf = k;
s1 = pi2 / (double) jf;
c1 = cos (s1);
s1 = sin (s1);
if (jf > maxFactors)
goto Memory_Error;
Cos [jf - 1] = 1.0;
Sin [jf - 1] = 0.0;
j = 1;
do {
Cos [j - 1] = Cos [k - 1] * c1 + Sin [k - 1] * s1;
Sin [j - 1] = Cos [k - 1] * s1 - Sin [k - 1] * c1;
k--;
Cos [k - 1] = Cos [j - 1];
Sin [k - 1] = -Sin [j - 1];
j++;
} while (j < k);
}
do {
do {
REAL aa, ak;
REAL bb, bk;
int k2;
aa = ak = Re_Data (kk);
bb = bk = Im_Data (kk);
k1 = kk;
k2 = kk + ispan;
j = 1;
k1 += kspan;
do {
k2 -= kspan;
Rtmp [j] = Re_Data (k1) + Re_Data (k2);
ak += Rtmp [j];
Itmp [j] = Im_Data (k1) + Im_Data (k2);
bk += Itmp [j];
j++;
Rtmp [j] = Re_Data (k1) - Re_Data (k2);
Itmp [j] = Im_Data (k1) - Im_Data (k2);
j++;
k1 += kspan;
} while (k1 < k2);
Re_Data (kk) = ak;
Im_Data (kk) = bk;
k1 = kk;
k2 = kk + ispan;
j = 1;
do {
REAL aj = 0.0;
REAL bj = 0.0;
k1 += kspan;
k2 -= kspan;
jj = j;
ak = aa;
bk = bb;
k = 1;
do {
ak += Rtmp [k] * Cos [jj - 1];
bk += Itmp [k] * Cos [jj - 1];
k++;
aj += Rtmp [k] * Sin [jj - 1];
bj += Itmp [k] * Sin [jj - 1];
k++;
jj += j;
if (jj > jf)
jj -= jf;
} while (k < jf);
k = jf - j;
Re_Data (k1) = ak - bj;
Im_Data (k1) = bk + aj;
Re_Data (k2) = ak + bj;
Im_Data (k2) = bk - aj;
j++;
} while (j < k);
kk += ispan;
} while (kk <= nn);
kk -= nn;
} while (kk <= kspan);
break;
}
/* multiply by rotation factor (except for factors of 2 and 4) */
if (ii == nFactor)
goto Permute_Results; /* exit infinite loop */
kk = jc + 1;
do {
c2 = 1.0 - cd;
s1 = sd;
do {
c1 = c2;
s2 = s1;
kk += kspan;
do {
REAL tmp;
do {
REAL ak;
ak = Re_Data (kk);
Re_Data (kk) = c2 * ak - s2 * Im_Data (kk);
Im_Data (kk) = s2 * ak + c2 * Im_Data (kk);
kk += ispan;
} while (kk <= nt);
tmp = s1 * s2;
s2 = s1 * c2 + c1 * s2;
c2 = c1 * c2 - tmp;
kk = kk - nt + kspan;
} while (kk <= ispan);
c2 = c1 - (cd * c1 + sd * s1);
s1 += sd * c1 - cd * s1;
c1 = 2.0 - (c2 * c2 + s1 * s1);
s1 *= c1;
c2 *= c1;
kk = kk - ispan + jc;
} while (kk <= kspan);
kk = kk - kspan + jc + inc;
} while (kk <= jc + jc);
break;
#endif /* FFT_RADIX4 */
}
}
/* permute the results to normal order -- done in two stages */
/* permutation for square factors of n */
Permute_Results:
Perm [0] = ns;
if (kt)
{
int k2;
k = kt + kt + 1;
if (k > nFactor)
k--;
Perm [k] = jc;
j = 1;
do {
Perm [j] = Perm [j - 1] / factor [j - 1];
Perm [k - 1] = Perm [k] * factor [j - 1];
j++;
k--;
} while (j < k);
k3 = Perm [k];
kspan = Perm [1];
kk = jc + 1;
k2 = kspan + 1;
j = 1;
if (nPass != nTotal)
{
/* permutation for multivariate transform */
Permute_Multi:
do {
do {
k = kk + jc;
do {
/* swap
* Re_Data (kk) <> Re_Data (k2)
* Im_Data (kk) <> Im_Data (k2)
*/
REAL tmp;
tmp = Re_Data (kk); Re_Data (kk) = Re_Data (k2); Re_Data (k2) = tmp;
tmp = Im_Data (kk); Im_Data (kk) = Im_Data (k2); Im_Data (k2) = tmp;
kk += inc;
k2 += inc;
} while (kk < k);
kk += (ns - jc);
k2 += (ns - jc);
} while (kk < nt);
k2 = k2 - nt + kspan;
kk = kk - nt + jc;
} while (k2 < ns);
do {
do {
k2 -= Perm [j - 1];
j++;
k2 = Perm [j] + k2;
} while (k2 > Perm [j - 1]);
j = 1;
do {
if (kk < k2)
goto Permute_Multi;
kk += jc;
k2 += kspan;
} while (k2 < ns);
} while (kk < ns);
}
else
{
/* permutation for single-variate transform (optional code) */
Permute_Single:
do {
/* swap
* Re_Data (kk) <> Re_Data (k2)
* Im_Data (kk) <> Im_Data (k2)
*/
REAL t;
t = Re_Data (kk); Re_Data (kk) = Re_Data (k2); Re_Data (k2) = t;
t = Im_Data (kk); Im_Data (kk) = Im_Data (k2); Im_Data (k2) = t;
kk += inc;
k2 += kspan;
} while (k2 < ns);
do {
do {
k2 -= Perm [j - 1];
j++;
k2 = Perm [j] + k2;
} while (k2 > Perm [j - 1]);
j = 1;
do {
if (kk < k2)
goto Permute_Single;
kk += inc;
k2 += kspan;
} while (k2 < ns);
} while (kk < ns);
}
jc = k3;
}
if ((kt << 1) + 1 >= nFactor)
return 0;
ispan = Perm [kt];
/* permutation for square-free factors of n */
j = nFactor - kt;
factor [j] = 1;
do {
factor [j - 1] *= factor [j];
j--;
} while (j != kt);
nn = factor [kt] - 1;
kt++;
if (nn > maxPerm)
goto Memory_Error;
j = jj = 0;
for (;;) {
int k2;
k = kt + 1;
k2 = factor [kt - 1];
kk = factor [k - 1];
j++;
if (j > nn)
break; /* exit infinite loop */
jj += kk;
while (jj >= k2)
{
jj -= k2;
k2 = kk;
kk = factor [k++];
jj += kk;
}
Perm [j - 1] = jj;
}
/* determine the permutation cycles of length greater than 1 */
j = 0;
for (;;) {
do {
kk = Perm [j++];
} while (kk < 0);
if (kk != j)
{
do {
k = kk;
kk = Perm [k - 1];
Perm [k - 1] = -kk;
} while (kk != j);
k3 = kk;
}
else
{
Perm [j - 1] = -j;
if (j == nn)
break; /* exit infinite loop */
}
}
maxFactors *= inc;
/* reorder a and b, following the permutation cycles */
for (;;) {
j = k3 + 1;
nt -= ispan;
ii = nt - inc + 1;
if (nt < 0)
break; /* exit infinite loop */
do {
do {
j--;
} while (Perm [j - 1] < 0);
jj = jc;
do {
int k2;
if (jj < maxFactors) kspan = jj; else kspan = maxFactors;
jj -= kspan;
k = Perm [j - 1];
kk = jc * k + ii + jj;
k1 = kk + kspan;
k2 = 0;
do {
Rtmp [k2] = Re_Data (k1);
Itmp [k2] = Im_Data (k1);
k2++;
k1 -= inc;
} while (k1 != kk);
do {
k1 = kk + kspan;
k2 = k1 - jc * (k + Perm [k - 1]);
k = -Perm [k - 1];
do {
Re_Data (k1) = Re_Data (k2);
Im_Data (k1) = Im_Data (k2);
k1 -= inc;
k2 -= inc;
} while (k1 != kk);
kk = k2;
} while (k != j);
k1 = kk + kspan;
k2 = 0;
do {
Re_Data (k1) = Rtmp [k2];
Im_Data (k1) = Itmp [k2];
k2++;
k1 -= inc;
} while (k1 != kk);
} while (jj);
} while (j != 1);
}
return 0; /* exit point here */
/* alloc or other problem, do some clean-up */
Memory_Error:
fprintf (stderr, "Error: " FFTRADIXS "() - insufficient memory.\n");
fft_free (); /* free-up memory */
return -1;
}
int
FFTN (int ndim,
const unsigned int dims [],
REAL Re [],
REAL Im [],
int iSign,
double scaling)
{
unsigned int nTotal;
unsigned int maxFactors, maxPerm;
/*
* tally the number of elements in the data array
* and determine the number of dimensions
*/
nTotal = 1;
if (ndim)
{
if (dims != NULL)
{
int i;
/* number of dimensions was specified */
for (i = 0; i < ndim; i++)
{
if (dims [i] <= 0) goto Dimension_Error;
nTotal *= dims [i];
}
}
else
nTotal *= ndim;
}
else
{
int i;
/* determine # of dimensions from zero-terminated list */
if (dims == NULL) goto Dimension_Error;
for (ndim = i = 0; dims [i]; i++)
{
if (dims [i] <= 0)
goto Dimension_Error;
nTotal *= dims [i];
ndim++;
}
}
/* determine maximum number of factors and permuations */
#if 1
/*
* follow John Beale's example, just use the largest dimension and don't
* worry about excess allocation. May be someone else will do it?
*/
if (dims != NULL)
{
int i;
for (maxFactors = maxPerm = 1, i = 0; i < ndim; i++)
{
if (dims [i] > maxFactors) maxFactors = dims [i];
if (dims [i] > maxPerm) maxPerm = dims [i];
}
}
else
{
maxFactors = maxPerm = nTotal;
}
#else
/* use the constants used in the original Fortran code */
maxFactors = 23;
maxPerm = 209;
#endif
/* loop over the dimensions: */
if (dims != NULL)
{
unsigned int nSpan = 1;
int i;
for (i = 0; i < ndim; i++)
{
int ret;
nSpan *= dims [i];
ret = FFTRADIX (Re, Im, nTotal, dims [i], nSpan, iSign,
maxFactors, maxPerm);
/* exit, clean-up already done */
if (ret)
return ret;
}
}
else
{
int ret;
ret = FFTRADIX (Re, Im, nTotal, nTotal, nTotal, iSign,
maxFactors, maxPerm);
/* exit, clean-up already done */
if (ret)
return ret;
}
/* Divide through by the normalizing constant: */
if (scaling && scaling != 1.0)
{
size_t ist;
if (iSign < 0) iSign = -iSign;
if (scaling < 0.0)
scaling = (scaling < -1.0) ? sqrt (nTotal) : nTotal;
scaling = 1.0 / scaling; /* multiply is often faster */
for (ist = 0; ist < nTotal; ist += iSign)
{
Re_Data (ist) *= scaling;
Im_Data (ist) *= scaling;
}
}
return 0;
Dimension_Error:
fprintf (stderr, "Error: " FFTNS "() - dimension error\n");
fft_free (); /* free-up memory */
return -1;
}
void fft2(unsigned int D, const long dimensions[D], unsigned long flags, const long ostrides[D], complex float* dst, const long istrides[D], const complex float* src)
{
const struct operator_s* plan = fft_create2(D, dimensions, flags, ostrides, dst, istrides, src, false);
fft_exec(plan, dst, src);
fft_free(plan);
}
void ifft(unsigned int D, const long dimensions[D], unsigned long flags, complex float* dst, const complex float* src)
{
const struct operator_s* plan = fft_create(D, dimensions, flags, dst, src, true);
fft_exec(plan, dst, src);
fft_free(plan);
}
int FMMGetEpsilon_Kottke(const S4_Simulation *S, const S4_Layer *L, const int n, std::complex<double> *Epsilon2, std::complex<double> *Epsilon_inv){
const int n2 = 2*n;
const int *G = S->G;
// Make grid
// Determine size of the grid
int ngrid[2] = {1,1};
for(int i = 0; i < 2; ++i){ // choose grid size
for(int j = 0; j < n; ++j){
if(abs(G[2*j+i]) > ngrid[i]){ ngrid[i] = abs(G[2*j+i]); }
}
if(ngrid[i] < 1){ ngrid[i] = 1; }
ngrid[i] *= S->options.resolution;
ngrid[i] = fft_next_fast_size(ngrid[i]);
}
S4_TRACE("I Subpixel smoothing on %d x %d grid\n", ngrid[0], ngrid[1]);
const int ng2 = ngrid[0]*ngrid[1];
const double ing2 = 1./(double)ng2;
// The grid needs to hold 5 matrix elements: xx,xy,yx,yy,zz
// We actually make 5 different grids to facilitate the fft routines
//std::complex<double> *work = (std::complex<double>*)S4_malloc(sizeof(std::complex<double>)*(6*ng2));
std::complex<double> *work = fft_alloc_complex(6*ng2);
std::complex<double>*fxx = work;
std::complex<double>*fxy = fxx + ng2;
std::complex<double>*fyx = fxy + ng2;
std::complex<double>*fyy = fyx + ng2;
std::complex<double>*fzz = fyy + ng2;
std::complex<double>*Fto = fzz + ng2;
//memset(work, 0, sizeof(std::complex<double>) * 6*ng2);
double *discval = (double*)S4_malloc(sizeof(double)*(L->pattern.nshapes+1));
fft_plan plans[5];
for(int i = 0; i <= 4; ++i){
plans[i] = fft_plan_dft_2d(ngrid, fxx+i*ng2, Fto, 1);
}
int ii[2];
for(ii[0] = 0; ii[0] < ngrid[0]; ++ii[0]){
const int si0 = ii[0] >= ngrid[0]/2 ? ii[0]-ngrid[0]/2 : ii[0]+ngrid[0]/2;
for(ii[1] = 0; ii[1] < ngrid[1]; ++ii[1]){
const int si1 = ii[1] >= ngrid[1]/2 ? ii[1]-ngrid[1]/2 : ii[1]+ngrid[1]/2;
Pattern_DiscretizeCell(&L->pattern, S->Lr, ngrid[0], ngrid[1], ii[0], ii[1], discval);
int nnz = 0;
int imat[2] = {-1,-1};
for(int i = 0; i <= L->pattern.nshapes; ++i){
if(fabs(discval[i]) > 2*std::numeric_limits<double>::epsilon()){
if(0 == nnz){ imat[0] = i; }
else if(1 == nnz){ imat[1] = i; }
++nnz;
}
}
//S4_TRACE("I %d,%d nnz = %d\n", ii[0], ii[1], nnz);
if(nnz < 2){ // just one material
//fprintf(stderr, "%d\t%d\t0\t0\n", ii[0], ii[1]);
const S4_Material *M;
if(0 == imat[0]){
M = &S->material[L->material];
}else{
M = &S->material[L->pattern.shapes[imat[0]-1].tag];
}
if(0 == M->type){
std::complex<double> eps_scalar(M->eps.s[0], M->eps.s[1]);
fxx[si1+si0*ngrid[1]] = eps_scalar;
fxy[si1+si0*ngrid[1]] = 0;
fyx[si1+si0*ngrid[1]] = 0;
fyy[si1+si0*ngrid[1]] = eps_scalar;
fzz[si1+si0*ngrid[1]] = eps_scalar;
}else{
fxx[si1+si0*ngrid[1]] = std::complex<double>(M->eps.abcde[0],M->eps.abcde[1]);
fyy[si1+si0*ngrid[1]] = std::complex<double>(M->eps.abcde[6],M->eps.abcde[7]);
fxy[si1+si0*ngrid[1]] = std::complex<double>(M->eps.abcde[2],M->eps.abcde[3]);
fyx[si1+si0*ngrid[1]] = std::complex<double>(M->eps.abcde[4],M->eps.abcde[5]);
fzz[si1+si0*ngrid[1]] = std::complex<double>(M->eps.abcde[8],M->eps.abcde[9]);
}
}else{
if(imat[1] > imat[0]){
std::swap(imat[0],imat[1]);
}
// imat[0] is the more-contained shape
double nvec[2];
const double nxvec[2] = { ((double)ii[0]+0.5)/(double)ngrid[0]-0.5, ((double)ii[1]+0.5)/(double)ngrid[1]-0.5 };
const double xvec[2] = { // center of current parallelogramic pixel
S->Lr[0]*nxvec[0] + S->Lr[2]*nxvec[1],
S->Lr[1]*nxvec[0] + S->Lr[3]*nxvec[1]
};
shape_get_normal(&(L->pattern.shapes[imat[0]-1]), xvec, nvec);
if(2 == nnz && imat[1] != imat[0] && !(0 == nvec[0] && 0 == nvec[1])){ // use Kottke averaging
//fprintf(stderr, "%d\t%d\t%f\t%f\n", ii[0], ii[1], nvec[0], nvec[1]);
const double fill = discval[imat[0]];
// Get the two tensors
std::complex<double> abcde[2][5];
for(int i = 0; i < 2; ++i){
const S4_Material *M;
if(0 == imat[i]){
M = &S->material[L->material];
}else{
M = &S->material[L->pattern.shapes[imat[i]-1].tag];
}
if(0 == M->type){
std::complex<double> eps_scalar(M->eps.s[0], M->eps.s[1]);
abcde[i][0] = eps_scalar;
abcde[i][1] = 0;
abcde[i][2] = 0;
abcde[i][3] = eps_scalar;
abcde[i][4] = eps_scalar;
}else{
for(int j = 0; j < 4; ++j){
abcde[i][j] = std::complex<double>(M->eps.abcde[2*j+0],M->eps.abcde[2*j+1]);
}
abcde[i][4] = std::complex<double>(M->eps.abcde[8],M->eps.abcde[9]);
}
}
// The zz component is just directly obtained by averaging
fzz[si1+si0*ngrid[1]] = discval[imat[0]] * abcde[0][4] + discval[imat[1]] * abcde[1][4];
// Compute rotated tensors
// abcd1 = Rot^T abcd1 Rot
// Rot = [ nvec[0] -nvec[1] ]
// [ nvec[1] nvec[0] ]
std::complex<double> abcd[2][4];
//fprintf(stderr, " nvec = {%f,%f}, fill = %f\n", nvec[0], nvec[1], fill);
//fprintf(stderr, " abcde[0] = {%f,%f,%f,%f}\n", abcde[0][0].real(), abcde[0][1].real(), abcde[0][2].real(), abcde[0][3].real());
//fprintf(stderr, " abcde[1] = {%f,%f,%f,%f}\n", abcde[1][0].real(), abcde[1][1].real(), abcde[1][2].real(), abcde[1][3].real());
sym2x2rot(abcde[0], nvec, abcd[0]);
sym2x2rot(abcde[1], nvec, abcd[1]);
// Compute the average tau tensor into abcde[0][0-3]
// tau(e__) = [ -1/e11 e12/e11 ]
// [ e21/e11 e22 - e21 e12/e11 ]
abcde[0][0] = fill * (-1./abcd[0][0]) + (1.-fill) * (-1./abcd[1][0]);
abcde[0][1] = fill * (abcd[0][1]/abcd[0][0]) + (1.-fill) * (abcd[1][1]/abcd[1][0]);
abcde[0][2] = fill * (abcd[0][2]/abcd[0][0]) + (1.-fill) * (abcd[1][2]/abcd[1][0]);
abcde[0][3] = fill * (abcd[0][3]-abcd[0][1]*abcd[0][2]/abcd[0][0]) + (1.-fill) * (abcd[1][3]-abcd[1][1]*abcd[1][2]/abcd[1][0]);
// Invert the tau transform into abcd[1]
// invtau(t__) = [ -1/t11 -t12/t11 ]
// [ -t21/t11 t22 - t21 t12/t11 ]
abcd[1][0] = -1./abcde[0][0];
abcd[1][1] = -abcde[0][1]/abcde[0][0];
abcd[1][2] = -abcde[0][2]/abcde[0][0];
abcd[1][3] = abcde[0][3] - abcde[0][1]*abcde[0][2]/abcde[0][0];
//fprintf(stderr, " abcd[1] = {%f,%f,%f,%f}\n", abcd[1][0].real(), abcd[1][1].real(), abcd[1][2].real(), abcd[1][3].real());
// Unrotate abcd[1] into abcd[0]
nvec[1] = -nvec[1];
sym2x2rot(abcd[1], nvec, abcd[0]);
//fprintf(stderr, " abcd[0] = {%f,%f,%f,%f}\n\n", abcd[0][0].real(), abcd[0][1].real(), abcd[0][2].real(), abcd[0][3].real());
fxx[si1+si0*ngrid[1]] = abcd[0][0];
fxy[si1+si0*ngrid[1]] = abcd[0][1];
fyx[si1+si0*ngrid[1]] = abcd[0][2];
fyy[si1+si0*ngrid[1]] = abcd[0][3];
}else{ // too many, just use the area weighting
//fprintf(stderr, "%d\t%d\t3\n", ii[0], ii[1]);
fxx[si1+si0*ngrid[1]] = 0;
fxy[si1+si0*ngrid[1]] = 0;
fyx[si1+si0*ngrid[1]] = 0;
fyy[si1+si0*ngrid[1]] = 0;
fzz[si1+si0*ngrid[1]] = 0;
for(int i = 0; i <= L->pattern.nshapes; ++i){
if(0 == discval[i]){ continue; }
int j = i-1;
const S4_Material *M;
if(-1 == j){
M = &S->material[L->material];
}else{
M = &S->material[L->pattern.shapes[j].tag];
}
if(0 == M->type){
std::complex<double> eps_scalar(M->eps.s[0], M->eps.s[1]);
fxx[si1+si0*ngrid[0]] += discval[i]*eps_scalar;
fyy[si1+si0*ngrid[0]] += discval[i]*eps_scalar;
fzz[si1+si0*ngrid[0]] += discval[i]*eps_scalar;
}else{
std::complex<double> ea(M->eps.abcde[0],M->eps.abcde[1]);
std::complex<double> eb(M->eps.abcde[2],M->eps.abcde[3]);
std::complex<double> ec(M->eps.abcde[4],M->eps.abcde[5]);
std::complex<double> ed(M->eps.abcde[6],M->eps.abcde[7]);
fxx[si1+si0*ngrid[1]] += discval[i]*ea;
fxy[si1+si0*ngrid[1]] += discval[i]*eb;
fyx[si1+si0*ngrid[1]] += discval[i]*ec;
fyy[si1+si0*ngrid[1]] += discval[i]*ed;
fzz[si1+si0*ngrid[1]] += discval[i]*std::complex<double>(M->eps.abcde[8],M->eps.abcde[9]);
}
}
}
}
/*
fprintf(stderr, "%d\t%d\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\n", ii[0], ii[1],
fxx[si1+si0*ngrid[1]].real(), fxx[si1+si0*ngrid[1]].imag(),
fxy[si1+si0*ngrid[1]].real(), fxy[si1+si0*ngrid[1]].imag(),
fyx[si1+si0*ngrid[1]].real(), fyx[si1+si0*ngrid[1]].imag(),
fyy[si1+si0*ngrid[1]].real(), fyy[si1+si0*ngrid[1]].imag(),
fzz[si1+si0*ngrid[1]].real(), fzz[si1+si0*ngrid[1]].imag()
);//*/
}
//fprintf(stderr, "\n");
}
// Make Epsilon_inv first
{
//fprintf(stderr, "fzz1[0] = %f+I %f\n", fzz[0].real(), fzz[0].imag());
//memset(Fto, 0, sizeof(std::complex<double>)*ng2);
fft_plan_exec(plans[4]);
//fprintf(stderr, "fzz2[0] = %f+I %f\n", fzz[0].real(), fzz[0].imag());
//fprintf(stderr, "Fto[0] = %f+I %f\n", Fto[0].real(), Fto[0].imag());
for(int j = 0; j < n; ++j){
for(int i = 0; i < n; ++i){
int f[2] = {G[2*i+0]-G[2*j+0],G[2*i+1]-G[2*j+1]};
if(f[0] < 0){ f[0] += ngrid[0]; }
if(f[1] < 0){ f[1] += ngrid[1]; }
Epsilon2[i+j*n] = ing2 * Fto[f[1]+f[0]*ngrid[1]];
}
}
}
//fprintf(stderr, "Epsilon2[0] = %f+I %f\n", Epsilon2[0].real(), Epsilon2[0].imag());
// Epsilon_inv needs inverting
RNP::TBLAS::SetMatrix<'A'>(n,n, 0.,1., Epsilon_inv,n);
int solve_info;
RNP::LinearSolve<'N'>(n,n, Epsilon2,n, Epsilon_inv,n, &solve_info, NULL);
//fprintf(stderr, "Epsilon_inv[0] = %f+I %f\n", Epsilon_inv[0].real(), Epsilon_inv[0].imag());
// We fill in the quarter blocks of F in Fortran order
for(int w = 0; w < 4; ++w){
int Ecol = (w&1 ? n : 0);
int Erow = (w&2 ? n : 0);
//memset(Fto, 0, sizeof(std::complex<double>)*ng2);
fft_plan_exec(plans[w]);
//fprintf(stderr, "Fto(%d)[0] = %f+I %f\n", w, Fto[0].real(), Fto[0].imag());
for(int j = 0; j < n; ++j){
for(int i = 0; i < n; ++i){
int f[2] = {G[2*i+0]-G[2*j+0],G[2*i+1]-G[2*j+1]};
if(f[0] < 0){ f[0] += ngrid[0]; }
if(f[1] < 0){ f[1] += ngrid[1]; }
Epsilon2[Erow+i+(Ecol+j)*n2] = ing2 * Fto[f[1]+f[0]*ngrid[1]];
}
}
}
//fprintf(stderr, "Epsilon2[0] = %f+I %f\n", Epsilon2[0].real(), Epsilon2[0].imag());
for(int i = 0; i <= 4; ++i){
fft_plan_destroy(plans[i]);
}
S4_free(discval);
//S4_free(work);
fft_free(work);
return 0;
}
void vspectra(void)
{
int i, j, k, kk, num, numm, i4, maxi, r;
int m, mm, blsiz, blsiz2, nsam, n_read;
double avsig, av, max, min, noise, wid;
uint8_t *bufferRead = malloc((NSAM) * sizeof(uint8_t));
static double vspec[NSPEC];
static int wtt[NSPEC];
static double re[NSPEC * 2], am[NSPEC * 2];
double smax;
fftwf_plan p0;
float *reamin0, *reamout0;
blsiz = NSPEC * 2;
blsiz2 = blsiz / 2;
d1.bw = 2.4; // fixed 2.4 for TV dongle 10 MHz for ADC card
if (d1.fbw == 0.0)
d1.fbw = 2.0; // use bandwidth default if not set in srt.cat
d1.f1 = 0.5 - (d1.fbw / d1.bw) * 0.5;
d1.f2 = 0.5 + (d1.fbw / d1.bw) * 0.5;
d1.fc = (d1.f1 + d1.f2) * 0.5;
d1.lofreq = 0; // not used but needs to be set to zero to flag the use of the dongle
d1.efflofreq = d1.freq - d1.bw * 0.5;
if (!d1.fftsim) {
fft_init(blsiz2, &p0, &reamin0, &reamout0);
}
num = d1.nblk; //was 20 // was 100
nsam = NSAM;
d1.nsam = NSAM * num;
avsig = 0;
numm = 0;
smax = 0;
max = -1e99;
min = 1e99;
for (i = 0; i < blsiz2; i++)
vspec[i] = 0.0;
for (k = 0; k < num; k++) {
if (!d1.radiosim)
// Read the raw data from the RTL Dongle
r = rtlsdr_read_sync(dev, bufferRead, nsam, &n_read);
else {
av = 5.0;
if (d1.elnow < 5.0)
av = av * (d1.tsys + d1.tcal) / d1.tsys;
if (strstr(soutrack, "Sun")) {
av = sqrt(d1.eloff * d1.eloff +
d1.azoff * d1.azoff * cos(d1.elnow * PI / 180.0) * cos(d1.elnow * PI / 180.0) +
1e-6);
if (av > d1.beamw)
av = d1.beamw;
av = 5.0 + 25.0 * cos(av * PI * 0.5 / d1.beamw) * cos(av * PI * 0.5 / d1.beamw);
}
for (i = 0; i < nsam; i++)
bufferRead[i] = (uint8_t) (sqrt(av) * gauss() + 127.397 + 0.5 + 0 * sin(2.0 * PI * 0.5 * (i / 2) / d1.bw)); // simulate data
}
// for(i=0;i<nsam;i+=0x10000) printf("%d %f\n",i,(double)(bufferRead[i]-127.397));
for (kk = 0; kk < nsam / blsiz; kk++) {
if (d1.fftsim)
for (i = 0; i < blsiz2; i++) {
re[i] = (double) (bufferRead[2 * i + kk * blsiz] - 127.397);
am[i] = (double) (bufferRead[2 * i + 1 + kk * blsiz] - 127.397);
if (re[i] > smax)
smax = re[i];
} else
for (i = 0; i < blsiz2; i++) {
reamin0[2 * i] = (float) (bufferRead[2 * i + kk * blsiz] - 127.397);
reamin0[2 * i + 1] = (float) (bufferRead[2 * i + 1 + kk * blsiz] - 127.397);
if (reamin0[2 * i] > smax)
smax = reamin0[2 * i];
}
if (d1.fftsim)
Four(re, am, blsiz2);
else {
cfft(&p0);
for (i = 0; i < blsiz2; i++) {
re[i] = reamout0[2 * i];
am[i] = reamout0[2 * i + 1];
}
}
// for(i = 0; i < blsiz2; i++) if(re[i] > max) max=re[i];
// for(i = 0; i < blsiz2; i++) if(re[i] < min) min=re[i];
for (i = 0; i < blsiz2; i++) {
if (i < blsiz2 / 2)
j = i + blsiz2 / 2;
else
j = i - blsiz2 / 2;
vspec[j] += re[i] * re[i] + am[i] * am[i];
numm++;
}
}
}
// printf("max %f min %f\n",max,min);
max = av = 0;
maxi = 0;
for (i = 0; i < blsiz2; i++)
wtt[i] = 1;
if (numm > 0) {
if (d1.nfreq == blsiz2) {
for (i = 0; i < blsiz2; i++) {
if (i > 10)
spec[i] = vspec[i] / (double) numm;
else
spec[i] = 0;
}
} else {
m = blsiz2 / d1.nfreq;
for (i = 0; i < d1.nrfi; i++) {
i4 = (d1.rfi[i] - d1.freq + d1.bw * 0.5) * blsiz2 / d1.bw + 0.5; // index of rfi MHz
wid = 0.5 * d1.rfiwid[i] / (d1.bw / NSPEC);
for (j = -wid; j <= wid; j++)
if ((i4 + j) >= 0 && (i4 + j) < blsiz2)
wtt[i4 + j] = 0;
}
for (j = 0; j < d1.nfreq; j++) {
av = mm = 0;
for (i = j * m - m / 2; i <= j * m + m / 2; i++) {
if (i > 10 && i < blsiz2 && wtt[i]) { // wtt=0 removal of spurs
av += vspec[i] / (double) numm;
if (vspec[i] > max) {
max = vspec[i];
maxi = i;
}
mm++;
}
}
if (mm > 0)
spec[j] = av / mm;
else {
spec[j] = 0;
if (j > 10)
printf("check RFI settings in srt.cat data deleted at %8.3f\n",
j * d1.bw / d1.nfreq + d1.freq - d1.bw * 0.5);
}
}
max = max / (double) numm;
noise = spec[maxi / m] * sqrt(2.0 * blsiz2 / (double) d1.nsam);
if (max > spec[maxi / m] + d1.rfisigma * noise && d1.printout) // rfisigma sigma
printf("check for RFI at %8.4f MHz max %5.0e av %5.0e smax %5.0f %3.0f sigma\n",
maxi * d1.bw / blsiz2 + d1.freq - d1.bw * 0.5, max, spec[maxi / m], smax,
(max - spec[maxi / m]) / noise);
}
}
d1.smax = smax;
if (!d1.fftsim)
fft_free(&p0, &reamin0, &reamout0);
free(bufferRead);
}
static void create_filter(struct eq_data *eq, unsigned size_log2,
struct eq_gain *gains, unsigned num_gains, double beta, const char *filter_path)
{
int i;
int half_block_size = eq->block_size >> 1;
double window_mod = 1.0 / kaiser_window_function(0.0, beta);
fft_t *fft = fft_new(size_log2);
float *time_filter = (float*)calloc(eq->block_size * 2 + 1, sizeof(*time_filter));
if (!fft || !time_filter)
goto end;
/* Make sure bands are in correct order. */
qsort(gains, num_gains, sizeof(*gains), gains_cmp);
/* Compute desired filter response. */
generate_response(eq->filter, gains, num_gains, half_block_size);
/* Get equivalent time-domain filter. */
fft_process_inverse(fft, time_filter, eq->filter, 1);
// ifftshift() to create the correct linear phase filter.
// The filter response was designed with zero phase, which won't work unless we compensate
// for the repeating property of the FFT here by flipping left and right blocks.
for (i = 0; i < half_block_size; i++)
{
float tmp = time_filter[i + half_block_size];
time_filter[i + half_block_size] = time_filter[i];
time_filter[i] = tmp;
}
/* Apply a window to smooth out the frequency repsonse. */
for (i = 0; i < (int)eq->block_size; i++)
{
/* Kaiser window. */
double phase = (double)i / eq->block_size;
phase = 2.0 * (phase - 0.5);
time_filter[i] *= window_mod * kaiser_window_function(phase, beta);
}
/* Debugging. */
if (filter_path)
{
FILE *file = fopen(filter_path, "w");
if (file)
{
for (i = 0; i < (int)eq->block_size - 1; i++)
fprintf(file, "%.8f\n", time_filter[i + 1]);
fclose(file);
}
}
/* Padded FFT to create our FFT filter.
* Make our even-length filter odd by discarding the first coefficient.
* For some interesting reason, this allows us to design an odd-length linear phase filter.
*/
fft_process_forward(eq->fft, eq->filter, time_filter + 1, 1);
end:
fft_free(fft);
free(time_filter);
}
AUD_Int32s denoise_mmse( AUD_Int16s *pInBuf, AUD_Int16s *pOutBuf, AUD_Int32s inLen )
{
Fft_16s *hFft = NULL;
Ifft_16s *hIfft = NULL;
AUD_Window16s *hWin = NULL;
AUD_Int32s frameSize = 512;
AUD_Int32s frameStride = 256;
AUD_Int32s frameOverlap = 256;
AUD_Int32s nFFT = frameSize;
AUD_Int32s nSpecLen = nFFT / 2 + 1;
AUD_Int32s nNoiseFrame = (AUD_Int32s)( ( 0.25 * SAMPLE_RATE - frameSize ) / frameStride + 1 );;
AUD_Int32s i, j, k;
AUD_Int32s cleanLen = 0;
// pre-emphasis
// sig_preemphasis( pInBuf, pInBuf, inLen );
// init hamming module
win16s_init( &hWin, AUD_WIN_HAMM, frameSize, 14 );
AUD_ASSERT( hWin );
// init fft handle
fft_init( &hFft, nFFT, 15 );
AUD_ASSERT( hFft );
// init ifft handle
ifft_init( &hIfft, nFFT, 15 );
AUD_ASSERT( hIfft );
AUD_Int16s *pFrame = (AUD_Int16s*)calloc( frameSize * sizeof(AUD_Int16s), 1 );
AUD_ASSERT( pFrame );
// FFT
AUD_Int32s *pFFTMag = (AUD_Int32s*)calloc( nFFT * sizeof(AUD_Int32s), 1 );
AUD_ASSERT( pFFTMag );
AUD_Int32s *pFFTRe = (AUD_Int32s*)calloc( nFFT * sizeof(AUD_Int32s), 1 );
AUD_ASSERT( pFFTRe );
AUD_Int32s *pFFTIm = (AUD_Int32s*)calloc( nFFT * sizeof(AUD_Int32s), 1 );
AUD_ASSERT( pFFTIm );
AUD_Int32s *pFFTCleanRe = (AUD_Int32s*)calloc( nFFT * sizeof(AUD_Int32s), 1 );
AUD_ASSERT( pFFTCleanRe );
AUD_Int32s *pFFTCleanIm = (AUD_Int32s*)calloc( nFFT * sizeof(AUD_Int32s), 1 );
AUD_ASSERT( pFFTCleanIm );
AUD_Double *pNoiseMean = (AUD_Double*)calloc( nSpecLen * sizeof(AUD_Double), 1 );
AUD_ASSERT( pNoiseMean );
AUD_Int32s *pFFTCleanMag = (AUD_Int32s*)calloc( nSpecLen * sizeof(AUD_Int32s), 1 );
AUD_ASSERT( pFFTCleanMag );
AUD_Int16s *pxOld = (AUD_Int16s*)calloc( frameSize * sizeof(AUD_Int16s), 1 );
AUD_ASSERT( pxOld );
AUD_Int16s *pxClean = (AUD_Int16s*)calloc( frameSize * sizeof(AUD_Int16s), 1 );
AUD_ASSERT( pxClean );
for ( i = 0; (i < nNoiseFrame) && ( (i * frameStride + frameSize) < inLen ); i++ )
{
win16s_calc( hWin, pInBuf + i * frameSize, pFrame );
fft_mag( hFft, pFrame, frameSize, pFFTMag );
for ( j = 0; j < nSpecLen; j++ )
{
pNoiseMean[j] += (AUD_Double)pFFTMag[j];
}
}
// compute noise mean
for ( j = 0; j < nSpecLen; j++ )
{
pNoiseMean[j] /= nNoiseFrame;
}
AUD_Double thres = 3.;
AUD_Double alpha = 2.;
AUD_Double flr = 0.002;
AUD_Double G = 0.9;
AUD_Double snr, beta;
AUD_Double tmp;
AUD_Double sigEnergy, noiseEnergy;
k = 0;
// start processing
for ( i = 0; (i * frameStride + frameSize) < inLen; i++ )
{
win16s_calc( hWin, pInBuf + i * frameStride, pFrame );
fft_calc( hFft, pFrame, frameSize, pFFTRe, pFFTIm );
// compute SNR
sigEnergy = 0.;
noiseEnergy = 0.;
for ( j = 0; j < nSpecLen; j++ )
{
tmp = pFFTRe[j] / 32768. * pFFTRe[j] / 32768. + pFFTIm[j] / 32768. * pFFTIm[j] / 32768.;
sigEnergy += tmp;
noiseEnergy += pNoiseMean[j] / 32768. * pNoiseMean[j] / 32768.;
tmp = sqrt( tmp ) * 32768.;
if ( tmp > MAX_INT32S )
{
AUD_ECHO
pFFTMag[j] = MAX_INT32S;
}
else
{
pFFTMag[j] = (AUD_Int32s)round( tmp );
}
}
snr = 10. * log10( sigEnergy / noiseEnergy );
beta = berouti( snr );
// AUDLOG( "signal energy: %.2f, noise energy: %.2f, snr: %.2f, beta: %.2f\n", sigEnergy, noiseEnergy, snr, beta );
#if 0
AUDLOG( "noisy FFT:\n" );
for ( j = 0; j < nSpecLen; j++ )
{
AUDLOG( "%d, ", pFFTMag[j] );
}
AUDLOG( "\n" );
#endif
tmp = 0.;
for ( j = 0; j < nSpecLen; j++ )
{
tmp = AUD_MAX( pow( pFFTMag[j], alpha ) - beta * pow( pNoiseMean[j], alpha ), flr * pow( pNoiseMean[j], alpha ) );
pFFTCleanMag[j] = (AUD_Int32s)round( pow( tmp, 1. / alpha ) );
}
// re-estimate noise
if ( snr < thres )
{
AUD_Double tmpNoise;
for ( j = 0; j < nSpecLen; j++ )
{
tmpNoise = G * pow( pNoiseMean[j], alpha ) + ( 1 - G ) * pow( pFFTMag[j], alpha );
pNoiseMean[j] = pow( tmpNoise, 1. / alpha );
}
}
#if 0
AUDLOG( "clean FFT:\n" );
for ( j = 0; j < nSpecLen; j++ )
{
AUDLOG( "%d, ", pFFTCleanMag[j] );
}
AUDLOG( "\n" );
#endif
pFFTCleanRe[0] = pFFTCleanMag[0];
pFFTCleanIm[0] = 0;
AUD_Double costheta, sintheta;
for ( j = 1; j < nSpecLen; j++ )
{
if ( pFFTMag[j] != 0 )
{
costheta = (AUD_Double)pFFTRe[j] / (AUD_Double)pFFTMag[j];
sintheta = (AUD_Double)pFFTIm[j] / (AUD_Double)pFFTMag[j];
pFFTCleanRe[nFFT - j] = pFFTCleanRe[j] = (AUD_Int32s)round( costheta * pFFTCleanMag[j] );
pFFTCleanIm[j] = (AUD_Int32s)round( sintheta * pFFTCleanMag[j] );
pFFTCleanIm[nFFT - j] = -pFFTCleanIm[j];
}
else
{
pFFTCleanRe[nFFT - j] = pFFTCleanRe[j] = pFFTCleanMag[j];
pFFTCleanIm[nFFT - j] = pFFTCleanIm[j] = 0;
}
}
#if 0
AUDLOG( "clean FFT with phase:\n" );
for ( j = 0; j < nFFT; j++ )
{
AUDLOG( "%d + j%d, ", pFFTCleanRe[j], pFFTCleanIm[j] );
}
AUDLOG( "\n" );
#endif
ifft_real( hIfft, pFFTCleanRe, pFFTCleanIm, nFFT, pxClean );
#if 0
AUDLOG( "clean speech:\n" );
for ( j = 0; j < nFFT; j++ )
{
AUDLOG( "%d, ", pxClean[j] );
}
AUDLOG( "\n" );
#endif
for ( j = 0; j < frameStride; j++ )
{
pOutBuf[k + j] = pxOld[j] + pxClean[j];
pxOld[j] = pxClean[frameOverlap + j];
}
k += frameStride;
cleanLen += frameStride;
}
// de-emphasis
// sig_deemphasis( pOutBuf, pOutBuf, cleanLen );
win16s_free( &hWin );
fft_free( &hFft );
ifft_free( &hIfft );
free( pFrame );
free( pNoiseMean );
free( pFFTMag );
free( pFFTRe );
free( pFFTIm );
free( pFFTCleanMag );
free( pFFTCleanRe );
free( pFFTCleanIm );
free( pxOld );
free( pxClean );
return cleanLen;
}
static void fconvolution( TrformStr *TrPtr, Image *psf )
{
int dims[2], i, k, dim, prog=0, delta = 100/15;
double **Re = NULL, **Im = NULL, **PRe = NULL, **PIm = NULL;
char percent[25];
dims[0] = TrPtr->src->width;
dims[1] = TrPtr->src->height;
dim = TrPtr->src->width * TrPtr->src->height;
Progress( _initProgress, "Convolution Filter" );
Re = (double**)mymalloc( dim * sizeof( double ) );
Im = (double**)mymalloc( dim * sizeof( double ) );
PRe = (double**)mymalloc( dim * sizeof( double ) );
PIm = (double**)mymalloc( dim * sizeof( double ) );
if( Re == NULL || Im == NULL || PRe == NULL || PIm == NULL)
{
PrintError("Not enough memory");
TrPtr->success = 0;
goto _fconvolution_exit;
}
for( i=0; i<3; i++ )
{
register double x,y,a,b;
UPDATE_PROGRESS_CONVOLUTION
makePSF( TrPtr->src->width, TrPtr->src->height, psf, *PRe, *PIm, i, 1 );
fftn (2, dims, *PRe, *PIm, 1, 1.0 );
UPDATE_PROGRESS_CONVOLUTION
if( makeDoubleImage ( TrPtr->src, *Re, *Im, i, TrPtr->gamma ) != 0 )
{
PrintError("Could not make Real-version of image");
TrPtr->success = 0;
goto _fconvolution_exit;
}
fftn (2, dims, *Re, *Im, 1, 1.0 ); // forward transform; don't scale
UPDATE_PROGRESS_CONVOLUTION
// Multiply with psf
for( k= 0; k<dim; k++)
{
x = (*Re)[k]; y = (*Im)[k];
a = (*PRe)[k]; b = (*PIm)[k];
(*Re)[k] = x*a-y*b;
(*Im)[k] = x*b+y*a;
}
UPDATE_PROGRESS_CONVOLUTION
fftn (2, dims, *Re, *Im, -1, -1.0 ); // backward transform; scale by dim;
UPDATE_PROGRESS_CONVOLUTION
makeUcharImage ( TrPtr->dest, *Re, i );
}
TrPtr->success = 1;
_fconvolution_exit:
Progress( _disposeProgress, percent );
fft_free();
if( Re != NULL ) myfree( (void**)Re );
if( Im != NULL ) myfree( (void**)Im );
if( PRe != NULL ) myfree( (void**)PRe );
if( PIm != NULL ) myfree( (void**)PIm );
}
AUD_Int32s denoise_aud( AUD_Int16s *pInBuf, AUD_Int16s *pOutBuf, AUD_Int32s inLen )
{
Fft_16s *hFft = NULL;
Ifft_16s *hIfft = NULL;
AUD_Window16s *hWin = NULL;
AUD_Int32s frameSize = 512;
AUD_Int32s frameStride = 256;
AUD_Int32s frameOverlap = 256;
AUD_Int32s nFFT = frameSize;
AUD_Int32s nSpecLen = nFFT / 2 + 1;
AUD_Int32s nNoiseFrame = 6; // (AUD_Int32s)( ( 0.25 * SAMPLE_RATE - frameSize ) / frameStride + 1 );
AUD_Int32s i, j, k, m, n, ret;
AUD_Int32s cleanLen = 0;
// pre-emphasis
// sig_preemphasis( pInBuf, pInBuf, inLen );
// init hamming module
win16s_init( &hWin, AUD_WIN_HAMM, frameSize, 14 );
AUD_ASSERT( hWin );
// init fft handle
fft_init( &hFft, nFFT, 15 );
AUD_ASSERT( hFft );
// init ifft handle
ifft_init( &hIfft, nFFT, 15 );
AUD_ASSERT( hIfft );
AUD_Int16s *pFrame = (AUD_Int16s*)calloc( frameSize * sizeof(AUD_Int16s), 1 );
AUD_ASSERT( pFrame );
// FFT
AUD_Int32s *pFFTMag = (AUD_Int32s*)calloc( nFFT * sizeof(AUD_Int32s), 1 );
AUD_ASSERT( pFFTMag );
AUD_Int32s *pFFTRe = (AUD_Int32s*)calloc( nFFT * sizeof(AUD_Int32s), 1 );
AUD_ASSERT( pFFTRe );
AUD_Int32s *pFFTIm = (AUD_Int32s*)calloc( nFFT * sizeof(AUD_Int32s), 1 );
AUD_ASSERT( pFFTIm );
AUD_Int32s *pFFTCleanRe = (AUD_Int32s*)calloc( nFFT * sizeof(AUD_Int32s), 1 );
AUD_ASSERT( pFFTCleanRe );
AUD_Int32s *pFFTCleanIm = (AUD_Int32s*)calloc( nFFT * sizeof(AUD_Int32s), 1 );
AUD_ASSERT( pFFTCleanIm );
// noise spectrum
AUD_Double *pNoiseEn = (AUD_Double*)calloc( nSpecLen * sizeof(AUD_Double), 1 );
AUD_ASSERT( pNoiseEn );
AUD_Double *pNoiseB = (AUD_Double*)calloc( nSpecLen * sizeof(AUD_Double), 1 );
AUD_ASSERT( pNoiseB );
AUD_Double *pXPrev = (AUD_Double*)calloc( nSpecLen * sizeof(AUD_Double), 1 );
AUD_ASSERT( pXPrev );
AUD_Double *pAb = (AUD_Double*)calloc( nSpecLen * sizeof(AUD_Double), 1 );
AUD_ASSERT( pAb );
AUD_Double *pH = (AUD_Double*)calloc( nSpecLen * sizeof(AUD_Double), 1 );
AUD_ASSERT( pH );
AUD_Double *pGammak = (AUD_Double*)calloc( nSpecLen * sizeof(AUD_Double), 1 );
AUD_ASSERT( pGammak );
AUD_Double *pKsi = (AUD_Double*)calloc( nSpecLen * sizeof(AUD_Double), 1 );
AUD_ASSERT( pKsi );
AUD_Double *pLogSigmak = (AUD_Double*)calloc( nSpecLen * sizeof(AUD_Double), 1 );
AUD_ASSERT( pLogSigmak );
AUD_Double *pAlpha = (AUD_Double*)calloc( nSpecLen * sizeof(AUD_Double), 1 );
AUD_ASSERT( pAlpha );
AUD_Int32s *pLinToBark = (AUD_Int32s*)calloc( nSpecLen * sizeof(AUD_Int32s), 1 );
AUD_ASSERT( pLinToBark );
AUD_Int16s *pxOld = (AUD_Int16s*)calloc( frameOverlap * sizeof(AUD_Int16s), 1 );
AUD_ASSERT( pxOld );
AUD_Int16s *pxClean = (AUD_Int16s*)calloc( nFFT * sizeof(AUD_Int16s), 1 );
AUD_ASSERT( pxClean );
/*
AUD_Int32s critBandEnds[22] = { 0, 100, 200, 300, 400, 510, 630, 770, 920, 1080, 1270,
1480, 1720, 2000, 2320, 2700, 3150, 3700, 4400, 5300, 6400, 7700 };
*/
AUD_Int32s critFFTEnds[CRITICAL_BAND_NUM + 1] = { 0, 4, 7, 10, 13, 17, 21, 25, 30, 35, 41, 48, 56, 64,
75, 87, 101, 119, 141, 170, 205, 247, 257 };
// generate linear->bark transform mapping
k = 0;
for ( i = 0; i < CRITICAL_BAND_NUM; i++ )
{
while ( k >= critFFTEnds[i] && k < critFFTEnds[i + 1] )
{
pLinToBark[k] = i;
k++;
}
}
AUD_Double absThr[CRITICAL_BAND_NUM] = { 38, 31, 22, 18.5, 15.5, 13, 11, 9.5, 8.75, 7.25, 4.75, 2.75,
1.5, 0.5, 0, 0, 0, 0, 2, 7, 12, 15.5 };
AUD_Double dbOffset[CRITICAL_BAND_NUM];
AUD_Double sumn[CRITICAL_BAND_NUM];
AUD_Double spread[CRITICAL_BAND_NUM];
for ( i = 0; i < CRITICAL_BAND_NUM; i++ )
{
absThr[i] = pow( 10., absThr[i] / 10. ) / nFFT / ( 65535. * 65535. );
dbOffset[i] = 10. + i;
sumn[i] = 0.474 + i;
spread[i] = pow( 10., ( 15.81 + 7.5 * sumn[i] - 17.5 * sqrt( 1. + sumn[i] * sumn[i] ) ) / 10. );
}
AUD_Double dcGain[CRITICAL_BAND_NUM];
for ( i = 0; i < CRITICAL_BAND_NUM; i++ )
{
dcGain[i] = 0.;
for ( j = 0; j < CRITICAL_BAND_NUM; j++ )
{
dcGain[i] += spread[MABS( i - j )];
}
}
AUD_Matrix exPatMatrix;
exPatMatrix.rows = CRITICAL_BAND_NUM;
exPatMatrix.cols = nSpecLen;
exPatMatrix.dataType = AUD_DATATYPE_DOUBLE;
ret = createMatrix( &exPatMatrix );
AUD_ASSERT( ret == 0 );
// excitation pattern
AUD_Int32s index = 0;
for ( i = 0; i < exPatMatrix.rows; i++ )
{
AUD_Double *pExpatRow = exPatMatrix.pDouble + i * exPatMatrix.cols;
for ( j = 0; j < exPatMatrix.cols; j++ )
{
index = MABS( i - pLinToBark[j] );
pExpatRow[j] = spread[index];
}
}
AUD_Int32s frameNum = (inLen - frameSize) / frameStride + 1;
AUD_ASSERT( frameNum > nNoiseFrame );
// compute noise mean
for ( i = 0; i < nNoiseFrame; i++ )
{
win16s_calc( hWin, pInBuf + i * frameSize, pFrame );
fft_mag( hFft, pFrame, frameSize, pFFTMag );
for ( j = 0; j < nSpecLen; j++ )
{
pNoiseEn[j] += pFFTMag[j] / 32768. * pFFTMag[j] / 32768.;
}
}
for ( j = 0; j < nSpecLen; j++ )
{
pNoiseEn[j] /= nNoiseFrame;
}
// get cirtical band mean filtered noise power
AUD_Int32s k1 = 0, k2 = 0;
for ( i = 0; i < CRITICAL_BAND_NUM; i++ )
{
k1 = k2;
AUD_Double segSum = 0.;
while ( k2 >= critFFTEnds[i] && k2 < critFFTEnds[i + 1] )
{
segSum += pNoiseEn[k2];
k2++;
}
segSum /= ( k2 - k1 );
for ( m = k1; m < k2; m++ )
{
pNoiseB[m] = segSum;
}
}
#if 0
AUDLOG( "noise band spectrum:\n" );
for ( j = 0; j < nSpecLen; j++ )
{
AUDLOG( "%.2f, ", pNoiseB[j] );
}
AUDLOG( "\n" );
#endif
AUD_Matrix frameMatrix;
frameMatrix.rows = nSpecLen;
frameMatrix.cols = 1;
frameMatrix.dataType = AUD_DATATYPE_DOUBLE;
ret = createMatrix( &frameMatrix );
AUD_ASSERT( ret == 0 );
AUD_Double *pFrameEn = frameMatrix.pDouble;
AUD_Matrix xMatrix;
xMatrix.rows = nSpecLen;
xMatrix.cols = 1;
xMatrix.dataType = AUD_DATATYPE_DOUBLE;
ret = createMatrix( &xMatrix );
AUD_ASSERT( ret == 0 );
AUD_Double *pX = xMatrix.pDouble;
AUD_Matrix cMatrix;
cMatrix.rows = CRITICAL_BAND_NUM;
cMatrix.cols = 1;
cMatrix.dataType = AUD_DATATYPE_DOUBLE;
ret = createMatrix( &cMatrix );
AUD_ASSERT( ret == 0 );
AUD_Double *pC = cMatrix.pDouble;
AUD_Matrix tMatrix;
tMatrix.rows = 1;
tMatrix.cols = CRITICAL_BAND_NUM;
tMatrix.dataType = AUD_DATATYPE_DOUBLE;
ret = createMatrix( &tMatrix );
AUD_ASSERT( ret == 0 );
AUD_Double *pT = tMatrix.pDouble;
AUD_Matrix tkMatrix;
tkMatrix.rows = 1;
tkMatrix.cols = nSpecLen;
tkMatrix.dataType = AUD_DATATYPE_DOUBLE;
ret = createMatrix( &tkMatrix );
AUD_ASSERT( ret == 0 );
AUD_Double *pTk = tkMatrix.pDouble;
AUD_Double dB0[CRITICAL_BAND_NUM];
AUD_Double epsilon = pow( 2, -52 );
#define ESTIMATE_MASKTHRESH( sigMatrix, tkMatrix )\
do {\
AUD_Double *pSig = sigMatrix.pDouble; \
for ( m = 0; m < exPatMatrix.rows; m++ ) \
{ \
AUD_Double suma = 0.; \
AUD_Double *pExpatRow = exPatMatrix.pDouble + m * exPatMatrix.cols; \
for ( n = 0; n < exPatMatrix.cols; n++ ) \
{ \
suma += pExpatRow[n] * pSig[n]; \
} \
pC[m] = suma; \
} \
AUD_Double product = 1.; \
AUD_Double sum = 0.; \
for ( m = 0; m < sigMatrix.rows; m++ ) \
{ \
product *= pSig[m]; \
sum += pSig[m]; \
} \
AUD_Double power = 1. / sigMatrix.rows;\
AUD_Double sfmDB = 10. * log10( pow( product, power ) / sum / sigMatrix.rows + epsilon ); \
AUD_Double alpha = AUD_MIN( 1., sfmDB / (-60.) ); \
for ( m = 0; m < tMatrix.cols; m++ ) \
{ \
dB0[m] = dbOffset[m] * alpha + 5.5; \
pT[m] = pC[m] / pow( 10., dB0[m] / 10. ) / dcGain[m]; \
pT[m] = AUD_MAX( pT[m], absThr[m] ); \
} \
for ( m = 0; m < tkMatrix.cols; m++ ) \
{ \
pTk[m] = pT[pLinToBark[m]]; \
} \
} while ( 0 )
AUD_Double aa = 0.98;
AUD_Double mu = 0.98;
AUD_Double eta = 0.15;
AUD_Double vadDecision;
k = 0;
// start processing
for ( i = 0; i < frameNum; i++ )
{
win16s_calc( hWin, pInBuf + i * frameStride, pFrame );
fft_calc( hFft, pFrame, frameSize, pFFTRe, pFFTIm );
// compute SNR
vadDecision = 0.;
for ( j = 0; j < nSpecLen; j++ )
{
pFrameEn[j] = pFFTRe[j] / 32768. * pFFTRe[j] / 32768. + pFFTIm[j] / 32768. * pFFTIm[j] / 32768.;
pGammak[j] = AUD_MIN( pFrameEn[j] / pNoiseEn[j], 40. );
if ( i > 0 )
{
pKsi[j] = aa * pXPrev[j] / pNoiseEn[j] + ( 1 - aa ) * AUD_MAX( pGammak[j] - 1., 0. );
}
else
{
pKsi[j] = aa + ( 1. - aa ) * AUD_MAX( pGammak[j] - 1., 0. );
}
pLogSigmak[j] = pGammak[j] * pKsi[j] / ( 1. + pKsi[j] ) - log( 1. + pKsi[j] );
vadDecision += ( j > 0 ? 2 : 1 ) * pLogSigmak[j];
}
vadDecision /= nFFT;
#if 0
AUDLOG( "X prev:\n" );
for ( j = 0; j < nSpecLen; j++ )
{
AUDLOG( "%.2f, ", pXPrev[j] );
}
AUDLOG( "\n" );
#endif
#if 0
AUDLOG( "gamma k:\n" );
for ( j = 0; j < nSpecLen; j++ )
{
AUDLOG( "%.2f, ", pGammak[j] );
}
AUDLOG( "\n" );
#endif
#if 0
AUDLOG( "ksi:\n" );
for ( j = 0; j < nSpecLen; j++ )
{
AUDLOG( "%.2f, ", pKsi[j] );
}
AUDLOG( "\n" );
#endif
#if 0
AUDLOG( "log sigma k:\n" );
for ( j = 0; j < nSpecLen; j++ )
{
AUDLOG( "%.2f, ", pLogSigmak[j] );
}
AUDLOG( "\n" );
#endif
// AUDLOG( "vadDecision: %.2f\n", vadDecision );
// re-estimate noise
if ( vadDecision < eta )
{
for ( j = 0; j < nSpecLen; j++ )
{
pNoiseEn[j] = mu * pNoiseEn[j] + ( 1. - mu ) * pFrameEn[j];
}
// re-estimate crital band based noise
AUD_Int32s k1 = 0, k2 = 0;
for ( int band = 0; band < CRITICAL_BAND_NUM; band++ )
{
k1 = k2;
AUD_Double segSum = 0.;
while ( k2 >= critFFTEnds[band] && k2 < critFFTEnds[band + 1] )
{
segSum += pNoiseEn[k2];
k2++;
}
segSum /= ( k2 - k1 );
for ( m = k1; m < k2; m++ )
{
pNoiseB[m] = segSum;
}
}
#if 0
AUDLOG( "noise band spectrum:\n" );
for ( j = 0; j < nSpecLen; j++ )
{
AUDLOG( "%.2f, ", pNoiseB[j] );
}
AUDLOG( "\n" );
#endif
}
for ( j = 0; j < nSpecLen; j++ )
{
pX[j] = AUD_MAX( pFrameEn[j] - pNoiseEn[j], 0.001 );
pXPrev[j] = pFrameEn[j];
}
ESTIMATE_MASKTHRESH( xMatrix, tkMatrix );
for ( int iter = 0; iter < 2; iter++ )
{
for ( j = 0; j < nSpecLen; j++ )
{
pAb[j] = pNoiseB[j] + pNoiseB[j] * pNoiseB[j] / pTk[j];
pFrameEn[j] = pFrameEn[j] * pFrameEn[j] / ( pFrameEn[j] + pAb[j] );
ESTIMATE_MASKTHRESH( frameMatrix, tkMatrix );
#if 0
showMatrix( &tMatrix );
#endif
}
}
#if 0
AUDLOG( "tk:\n" );
for ( j = 0; j < nSpecLen; j++ )
{
AUDLOG( "%.2f, ", pTk[j] );
}
AUDLOG( "\n" );
#endif
pAlpha[0] = ( pNoiseB[0] + pTk[0] ) * ( pNoiseB[0] / pTk[0] );
pH[0] = pFrameEn[0] / ( pFrameEn[0] + pAlpha[0] );
pXPrev[0] *= pH[0] * pH[0];
pFFTCleanRe[0] = 0;
pFFTCleanIm[0] = 0;
for ( j = 1; j < nSpecLen; j++ )
{
pAlpha[j] = ( pNoiseB[j] + pTk[j] ) * ( pNoiseB[j] / pTk[j] );
pH[j] = pFrameEn[j] / ( pFrameEn[j] + pAlpha[j] );
pFFTCleanRe[j] = pFFTCleanRe[nFFT - j] = (AUD_Int32s)round( pH[j] * pFFTRe[j] );
pFFTCleanIm[j] = (AUD_Int32s)round( pH[j] * pFFTIm[j] );
pFFTCleanIm[nFFT - j] = -pFFTCleanIm[j];
pXPrev[j] *= pH[j] * pH[j];
}
#if 0
AUDLOG( "denoise transfer function:\n" );
for ( j = 0; j < nSpecLen; j++ )
{
AUDLOG( "%.2f, ", pH[j] );
}
AUDLOG( "\n" );
#endif
#if 0
AUDLOG( "clean FFT with phase:\n" );
for ( j = 0; j < nFFT; j++ )
{
AUDLOG( "%d + j%d, ", pFFTCleanRe[j], pFFTCleanIm[j] );
}
AUDLOG( "\n" );
#endif
ifft_real( hIfft, pFFTCleanRe, pFFTCleanIm, nFFT, pxClean );
#if 0
AUDLOG( "clean speech:\n" );
for ( j = 0; j < nFFT; j++ )
{
AUDLOG( "%d, ", pxClean[j] );
}
AUDLOG( "\n" );
#endif
for ( j = 0; j < frameStride; j++ )
{
if ( j < frameOverlap )
{
pOutBuf[k + j] = pxOld[j] + pxClean[j];
pxOld[j] = pxClean[frameStride + j];
}
else
{
pOutBuf[k + j] = pxClean[j];
}
}
k += frameStride;
cleanLen += frameStride;
}
// de-emphasis
// sig_deemphasis( pOutBuf, pOutBuf, cleanLen );
win16s_free( &hWin );
fft_free( &hFft );
ifft_free( &hIfft );
free( pFrame );
free( pNoiseEn );
free( pNoiseB );
free( pFFTMag );
free( pFFTRe );
free( pFFTIm );
free( pXPrev );
free( pAb );
free( pH );
free( pFFTCleanRe );
free( pFFTCleanIm );
free( pxOld );
free( pxClean );
free( pGammak );
free( pKsi );
free( pLogSigmak );
free( pAlpha );
free( pLinToBark );
ret = createMatrix( &xMatrix );
AUD_ASSERT( ret == 0 );
ret = destroyMatrix( &exPatMatrix );
AUD_ASSERT( ret == 0 );
ret = destroyMatrix( &frameMatrix );
AUD_ASSERT( ret == 0 );
ret = destroyMatrix( &cMatrix );
AUD_ASSERT( ret == 0 );
ret = destroyMatrix( &tMatrix );
AUD_ASSERT( ret == 0 );
ret = destroyMatrix( &tkMatrix );
AUD_ASSERT( ret == 0 );
return cleanLen;
}
/*
* singleton's mixed radix routine
*
* could move allocation out to fftn(), but leave it here so that it's
* possible to make this a standalone function
*/
static int
FFTRADIX (REAL Re[],
REAL Im[],
size_t nTotal,
size_t nPass,
size_t nSpan,
int iSign,
int max_factors,
int max_perm)
{
int ii, mfactor, kspan, ispan, inc;
int j, jc, jf, jj, k, k1, k2, k3, k4, kk, kt, nn, ns, nt;
REAL radf;
REAL c1, c2, c3, cd, aa, aj, ak, ajm, ajp, akm, akp;
REAL s1, s2, s3, sd, bb, bj, bk, bjm, bjp, bkm, bkp;
REAL *Rtmp = NULL; /* temp space for real part*/
REAL *Itmp = NULL; /* temp space for imaginary part */
REAL *Cos = NULL; /* Cosine values */
REAL *Sin = NULL; /* Sine values */
REAL s60 = SIN60; /* sin(60 deg) */
REAL c72 = COS72; /* cos(72 deg) */
REAL s72 = SIN72; /* sin(72 deg) */
REAL pi2 = M_PI; /* use PI first, 2 PI later */
/* gcc complains about k3 being uninitialized, but I can't find out where
* or why ... it looks okay to me.
*
* initialize to make gcc happy
*/
k3 = 0;
/* gcc complains about c2, c3, s2,s3 being uninitialized, but they're
* only used for the radix 4 case and only AFTER the (s1 == 0.0) pass
* through the loop at which point they will have been calculated.
*
* initialize to make gcc happy
*/
c2 = c3 = s2 = s3 = 0.0;
/* Parameter adjustments, was fortran so fix zero-offset */
Re--;
Im--;
if (nPass < 2)
return 0;
/* assign pointers */
Rtmp = (REAL *) Tmp0;
Itmp = (REAL *) Tmp1;
Cos = (REAL *) Tmp2;
Sin = (REAL *) Tmp3;
/*
* Function Body
*/
inc = iSign;
if (iSign < 0) {
s72 = -s72;
s60 = -s60;
pi2 = -pi2;
inc = -inc; /* absolute value */
}
/* adjust for strange increments */
nt = inc * nTotal;
ns = inc * nSpan;
kspan = ns;
nn = nt - inc;
jc = ns / nPass;
radf = pi2 * (double) jc;
pi2 *= 2.0; /* use 2 PI from here on */
ii = 0;
jf = 0;
/* determine the factors of n */
mfactor = 0;
k = nPass;
while (k % 16 == 0) {
mfactor++;
factor [mfactor - 1] = 4;
k /= 16;
}
j = 3;
jj = 9;
do {
while (k % jj == 0) {
mfactor++;
factor [mfactor - 1] = j;
k /= jj;
}
j += 2;
jj = j * j;
} while (jj <= k);
if (k <= 4) {
kt = mfactor;
factor [mfactor] = k;
if (k != 1)
mfactor++;
} else {
if (k - (k / 4 << 2) == 0) {
mfactor++;
factor [mfactor - 1] = 2;
k /= 4;
}
kt = mfactor;
j = 2;
do {
if (k % j == 0) {
mfactor++;
factor [mfactor - 1] = j;
k /= j;
}
j = ((j + 1) / 2 << 1) + 1;
} while (j <= k);
}
if (kt) {
j = kt;
do {
mfactor++;
factor [mfactor - 1] = factor [j - 1];
j--;
} while (j);
}
/* test that mfactors is in range */
if (mfactor > NFACTOR)
{
fputs ("Error: " FFTRADIXS "() - exceeded number of factors\n", stderr);
goto Memory_Error_Label;
}
/* compute fourier transform */
for (;;) {
sd = radf / (double) kspan;
cd = sin(sd);
cd = 2.0 * cd * cd;
sd = sin(sd + sd);
kk = 1;
ii++;
switch (factor [ii - 1]) {
case 2:
/* transform for factor of 2 (including rotation factor) */
kspan /= 2;
k1 = kspan + 2;
do {
do {
k2 = kk + kspan;
ak = Re [k2];
bk = Im [k2];
Re [k2] = Re [kk] - ak;
Im [k2] = Im [kk] - bk;
Re [kk] += ak;
Im [kk] += bk;
kk = k2 + kspan;
} while (kk <= nn);
kk -= nn;
} while (kk <= jc);
if (kk > kspan)
goto Permute_Results_Label; /* exit infinite loop */
do {
c1 = 1.0 - cd;
s1 = sd;
do {
do {
do {
k2 = kk + kspan;
ak = Re [kk] - Re [k2];
bk = Im [kk] - Im [k2];
Re [kk] += Re [k2];
Im [kk] += Im [k2];
Re [k2] = c1 * ak - s1 * bk;
Im [k2] = s1 * ak + c1 * bk;
kk = k2 + kspan;
} while (kk < nt);
k2 = kk - nt;
c1 = -c1;
kk = k1 - k2;
} while (kk > k2);
ak = c1 - (cd * c1 + sd * s1);
s1 = sd * c1 - cd * s1 + s1;
c1 = 2.0 - (ak * ak + s1 * s1);
s1 *= c1;
c1 *= ak;
kk += jc;
} while (kk < k2);
k1 += inc + inc;
kk = (k1 - kspan) / 2 + jc;
} while (kk <= jc + jc);
break;
case 4: /* transform for factor of 4 */
ispan = kspan;
kspan /= 4;
do {
c1 = 1.0;
s1 = 0.0;
do {
do {
k1 = kk + kspan;
k2 = k1 + kspan;
k3 = k2 + kspan;
akp = Re [kk] + Re [k2];
akm = Re [kk] - Re [k2];
ajp = Re [k1] + Re [k3];
ajm = Re [k1] - Re [k3];
bkp = Im [kk] + Im [k2];
bkm = Im [kk] - Im [k2];
bjp = Im [k1] + Im [k3];
bjm = Im [k1] - Im [k3];
Re [kk] = akp + ajp;
Im [kk] = bkp + bjp;
ajp = akp - ajp;
bjp = bkp - bjp;
if (iSign < 0) {
akp = akm + bjm;
bkp = bkm - ajm;
akm -= bjm;
bkm += ajm;
} else {
akp = akm - bjm;
bkp = bkm + ajm;
akm += bjm;
bkm -= ajm;
}
/* avoid useless multiplies */
if (s1 == 0.0) {
Re [k1] = akp;
Re [k2] = ajp;
Re [k3] = akm;
Im [k1] = bkp;
Im [k2] = bjp;
Im [k3] = bkm;
} else {
Re [k1] = akp * c1 - bkp * s1;
Re [k2] = ajp * c2 - bjp * s2;
Re [k3] = akm * c3 - bkm * s3;
Im [k1] = akp * s1 + bkp * c1;
Im [k2] = ajp * s2 + bjp * c2;
Im [k3] = akm * s3 + bkm * c3;
}
kk = k3 + kspan;
} while (kk <= nt);
c2 = c1 - (cd * c1 + sd * s1);
s1 = sd * c1 - cd * s1 + s1;
c1 = 2.0 - (c2 * c2 + s1 * s1);
s1 *= c1;
c1 *= c2;
/* values of c2, c3, s2, s3 that will get used next time */
c2 = c1 * c1 - s1 * s1;
s2 = 2.0 * c1 * s1;
c3 = c2 * c1 - s2 * s1;
s3 = c2 * s1 + s2 * c1;
kk = kk - nt + jc;
} while (kk <= kspan);
kk = kk - kspan + inc;
} while (kk <= jc);
if (kspan == jc)
goto Permute_Results_Label; /* exit infinite loop */
break;
default:
/* transform for odd factors */
#ifdef FFT_RADIX4
fputs ("Error: " FFTRADIXS "(): compiled for radix 2/4 only\n", stderr);
fft_free (); /* free-up memory */
return -1;
break;
#else /* FFT_RADIX4 */
k = factor [ii - 1];
ispan = kspan;
kspan /= k;
switch (k) {
case 3: /* transform for factor of 3 (optional code) */
do {
do {
k1 = kk + kspan;
k2 = k1 + kspan;
ak = Re [kk];
bk = Im [kk];
aj = Re [k1] + Re [k2];
bj = Im [k1] + Im [k2];
Re [kk] = ak + aj;
Im [kk] = bk + bj;
ak -= 0.5 * aj;
bk -= 0.5 * bj;
aj = (Re [k1] - Re [k2]) * s60;
bj = (Im [k1] - Im [k2]) * s60;
Re [k1] = ak - bj;
Re [k2] = ak + bj;
Im [k1] = bk + aj;
Im [k2] = bk - aj;
kk = k2 + kspan;
} while (kk < nn);
kk -= nn;
} while (kk <= kspan);
break;
case 5: /* transform for factor of 5 (optional code) */
c2 = c72 * c72 - s72 * s72;
s2 = 2.0 * c72 * s72;
do {
do {
k1 = kk + kspan;
k2 = k1 + kspan;
k3 = k2 + kspan;
k4 = k3 + kspan;
akp = Re [k1] + Re [k4];
akm = Re [k1] - Re [k4];
bkp = Im [k1] + Im [k4];
bkm = Im [k1] - Im [k4];
ajp = Re [k2] + Re [k3];
ajm = Re [k2] - Re [k3];
bjp = Im [k2] + Im [k3];
bjm = Im [k2] - Im [k3];
aa = Re [kk];
bb = Im [kk];
Re [kk] = aa + akp + ajp;
Im [kk] = bb + bkp + bjp;
ak = akp * c72 + ajp * c2 + aa;
bk = bkp * c72 + bjp * c2 + bb;
aj = akm * s72 + ajm * s2;
bj = bkm * s72 + bjm * s2;
Re [k1] = ak - bj;
Re [k4] = ak + bj;
Im [k1] = bk + aj;
Im [k4] = bk - aj;
ak = akp * c2 + ajp * c72 + aa;
bk = bkp * c2 + bjp * c72 + bb;
aj = akm * s2 - ajm * s72;
bj = bkm * s2 - bjm * s72;
Re [k2] = ak - bj;
Re [k3] = ak + bj;
Im [k2] = bk + aj;
Im [k3] = bk - aj;
kk = k4 + kspan;
} while (kk < nn);
kk -= nn;
} while (kk <= kspan);
break;
default:
if (k != jf) {
jf = k;
s1 = pi2 / (double) k;
c1 = cos(s1);
s1 = sin(s1);
if (jf > max_factors)
goto Memory_Error_Label;
Cos [jf - 1] = 1.0;
Sin [jf - 1] = 0.0;
j = 1;
do {
Cos [j - 1] = Cos [k - 1] * c1 + Sin [k - 1] * s1;
Sin [j - 1] = Cos [k - 1] * s1 - Sin [k - 1] * c1;
k--;
Cos [k - 1] = Cos [j - 1];
Sin [k - 1] = -Sin [j - 1];
j++;
} while (j < k);
}
do {
do {
k1 = kk;
k2 = kk + ispan;
ak = aa = Re [kk];
bk = bb = Im [kk];
j = 1;
k1 += kspan;
do {
k2 -= kspan;
j++;
Rtmp [j - 1] = Re [k1] + Re [k2];
ak += Rtmp [j - 1];
Itmp [j - 1] = Im [k1] + Im [k2];
bk += Itmp [j - 1];
j++;
Rtmp [j - 1] = Re [k1] - Re [k2];
Itmp [j - 1] = Im [k1] - Im [k2];
k1 += kspan;
} while (k1 < k2);
Re [kk] = ak;
Im [kk] = bk;
k1 = kk;
k2 = kk + ispan;
j = 1;
do {
k1 += kspan;
k2 -= kspan;
jj = j;
ak = aa;
bk = bb;
aj = 0.0;
bj = 0.0;
k = 1;
do {
k++;
ak += Rtmp [k - 1] * Cos [jj - 1];
bk += Itmp [k - 1] * Cos [jj - 1];
k++;
aj += Rtmp [k - 1] * Sin [jj - 1];
bj += Itmp [k - 1] * Sin [jj - 1];
jj += j;
if (jj > jf) {
jj -= jf;
}
} while (k < jf);
k = jf - j;
Re [k1] = ak - bj;
Im [k1] = bk + aj;
Re [k2] = ak + bj;
Im [k2] = bk - aj;
j++;
} while (j < k);
kk += ispan;
} while (kk <= nn);
kk -= nn;
} while (kk <= kspan);
break;
}
/* multiply by rotation factor (except for factors of 2 and 4) */
if (ii == mfactor)
goto Permute_Results_Label; /* exit infinite loop */
kk = jc + 1;
do {
c2 = 1.0 - cd;
s1 = sd;
do {
c1 = c2;
s2 = s1;
kk += kspan;
do {
do {
ak = Re [kk];
Re [kk] = c2 * ak - s2 * Im [kk];
Im [kk] = s2 * ak + c2 * Im [kk];
kk += ispan;
} while (kk <= nt);
ak = s1 * s2;
s2 = s1 * c2 + c1 * s2;
c2 = c1 * c2 - ak;
kk = kk - nt + kspan;
} while (kk <= ispan);
c2 = c1 - (cd * c1 + sd * s1);
s1 += sd * c1 - cd * s1;
c1 = 2.0 - (c2 * c2 + s1 * s1);
s1 *= c1;
c2 *= c1;
kk = kk - ispan + jc;
} while (kk <= kspan);
kk = kk - kspan + jc + inc;
} while (kk <= jc + jc);
break;
#endif /* FFT_RADIX4 */
}
}
/* permute the results to normal order---done in two stages */
/* permutation for square factors of n */
Permute_Results_Label:
Perm [0] = ns;
if (kt) {
k = kt + kt + 1;
if (mfactor < k)
k--;
j = 1;
Perm [k] = jc;
do {
Perm [j] = Perm [j - 1] / factor [j - 1];
Perm [k - 1] = Perm [k] * factor [j - 1];
j++;
k--;
} while (j < k);
k3 = Perm [k];
kspan = Perm [1];
kk = jc + 1;
k2 = kspan + 1;
j = 1;
if (nPass != nTotal) {
/* permutation for multivariate transform */
Permute_Multi_Label:
do {
do {
k = kk + jc;
do {
/* swap Re [kk] <> Re [k2], Im [kk] <> Im [k2] */
ak = Re [kk]; Re [kk] = Re [k2]; Re [k2] = ak;
bk = Im [kk]; Im [kk] = Im [k2]; Im [k2] = bk;
kk += inc;
k2 += inc;
} while (kk < k);
kk += ns - jc;
k2 += ns - jc;
} while (kk < nt);
k2 = k2 - nt + kspan;
kk = kk - nt + jc;
} while (k2 < ns);
do {
do {
k2 -= Perm [j - 1];
j++;
k2 = Perm [j] + k2;
} while (k2 > Perm [j - 1]);
j = 1;
do {
if (kk < k2)
goto Permute_Multi_Label;
kk += jc;
k2 += kspan;
} while (k2 < ns);
} while (kk < ns);
} else {
/* permutation for single-variate transform (optional code) */
Permute_Single_Label:
do {
/* swap Re [kk] <> Re [k2], Im [kk] <> Im [k2] */
ak = Re [kk]; Re [kk] = Re [k2]; Re [k2] = ak;
bk = Im [kk]; Im [kk] = Im [k2]; Im [k2] = bk;
kk += inc;
k2 += kspan;
} while (k2 < ns);
do {
do {
k2 -= Perm [j - 1];
j++;
k2 = Perm [j] + k2;
} while (k2 > Perm [j - 1]);
j = 1;
do {
if (kk < k2)
goto Permute_Single_Label;
kk += inc;
k2 += kspan;
} while (k2 < ns);
} while (kk < ns);
}
jc = k3;
}
if ((kt << 1) + 1 >= mfactor)
return 0;
ispan = Perm [kt];
/* permutation for square-free factors of n */
j = mfactor - kt;
factor [j] = 1;
do {
factor [j - 1] *= factor [j];
j--;
} while (j != kt);
kt++;
nn = factor [kt - 1] - 1;
if (nn > max_perm)
goto Memory_Error_Label;
j = jj = 0;
for (;;) {
k = kt + 1;
k2 = factor [kt - 1];
kk = factor [k - 1];
j++;
if (j > nn)
break; /* exit infinite loop */
jj += kk;
while (jj >= k2) {
jj -= k2;
k2 = kk;
k++;
kk = factor [k - 1];
jj += kk;
}
Perm [j - 1] = jj;
}
/* determine the permutation cycles of length greater than 1 */
j = 0;
for (;;) {
do {
j++;
kk = Perm [j - 1];
} while (kk < 0);
if (kk != j) {
do {
k = kk;
kk = Perm [k - 1];
Perm [k - 1] = -kk;
} while (kk != j);
k3 = kk;
} else {
Perm [j - 1] = -j;
if (j == nn)
break; /* exit infinite loop */
}
}
max_factors *= inc;
/* reorder a and b, following the permutation cycles */
for (;;) {
j = k3 + 1;
nt -= ispan;
ii = nt - inc + 1;
if (nt < 0)
break; /* exit infinite loop */
do {
do {
j--;
} while (Perm [j - 1] < 0);
jj = jc;
do {
kspan = jj;
if (jj > max_factors) {
kspan = max_factors;
}
jj -= kspan;
k = Perm [j - 1];
kk = jc * k + ii + jj;
k1 = kk + kspan;
k2 = 0;
do {
k2++;
Rtmp [k2 - 1] = Re [k1];
Itmp [k2 - 1] = Im [k1];
k1 -= inc;
} while (k1 != kk);
do {
k1 = kk + kspan;
k2 = k1 - jc * (k + Perm [k - 1]);
k = -Perm [k - 1];
do {
Re [k1] = Re [k2];
Im [k1] = Im [k2];
k1 -= inc;
k2 -= inc;
} while (k1 != kk);
kk = k2;
} while (k != j);
k1 = kk + kspan;
k2 = 0;
do {
k2++;
Re [k1] = Rtmp [k2 - 1];
Im [k1] = Itmp [k2 - 1];
k1 -= inc;
} while (k1 != kk);
} while (jj);
} while (j != 1);
}
return 0; /* exit point here */
/* alloc or other problem, do some clean-up */
Memory_Error_Label:
fputs ("Error: " FFTRADIXS "() - insufficient memory.\n", stderr);
fft_free (); /* free-up memory */
return -1;
}
int
FFTN (int ndim, const int dims[],
REAL Re [],
REAL Im [],
int iSign,
double scaling)
{
size_t nSpan, nPass, nTotal;
int ret, max_factors, max_perm;
int i;
/*
* tally the number of elements in the data array
* and determine the number of dimensions
*/
nTotal = 1;
if (ndim && dims [0])
{
for (i = 0; i < ndim; i++)
{
if (dims [i] <= 0)
{
fputs ("Error: " FFTNS "() - dimension error\n", stderr);
fft_free (); /* free-up memory */
return -1;
}
nTotal *= dims [i];
}
}
else
{
ndim = 0;
for (i = 0; dims [i]; i++)
{
if (dims [i] <= 0)
{
fputs ("Error: " FFTNS "() - dimension error\n", stderr);
fft_free (); /* free-up memory */
return -1;
}
nTotal *= dims [i];
ndim++;
}
}
/* determine maximum number of factors and permuations */
#if 1
/*
* follow John Beale's example, just use the largest dimension and don't
* worry about excess allocation. May be someone else will do it?
*/
max_factors = max_perm = 1;
for (i = 0; i < ndim; i++)
{
nSpan = dims [i];
if (nSpan > (unsigned int)max_factors) max_factors = nSpan;
if (nSpan > (unsigned int)max_perm) max_perm = nSpan;
}
#else
/* use the constants used in the original Fortran code */
max_factors = 23;
max_perm = 209;
#endif
/* loop over the dimensions: */
nPass = 1;
for (i = 0; i < ndim; i++)
{
nSpan = dims [i];
nPass *= nSpan;
ret = FFTRADIX (Re, Im, nTotal, nSpan, nPass, iSign,
max_factors, max_perm);
/* exit, clean-up already done */
if (ret)
return ret;
}
/* Divide through by the normalizing constant: */
if (scaling && scaling != 1.0)
{
if (iSign < 0) iSign = -iSign;
if (scaling < 0.0)
{
scaling = nTotal;
if (scaling < -1.0)
scaling = sqrt (scaling);
}
scaling = 1.0 / scaling; /* multiply is often faster */
for (i = 0; i < (int)nTotal; i += iSign)
{
Re [i] *= scaling;
Im [i] *= scaling;
}
}
return 0;
}
void create_windows(void)
{
char *r_a = gettext("realloc issue");
int nr = 0;
if (w_stats)
{
delwin(w_stats);
delwin(w_line1);
delwin(w_slow);
delwin(w_line2);
delwin(w_fast);
}
#ifdef FW
fft_free();
fft_init(max_x);
#endif
if (max_x > history_n)
{
history = (double *)realloc(history, sizeof(double) * max_x);
if (!history)
error_exit(r_a);
history_temp = (double *)realloc(history_temp, sizeof(double) * max_x);
if (!history_temp)
error_exit(r_a);
/* halve of it is enough really */
history_fft_magn = (double *)realloc(history_fft_magn, sizeof(double) * max_x);
if (!history_fft_magn)
error_exit(r_a);
history_fft_phase = (double *)realloc(history_fft_phase, sizeof(double) * max_x);
if (!history_fft_phase)
error_exit(r_a);
history_set = (char *)realloc(history_set, sizeof(char) * max_x);
if (!history_set)
error_exit(r_a);
memset(&history[history_n], 0x00, (max_x - history_n) * sizeof(double));
memset(&history_set[history_n], 0x00, (max_x - history_n) * sizeof(char));
history_n = max_x;
}
if ((int)max_y > window_history_n)
{
slow_history = (char **)realloc(slow_history, sizeof(char *) * max_y);
if (!slow_history)
error_exit(r_a);
fast_history = (char **)realloc(fast_history, sizeof(char *) * max_y);
if (!fast_history)
error_exit(r_a);
memset(&slow_history[window_history_n], 0x00, (max_y - window_history_n) * sizeof(char *));
memset(&fast_history[window_history_n], 0x00, (max_y - window_history_n) * sizeof(char *));
window_history_n = max_y;
}
w_stats = newwin(stats_h, max_x, 0, 0);
scrollok(w_stats, false);
w_line1 = newwin(1, max_x, stats_h, 0);
scrollok(w_line1, false);
wnoutrefresh(w_line1);
logs_n = max_y - (stats_h + 1 + 1);
fast_n = logs_n * 11 / 20;
slow_n = logs_n - fast_n;
w_slow = newwin(slow_n, max_x, (stats_h + 1), 0);
scrollok(w_slow, true);
w_line2 = newwin(1, max_x, (stats_h + 1) + slow_n, 0);
scrollok(w_line2, false);
wnoutrefresh(w_line2);
w_fast = newwin(fast_n, max_x, (stats_h + 1) + slow_n + 1, 0);
scrollok(w_fast, true);
wattron(w_line1, A_REVERSE);
wattron(w_line2, A_REVERSE);
for(nr=0; nr<max_x; nr++)
{
wprintw(w_line1, " ");
wprintw(w_line2, " ");
}
wattroff(w_line2, A_REVERSE);
wattroff(w_line1, A_REVERSE);
wnoutrefresh(w_line1);
wnoutrefresh(w_line2);
doupdate();
signal(SIGWINCH, handler);
}
static void fresize( TrformStr *TrPtr )
{
int dims[2], dest_dims[2], i, dim, prog=0, delta = 100/12;
double **Re = NULL, **Im = NULL;
char percent[25];
double *re,*im;
// unsigned char *ch;
// int x,y,dy,sy,x1,x2,y1,y2,rx,ry;
int x,y,dy,sy,x1,y1,rx,ry;
dims[0] = TrPtr->src->width;
dims[1] = TrPtr->src->height;
dim = max( TrPtr->src->width * TrPtr->src->height, TrPtr->dest->width * TrPtr->dest->height );
dest_dims[1] = TrPtr->dest->height;
dest_dims[0] = TrPtr->dest->width;
ry = (TrPtr->src->height - TrPtr->dest->height);
rx = (TrPtr->src->width - TrPtr->dest->width);
x1 = min( TrPtr->dest->width/2, TrPtr->src->width/2) ;
y1 = min( TrPtr->dest->height/2, TrPtr->src->height/2) ;
Progress( _initProgress, "Resize Filter" );
Re = (double**)mymalloc( dim * sizeof( double ) );
Im = (double**)mymalloc( dim * sizeof( double ) );
if( Re == NULL || Im == NULL )
{
PrintError("Not enough memory");
TrPtr->success = 0;
goto _antialias_exit;
}
re = *Re; im = *Im;
for( i=0; i<3; i++ )
{
UPDATE_PROGRESS_ANTIALIAS
if( makeDoubleImage ( TrPtr->src, *Re, *Im, i, TrPtr->gamma ) != 0 )
{
PrintError("Could not make Real-version of image");
TrPtr->success = 0;
goto _antialias_exit;
}
fftn (2, dims, *Re, *Im, 1, -1.0 ); // forward transform; don't scale
UPDATE_PROGRESS_ANTIALIAS
if( TrPtr->dest->width < TrPtr->src->width )
{
// Cut frame if decimating
for( y=0; y<y1; y++)
{
sy = y * TrPtr->src->width;
dy = y * TrPtr->dest->width;
for(x=0; x<TrPtr->dest->width; x++)
{
if(x<x1)
{
re[dy+x] = re[sy+x];im[dy+x] = im[sy+x];
}
if(x>=x1)
{
re[dy+x] = re[sy+x+rx];im[dy+x] = im[sy+x+rx];
}
}
}
for( y=y1; y<TrPtr->dest->height; y++ )
{
sy = (y+ry) * TrPtr->src->width;
dy = y * TrPtr->dest->width;
for(x=0; x<TrPtr->dest->width; x++)
{
if(x<x1)
{
re[dy+x] = re[sy+x];im[dy+x] = im[sy+x];
}
if(x>=x1)
{
re[dy+x] = re[sy+x+rx];im[dy+x] = im[sy+x+rx];
}
}
}
}
else
{
// Pad if enlarging
for( y=TrPtr->dest->height-1;y>=y1; y-- )
{
sy = (y+ry) * TrPtr->src->width;
dy = y * TrPtr->dest->width;
for(x=TrPtr->dest->width-1;x>=0; x--)
{
if(x<x1)
{
re[dy+x] = re[sy+x];im[dy+x] = im[sy+x];
}
else if(x>=x1)
{
re[dy+x] = re[sy+x+rx];im[dy+x] = im[sy+x+rx];
}
else
{
re[dy+x] = 0.0;im[dy+x] = 0.0;
}
}
}
for( y=y1-1;y>=0;y--)
{
sy = y * TrPtr->src->width;
dy = y * TrPtr->dest->width;
for(x=TrPtr->dest->width-1;x>=0; x--)
{
if(x<x1)
{
re[dy+x] = re[sy+x];im[dy+x] = im[sy+x];
}
else if(x>=x1)
{
re[dy+x] = re[sy+x+rx];im[dy+x] = im[sy+x+rx];
}
else
{
re[dy+x] = 0.0;im[dy+x] = 0.0;
}
}
}
}
UPDATE_PROGRESS_ANTIALIAS
fftn (2, dest_dims, *Re, *Im, -1, 1.0 ); // backward transform; scale by dim;
UPDATE_PROGRESS_ANTIALIAS
makeUcharImage ( TrPtr->dest, *Re, i );
}
TrPtr->success = 1;
_antialias_exit:
Progress( _disposeProgress, percent );
fft_free();
if( Re != NULL ) myfree( (void**)Re );
if( Im != NULL ) myfree( (void**)Im );
}
AUD_Int32s denoise_wiener( AUD_Int16s *pInBuf, AUD_Int16s *pOutBuf, AUD_Int32s inLen )
{
Fft_16s *hFft = NULL;
Ifft_16s *hIfft = NULL;
AUD_Window16s *hWin = NULL;
_VAD_Nest vadState;
AUD_Int32s frameSize = 512;
AUD_Int32s frameStride = 256;
AUD_Int32s nFFT = frameSize;
AUD_Int32s nSpecLen = nFFT / 2 + 1;
AUD_Int32s nNoiseFrame = (AUD_Int32s)( ( 0.25 * SAMPLE_RATE - frameSize ) / frameStride + 1 );
AUD_Int32s i, j, k;
AUD_Int32s cleanLen = 0;
vadState.meanEn = 280;
vadState.nbSpeechFrame = 0;
vadState.hangOver = 0;
AUD_Int16s *pFrame = (AUD_Int16s*)calloc( frameSize * sizeof(AUD_Int16s), 1 );
AUD_ASSERT( pFrame );
AUD_Double *pNoiseMean = (AUD_Double*)calloc( nSpecLen * sizeof(AUD_Double), 1 );
AUD_ASSERT( pNoiseMean );
AUD_Double *pLamdaD = (AUD_Double*)calloc( nSpecLen * sizeof(AUD_Double), 1 );
AUD_ASSERT( pLamdaD );
AUD_Double *pXi = (AUD_Double*)calloc( nSpecLen * sizeof(AUD_Double), 1 );
AUD_ASSERT( pXi );
AUD_Double *pGamma = (AUD_Double*)calloc( nSpecLen * sizeof(AUD_Double), 1 );
AUD_ASSERT( pGamma );
AUD_Double *pG = (AUD_Double*)calloc( nSpecLen * sizeof(AUD_Double), 1 );
AUD_ASSERT( pG );
AUD_Double *pGammaNew = (AUD_Double*)calloc( nSpecLen * sizeof(AUD_Double), 1 );
AUD_ASSERT( pGammaNew );
for ( j = 0; j < nSpecLen; j++ )
{
pG[j] = 1.;
pGamma[j] = 1.;
}
// FFT
AUD_Int32s *pFFTMag = (AUD_Int32s*)calloc( nFFT * sizeof(AUD_Int32s), 1 );
AUD_ASSERT( pFFTMag );
AUD_Int32s *pFFTRe = (AUD_Int32s*)calloc( nFFT * sizeof(AUD_Int32s), 1 );
AUD_ASSERT( pFFTRe );
AUD_Int32s *pFFTIm = (AUD_Int32s*)calloc( nFFT * sizeof(AUD_Int32s), 1 );
AUD_ASSERT( pFFTIm );
AUD_Double *pFFTCleanMag = (AUD_Double*)calloc( nFFT * sizeof(AUD_Double), 1 );
AUD_ASSERT( pFFTCleanMag );
AUD_Int32s *pFFTCleanRe = (AUD_Int32s*)calloc( nFFT * sizeof(AUD_Int32s), 1 );
AUD_ASSERT( pFFTCleanRe );
AUD_Int32s *pFFTCleanIm = (AUD_Int32s*)calloc( nFFT * sizeof(AUD_Int32s), 1 );
AUD_ASSERT( pFFTCleanIm );
AUD_Int16s *pxClean = (AUD_Int16s*)calloc( nFFT * sizeof(AUD_Int16s), 1 );
AUD_ASSERT( pxClean );
memset( pOutBuf, 0, inLen * sizeof(AUD_Int16s) );
// init hamming module
win16s_init( &hWin, AUD_WIN_HAMM, frameSize, 14 );
AUD_ASSERT( hWin );
// init fft handle
fft_init( &hFft, nFFT, 15 );
AUD_ASSERT( hFft );
// init ifft handle
ifft_init( &hIfft, nFFT, 15 );
AUD_ASSERT( hIfft );
// noise manipulation
for ( i = 0; (i < nNoiseFrame) && ( (i * frameStride + frameSize) < inLen ); i++ )
{
win16s_calc( hWin, pInBuf + i * frameStride, pFrame );
fft_mag( hFft, pFrame, frameSize, pFFTMag );
for ( j = 0; j < nSpecLen; j++ )
{
pNoiseMean[j] += (AUD_Double)pFFTMag[j];
pLamdaD[j] += (AUD_Double)pFFTMag[j] * (AUD_Double)pFFTMag[j];
}
}
// compute noise mean
for ( j = 0; j < nSpecLen; j++ )
{
pNoiseMean[j] /= nNoiseFrame;
pLamdaD[j] /= nNoiseFrame;
}
AUD_Int32s vadFlag = 0;
AUD_Double noiseLen = 9.;
AUD_Double alpha = 0.99;
k = 0;
for ( i = 0; (i * frameStride + frameSize) < inLen; i++ )
{
win16s_calc( hWin, pInBuf + i * frameStride, pFrame );
fft_calc( hFft, pFrame, frameSize, pFFTRe, pFFTIm );
for ( j = 0; j < nSpecLen; j++ )
{
pFFTMag[j] = (AUD_Int32s)round( sqrt( (AUD_Double)pFFTRe[j] * pFFTRe[j] + (AUD_Double)pFFTIm[j] * pFFTIm[j] ) );
}
#if 0
AUDLOG( "noisy FFT:\n" );
for ( j = 0; j < nSpecLen; j++ )
{
AUDLOG( "%d, ", pFFTMag[j] );
}
AUDLOG( "\n" );
#endif
vadFlag = vad_nest( &vadState, pFrame, frameSize );
if ( vadFlag == 0 )
{
for ( j = 0; j < nSpecLen; j++ )
{
pNoiseMean[j] = ( noiseLen * pNoiseMean[j] + (AUD_Double)pFFTMag[j] ) / ( noiseLen + 1. );
pLamdaD[j] = ( noiseLen * pLamdaD[j] + (AUD_Double)pFFTMag[j] * pFFTMag[j] ) / ( noiseLen + 1. );
}
}
for ( j = 0; j < nSpecLen; j++ )
{
pGammaNew[j] = (AUD_Double)pFFTMag[j] * pFFTMag[j] / pLamdaD[j];
pXi[j] = alpha * pG[j] * pG[j] * pGamma[j] + ( 1. - alpha ) * AUD_MAX( pGammaNew[j] - 1., 0. );
pGamma[j] = pGammaNew[j];
pG[j] = pXi[j] / ( pXi[j] + 1. );
pFFTCleanMag[j] = pG[j] * pFFTMag[j];
}
#if 0
AUDLOG( "clean FFT:\n" );
for ( j = 0; j < nSpecLen; j++ )
{
AUDLOG( "%.2f, ", pFFTCleanMag[j] );
}
AUDLOG( "\n" );
#endif
// combine to real/im part of IFFT
pFFTCleanRe[0] = pFFTCleanMag[0];
pFFTCleanIm[0] = 0;
AUD_Double costheta, sintheta;
for ( j = 1; j < nSpecLen; j++ )
{
if ( pFFTMag[j] != 0 )
{
costheta = (AUD_Double)pFFTRe[j] / (AUD_Double)pFFTMag[j];
sintheta = (AUD_Double)pFFTIm[j] / (AUD_Double)pFFTMag[j];
pFFTCleanRe[nFFT - j] = pFFTCleanRe[j] = (AUD_Int32s)round( costheta * pFFTCleanMag[j] );
pFFTCleanIm[j] = (AUD_Int32s)round( sintheta * pFFTCleanMag[j] );
pFFTCleanIm[nFFT - j] = -pFFTCleanIm[j];
}
else
{
pFFTCleanRe[nFFT - j] = pFFTCleanRe[j] = pFFTCleanMag[j];
pFFTCleanIm[nFFT - j] = pFFTCleanIm[j] = 0;
}
}
ifft_real( hIfft, pFFTCleanRe, pFFTCleanIm, nFFT, pxClean );
#if 0
AUDLOG( "clean FFT with phase:\n" );
for ( j = 0; j < nFFT; j++ )
{
AUDLOG( "%d + j%d, ", pFFTCleanRe[j], pFFTCleanIm[j] );
}
AUDLOG( "\n" );
#endif
for ( j = 0; j < frameSize; j++ )
{
pOutBuf[k + j] = pOutBuf[k + j] + pxClean[j];
}
k += frameStride;
cleanLen += frameStride;
}
win16s_free( &hWin );
fft_free( &hFft );
ifft_free( &hIfft );
free( pFrame );
free( pNoiseMean );
free( pLamdaD );
free( pXi );
free( pGamma );
free( pG );
free( pGammaNew );
free( pFFTMag );
free( pFFTRe );
free( pFFTIm );
free( pFFTCleanMag );
free( pFFTCleanRe );
free( pFFTCleanIm );
free( pxClean );
return cleanLen;
}
