void cc(float *y,float *H)
{
// Complete el código
fftr(y, NFFT, 1); // FFT directa
// Complete el código
fftr(y, NFFT, -1); // FFT inversa
}
void fftcep(double *sp, const int flng, double *c, const int m, int itr,
double ac)
{
double temp;
static double *x = NULL, *y;
static double size;
int k;
if (x == NULL) {
x = dgetmem(flng + flng);
y = x + flng;
}
if (flng > size) {
free(x);
x = dgetmem(flng + flng);
y = x + flng;
size = flng;
}
movem(sp, x, sizeof(*sp), flng);
fftr(x, y, flng);
for (k = 0; k < flng; k++)
x[k] /= flng;
for (k = 0; k <= m; k++) {
c[k] = x[k];
x[k] = 0;
}
ac += 1.0;
while (--itr > 0) {
for (k = 1; k <= m; k++)
x[flng - k] = x[k];
fftr(x, y, flng);
for (k = 0; k < flng; k++)
if (x[k] < 0.0)
x[k] = 0.0;
else
x[k] /= flng;
fftr(x, y, flng);
for (k = 0; k <= m; k++) {
temp = x[k] * ac;
c[k] += temp;
x[k] -= temp;
}
}
c[0] *= 0.5;
if (m == flng / 2)
c[m] *= 0.5;
}
void phase(double *p, const int mp, double *z, const int mz, double *ph,
const int flng, const int unlap)
{
static double *x;
static int fsize = 0;
double *y, *xx, *yy, *py;
int no, i, offset;
double pi;
pi = atan(1.) * 4.;
no = flng / 2 + 1;
if (flng > fsize) {
if (x != NULL)
free(x);
fsize = flng;
x = dgetmem(4 * flng + no);
}
y = &x[flng];
xx = &y[flng];
yy = &xx[flng];
py = &yy[flng];
fillz(x, sizeof(*x), flng);
fillz(xx, sizeof(*xx), flng);
movem(z, x, mz + 1, sizeof(*z));
movem(p, xx, mp + 1, sizeof(*p));
fftr(x, y, flng);
xx[0] = 1;
fftr(xx, yy, flng);
for (i = 0; i < no; i++) {
ph[i] = x[i] * xx[i] + y[i] * yy[i];
py[i] = y[i] * xx[i] - x[i] * yy[i];
}
offset = 0;
i = 0;
ph[i] = atan2(py[i], ph[i]) / pi;
i++;
for (; i < no; i++) {
ph[i] = atan2(py[i], ph[i]) / pi;
if (unlap) {
if (ph[i - 1] - ph[i] - offset > 1)
offset += 2;
else if (ph[i] + offset - ph[i - 1] > 1)
offset -= 2;
ph[i] += offset;
}
}
return;
}
void c2sp(double *c, const int m, double *x, double *y, const int l)
{
int m1;
m1 = m + 1;
movem(c, x, sizeof(*c), m1);
fillz(x + m1, sizeof(*x), l - m1);
fftr(x, y, l);
}
void grpdelay(double *x, double *gd, const int size, const int is_arma)
{
static double *y;
static int fsize;
double *u, *v;
int k, size_2;
if (fsize < size) {
if (y != NULL)
free(y);
fsize = size;
y = dgetmem(3 * size);
}
movem(x, gd, sizeof(*x), size);
u = y + size;
v = u + size;
size_2 = size / 2;
if (is_arma)
gd[0] = 1;
for (k = 0; k < size; ++k)
u[k] = gd[k] * k;
fftr(gd, y, size);
fftr(u, v, size);
for (k = 0; k <= size_2; k++) {
gd[k] = (gd[k] * u[k] + y[k] * v[k]) / (gd[k] * gd[k] + y[k] * y[k]);
if (is_arma)
gd[k] *= -1;
}
return;
}
void init_process() // Filtrado FIR con coeficientes obtenidos en MATLAB
{
// Pegue entre corchetes en la definición de hh la lista de los coeficientes
//obtenidos con MATLAB
float hh[]={0};
int n;
for (n=0; n<L_FIR; n++)
h[n] = hh[n];
for (; n<NFFT; n++)
h[n] = 0.0;
fftr(h, NFFT, 1);
}
int ifftr(double *x, double *y, const int l)
{
int i;
double *xp, *yp;
fftr(x, y, l);
xp = x;
yp = y;
i = l;
while (i--) {
*xp++ /= l;
*yp++ /= -l;
}
return (0);
}
void c2ndps(double *c, const int m, double *n, const int l)
{
int i;
double *tmp;
fillz(n, sizeof(*n), l);
tmp = dgetmem(l);
// generate mc(m)
for (i = 1; i < m + 1; i++) {
n[i] = c[i] * i / 2.0;
n[l - i] = n[i];
}
fftr(n, tmp, l);
free(tmp);
}
void analisi(float *io_data, float *blockIn, float *x, NRstruct *nr)
{
// Suposa que cada bloc de processament (tram) conté dos blocs de dades.
// Usa el vector auxiliar blockIn.
// Posa a x la transformada de Fourier del tram de senyal.
int n;
for(n=0;n<IO_N;n++)
blockIn[n+IO_N] = io_data[n]; // Posa el bloc actual al final de blockIn
for(n=0;n<N;n++)
x[n] = blockIn[n]*(nr->w[n]); // Multiplica el tram que s'analitza per la finestra
fftr(x,N,1); // FFT directa
for(n=0;n<IO_N;n++)
blockIn[n] = blockIn[n+IO_N]; // Posa el bloc actual al principi de blockIn per a la iteració següent
}
void sintesi(float *io_data, float *blockOut, float *x)
{
// Suposa que cada bloc de processament (tram) conté dos blocs de dades.
// Usa el vector auxiliar blockOut.
// Rep a través de x la transformada de Fourier del tram de senyal modificat.
int n;
fftr(x, N, -1); // FFT inversa
for(n=0;n<N;n++)
blockOut[n]+=x[n]; // Acumula el tram actual amb la part solapada de l'anterior
for (n=0;n<IO_N;n++)
io_data[n] = blockOut[n]; // Passa a la sortida D/A el bloc resultant de l'acumulació
//Prepara blockOut per a la iteració següent
/*************************************************************************/
/*** ESCRIURE CODI ***/
/*************************************************************************/
}
int smcep(double *xw, const int flng, double *mc, const int m, const int fftsz,
const double a, const double t, const int itr1, const int itr2,
const double dd, const int etype, const double e, const double f,
const int itype)
{
int i, j;
int flag = 0, f2, m2;
double u, s, eps = 0.0, min, max;
static double *x = NULL, *y, *c, *d, *al, *b;
static int size_x, size_d;
if (etype == 1 && e < 0.0) {
fprintf(stderr, "smcep : value of e must be e>=0!\n");
exit(1);
}
if (etype == 2 && e >= 0.0) {
fprintf(stderr, "smcep : value of E must be E<0!\n");
exit(1);
}
if (etype == 1) {
eps = e;
}
if (x == NULL) {
x = dgetmem(3 * flng);
y = x + flng;
c = y + flng;
size_x = flng;
d = dgetmem(3 * m + 3);
al = d + (m + 1);
b = al + (m + 1);
size_d = m;
}
if (flng > size_x) {
free(x);
x = dgetmem(3 * flng);
y = x + flng;
c = y + flng;
size_x = flng;
}
if (m > size_d) {
free(d);
d = dgetmem(3 * m + 3);
al = d + (m + 1);
b = al + (m + 1);
size_d = m;
}
f2 = flng / 2.;
m2 = m + m;
movem(xw, x, sizeof(*x), flng);
switch (itype) {
case 0: /* windowed data sequence */
fftr(x, y, flng);
for (i = 0; i < flng; i++) {
x[i] = x[i] * x[i] + y[i] * y[i] + eps; /* periodogram */
}
break;
case 1: /* dB */
for (i = 0; i <= flng / 2; i++) {
x[i] = exp((x[i] / 20.0) * log(10.0)); /* dB -> amplitude spectrum */
x[i] = x[i] * x[i] + eps; /* amplitude -> periodogram */
}
break;
case 2: /* log */
for (i = 0; i <= flng / 2; i++) {
x[i] = exp(x[i]); /* log -> amplitude spectrum */
x[i] = x[i] * x[i] + eps; /* amplitude -> periodogram */
}
break;
case 3: /* amplitude */
for (i = 0; i <= flng / 2; i++) {
x[i] = x[i] * x[i] + eps; /* amplitude -> periodogram */
}
break;
case 4: /* periodogram */
for (i = 0; i <= flng / 2; i++) {
x[i] = x[i] + eps;
}
break;
default:
fprintf(stderr, "smcep : Input type %d is not supported!\n", itype);
exit(1);
}
if (itype > 0) {
for (i = 1; i < flng / 2; i++)
x[flng - i] = x[i];
}
if (etype == 2 && e < 0.0) {
max = x[0];
for (i = 1; i < flng; i++) {
if (max < x[i])
max = x[i];
}
max = sqrt(max);
min = max * pow(10.0, e / 20.0); /* floor is 20*log10(min/max) */
min = min * min;
for (i = 0; i < flng; i++) {
if (x[i] < min)
x[i] = min;
}
}
for (i = 0; i < flng; i++)
c[i] = log(x[i]);
/* 1, (-a), (-a)^2, ..., (-a)^M */
al[0] = 1.0;
for (i = 1; i <= m; i++)
al[i] = 0.0;
frqt_a(al, m, fftsz, a, t);
/* initial value of cepstrum */
ifftr(c, y, flng); /* c : IFFT[x] */
c[0] /= 2.0;
c[flng / 2] /= 2.0;
freqt2(c, f2, mc, m, fftsz, a, t); /* mc : mel cep. */
s = c[0];
/* Newton Raphson method */
for (j = 1; j <= itr2; j++) {
fillz(c, sizeof(*c), flng);
ifreqt2(mc, m, c, f2, fftsz, a, t); /* mc : mel cep. */
fftr(c, y, flng); /* c, y : FFT[mc] */
for (i = 0; i < flng; i++)
c[i] = x[i] / exp(c[i] + c[i]);
ifftr(c, y, flng);
frqtr2(c, f2, c, m2, fftsz, a, t); /* c : r(k) */
u = c[0];
if (j >= itr1) {
if (fabs((u - s) / u) < dd) {
flag = 1;
break;
}
s = u;
}
for (i = 0; i <= m; i++)
b[i] = c[i] - al[i];
for (i = 0; i <= m2; i++)
y[i] = c[i];
for (i = 0; i <= m2; i += 2)
y[i] -= c[0];
for (i = 2; i <= m; i += 2)
c[i] += c[0];
c[0] += c[0];
if (theq(c, y, d, b, m + 1, f)) {
fprintf(stderr, "smcep : Error in theq() at %dth iteration!\n", j);
exit(1);
}
for (i = 0; i <= m; i++)
mc[i] += d[i];
}
if (flag)
return (0);
else
return (-1);
}
int main(int argc, char *argv[])
{
FILE *fp;
char *s, *infile = NULL, c;
int size = SIZE, nout = 0, k, nd = -1, out = ' ';
double *x, *y;
if ((cmnd = strrchr(argv[0], '/')) == NULL)
cmnd = argv[0];
else
cmnd++;
while (--argc) {
if (*(s = *++argv) == '-') {
c = *++s;
if ((c == 'l' || c == 'm') && (*++s == '\0')) {
s = *++argv;
--argc;
}
switch (c) {
case 'l':
size = atoi(s);
break;
case 'm':
nd = atoi(s) + 1;
break;
case 'H':
nout = 1;
break;
case 'i':
case 'p':
case 'r':
c -= ('a' - 'A');
case 'A':
case 'I':
case 'P':
case 'R':
out = c;
break;
case 'h':
usage(0);
default:
fprintf(stderr, "%s : Invalid option '%c'!\n", cmnd, c);
usage(1);
}
} else
infile = s;
}
if (nd == -1)
nd = size;
if (nd > size) {
fprintf(stderr,
"%s : Order of sequence %d should be less than the FFT size %d!\n",
cmnd, nd, size);
return (1);
}
nout = (nout) ? size / 2 + 1 : size;
fp = stdin;
if (infile) {
fp = getfp(infile, "rb");
}
x = dgetmem(size + size);
y = x + size;
while (!feof(fp)) {
fillz(x, size, sizeof(*x));
if (freadf(x, sizeof(*x), nd, fp) == 0)
break;
fftr(x, y, size);
if (out == 'P')
for (k = 0; k < size; k++)
x[k] = x[k] * x[k] + y[k] * y[k];
else if (out == 'A')
for (k = 0; k < size; k++)
x[k] = sqrt(x[k] * x[k] + y[k] * y[k]);
if (out != 'I')
fwritef(x, sizeof(*x), nout, stdout);
if (out == ' ' || out == 'I')
fwritef(y, sizeof(*y), nout, stdout);
}
if (infile) {
fclose(fp);
}
return (0);
}
double mlsacheck(double *in, double *out, int m, int fftlen,
double a, double r, int c)
{
int i;
double gain, *x, *y, *mag = NULL, max = 0.0;
x = dgetmem(fftlen);
y = dgetmem(fftlen);
fillz(x, sizeof(*x), fftlen);
fillz(y, sizeof(*y), fftlen);
/* calculate gain factor */
for (i = 0, gain = 0.0; i <= m; i++) {
x[i] = in[i];
gain += x[i] * pow(-a, i);
}
/* gain normalization */
x[0] -= gain;
/* check stability */
if (c != 1 && c != 4) { /* usual mode */
mag = dgetmem(fftlen);
fillz(mag, sizeof(*mag), fftlen);
fftr(x, y, fftlen);
for (i = 0; i < fftlen; i++) {
mag[i] = sqrt(x[i] * x[i] + y[i] * y[i]);
if (mag[i] > max)
max = mag[i];
}
} else { /* fast mode */
for (i = 0; i <= m; i++)
max += x[i];
}
/* modify MLSA filter coefficients */
if (c == 0 || c == 1 || max <= r) {
memcpy(out, in, sizeof(*out) * (m + 1));
} else {
if (c == 2) { /* clipping */
for (i = 0; i < fftlen; i++) {
if (mag[i] > r) {
x[i] *= r / mag[i];
y[i] *= r / mag[i];
}
}
ifft(x, y, fftlen);
x[0] += gain;
memcpy(out, x, sizeof(*out) * (m + 1));
} else if (c == 3) { /* scaling */
for (i = 0; i < fftlen; i++) {
x[i] *= r / max;
y[i] *= r / max;
}
ifft(x, y, fftlen);
x[0] += gain;
memcpy(out, x, sizeof(*out) * (m + 1));
} else if (c == 4) { /* fast mode */
for (i = 0; i <= m; i++)
x[i] *= r / max;
x[0] += gain;
memcpy(out, x, sizeof(*out) * (m + 1));
}
}
free(x);
free(y);
if (c != 1 && c != 4) {
free(mag);
}
return max;
}
double * mlsacheck(double *mcep, int m, int fftlen, int frame,
double a, double R1, double R2,
int modify_filter, int stable_condition)
{
int i;
double K, r, *x, *y, *mag;
x = dgetmem(fftlen);
y = dgetmem(fftlen);
mag = dgetmem(fftlen / 2);
for (i = 0; i < fftlen; i++)
{
x[i] = 0;
y[i] = 0;
}
for (i = 0; i < m + 1; i++)
x[i] = mcep[i];
/* calculate gain factor */
for (i = 0, K = 0.0; i < m + 1; i++)
K += x[i] * pow(a, i);
/* gain normalization */
x[0] -= K;
fftr(x, y, fftlen);
/* check stability */
for (i = 0; i < fftlen / 2; i++)
{
mag[i] = x[i] * x[i] + y[i] * y[i];
mag[i] = sqrt(mag[i]);
if (mag[i] > R1 || mag[i] > R2) /* unstable */
{
/* output ascii report */
// fprintf(stderr, "[ unstable frame number : %d ]\n", frame);
// for (i = 0; i < m + 1; i++)
// fprintf(stderr, "%f\n", mcep[i]);
if (modify_filter == TR) /* -c option */
{
switch (stable_condition) /* -r option */
{
case STABLE1:
if (mag[i] > R1)
{
r = R1 / mag[i];
x[i] *= r;
y[i] *= r;
x[fftlen - 1 - i] *= r;
y[fftlen - 1 - i] *= r;
}
break;
case STABLE2:
if (mag[i] > R2)
{
r = R2 / mag[i];
x[i] *= r;
y[i] *= r;
x[fftlen - 1 - i] *= r;
y[fftlen - 1 - i] *= r;
}
}
}
}
}
ifft(x, y, fftlen);
/* revert gain factor */
x[0] += K;
for (i = 0; i < m + 1; i++)
mcep[i] = x[i];
// fwrite(x, sizeof(*x), m + 1, stdout);
//free(x);
free(y);
free(mag);
return( x );
}
void WaveInData(WPARAM wParam, LPARAM lParam)
{
enum
{FREQ_TIME = 64,
DISPLAY_TIME = 16,
SPECTRUM_TIME = 4};
// Create buffers for processing the audio data
static double buffer[SAMPLES];
static complex x[SAMPLES];
static double xa[RANGE];
static double xp[RANGE];
static double xf[RANGE];
static double fps = (double)SAMPLE_RATE / (double)SAMPLES;
static double expect = 2.0 * M_PI * (double)STEP / (double)SAMPLES;
// Initialise data structs
if (spectrum.data == NULL)
{
spectrum.data = xa;
spectrum.length = RANGE;
}
// Copy the input data
memmove(buffer, buffer + STEP, (SAMPLES - STEP) * sizeof(double));
short *data = (short *)((WAVEHDR *)lParam)->lpData;
for (int i = 0; i < STEP; i++)
buffer[SAMPLES - STEP + i] = (double)data[i];
// Give the buffer back
waveInAddBuffer(audio.hwi, (WAVEHDR *)lParam, sizeof(WAVEHDR));
// Maximum data value
static double dmax;
if (dmax < 4096.0)
dmax = 4096.0;
// Calculate normalising value
double norm = dmax;
dmax = 0.0;
// Copy data to FFT input arrays
for (int i = 0; i < SAMPLES; i++)
{
// Find the magnitude
if (dmax < fabs(buffer[i]))
dmax = fabs(buffer[i]);
// Calculate the window
double window =
0.5 - 0.5 * cos(2.0 * M_PI *
i / SAMPLES);
// Normalise and window the input data
x[i].r = (double)buffer[i] / norm * window;
}
// do FFT
fftr(x, SAMPLES);
// Process FFT output
for (int i = 1; i < RANGE; i++)
{
double real = x[i].r;
double imag = x[i].i;
xa[i] = hypot(real, imag);
// Do frequency calculation
double p = atan2(imag, real);
double dp = xp[i] - p;
xp[i] = p;
// Calculate phase difference
dp -= (double)i * expect;
int qpd = dp / M_PI;
if (qpd >= 0)
qpd += qpd & 1;
else
qpd -= qpd & 1;
dp -= M_PI * (double)qpd;
// Calculate frequency difference
double df = OVERSAMPLE * dp / (2.0 * M_PI);
// Calculate actual frequency from slot frequency plus
// frequency difference
xf[i] = (i * fps + df * fps);
}
// Maximum FFT output
double max = 0.0;
double f = 0.0;
// Find maximum value
for (int i = 1; i < RANGE - 1; i++)
{
if (xa[i] > max)
{
max = xa[i];
f = xf[i];
}
}
static long freqTimer;
if (max > MIN)
{
display.f = f;
freqTimer = 0;
}
else
{
if (freqTimer == 64)
display.f = 0.0;
}
freqTimer++;
double level = 0.0;
for (int i = 0; i < STEP; i++)
level += ((double)data[i] / 32768.0) *
((double)data[i] / 32768.0);
level = sqrt(level / STEP) * 2.0;
double dB = log10(level) * 20.0;
if (dB < -80.0)
dB = -80.0;
display.l = dB;
meter.l = level / pow(10.0, 0.15);
static long displayTimer;
// Update display
if ((displayTimer % SPECTRUM_TIME) == 0)
InvalidateRgn(spectrum.hwnd, NULL, TRUE);
if ((displayTimer % DISPLAY_TIME) == 0)
InvalidateRgn(display.hwnd, NULL, TRUE);
displayTimer++;
}
void mlsacheck(double *mcep, int m, int fftlen, int frame,
double a, double r, int c)
{
int i;
double gain, *x, *y, *mag = NULL, max = 0.0;
x = dgetmem(fftlen);
y = dgetmem(fftlen);
fillz(x, sizeof(*x), fftlen);
fillz(y, sizeof(*y), fftlen);
/* calculate gain factor */
for (i = 0, gain = 0.0; i <= m; i++) {
x[i] = mcep[i];
gain += x[i] * pow(-a, i);
}
/* gain normalization */
x[0] -= gain;
/* check stability */
if (c == 0 || c == 2 || c == 3) { /* usual mode */
mag = dgetmem(fftlen);
fillz(mag, sizeof(*mag), fftlen);
fftr(x, y, fftlen);
for (i = 0; i < fftlen; i++) {
mag[i] = sqrt(x[i] * x[i] + y[i] * y[i]);
if (mag[i] > max)
max = mag[i];
}
} else { /* fast mode */
for (i = 0; i <= m; i++)
max += x[i];
}
/* modification MLSA filter coefficients */
if (max > r) {
/* output ascii report */
fprintf(stderr,
"[No. %d] is unstable frame (maximum = %f, threshold = %f)\n",
frame, max, r);
/* modification */
if (c == 2) { /* clipping */
for (i = 0; i < fftlen; i++) {
if (mag[i] > r) {
x[i] *= r / mag[i];
y[i] *= r / mag[i];
}
}
} else if (c == 3) { /* scaling */
for (i = 0; i < fftlen; i++) {
x[i] *= r / max;
y[i] *= r / max;
}
} else if (c == 4) { /* fast mode */
for (i = 0; i <= m; i++)
x[i] *= r / max;
}
}
/* output MLSA filter coefficients */
if (c == 0 || c == 1 || max <= r) { /* no modification */
fwritef(mcep, sizeof(*mcep), m + 1, stdout);
} else {
if (c == 2 || c == 3)
ifft(x, y, fftlen);
x[0] += gain;
fwritef(x, sizeof(*x), m + 1, stdout);
}
free(x);
free(y);
if (c == 0 || c == 2 || c == 3)
free(mag);
}
int gcep(double *xw, const int flng, double *gc, const int m, const double g,
const int itr1, const int itr2, const double d, const double e,
const double f, const int itype)
{
int i, j, flag = 0;
double t, s, dd = 0.0;
static double *x = NULL, *y, *cr, *ci, *rr, *hr, *hi, *er, *ei;
static int size;
if (x == NULL) {
x = dgetmem(9 * flng);
size = flng;
y = x + flng;
cr = y + flng;
ci = cr + flng;
rr = ci + flng;
hr = rr + flng;
hi = hr + flng;
er = hi + flng;
ei = er + flng;
}
if (flng > size) {
free(x);
x = dgetmem(9 * flng);
size = flng;
y = x + flng;
cr = y + flng;
ci = cr + flng;
rr = ci + flng;
hr = rr + flng;
hi = hr + flng;
er = hi + flng;
ei = er + flng;
}
movem(xw, x, sizeof(*x), flng);
switch (itype) {
case 0: /* windowed data sequence */
fftr(x, y, flng);
for (i = 0; i < flng; i++) {
x[i] = x[i] * x[i] + y[i] * y[i] + e; /* periodegram */
}
break;
case 1: /* dB */
for (i = 0; i <= flng / 2; i++) {
x[i] /= 20.0 / log(10.0); /* dB -> amplitude spectrum */
x[i] = x[i] * x[i] + e; /* amplitude -> periodgram */
}
break;
case 2: /* log */
for (i = 0; i <= flng / 2; i++) {
x[i] = exp(x[i]); /* log -> amplitude spectrum */
x[i] = x[i] * x[i] + e; /* amplitude -> periodgram */
}
break;
case 3: /* amplitude */
for (i = 0; i <= flng / 2; i++) {
x[i] = x[i] * x[i] + e; /* amplitude -> periodgram */
}
break;
case 4: /* periodgram */
for (i = 0; i <= flng / 2; i++) {
x[i] = x[i] + e;
}
break;
default:
fprintf(stderr, "mgcep : Input type %d is not supported!\n", itype);
exit(1);
}
if (itype > 0) {
for (i = 1; i < flng / 2; i++)
x[flng - i] = x[i];
}
for (i = 0; i < flng; i++)
cr[i] = log(x[i]);
/* initial value of generalized cepstrum */
ifftr(cr, y, flng); /* x : IFFT[x] */
cr[0] = exp(cr[0] / 2);
gc2gc(cr, m, 0.0, gc, m, g); /* gc : generalized cepstrum */
/* Newton-Raphson method */
for (j = 1; j <= itr2; j++) {
fillz(cr, sizeof(*cr), flng);
movem(&gc[1], &cr[1], sizeof(*cr), m);
fftr(cr, ci, flng); /* cr+jci : FFT[gc] */
for (i = 0; i < flng; i++) {
t = x[i] / agexp(g, cr[i], ci[i]);
cr[i] = 1 + g * cr[i];
ci[i] = g * ci[i];
s = cr[i] * cr[i] + ci[i] * ci[i];
rr[i] = t / s;
hr[i] = (cr[i] * cr[i] - ci[i] * ci[i]) * t / (s * s);
hi[i] = 2 * cr[i] * ci[i] * t / (s * s);
er[i] = cr[i] * t / s;
ei[i] = ci[i] * t / s;
}
ifftr(rr, y, flng); /* rr : r(k) */
ifft(hr, hi, flng); /* hr : h(k) */
ifft(er, ei, flng); /* er : e(k) */
s = gc[0]; /* gc[0] : gain */
for (i = 1, t = 0.0; i <= m; i++)
t += er[i] * gc[i];
t = er[0] + g * t;
t = sqrt(fabs(t));
if (j >= itr1) {
if (fabs((t - dd) / t) < d) {
flag = 1;
break;
}
dd = t;
}
for (i = 2; i <= m + m; i++)
hr[i] *= 1 + g;
if (theq(rr, &hr[2], &y[1], &er[1], m, f)) {
fprintf(stderr, "gcep : Error in theq() at %dth iteration!\n", j);
exit(1);
}
gc[0] = t;
for (i = 1; i <= m; i++)
gc[i] += y[i];
}
if (flag)
return (0);
else
return (-1);
}
void WaveInData(WPARAM wParam, LPARAM lParam)
{
// Create buffers for processing the audio data
static double buffer[SAMPLES];
static complex x[STEP];
static double xa[RANGE];
static double K = 2.0 * M_PI / (double)SAMPLE_RATE;
// Initialise data structs
if (spectrum.data == NULL)
{
spectrum.data = xa;
spectrum.length = RANGE;
}
// Copy the input data
memmove(buffer, buffer + STEP, (SAMPLES - STEP) * sizeof(double));
short *data = (short *)((WAVEHDR *)lParam)->lpData;
for (int i = 0; i < STEP; i++)
buffer[SAMPLES - STEP + i] = (double)data[i];
// Give the buffer back
waveInAddBuffer(audio.hwi, (WAVEHDR *)lParam, sizeof(WAVEHDR));
// Maximum data value
static double dmax;
if (dmax < 4096.0)
dmax = 4096.0;
// Calculate normalising value
double norm = dmax;
dmax = 0.0;
// Copy data to FFT input arrays
for (int i = 0; i < STEP; i++)
{
// Find the magnitude
if (dmax < fabs(buffer[i]))
dmax = fabs(buffer[i]);
// Calculate the window
double window =
0.5 - 0.5 * cos(2.0 * M_PI *
i / STEP);
// Normalise and window the input data
x[i].r = (double)buffer[i] / norm * window;
}
// do FFT
fftr(x, STEP);
// Process FFT output
for (int i = 1; i < RANGE; i++)
{
double real = x[i].r;
double imag = x[i].i;
xa[i] = hypot(real, imag);
}
// Do cross correlation
double imag = 0.0;
double real = 0.0;
for (int i = 0; i < SAMPLES; i++)
{
double window =
(0.5 - 0.5 * cos(2.0 * M_PI * i / SAMPLES));
imag += (buffer[i] / 32768.0) * window * sin(i * display.f * K);
real += (buffer[i] / 32768.0) * window * cos(i * display.f * K);
}
double level = hypot(real, imag);
level = level / (SAMPLES / (4.0 * sqrt(2.0)));
meter.l = level / pow(10.0, 0.15);
double dB = log10(level) * 20.0;
if (dB < -80.0)
dB = -80.0;
static long timer;
// Update display
if ((timer % 4) == 0)
InvalidateRgn(spectrum.hwnd, NULL, TRUE);
if ((timer % 16) == 0)
{
display.l = dB;
InvalidateRgn(display.hwnd, NULL, TRUE);
}
timer++;
}
