Now using fixed-point FFTs in the preprocessor.
[speexdsp.git] / libspeex / preprocess.c
1 /* Copyright (C) 2003 Epic Games (written by Jean-Marc Valin)
2    Copyright (C) 2004-2006 Epic Games 
3    
4    File: preprocess.c
5    Preprocessor with denoising based on the algorithm by Ephraim and Malah
6
7    Redistribution and use in source and binary forms, with or without
8    modification, are permitted provided that the following conditions are
9    met:
10
11    1. Redistributions of source code must retain the above copyright notice,
12    this list of conditions and the following disclaimer.
13
14    2. Redistributions in binary form must reproduce the above copyright
15    notice, this list of conditions and the following disclaimer in the
16    documentation and/or other materials provided with the distribution.
17
18    3. The name of the author may not be used to endorse or promote products
19    derived from this software without specific prior written permission.
20
21    THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
22    IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
23    OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
24    DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT,
25    INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
26    (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
27    SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
28    HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
29    STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31    POSSIBILITY OF SUCH DAMAGE.
32 */
33
34
35 /*
36    Recommended papers:
37    
38    Y. Ephraim and D. Malah, "Speech enhancement using minimum mean-square error
39    short-time spectral amplitude estimator". IEEE Transactions on Acoustics, 
40    Speech and Signal Processing, vol. ASSP-32, no. 6, pp. 1109-1121, 1984.
41    
42    Y. Ephraim and D. Malah, "Speech enhancement using minimum mean-square error
43    log-spectral amplitude estimator". IEEE Transactions on Acoustics, Speech and 
44    Signal Processing, vol. ASSP-33, no. 2, pp. 443-445, 1985.
45    
46    I. Cohen and B. Berdugo, "Speech enhancement for non-stationary noise environments".
47    Signal Processing, vol. 81, no. 2, pp. 2403-2418, 2001.
48
49    Stefan Gustafsson, Rainer Martin, Peter Jax, and Peter Vary. "A psychoacoustic 
50    approach to combined acoustic echo cancellation and noise reduction". IEEE 
51    Transactions on Speech and Audio Processing, 2002.
52    
53    J.-M. Valin, J. Rouat, and F. Michaud, "Microphone array post-filter for separation
54    of simultaneous non-stationary sources". In Proceedings IEEE International 
55    Conference on Acoustics, Speech, and Signal Processing, 2004.
56 */
57
58 #ifdef HAVE_CONFIG_H
59 #include "config.h"
60 #endif
61
62 #include <math.h>
63 #include "speex/speex_preprocess.h"
64 #include "speex/speex_echo.h"
65 #include "misc.h"
66 #include "fftwrap.h"
67 #include "filterbank.h"
68
69 #define max(a,b) ((a) > (b) ? (a) : (b))
70 #define min(a,b) ((a) < (b) ? (a) : (b))
71
72 #ifndef M_PI
73 #define M_PI 3.14159263
74 #endif
75
76 #define LOUDNESS_EXP 2.5
77
78 #define NB_BANDS 24
79
80 #define SPEEX_PROB_START_DEFAULT       0.35f
81 #define SPEEX_PROB_CONTINUE_DEFAULT    0.20f
82 #define NOISE_SUPPRESS_DEFAULT       -25
83 #define ECHO_SUPPRESS_DEFAULT        -45
84 #define ECHO_SUPPRESS_ACTIVE_DEFAULT -15
85
86 #ifndef NULL
87 #define NULL 0
88 #endif
89
90 /** Speex pre-processor state. */
91 struct SpeexPreprocessState_ {
92    /* Basic info */
93    int    frame_size;        /**< Number of samples processed each time */
94    int    ps_size;           /**< Number of points in the power spectrum */
95    int    sampling_rate;     /**< Sampling rate of the input/output */
96    int    nbands;
97    FilterBank *bank;
98    
99    /* Parameters */
100    int    denoise_enabled;
101    int    agc_enabled;
102    float  agc_level;
103    int    vad_enabled;
104    int    dereverb_enabled;
105    float  reverb_decay;
106    float  reverb_level;
107    float  speech_prob_start;
108    float  speech_prob_continue;
109    int    noise_suppress;
110    int    echo_suppress;
111    int    echo_suppress_active;
112    SpeexEchoState *echo_state;
113    
114    /* DSP-related arrays */
115    spx_word16_t *frame;      /**< Processing frame (2*ps_size) */
116    spx_word16_t *ft;         /**< Processing frame in freq domain (2*ps_size) */
117    float *ps;                /**< Current power spectrum */
118    float *gain2;             /**< Adjusted gains */
119    float *gain_floor;        /**< Minimum gain allowed */
120    float *window;            /**< Analysis/Synthesis window */
121    float *noise;             /**< Noise estimate */
122    float *reverb_estimate;   /**< Estimate of reverb energy */
123    float *old_ps;            /**< Power spectrum for last frame */
124    float *gain;              /**< Ephraim Malah gain */
125    float *prior;             /**< A-priori SNR */
126    float *post;              /**< A-posteriori SNR */
127
128    float *S;                 /**< Smoothed power spectrum */
129    float *Smin;              /**< See Cohen paper */
130    float *Stmp;              /**< See Cohen paper */
131    float *update_prob;       /**< Propability of speech presence for noise update */
132
133    float *zeta;              /**< Smoothed a priori SNR */
134
135    float *loudness_weight;   /**< Perceptual loudness curve */
136
137    float *echo_noise;
138    spx_word32_t *residual_echo;
139
140    /* Misc */
141    float *inbuf;             /**< Input buffer (overlapped analysis) */
142    float *outbuf;            /**< Output buffer (for overlap and add) */
143
144    int    was_speech;
145    float  loudness;          /**< loudness estimate */
146    float  loudness2;         /**< loudness estimate */
147    int    nb_adapt;          /**< Number of frames used for adaptation so far */
148    int    nb_loudness_adapt; /**< Number of frames used for loudness adaptation so far */
149    int    min_count;         /**< Number of frames processed so far */
150    void  *fft_lookup;        /**< Lookup table for the FFT */
151
152 };
153
154
155 static void conj_window(float *w, int len)
156 {
157    int i;
158    for (i=0;i<len;i++)
159    {
160       float x=4*((float)i)/len;
161       int inv=0;
162       if (x<1)
163       {
164       } else if (x<2)
165       {
166          x=2-x;
167          inv=1;
168       } else if (x<3)
169       {
170          x=x-2;
171          inv=1;
172       } else {
173          x=4-x;
174       }
175       x*=1.9979;
176       w[i]=(.5-.5*cos(x))*(.5-.5*cos(x));
177       if (inv)
178          w[i]=1-w[i];
179       w[i]=sqrt(w[i]);
180    }
181 }
182
183 /* This function approximates the gain function 
184    y = gamma(1.25)^2 * M(-.25;1;-x) / sqrt(x)  
185    which multiplied by xi/(1+xi) is the optimal gain
186    in the loudness domain ( sqrt[amplitude] )
187 */
188 static inline float hypergeom_gain(float x)
189 {
190    int ind;
191    float integer, frac;
192    static const float table[21] = {
193       0.82157f, 1.02017f, 1.20461f, 1.37534f, 1.53363f, 1.68092f, 1.81865f,
194       1.94811f, 2.07038f, 2.18638f, 2.29688f, 2.40255f, 2.50391f, 2.60144f,
195       2.69551f, 2.78647f, 2.87458f, 2.96015f, 3.04333f, 3.12431f, 3.20326f};
196       
197    integer = floor(2*x);
198    ind = (int)integer;
199    if (ind<0)
200       return 1;
201    if (ind>19)
202       return 1+.1296/x;
203    frac = 2*x-integer;
204    return ((1-frac)*table[ind] + frac*table[ind+1])/sqrt(x+.0001f);
205 }
206
207 static inline float qcurve(float x)
208 {
209    return 1.f/(1.f+.15f/(x));
210 }
211
212 SpeexPreprocessState *speex_preprocess_state_init(int frame_size, int sampling_rate)
213 {
214    int i;
215    int N, N3, N4, M;
216
217    SpeexPreprocessState *st = (SpeexPreprocessState *)speex_alloc(sizeof(SpeexPreprocessState));
218    st->frame_size = frame_size;
219
220    /* Round ps_size down to the nearest power of two */
221 #if 0
222    i=1;
223    st->ps_size = st->frame_size;
224    while(1)
225    {
226       if (st->ps_size & ~i)
227       {
228          st->ps_size &= ~i;
229          i<<=1;
230       } else {
231          break;
232       }
233    }
234    
235    
236    if (st->ps_size < 3*st->frame_size/4)
237       st->ps_size = st->ps_size * 3 / 2;
238 #else
239    st->ps_size = st->frame_size;
240 #endif
241
242    N = st->ps_size;
243    N3 = 2*N - st->frame_size;
244    N4 = st->frame_size - N3;
245    
246    st->sampling_rate = sampling_rate;
247    st->denoise_enabled = 1;
248    st->agc_enabled = 0;
249    st->agc_level = 8000;
250    st->vad_enabled = 0;
251    st->dereverb_enabled = 0;
252    st->reverb_decay = .0;
253    st->reverb_level = .0;
254    st->noise_suppress = NOISE_SUPPRESS_DEFAULT;
255    st->echo_suppress = ECHO_SUPPRESS_DEFAULT;
256    st->echo_suppress_active = ECHO_SUPPRESS_ACTIVE_DEFAULT;
257
258    st->speech_prob_start = SPEEX_PROB_START_DEFAULT;
259    st->speech_prob_continue = SPEEX_PROB_CONTINUE_DEFAULT;
260
261    st->echo_state = NULL;
262    
263    st->nbands = NB_BANDS;
264    M = st->nbands;
265    st->bank = filterbank_new(M, sampling_rate, N, 1);
266    
267    st->frame = (spx_word16_t*)speex_alloc(2*N*sizeof(float));
268    st->window = (float*)speex_alloc(2*N*sizeof(float));
269    st->ft = (spx_word16_t*)speex_alloc(2*N*sizeof(float));
270    
271    st->ps = (float*)speex_alloc((N+M)*sizeof(float));
272    st->noise = (float*)speex_alloc((N+M)*sizeof(float));
273    st->echo_noise = (float*)speex_alloc((N+M)*sizeof(float));
274    st->residual_echo = (spx_word32_t*)speex_alloc((N+M)*sizeof(float));
275    st->reverb_estimate = (float*)speex_alloc((N+M)*sizeof(float));
276    st->old_ps = (float*)speex_alloc((N+M)*sizeof(float));
277    st->prior = (float*)speex_alloc((N+M)*sizeof(float));
278    st->post = (float*)speex_alloc((N+M)*sizeof(float));
279    st->gain = (float*)speex_alloc((N+M)*sizeof(float));
280    st->gain2 = (float*)speex_alloc((N+M)*sizeof(float));
281    st->gain_floor = (float*)speex_alloc((N+M)*sizeof(float));
282    st->zeta = (float*)speex_alloc((N+M)*sizeof(float));
283    
284    st->S = (float*)speex_alloc(N*sizeof(float));
285    st->Smin = (float*)speex_alloc(N*sizeof(float));
286    st->Stmp = (float*)speex_alloc(N*sizeof(float));
287    st->update_prob = (float*)speex_alloc(N*sizeof(float));
288    
289    st->loudness_weight = (float*)speex_alloc(N*sizeof(float));
290    st->inbuf = (float*)speex_alloc(N3*sizeof(float));
291    st->outbuf = (float*)speex_alloc(N3*sizeof(float));
292
293    conj_window(st->window, 2*N3);
294    for (i=2*N3;i<2*st->ps_size;i++)
295       st->window[i]=1;
296    
297    if (N4>0)
298    {
299       for (i=N3-1;i>=0;i--)
300       {
301          st->window[i+N3+N4]=st->window[i+N3];
302          st->window[i+N3]=1;
303       }
304    }
305    for (i=0;i<N+M;i++)
306    {
307       st->noise[i]=1.;
308       st->reverb_estimate[i]=0.;
309       st->old_ps[i]=1.;
310       st->gain[i]=1;
311       st->post[i]=1;
312       st->prior[i]=1;
313    }
314
315    for (i=0;i<N;i++)
316       st->update_prob[i] = 1;
317    for (i=0;i<N3;i++)
318    {
319       st->inbuf[i]=0;
320       st->outbuf[i]=0;
321    }
322
323    for (i=0;i<N;i++)
324    {
325       float ff=((float)i)*.5*sampling_rate/((float)N);
326       st->loudness_weight[i] = .35f-.35f*ff/16000.f+.73f*exp(-.5f*(ff-3800)*(ff-3800)/9e5f);
327       if (st->loudness_weight[i]<.01f)
328          st->loudness_weight[i]=.01f;
329       st->loudness_weight[i] *= st->loudness_weight[i];
330    }
331
332    st->was_speech = 0;
333    st->loudness = pow(6000,LOUDNESS_EXP);
334    st->loudness2 = 6000;
335    st->nb_loudness_adapt = 0;
336
337    st->fft_lookup = spx_fft_init(2*N);
338
339    st->nb_adapt=0;
340    st->min_count=0;
341    return st;
342 }
343
344 void speex_preprocess_state_destroy(SpeexPreprocessState *st)
345 {
346    speex_free(st->frame);
347    speex_free(st->ft);
348    speex_free(st->ps);
349    speex_free(st->gain2);
350    speex_free(st->gain_floor);
351    speex_free(st->window);
352    speex_free(st->noise);
353    speex_free(st->reverb_estimate);
354    speex_free(st->old_ps);
355    speex_free(st->gain);
356    speex_free(st->prior);
357    speex_free(st->post);
358    speex_free(st->loudness_weight);
359    speex_free(st->echo_noise);
360    speex_free(st->residual_echo);
361
362    speex_free(st->S);
363    speex_free(st->Smin);
364    speex_free(st->Stmp);
365    speex_free(st->update_prob);
366    speex_free(st->zeta);
367
368    speex_free(st->inbuf);
369    speex_free(st->outbuf);
370
371    spx_fft_destroy(st->fft_lookup);
372    filterbank_destroy(st->bank);
373    speex_free(st);
374 }
375
376 static void speex_compute_agc(SpeexPreprocessState *st)
377 {
378    int i;
379    int N = st->ps_size;
380    float scale=.5f/N;
381    float agc_gain;
382    int freq_start, freq_end;
383    float active_bands = 0;
384
385    freq_start = (int)(300.0f*2*N/st->sampling_rate);
386    freq_end   = (int)(2000.0f*2*N/st->sampling_rate);
387    for (i=freq_start;i<freq_end;i++)
388    {
389       if (st->S[i] > 20.f*st->Smin[i]+1000.f)
390          active_bands+=1;
391    }
392    active_bands /= (freq_end-freq_start+1);
393
394    if (active_bands > .2f)
395    {
396       float loudness=0.f;
397       float rate, rate2=.2f;
398       st->nb_loudness_adapt++;
399       rate=2.0f/(1+st->nb_loudness_adapt);
400       if (rate < .05f)
401          rate = .05f;
402       if (rate < .1f && pow(loudness, LOUDNESS_EXP) > st->loudness)
403          rate = .1f;
404       if (rate < .2f && pow(loudness, LOUDNESS_EXP) > 3.f*st->loudness)
405          rate = .2f;
406       if (rate < .4f && pow(loudness, LOUDNESS_EXP) > 10.f*st->loudness)
407          rate = .4f;
408
409       for (i=2;i<N;i++)
410       {
411          loudness += scale*st->ps[i] * st->gain2[i] * st->gain2[i] * st->loudness_weight[i];
412       }
413       loudness=sqrt(loudness);
414       /*if (loudness < 2*pow(st->loudness, 1.0/LOUDNESS_EXP) &&
415         loudness*2 > pow(st->loudness, 1.0/LOUDNESS_EXP))*/
416       st->loudness = (1-rate)*st->loudness + (rate)*pow(loudness, LOUDNESS_EXP);
417       
418       st->loudness2 = (1-rate2)*st->loudness2 + rate2*pow(st->loudness, 1.0f/LOUDNESS_EXP);
419
420       loudness = pow(st->loudness, 1.0f/LOUDNESS_EXP);
421
422       /*fprintf (stderr, "%f %f %f\n", loudness, st->loudness2, rate);*/
423    }
424    
425    agc_gain = st->agc_level/st->loudness2;
426    /*fprintf (stderr, "%f %f %f %f\n", active_bands, st->loudness, st->loudness2, agc_gain);*/
427    if (agc_gain>200)
428       agc_gain = 200;
429
430    for (i=0;i<N;i++)
431       st->gain2[i] *= agc_gain;
432    
433 }
434
435 static void preprocess_analysis(SpeexPreprocessState *st, spx_int16_t *x)
436 {
437    int i;
438    int N = st->ps_size;
439    int N3 = 2*N - st->frame_size;
440    int N4 = st->frame_size - N3;
441    float *ps=st->ps;
442
443    /* 'Build' input frame */
444    for (i=0;i<N3;i++)
445       st->frame[i]=st->inbuf[i];
446    for (i=0;i<st->frame_size;i++)
447       st->frame[N3+i]=x[i];
448    
449    /* Update inbuf */
450    for (i=0;i<N3;i++)
451       st->inbuf[i]=x[N4+i];
452
453    /* Windowing */
454    for (i=0;i<2*N;i++)
455       st->frame[i] *= st->window[i];
456
457    /* Perform FFT */
458    spx_fft(st->fft_lookup, st->frame, st->ft);
459          
460    /* Power spectrum */
461    ps[0]=1;
462    for (i=1;i<N;i++)
463       ps[i]=1+MULT16_16(st->ft[2*i-1],st->ft[2*i-1]) + MULT16_16(st->ft[2*i],st->ft[2*i]);
464
465    filterbank_compute_bank(st->bank, ps, ps+N);
466 }
467
468 static void update_noise_prob(SpeexPreprocessState *st)
469 {
470    int i;
471    int min_range;
472    int N = st->ps_size;
473
474    for (i=1;i<N-1;i++)
475       st->S[i] = 100.f+ .8f*st->S[i] + .05f*st->ps[i-1]+.1f*st->ps[i]+.05f*st->ps[i+1];
476    st->S[0] = 100.f+ .8f*st->S[0] + .2*st->ps[0];
477    st->S[N-1] = 100.f+ .8f*st->S[N-1] + .2*st->ps[N-1];
478    
479    if (st->nb_adapt==1)
480    {
481       for (i=0;i<N;i++)
482          st->Smin[i] = st->Stmp[i] = 0.f;
483    }
484
485    if (st->nb_adapt < 100)
486       min_range = 15;
487    else if (st->nb_adapt < 1000)
488       min_range = 50;
489    else if (st->nb_adapt < 10000)
490       min_range = 150;
491    else
492       min_range = 300;
493    if (st->min_count > min_range)
494    {
495       st->min_count = 0;
496       for (i=0;i<N;i++)
497       {
498          st->Smin[i] = min(st->Stmp[i], st->S[i]);
499          st->Stmp[i] = st->S[i];
500       }
501    } else {
502       for (i=0;i<N;i++)
503       {
504          st->Smin[i] = min(st->Smin[i], st->S[i]);
505          st->Stmp[i] = min(st->Stmp[i], st->S[i]);      
506       }
507    }
508    for (i=0;i<N;i++)
509    {
510       st->update_prob[i] *= .2f;
511       if (st->S[i] > 2.5*st->Smin[i])
512          st->update_prob[i] += .8f;
513       /*fprintf (stderr, "%f ", st->S[i]/st->Smin[i]);*/
514       /*fprintf (stderr, "%f ", st->update_prob[i]);*/
515    }
516
517 }
518
519 #define NOISE_OVERCOMPENS 1.
520
521 void speex_echo_get_residual(SpeexEchoState *st, spx_word32_t *Yout, int len);
522
523 int speex_preprocess(SpeexPreprocessState *st, spx_int16_t *x, spx_int32_t *echo)
524 {
525    return speex_preprocess_run(st, x);
526 }
527
528 int speex_preprocess_run(SpeexPreprocessState *st, spx_int16_t *x)
529 {
530    int i;
531    int M;
532    int N = st->ps_size;
533    int N3 = 2*N - st->frame_size;
534    int N4 = st->frame_size - N3;
535    float *ps=st->ps;
536    float Zframe=0, Pframe;
537    float beta, beta_1;
538    float echo_floor;
539    float noise_floor;
540    
541    st->nb_adapt++;
542    st->min_count++;
543    
544    beta =1.0f/st->nb_adapt;
545    if (beta < .03f)
546       beta=.03f;
547    beta_1 = 1.0f-beta;
548    M = st->nbands;
549    /* Deal with residual echo if provided */
550    if (st->echo_state)
551    {
552       speex_echo_get_residual(st->echo_state, st->residual_echo, N);
553       for (i=0;i<N;i++)
554          st->echo_noise[i] = MAX32(.6f*st->echo_noise[i], st->residual_echo[i]);
555       filterbank_compute_bank(st->bank, st->echo_noise, st->echo_noise+N);
556    } else {
557       for (i=0;i<N+M;i++)
558          st->echo_noise[i] = 0;
559    }
560    preprocess_analysis(st, x);
561
562    update_noise_prob(st);
563
564    /* Noise estimation always updated for the 10 first frames */
565    /*if (st->nb_adapt<10)
566    {
567       for (i=1;i<N-1;i++)
568          st->update_prob[i] = 0;
569    }
570    */
571    
572    /* Update the noise estimate for the frequencies where it can be */
573    for (i=0;i<N;i++)
574    {
575       if (st->update_prob[i]<.5f || st->ps[i] < st->noise[i])
576          st->noise[i] = beta_1*st->noise[i] + beta*NOISE_OVERCOMPENS*st->ps[i];
577    }
578    filterbank_compute_bank(st->bank, st->noise, st->noise+N);
579
580    /* Special case for first frame */
581    if (st->nb_adapt==1)
582       for (i=0;i<N+M;i++)
583          st->old_ps[i] = ps[i];
584
585    /* Compute a posteriori SNR */
586    for (i=0;i<N+M;i++)
587    {
588       float gamma = .1;
589       float tot_noise = 1.f+ st->noise[i] + st->echo_noise[i] + st->reverb_estimate[i];
590       st->post[i] = ps[i]/tot_noise - 1.f;
591       if (st->post[i]>100.f)
592          st->post[i]=100.f;
593       /*gamma = .15+.85*st->prior[i]*st->prior[i]/((1+st->prior[i])*(1+st->prior[i]));*/
594       gamma = .1+.9*(st->old_ps[i]/(1+st->old_ps[i]+tot_noise))*(st->old_ps[i]/(1+st->old_ps[i]+tot_noise));
595       /* A priori SNR update */
596       st->prior[i] = gamma*max(0.0f,st->post[i]) + (1.f-gamma)*st->old_ps[i]/tot_noise;
597       if (st->prior[i]>100.f)
598          st->prior[i]=100.f;
599    }
600
601    /*print_vec(st->post, N+M, "");*/
602
603    /* Recursive average of the a priori SNR. A bit smoothed for the psd components */
604    st->zeta[0] = .7f*st->zeta[0] + .3f*st->prior[0];
605    for (i=1;i<N-1;i++)
606       st->zeta[i] = .7f*st->zeta[i] + .3f*(.5f*st->prior[i]+.25f*st->prior[i+1]+.25f*st->prior[i-1]);
607    for (i=N-1;i<N+M;i++)
608       st->zeta[i] = .7f*st->zeta[i] + .3f*st->prior[i];
609
610    /* Speech probability of presence for the entire frame is based on the average filterbank a priori SNR */
611    Zframe = 0;
612    for (i=N;i<N+M;i++)
613       Zframe += st->zeta[i];
614    Zframe /= st->nbands;
615    Pframe = .1+.9*qcurve(Zframe);
616    
617    noise_floor = exp(.2302585f*st->noise_suppress);
618    echo_floor = exp(.2302585f* (st->echo_suppress*(1-Pframe) + st->echo_suppress_active*Pframe));
619    /*print_vec(&Pframe, 1, "");*/
620    /*for (i=N;i<N+M;i++)
621       st->gain2[i] = qcurve (st->zeta[i]);
622    filterbank_compute_psd(st->bank,st->gain2+N, st->gain2);*/
623    
624    for (i=N;i<N+M;i++)
625    {
626       float theta, MM;
627       float prior_ratio;
628       float q;
629       float P1;
630
631       /* Compute the gain floor based on different floors for the background noise and residual echo */
632       st->gain_floor[i] = sqrt((noise_floor*st->noise[i] + echo_floor*st->echo_noise[i])/(1+st->noise[i] + st->echo_noise[i]));
633       prior_ratio = st->prior[i]/(1.f+st->prior[i]);
634       theta = (1.f+st->post[i])*prior_ratio;
635
636       MM = hypergeom_gain(theta);
637       st->gain[i] = prior_ratio * MM;
638       /* Bound on the gain */
639       if (st->gain[i]>1.f)
640          st->gain[i]=1.f;
641       
642       /* Save old Bark power spectrum */
643       st->old_ps[i] = .2*st->old_ps[i] + .8*st->gain[i]*st->gain[i]*ps[i];
644
645       P1 = .2+.8*qcurve (st->zeta[i]);
646       q = 1-Pframe*P1;
647       st->gain2[i]=1.f/(1.f + (q/(1.f-q))*(1.f+st->prior[i])*exp(-theta));
648    }
649    filterbank_compute_psd(st->bank,st->gain2+N, st->gain2);
650    filterbank_compute_psd(st->bank,st->gain+N, st->gain);
651    
652    /* Use 1 for linear gain resolution (best) or 0 for Bark gain resolution (faster) */
653    if (1)
654    {
655       filterbank_compute_psd(st->bank,st->gain_floor+N, st->gain_floor);
656    
657       /* Compute gain according to the Ephraim-Malah algorithm */
658       for (i=0;i<N;i++)
659       {
660          float MM;
661          float theta;
662          float prior_ratio;
663          float p;
664          float g;
665          
666          /* Wiener filter gain */
667          prior_ratio = st->prior[i]/(1.f+st->prior[i]);
668          theta = (1.f+st->post[i])*prior_ratio;
669          p = st->gain2[i];
670          
671          /* Optimal estimator for loudness domain */
672          MM = hypergeom_gain(theta);
673          g = prior_ratio * MM;
674          
675          /* Constrain the gain to be close to the Bark scale gain */
676          if (g > 3*st->gain[i])
677             g = 3*st->gain[i];
678          st->gain[i] = g;
679          
680          /* Bound on the gain */
681          if (st->gain[i]>1.f)
682             st->gain[i]=1.f;
683          
684          /* Save old power spectrum */
685          st->old_ps[i] = .2*st->old_ps[i] + .8*st->gain[i]*st->gain[i]*ps[i];
686          
687          /* Apply gain floor */
688          if (st->gain[i] < st->gain_floor[i])
689             st->gain[i] = st->gain_floor[i];
690          
691          /* Exponential decay model for reverberation (unused) */
692          /*st->reverb_estimate[i] = st->reverb_decay*st->reverb_estimate[i] + st->reverb_decay*st->reverb_level*st->gain[i]*st->gain[i]*st->ps[i];*/
693          
694          /* Take into account speech probability of presence (loudness domain MMSE estimator) */
695          st->gain2[i]=(p*sqrt(st->gain[i])+sqrt(st->gain_floor[i])*(1-p)) * (p*sqrt(st->gain[i])+sqrt(st->gain_floor[i])*(1-p));
696          
697          /* Use this if you want a log-domain MMSE estimator instead */
698          /*st->gain2[i] = pow(st->gain[i], p) * pow(st->gain_floor[i],1.f-p);*/
699          
700       }
701    } else {
702       for (i=N;i<N+M;i++)
703       {
704          float p = st->gain2[i];
705          if (st->gain[i] < st->gain_floor[i])
706             st->gain[i] = st->gain_floor[i];
707          st->gain2[i]=(p*sqrt(st->gain[i])+sqrt(st->gain_floor[i])*(1-p)) * (p*sqrt(st->gain[i])+sqrt(st->gain_floor[i])*(1-p));
708       }
709       filterbank_compute_psd(st->bank,st->gain2+N, st->gain2);
710       
711    }
712    
713    if (!st->denoise_enabled)
714    {
715       for (i=0;i<N+M;i++)
716          st->gain2[i]=1.f;
717    }
718    
719    if (st->agc_enabled)
720       speex_compute_agc(st);
721
722    /* Apply computed gain */
723    for (i=1;i<N;i++)
724    {
725       st->ft[2*i-1] *= st->gain2[i];
726       st->ft[2*i] *= st->gain2[i];
727    }
728    st->ft[0] *= st->gain2[0];
729    st->ft[2*N-1] *= st->gain2[N-1];
730
731    /* Inverse FFT with 1/N scaling */
732    spx_ifft(st->fft_lookup, st->ft, st->frame);
733
734    {
735       float max_sample=0;
736       for (i=0;i<2*N;i++)
737          if (fabs(st->frame[i])>max_sample)
738             max_sample = fabs(st->frame[i]);
739       if (max_sample>28000.f)
740       {
741          float damp = 28000.f/max_sample;
742          for (i=0;i<2*N;i++)
743             st->frame[i] *= damp;
744       }
745    }
746
747    for (i=0;i<2*N;i++)
748       st->frame[i] *= st->window[i];
749
750    /* Perform overlap and add */
751    for (i=0;i<N3;i++)
752       x[i] = st->outbuf[i] + st->frame[i];
753    for (i=0;i<N4;i++)
754       x[N3+i] = st->frame[N3+i];
755    
756    /* Update outbuf */
757    for (i=0;i<N3;i++)
758       st->outbuf[i] = st->frame[st->frame_size+i];
759
760    if (st->vad_enabled)
761    {
762       if (Pframe > st->speech_prob_start || (st->was_speech && Pframe > st->speech_prob_continue))
763       {
764          st->was_speech=1;
765          return 1;
766       } else
767       {
768          st->was_speech=0;
769          return 0;
770       }
771    } else {
772       return 1;
773    }
774 }
775
776 void speex_preprocess_estimate_update(SpeexPreprocessState *st, spx_int16_t *x, spx_int32_t *echo)
777 {
778    int i;
779    int N = st->ps_size;
780    int N3 = 2*N - st->frame_size;
781    int M;
782    float *ps=st->ps;
783
784    M = st->nbands;
785    st->min_count++;
786    
787    preprocess_analysis(st, x);
788
789    update_noise_prob(st);
790    
791    for (i=1;i<N-1;i++)
792    {
793       if (st->update_prob[i]<.5f || st->ps[i] < st->noise[i])
794       {
795          if (echo)
796             st->noise[i] = .95f*st->noise[i] + .1f*max(1.0f,st->ps[i]-1.0*echo[i]);
797          else
798             st->noise[i] = .95f*st->noise[i] + .1f*st->ps[i];
799       }
800    }
801
802    for (i=0;i<N3;i++)
803       st->outbuf[i] = x[st->frame_size-N3+i]*st->window[st->frame_size+i];
804
805    /* Save old power spectrum */
806    for (i=0;i<N+M;i++)
807       st->old_ps[i] = ps[i];
808
809    for (i=1;i<N;i++)
810       st->reverb_estimate[i] *= st->reverb_decay;
811 }
812
813
814 int speex_preprocess_ctl(SpeexPreprocessState *state, int request, void *ptr)
815 {
816    int i;
817    SpeexPreprocessState *st;
818    st=(SpeexPreprocessState*)state;
819    switch(request)
820    {
821    case SPEEX_PREPROCESS_SET_DENOISE:
822       st->denoise_enabled = (*(int*)ptr);
823       break;
824    case SPEEX_PREPROCESS_GET_DENOISE:
825       (*(int*)ptr) = st->denoise_enabled;
826       break;
827
828    case SPEEX_PREPROCESS_SET_AGC:
829       st->agc_enabled = (*(int*)ptr);
830       break;
831    case SPEEX_PREPROCESS_GET_AGC:
832       (*(int*)ptr) = st->agc_enabled;
833       break;
834
835    case SPEEX_PREPROCESS_SET_AGC_LEVEL:
836       st->agc_level = (*(float*)ptr);
837       if (st->agc_level<1)
838          st->agc_level=1;
839       if (st->agc_level>32768)
840          st->agc_level=32768;
841       break;
842    case SPEEX_PREPROCESS_GET_AGC_LEVEL:
843       (*(float*)ptr) = st->agc_level;
844       break;
845
846    case SPEEX_PREPROCESS_SET_VAD:
847       speex_warning("The VAD has been removed pending a complete rewrite");
848       st->vad_enabled = (*(spx_int32_t*)ptr);
849       break;
850    case SPEEX_PREPROCESS_GET_VAD:
851       (*(spx_int32_t*)ptr) = st->vad_enabled;
852       break;
853    
854    case SPEEX_PREPROCESS_SET_DEREVERB:
855       st->dereverb_enabled = (*(int*)ptr);
856       for (i=0;i<st->ps_size;i++)
857          st->reverb_estimate[i]=0;
858       break;
859    case SPEEX_PREPROCESS_GET_DEREVERB:
860       (*(int*)ptr) = st->dereverb_enabled;
861       break;
862
863    case SPEEX_PREPROCESS_SET_DEREVERB_LEVEL:
864       st->reverb_level = (*(float*)ptr);
865       break;
866    case SPEEX_PREPROCESS_GET_DEREVERB_LEVEL:
867       (*(float*)ptr) = st->reverb_level;
868       break;
869    
870    case SPEEX_PREPROCESS_SET_DEREVERB_DECAY:
871       st->reverb_decay = (*(float*)ptr);
872       break;
873    case SPEEX_PREPROCESS_GET_DEREVERB_DECAY:
874       (*(float*)ptr) = st->reverb_decay;
875       break;
876
877    case SPEEX_PREPROCESS_SET_PROB_START:
878       st->speech_prob_start = (*(int*)ptr) / 100.0;
879       if ( st->speech_prob_start > 1 || st->speech_prob_start < 0 )
880          st->speech_prob_start = SPEEX_PROB_START_DEFAULT;
881       break;
882    case SPEEX_PREPROCESS_GET_PROB_START:
883       (*(int*)ptr) = st->speech_prob_start * 100;
884       break;
885
886    case SPEEX_PREPROCESS_SET_PROB_CONTINUE:
887       st->speech_prob_continue = (*(int*)ptr) / 100.0;
888       if ( st->speech_prob_continue > 1 || st->speech_prob_continue < 0 )
889          st->speech_prob_continue = SPEEX_PROB_CONTINUE_DEFAULT;
890       break;
891    case SPEEX_PREPROCESS_GET_PROB_CONTINUE:
892       (*(int*)ptr) = st->speech_prob_continue * 100;
893       break;
894
895    case SPEEX_PREPROCESS_SET_NOISE_SUPPRESS:
896       st->noise_suppress = -ABS(*(spx_int32_t*)ptr);
897       break;
898    case SPEEX_PREPROCESS_GET_NOISE_SUPPRESS:
899       (*(spx_int32_t*)ptr) = st->noise_suppress;
900       break;
901    case SPEEX_PREPROCESS_SET_ECHO_SUPPRESS:
902       st->echo_suppress = -ABS(*(spx_int32_t*)ptr);
903       break;
904    case SPEEX_PREPROCESS_GET_ECHO_SUPPRESS:
905       (*(spx_int32_t*)ptr) = st->echo_suppress;
906       break;
907    case SPEEX_PREPROCESS_SET_ECHO_SUPPRESS_ACTIVE:
908       st->echo_suppress_active = -ABS(*(spx_int32_t*)ptr);
909       break;
910    case SPEEX_PREPROCESS_GET_ECHO_SUPPRESS_ACTIVE:
911       (*(spx_int32_t*)ptr) = st->echo_suppress_active;
912       break;
913    case SPEEX_PREPROCESS_SET_ECHO_STATE:
914       st->echo_state = (SpeexEchoState*)ptr;
915       break;
916    case SPEEX_PREPROCESS_GET_ECHO_STATE:
917       ptr = (void*)st->echo_state;
918       break;
919
920    default:
921       speex_warning_int("Unknown speex_preprocess_ctl request: ", request);
922       return -1;
923    }
924    return 0;
925 }