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