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