a priori SNR and a posteriori SNR computed with fixed-point
[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 #define SQR(x) ((x)*(x))
91 #define SQR16(x) (MULT16_16((x),(x)))
92 #define SQR16_Q15(x) (MULT16_16_Q15((x),(x)))
93
94 #ifdef FIXED_POINT
95 static inline spx_word16_t DIV32_16_Q8(spx_word32_t a, spx_word32_t b)
96 {
97    if (SHR32(a,8) >= b)
98    {
99       return 32767;
100    } else {
101       if (b>=QCONST32(1,23))
102       {
103          a = SHR32(a,8);
104          b = SHR32(b,8);
105       }
106       if (b>=QCONST32(1,19))
107       {
108          a = SHR32(a,4);
109          b = SHR32(b,4);
110       }
111       if (b>=QCONST32(1,15))
112       {
113          a = SHR32(a,4);
114          b = SHR32(b,4);
115       }
116       a = SHL32(a,8);
117       return DIV32_16(a,b);
118    }
119    
120 }
121 static inline spx_word16_t DIV32_16_Q15(spx_word32_t a, spx_word32_t b)
122 {
123    if (SHR32(a,15) >= b)
124    {
125       return 32767;
126    } else {
127       if (b>=QCONST32(1,23))
128       {
129          a = SHR32(a,8);
130          b = SHR32(b,8);
131       }
132       if (b>=QCONST32(1,19))
133       {
134          a = SHR32(a,4);
135          b = SHR32(b,4);
136       }
137       if (b>=QCONST32(1,15))
138       {
139          a = SHR32(a,4);
140          b = SHR32(b,4);
141       }
142       a = SHL32(a,15);      
143       return DIV32_16(a,b);
144    }
145 }
146 #define SNR_SCALING 256.f
147 #define SNR_SCALING_1 0.0039062f
148 #define SNR_SHIFT 8
149
150 #define GAIN_SCALING 32768.f
151 #define GAIN_SCALING_1 3.0518e-05
152 #define GAIN_SHIFT 1
153
154 #else
155
156 #define DIV32_16_Q8(a,b) ((a)/(b))
157 #define DIV32_16_Q15(a,b) ((a)/(b))
158 #define SNR_SCALING 1.f
159 #define SNR_SCALING_1 1.f
160 #define SNR_SHIFT 0
161 #define GAIN_SCALING 1.f
162 #define GAIN_SCALING_1 1.f
163 #define GAIN_SHIFT 0
164
165 #endif
166
167
168 /** Speex pre-processor state. */
169 struct SpeexPreprocessState_ {
170    /* Basic info */
171    int    frame_size;        /**< Number of samples processed each time */
172    int    ps_size;           /**< Number of points in the power spectrum */
173    int    sampling_rate;     /**< Sampling rate of the input/output */
174    int    nbands;
175    FilterBank *bank;
176    
177    /* Parameters */
178    int    denoise_enabled;
179    int    agc_enabled;
180    float  agc_level;
181    int    vad_enabled;
182    int    dereverb_enabled;
183    float  reverb_decay;
184    float  reverb_level;
185    float  speech_prob_start;
186    float  speech_prob_continue;
187    int    noise_suppress;
188    int    echo_suppress;
189    int    echo_suppress_active;
190    SpeexEchoState *echo_state;
191    
192    /* DSP-related arrays */
193    spx_word16_t *frame;      /**< Processing frame (2*ps_size) */
194    spx_word16_t *ft;         /**< Processing frame in freq domain (2*ps_size) */
195    spx_word32_t *ps;         /**< Current power spectrum */
196    float *gain2;             /**< Adjusted gains */
197    float *gain_floor;        /**< Minimum gain allowed */
198    spx_word16_t *window;     /**< Analysis/Synthesis window */
199    spx_word32_t *noise;      /**< Noise estimate */
200    spx_word32_t *reverb_estimate; /**< Estimate of reverb energy */
201    spx_word32_t *old_ps;     /**< Power spectrum for last frame */
202    float *gain;              /**< Ephraim Malah gain */
203    float *prior;             /**< A-priori SNR */
204    float *post;              /**< A-posteriori SNR */
205
206    spx_word32_t *S;          /**< Smoothed power spectrum */
207    spx_word32_t *Smin;       /**< See Cohen paper */
208    spx_word32_t *Stmp;       /**< See Cohen paper */
209    float *update_prob;       /**< Propability of speech presence for noise update */
210
211    float *zeta;              /**< Smoothed a priori SNR */
212
213    float *loudness_weight;   /**< Perceptual loudness curve */
214
215    spx_word32_t *echo_noise;
216    spx_word32_t *residual_echo;
217
218    /* Misc */
219    spx_word16_t *inbuf;      /**< Input buffer (overlapped analysis) */
220    spx_word16_t *outbuf;     /**< Output buffer (for overlap and add) */
221
222    int    was_speech;
223    float  loudness;          /**< loudness estimate */
224    float  loudness2;         /**< loudness estimate */
225    int    nb_adapt;          /**< Number of frames used for adaptation so far */
226    int    nb_loudness_adapt; /**< Number of frames used for loudness adaptation so far */
227    int    min_count;         /**< Number of frames processed so far */
228    void  *fft_lookup;        /**< Lookup table for the FFT */
229
230 };
231
232
233 static void conj_window(spx_word16_t *w, int len)
234 {
235    int i;
236    for (i=0;i<len;i++)
237    {
238       float tmp;
239       float x=4*((float)i)/len;
240       int inv=0;
241       if (x<1)
242       {
243       } else if (x<2)
244       {
245          x=2-x;
246          inv=1;
247       } else if (x<3)
248       {
249          x=x-2;
250          inv=1;
251       } else {
252          x=4-x;
253       }
254       x*=1.9979;
255       tmp=(.5-.5*cos(x))*(.5-.5*cos(x));
256       if (inv)
257          tmp=1-tmp;
258       w[i]=QCONST16(.999,15)*sqrt(tmp);
259    }
260 }
261
262 /* This function approximates the gain function 
263    y = gamma(1.25)^2 * M(-.25;1;-x) / sqrt(x)  
264    which multiplied by xi/(1+xi) is the optimal gain
265    in the loudness domain ( sqrt[amplitude] )
266 */
267 static inline float hypergeom_gain(float x)
268 {
269    int ind;
270    float integer, frac;
271    static const float table[21] = {
272       0.82157f, 1.02017f, 1.20461f, 1.37534f, 1.53363f, 1.68092f, 1.81865f,
273       1.94811f, 2.07038f, 2.18638f, 2.29688f, 2.40255f, 2.50391f, 2.60144f,
274       2.69551f, 2.78647f, 2.87458f, 2.96015f, 3.04333f, 3.12431f, 3.20326f};
275       
276    integer = floor(2*x);
277    ind = (int)integer;
278    if (ind<0)
279       return 1;
280    if (ind>19)
281       return 1+.1296/x;
282    frac = 2*x-integer;
283    return ((1-frac)*table[ind] + frac*table[ind+1])/sqrt(x+.0001f);
284 }
285
286 static inline float qcurve(float x)
287 {
288    return 1.f/(1.f+.15f/(x));
289 }
290
291 SpeexPreprocessState *speex_preprocess_state_init(int frame_size, int sampling_rate)
292 {
293    int i;
294    int N, N3, N4, M;
295
296    SpeexPreprocessState *st = (SpeexPreprocessState *)speex_alloc(sizeof(SpeexPreprocessState));
297    st->frame_size = frame_size;
298
299    /* Round ps_size down to the nearest power of two */
300 #if 0
301    i=1;
302    st->ps_size = st->frame_size;
303    while(1)
304    {
305       if (st->ps_size & ~i)
306       {
307          st->ps_size &= ~i;
308          i<<=1;
309       } else {
310          break;
311       }
312    }
313    
314    
315    if (st->ps_size < 3*st->frame_size/4)
316       st->ps_size = st->ps_size * 3 / 2;
317 #else
318    st->ps_size = st->frame_size;
319 #endif
320
321    N = st->ps_size;
322    N3 = 2*N - st->frame_size;
323    N4 = st->frame_size - N3;
324    
325    st->sampling_rate = sampling_rate;
326    st->denoise_enabled = 1;
327    st->agc_enabled = 0;
328    st->agc_level = 8000;
329    st->vad_enabled = 0;
330    st->dereverb_enabled = 0;
331    st->reverb_decay = .0;
332    st->reverb_level = .0;
333    st->noise_suppress = NOISE_SUPPRESS_DEFAULT;
334    st->echo_suppress = ECHO_SUPPRESS_DEFAULT;
335    st->echo_suppress_active = ECHO_SUPPRESS_ACTIVE_DEFAULT;
336
337    st->speech_prob_start = SPEEX_PROB_START_DEFAULT;
338    st->speech_prob_continue = SPEEX_PROB_CONTINUE_DEFAULT;
339
340    st->echo_state = NULL;
341    
342    st->nbands = NB_BANDS;
343    M = st->nbands;
344    st->bank = filterbank_new(M, sampling_rate, N, 1);
345    
346    st->frame = (spx_word16_t*)speex_alloc(2*N*sizeof(float));
347    st->window = (spx_word16_t*)speex_alloc(2*N*sizeof(float));
348    st->ft = (spx_word16_t*)speex_alloc(2*N*sizeof(float));
349    
350    st->ps = (spx_word32_t*)speex_alloc((N+M)*sizeof(float));
351    st->noise = (spx_word32_t*)speex_alloc((N+M)*sizeof(float));
352    st->echo_noise = (spx_word32_t*)speex_alloc((N+M)*sizeof(float));
353    st->residual_echo = (spx_word32_t*)speex_alloc((N+M)*sizeof(float));
354    st->reverb_estimate = (spx_word32_t*)speex_alloc((N+M)*sizeof(float));
355    st->old_ps = (spx_word32_t*)speex_alloc((N+M)*sizeof(float));
356    st->prior = (float*)speex_alloc((N+M)*sizeof(float));
357    st->post = (float*)speex_alloc((N+M)*sizeof(float));
358    st->gain = (float*)speex_alloc((N+M)*sizeof(float));
359    st->gain2 = (float*)speex_alloc((N+M)*sizeof(float));
360    st->gain_floor = (float*)speex_alloc((N+M)*sizeof(float));
361    st->zeta = (float*)speex_alloc((N+M)*sizeof(float));
362    
363    st->S = (spx_word32_t*)speex_alloc(N*sizeof(float));
364    st->Smin = (spx_word32_t*)speex_alloc(N*sizeof(float));
365    st->Stmp = (spx_word32_t*)speex_alloc(N*sizeof(float));
366    st->update_prob = (float*)speex_alloc(N*sizeof(float));
367    
368    st->loudness_weight = (float*)speex_alloc(N*sizeof(float));
369    st->inbuf = (spx_word16_t*)speex_alloc(N3*sizeof(float));
370    st->outbuf = (spx_word16_t*)speex_alloc(N3*sizeof(float));
371
372    conj_window(st->window, 2*N3);
373    for (i=2*N3;i<2*st->ps_size;i++)
374       st->window[i]=QCONST16(.999,15);
375    
376    if (N4>0)
377    {
378       for (i=N3-1;i>=0;i--)
379       {
380          st->window[i+N3+N4]=st->window[i+N3];
381          st->window[i+N3]=1;
382       }
383    }
384    for (i=0;i<N+M;i++)
385    {
386       st->noise[i]=1.;
387       st->reverb_estimate[i]=0.;
388       st->old_ps[i]=1.;
389       st->gain[i]=1;
390       st->post[i]=1;
391       st->prior[i]=1;
392    }
393
394    for (i=0;i<N;i++)
395       st->update_prob[i] = 1;
396    for (i=0;i<N3;i++)
397    {
398       st->inbuf[i]=0;
399       st->outbuf[i]=0;
400    }
401
402    for (i=0;i<N;i++)
403    {
404       float ff=((float)i)*.5*sampling_rate/((float)N);
405       st->loudness_weight[i] = .35f-.35f*ff/16000.f+.73f*exp(-.5f*(ff-3800)*(ff-3800)/9e5f);
406       if (st->loudness_weight[i]<.01f)
407          st->loudness_weight[i]=.01f;
408       st->loudness_weight[i] *= st->loudness_weight[i];
409    }
410
411    st->was_speech = 0;
412    st->loudness = pow(6000,LOUDNESS_EXP);
413    st->loudness2 = 6000;
414    st->nb_loudness_adapt = 0;
415
416    st->fft_lookup = spx_fft_init(2*N);
417
418    st->nb_adapt=0;
419    st->min_count=0;
420    return st;
421 }
422
423 void speex_preprocess_state_destroy(SpeexPreprocessState *st)
424 {
425    speex_free(st->frame);
426    speex_free(st->ft);
427    speex_free(st->ps);
428    speex_free(st->gain2);
429    speex_free(st->gain_floor);
430    speex_free(st->window);
431    speex_free(st->noise);
432    speex_free(st->reverb_estimate);
433    speex_free(st->old_ps);
434    speex_free(st->gain);
435    speex_free(st->prior);
436    speex_free(st->post);
437    speex_free(st->loudness_weight);
438    speex_free(st->echo_noise);
439    speex_free(st->residual_echo);
440
441    speex_free(st->S);
442    speex_free(st->Smin);
443    speex_free(st->Stmp);
444    speex_free(st->update_prob);
445    speex_free(st->zeta);
446
447    speex_free(st->inbuf);
448    speex_free(st->outbuf);
449
450    spx_fft_destroy(st->fft_lookup);
451    filterbank_destroy(st->bank);
452    speex_free(st);
453 }
454
455 static void speex_compute_agc(SpeexPreprocessState *st)
456 {
457    int i;
458    int N = st->ps_size;
459    float scale=.5f/N;
460    float agc_gain;
461    int freq_start, freq_end;
462    float active_bands = 0;
463
464    freq_start = (int)(300.0f*2*N/st->sampling_rate);
465    freq_end   = (int)(2000.0f*2*N/st->sampling_rate);
466    for (i=freq_start;i<freq_end;i++)
467    {
468       if (st->S[i] > 20.f*st->Smin[i]+1000.f)
469          active_bands+=1;
470    }
471    active_bands /= (freq_end-freq_start+1);
472
473    if (active_bands > .2f)
474    {
475       float loudness=0.f;
476       float rate, rate2=.2f;
477       st->nb_loudness_adapt++;
478       rate=2.0f/(1+st->nb_loudness_adapt);
479       if (rate < .05f)
480          rate = .05f;
481       if (rate < .1f && pow(loudness, LOUDNESS_EXP) > st->loudness)
482          rate = .1f;
483       if (rate < .2f && pow(loudness, LOUDNESS_EXP) > 3.f*st->loudness)
484          rate = .2f;
485       if (rate < .4f && pow(loudness, LOUDNESS_EXP) > 10.f*st->loudness)
486          rate = .4f;
487
488       for (i=2;i<N;i++)
489       {
490          loudness += scale*st->ps[i] * st->gain2[i] * st->gain2[i] * st->loudness_weight[i];
491       }
492       loudness=sqrt(loudness);
493       /*if (loudness < 2*pow(st->loudness, 1.0/LOUDNESS_EXP) &&
494         loudness*2 > pow(st->loudness, 1.0/LOUDNESS_EXP))*/
495       st->loudness = (1-rate)*st->loudness + (rate)*pow(loudness, LOUDNESS_EXP);
496       
497       st->loudness2 = (1-rate2)*st->loudness2 + rate2*pow(st->loudness, 1.0f/LOUDNESS_EXP);
498
499       loudness = pow(st->loudness, 1.0f/LOUDNESS_EXP);
500
501       /*fprintf (stderr, "%f %f %f\n", loudness, st->loudness2, rate);*/
502    }
503    
504    agc_gain = st->agc_level/st->loudness2;
505    /*fprintf (stderr, "%f %f %f %f\n", active_bands, st->loudness, st->loudness2, agc_gain);*/
506    if (agc_gain>200)
507       agc_gain = 200;
508
509    for (i=0;i<N;i++)
510       st->gain2[i] *= agc_gain;
511    
512 }
513
514 static void preprocess_analysis(SpeexPreprocessState *st, spx_int16_t *x)
515 {
516    int i;
517    int N = st->ps_size;
518    int N3 = 2*N - st->frame_size;
519    int N4 = st->frame_size - N3;
520    spx_word32_t *ps=st->ps;
521
522    /* 'Build' input frame */
523    for (i=0;i<N3;i++)
524       st->frame[i]=st->inbuf[i];
525    for (i=0;i<st->frame_size;i++)
526       st->frame[N3+i]=x[i];
527    
528    /* Update inbuf */
529    for (i=0;i<N3;i++)
530       st->inbuf[i]=x[N4+i];
531
532    /* Windowing */
533    for (i=0;i<2*N;i++)
534       st->frame[i] = MULT16_16_Q15(st->window[i], st->frame[i]);
535
536    /* Perform FFT */
537    spx_fft(st->fft_lookup, st->frame, st->ft);
538          
539    /* Power spectrum */
540    ps[0]=1;
541    for (i=1;i<N;i++)
542       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]);
543
544    filterbank_compute_bank32(st->bank, ps, ps+N);
545 }
546
547 static void update_noise_prob(SpeexPreprocessState *st)
548 {
549    int i;
550    int min_range;
551    int N = st->ps_size;
552
553    for (i=1;i<N-1;i++)
554       st->S[i] = 100.f+ .8f*st->S[i] + .05f*st->ps[i-1]+.1f*st->ps[i]+.05f*st->ps[i+1];
555    st->S[0] = 100.f+ .8f*st->S[0] + .2*st->ps[0];
556    st->S[N-1] = 100.f+ .8f*st->S[N-1] + .2*st->ps[N-1];
557    
558    if (st->nb_adapt==1)
559    {
560       for (i=0;i<N;i++)
561          st->Smin[i] = st->Stmp[i] = 0.f;
562    }
563
564    if (st->nb_adapt < 100)
565       min_range = 15;
566    else if (st->nb_adapt < 1000)
567       min_range = 50;
568    else if (st->nb_adapt < 10000)
569       min_range = 150;
570    else
571       min_range = 300;
572    if (st->min_count > min_range)
573    {
574       st->min_count = 0;
575       for (i=0;i<N;i++)
576       {
577          st->Smin[i] = min(st->Stmp[i], st->S[i]);
578          st->Stmp[i] = st->S[i];
579       }
580    } else {
581       for (i=0;i<N;i++)
582       {
583          st->Smin[i] = min(st->Smin[i], st->S[i]);
584          st->Stmp[i] = min(st->Stmp[i], st->S[i]);      
585       }
586    }
587    for (i=0;i<N;i++)
588    {
589       st->update_prob[i] *= .2f;
590       if (st->S[i] > 2.5*st->Smin[i])
591          st->update_prob[i] += .8f;
592       /*fprintf (stderr, "%f ", st->S[i]/st->Smin[i]);*/
593       /*fprintf (stderr, "%f ", st->update_prob[i]);*/
594    }
595
596 }
597
598 #define NOISE_OVERCOMPENS 1.
599
600 void speex_echo_get_residual(SpeexEchoState *st, spx_word32_t *Yout, int len);
601
602 int speex_preprocess(SpeexPreprocessState *st, spx_int16_t *x, spx_int32_t *echo)
603 {
604    return speex_preprocess_run(st, x);
605 }
606
607 int speex_preprocess_run(SpeexPreprocessState *st, spx_int16_t *x)
608 {
609    int i;
610    int M;
611    int N = st->ps_size;
612    int N3 = 2*N - st->frame_size;
613    int N4 = st->frame_size - N3;
614    spx_word32_t *ps=st->ps;
615    float Zframe=0, Pframe;
616    float beta, beta_1;
617    float echo_floor;
618    float noise_floor;
619    
620    st->nb_adapt++;
621    st->min_count++;
622    
623    beta =1.0f/st->nb_adapt;
624    if (beta < .03f)
625       beta=.03f;
626    beta_1 = 1.0f-beta;
627    M = st->nbands;
628    /* Deal with residual echo if provided */
629    if (st->echo_state)
630    {
631       speex_echo_get_residual(st->echo_state, st->residual_echo, N);
632       for (i=0;i<N;i++)
633          st->echo_noise[i] = MAX32(.6f*st->echo_noise[i], st->residual_echo[i]);
634       filterbank_compute_bank32(st->bank, st->echo_noise, st->echo_noise+N);
635    } else {
636       for (i=0;i<N+M;i++)
637          st->echo_noise[i] = 0;
638    }
639    preprocess_analysis(st, x);
640
641    update_noise_prob(st);
642
643    /* Noise estimation always updated for the 10 first frames */
644    /*if (st->nb_adapt<10)
645    {
646       for (i=1;i<N-1;i++)
647          st->update_prob[i] = 0;
648    }
649    */
650    
651    /* Update the noise estimate for the frequencies where it can be */
652    for (i=0;i<N;i++)
653    {
654       if (st->update_prob[i]<.5f || st->ps[i] < st->noise[i])
655          st->noise[i] = beta_1*st->noise[i] + beta*NOISE_OVERCOMPENS*st->ps[i];
656    }
657    filterbank_compute_bank32(st->bank, st->noise, st->noise+N);
658
659    /* Special case for first frame */
660    if (st->nb_adapt==1)
661       for (i=0;i<N+M;i++)
662          st->old_ps[i] = ps[i];
663
664    /* Compute a posteriori SNR */
665    for (i=0;i<N+M;i++)
666    {
667       float gamma = .1;
668       spx_word32_t tot_noise = 1+st->noise[i] + st->echo_noise[i] + st->reverb_estimate[i];
669       st->post[i] = SNR_SCALING_1*SUB16(DIV32_16_Q8(ps[i],tot_noise), QCONST16(1.f,8));
670       if (st->post[i]>100.f)
671          st->post[i]=100.f;
672       /*gamma = .15+.85*st->prior[i]*st->prior[i]/((1+st->prior[i])*(1+st->prior[i]));*/
673       gamma = .1+.9*GAIN_SCALING_1*SQR16_Q15(DIV32_16_Q15(st->old_ps[i],(st->old_ps[i]+tot_noise)));
674       /* A priori SNR update */
675       st->prior[i] = gamma*max(0.0f,st->post[i]) + (1.f-gamma)*SNR_SCALING_1*DIV32_16_Q8(st->old_ps[i],tot_noise);
676       if (st->prior[i]>100.f)
677          st->prior[i]=100.f;
678    }
679
680    /*print_vec(st->post, N+M, "");*/
681
682    /* Recursive average of the a priori SNR. A bit smoothed for the psd components */
683    st->zeta[0] = .7f*st->zeta[0] + .3f*st->prior[0];
684    for (i=1;i<N-1;i++)
685       st->zeta[i] = .7f*st->zeta[i] + .3f*(.5f*st->prior[i]+.25f*st->prior[i+1]+.25f*st->prior[i-1]);
686    for (i=N-1;i<N+M;i++)
687       st->zeta[i] = .7f*st->zeta[i] + .3f*st->prior[i];
688
689    /* Speech probability of presence for the entire frame is based on the average filterbank a priori SNR */
690    Zframe = 0;
691    for (i=N;i<N+M;i++)
692       Zframe += st->zeta[i];
693    Zframe /= st->nbands;
694    Pframe = .1+.9*qcurve(Zframe);
695    
696    noise_floor = exp(.2302585f*st->noise_suppress);
697    echo_floor = exp(.2302585f* (st->echo_suppress*(1-Pframe) + st->echo_suppress_active*Pframe));
698    /*print_vec(&Pframe, 1, "");*/
699    /*for (i=N;i<N+M;i++)
700       st->gain2[i] = qcurve (st->zeta[i]);
701    filterbank_compute_psd(st->bank,st->gain2+N, st->gain2);*/
702    
703    for (i=N;i<N+M;i++)
704    {
705       float theta, MM;
706       float prior_ratio;
707       float q;
708       float P1;
709
710       /* Compute the gain floor based on different floors for the background noise and residual echo */
711       st->gain_floor[i] = sqrt((noise_floor*st->noise[i] + echo_floor*st->echo_noise[i])/(1+st->noise[i] + st->echo_noise[i]));
712       prior_ratio = st->prior[i]/(1.f+st->prior[i]);
713       theta = (1.f+st->post[i])*prior_ratio;
714
715       MM = hypergeom_gain(theta);
716       st->gain[i] = prior_ratio * MM;
717       /* Bound on the gain */
718       if (st->gain[i]>1.f)
719          st->gain[i]=1.f;
720       
721       /* Save old Bark power spectrum */
722       st->old_ps[i] = .2*st->old_ps[i] + .8*st->gain[i]*st->gain[i]*ps[i];
723
724       P1 = .2+.8*qcurve (st->zeta[i]);
725       q = 1-Pframe*P1;
726       st->gain2[i]=1.f/(1.f + (q/(1.f-q))*(1.f+st->prior[i])*exp(-theta));
727    }
728    filterbank_compute_psd(st->bank,st->gain2+N, st->gain2);
729    filterbank_compute_psd(st->bank,st->gain+N, st->gain);
730    
731    /* Use 1 for linear gain resolution (best) or 0 for Bark gain resolution (faster) */
732    if (1)
733    {
734       filterbank_compute_psd(st->bank,st->gain_floor+N, st->gain_floor);
735    
736       /* Compute gain according to the Ephraim-Malah algorithm */
737       for (i=0;i<N;i++)
738       {
739          float MM;
740          float theta;
741          float prior_ratio;
742          float p;
743          float g;
744          
745          /* Wiener filter gain */
746          prior_ratio = st->prior[i]/(1.f+st->prior[i]);
747          theta = (1.f+st->post[i])*prior_ratio;
748          p = st->gain2[i];
749          
750          /* Optimal estimator for loudness domain */
751          MM = hypergeom_gain(theta);
752          g = prior_ratio * MM;
753          
754          /* Constrain the gain to be close to the Bark scale gain */
755          if (g > 3*st->gain[i])
756             g = 3*st->gain[i];
757          st->gain[i] = g;
758          
759          /* Bound on the gain */
760          if (st->gain[i]>1.f)
761             st->gain[i]=1.f;
762          
763          /* Save old power spectrum */
764          st->old_ps[i] = .2*st->old_ps[i] + .8*st->gain[i]*st->gain[i]*ps[i];
765          
766          /* Apply gain floor */
767          if (st->gain[i] < st->gain_floor[i])
768             st->gain[i] = st->gain_floor[i];
769          
770          /* Exponential decay model for reverberation (unused) */
771          /*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];*/
772          
773          /* Take into account speech probability of presence (loudness domain MMSE estimator) */
774          st->gain2[i]=SQR(p*sqrt(st->gain[i])+sqrt(st->gain_floor[i])*(1-p));
775          
776          /* Use this if you want a log-domain MMSE estimator instead */
777          /*st->gain2[i] = pow(st->gain[i], p) * pow(st->gain_floor[i],1.f-p);*/
778          
779       }
780    } else {
781       for (i=N;i<N+M;i++)
782       {
783          float p = st->gain2[i];
784          if (st->gain[i] < st->gain_floor[i])
785             st->gain[i] = st->gain_floor[i];
786          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));
787       }
788       filterbank_compute_psd(st->bank,st->gain2+N, st->gain2);
789       
790    }
791    
792    if (!st->denoise_enabled)
793    {
794       for (i=0;i<N+M;i++)
795          st->gain2[i]=1.f;
796    }
797    
798    if (st->agc_enabled)
799       speex_compute_agc(st);
800
801    /* Apply computed gain */
802    for (i=1;i<N;i++)
803    {
804       st->ft[2*i-1] *= st->gain2[i];
805       st->ft[2*i] *= st->gain2[i];
806    }
807    st->ft[0] *= st->gain2[0];
808    st->ft[2*N-1] *= st->gain2[N-1];
809
810    /* Inverse FFT with 1/N scaling */
811    spx_ifft(st->fft_lookup, st->ft, st->frame);
812
813    {
814       float max_sample=0;
815       for (i=0;i<2*N;i++)
816          if (fabs(st->frame[i])>max_sample)
817             max_sample = fabs(st->frame[i]);
818       if (max_sample>28000.f)
819       {
820          float damp = 28000.f/max_sample;
821          for (i=0;i<2*N;i++)
822             st->frame[i] *= damp;
823       }
824    }
825
826    for (i=0;i<2*N;i++)
827       st->frame[i] = MULT16_16_Q15(st->window[i], st->frame[i]);
828
829    /* Perform overlap and add */
830    for (i=0;i<N3;i++)
831       x[i] = st->outbuf[i] + st->frame[i];
832    for (i=0;i<N4;i++)
833       x[N3+i] = st->frame[N3+i];
834    
835    /* Update outbuf */
836    for (i=0;i<N3;i++)
837       st->outbuf[i] = st->frame[st->frame_size+i];
838
839    if (st->vad_enabled)
840    {
841       if (Pframe > st->speech_prob_start || (st->was_speech && Pframe > st->speech_prob_continue))
842       {
843          st->was_speech=1;
844          return 1;
845       } else
846       {
847          st->was_speech=0;
848          return 0;
849       }
850    } else {
851       return 1;
852    }
853 }
854
855 void speex_preprocess_estimate_update(SpeexPreprocessState *st, spx_int16_t *x, spx_int32_t *echo)
856 {
857    int i;
858    int N = st->ps_size;
859    int N3 = 2*N - st->frame_size;
860    int M;
861    spx_word32_t *ps=st->ps;
862
863    M = st->nbands;
864    st->min_count++;
865    
866    preprocess_analysis(st, x);
867
868    update_noise_prob(st);
869    
870    for (i=1;i<N-1;i++)
871    {
872       if (st->update_prob[i]<.5f || st->ps[i] < st->noise[i])
873       {
874          if (echo)
875             st->noise[i] = .95f*st->noise[i] + .1f*max(1.0f,st->ps[i]-1.0*echo[i]);
876          else
877             st->noise[i] = .95f*st->noise[i] + .1f*st->ps[i];
878       }
879    }
880
881    for (i=0;i<N3;i++)
882       st->outbuf[i] = MULT16_16_Q15(x[st->frame_size-N3+i],st->window[st->frame_size+i]);
883
884    /* Save old power spectrum */
885    for (i=0;i<N+M;i++)
886       st->old_ps[i] = ps[i];
887
888    for (i=1;i<N;i++)
889       st->reverb_estimate[i] *= st->reverb_decay;
890 }
891
892
893 int speex_preprocess_ctl(SpeexPreprocessState *state, int request, void *ptr)
894 {
895    int i;
896    SpeexPreprocessState *st;
897    st=(SpeexPreprocessState*)state;
898    switch(request)
899    {
900    case SPEEX_PREPROCESS_SET_DENOISE:
901       st->denoise_enabled = (*(int*)ptr);
902       break;
903    case SPEEX_PREPROCESS_GET_DENOISE:
904       (*(int*)ptr) = st->denoise_enabled;
905       break;
906
907    case SPEEX_PREPROCESS_SET_AGC:
908       st->agc_enabled = (*(int*)ptr);
909       break;
910    case SPEEX_PREPROCESS_GET_AGC:
911       (*(int*)ptr) = st->agc_enabled;
912       break;
913
914    case SPEEX_PREPROCESS_SET_AGC_LEVEL:
915       st->agc_level = (*(float*)ptr);
916       if (st->agc_level<1)
917          st->agc_level=1;
918       if (st->agc_level>32768)
919          st->agc_level=32768;
920       break;
921    case SPEEX_PREPROCESS_GET_AGC_LEVEL:
922       (*(float*)ptr) = st->agc_level;
923       break;
924
925    case SPEEX_PREPROCESS_SET_VAD:
926       speex_warning("The VAD has been removed pending a complete rewrite");
927       st->vad_enabled = (*(spx_int32_t*)ptr);
928       break;
929    case SPEEX_PREPROCESS_GET_VAD:
930       (*(spx_int32_t*)ptr) = st->vad_enabled;
931       break;
932    
933    case SPEEX_PREPROCESS_SET_DEREVERB:
934       st->dereverb_enabled = (*(int*)ptr);
935       for (i=0;i<st->ps_size;i++)
936          st->reverb_estimate[i]=0;
937       break;
938    case SPEEX_PREPROCESS_GET_DEREVERB:
939       (*(int*)ptr) = st->dereverb_enabled;
940       break;
941
942    case SPEEX_PREPROCESS_SET_DEREVERB_LEVEL:
943       st->reverb_level = (*(float*)ptr);
944       break;
945    case SPEEX_PREPROCESS_GET_DEREVERB_LEVEL:
946       (*(float*)ptr) = st->reverb_level;
947       break;
948    
949    case SPEEX_PREPROCESS_SET_DEREVERB_DECAY:
950       st->reverb_decay = (*(float*)ptr);
951       break;
952    case SPEEX_PREPROCESS_GET_DEREVERB_DECAY:
953       (*(float*)ptr) = st->reverb_decay;
954       break;
955
956    case SPEEX_PREPROCESS_SET_PROB_START:
957       st->speech_prob_start = (*(int*)ptr) / 100.0;
958       if ( st->speech_prob_start > 1 || st->speech_prob_start < 0 )
959          st->speech_prob_start = SPEEX_PROB_START_DEFAULT;
960       break;
961    case SPEEX_PREPROCESS_GET_PROB_START:
962       (*(int*)ptr) = st->speech_prob_start * 100;
963       break;
964
965    case SPEEX_PREPROCESS_SET_PROB_CONTINUE:
966       st->speech_prob_continue = (*(int*)ptr) / 100.0;
967       if ( st->speech_prob_continue > 1 || st->speech_prob_continue < 0 )
968          st->speech_prob_continue = SPEEX_PROB_CONTINUE_DEFAULT;
969       break;
970    case SPEEX_PREPROCESS_GET_PROB_CONTINUE:
971       (*(int*)ptr) = st->speech_prob_continue * 100;
972       break;
973
974    case SPEEX_PREPROCESS_SET_NOISE_SUPPRESS:
975       st->noise_suppress = -ABS(*(spx_int32_t*)ptr);
976       break;
977    case SPEEX_PREPROCESS_GET_NOISE_SUPPRESS:
978       (*(spx_int32_t*)ptr) = st->noise_suppress;
979       break;
980    case SPEEX_PREPROCESS_SET_ECHO_SUPPRESS:
981       st->echo_suppress = -ABS(*(spx_int32_t*)ptr);
982       break;
983    case SPEEX_PREPROCESS_GET_ECHO_SUPPRESS:
984       (*(spx_int32_t*)ptr) = st->echo_suppress;
985       break;
986    case SPEEX_PREPROCESS_SET_ECHO_SUPPRESS_ACTIVE:
987       st->echo_suppress_active = -ABS(*(spx_int32_t*)ptr);
988       break;
989    case SPEEX_PREPROCESS_GET_ECHO_SUPPRESS_ACTIVE:
990       (*(spx_int32_t*)ptr) = st->echo_suppress_active;
991       break;
992    case SPEEX_PREPROCESS_SET_ECHO_STATE:
993       st->echo_state = (SpeexEchoState*)ptr;
994       break;
995    case SPEEX_PREPROCESS_GET_ECHO_STATE:
996       ptr = (void*)st->echo_state;
997       break;
998
999    default:
1000       speex_warning_int("Unknown speex_preprocess_ctl request: ", request);
1001       return -1;
1002    }
1003    return 0;
1004 }