Converted Pframe, P1 and q 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       spx_word32_t theta;
772       spx_word32_t MM;
773       spx_word16_t prior_ratio;
774       spx_word16_t q;
775       spx_word16_t P1;
776
777       /* Compute the gain floor based on different floors for the background noise and residual echo */
778       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]);
779       prior_ratio = PDIV32_16(SHL32(EXTEND32(st->prior[i]), 15), ADD16(st->prior[i], SHL32(1,SNR_SHIFT)));
780       theta = MULT16_32_P15(prior_ratio, QCONST32(1.f,EXPIN_SHIFT)+SHL32(EXTEND32(st->post[i]),EXPIN_SHIFT-SNR_SHIFT));
781
782       MM = hypergeom_gain(theta);
783       /* Gain with bound */
784       st->gain[i] = EXTRACT16(MIN32(Q15_ONE, MULT16_32_Q15(prior_ratio, MM)));
785       /* Save old Bark power spectrum */
786       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]);
787
788       P1 = QCONST16(.199f,15)+MULT16_16_Q15(QCONST16(.8f,15),qcurve (st->zeta[i]));
789       q = Q15_ONE-MULT16_16_Q15(Pframe,P1);
790 #ifdef FIXED_POINT
791       theta = MIN32(theta, 32767);
792       st->gain2[i]=FRAC_SCALING/(1.f + (q/(1.f*Q15_ONE-q))*SNR_SCALING_1*MULT16_16_Q15((SHL32(1,SNR_SHIFT)+st->prior[i]),EXTRACT16(MIN32(Q15ONE,SHR32(spx_exp(-EXTRACT16(theta)),1)))));
793 #else
794       st->gain2[i]=1/(1.f + (q/(1.f-q))*(1+st->prior[i])*exp(-theta));
795 #endif
796    }
797    /* Convert the EM gains and speech prob to linear frequency */
798    filterbank_compute_psd16(st->bank,st->gain2+N, st->gain2);
799    filterbank_compute_psd16(st->bank,st->gain+N, st->gain);
800    
801    /* Use 1 for linear gain resolution (best) or 0 for Bark gain resolution (faster) */
802    if (1)
803    {
804       filterbank_compute_psd16(st->bank,st->gain_floor+N, st->gain_floor);
805    
806       /* Compute gain according to the Ephraim-Malah algorithm -- linear frequency */
807       for (i=0;i<N;i++)
808       {
809          spx_word32_t MM;
810          spx_word32_t theta;
811          spx_word16_t prior_ratio;
812          spx_word16_t tmp;
813          spx_word16_t p;
814          spx_word16_t g;
815          
816          /* Wiener filter gain */
817          prior_ratio = PDIV32_16(SHL32(EXTEND32(st->prior[i]), 15), ADD16(st->prior[i], SHL32(1,SNR_SHIFT)));
818          theta = MULT16_32_P15(prior_ratio, QCONST32(1.f,EXPIN_SHIFT)+SHL32(EXTEND32(st->post[i]),EXPIN_SHIFT-SNR_SHIFT));
819
820          /* Optimal estimator for loudness domain */
821          MM = hypergeom_gain(theta);
822          /* EM gain with bound */
823          g = EXTRACT16(MIN32(Q15_ONE, MULT16_32_Q15(prior_ratio, MM)));
824          /* Interpolated speech probability of presence */
825          p = st->gain2[i];
826                   
827          /* Constrain the gain to be close to the Bark scale gain */
828          if (MULT16_16_Q15(QCONST16(.333f,15),g) > st->gain[i])
829             g = MULT16_16(3,st->gain[i]);
830          st->gain[i] = g;
831          
832          /* Save old power spectrum */
833          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]);
834          
835          /* Apply gain floor */
836          if (st->gain[i] < st->gain_floor[i])
837             st->gain[i] = st->gain_floor[i];
838
839          /* Exponential decay model for reverberation (unused) */
840          /*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];*/
841          
842          /* Take into account speech probability of presence (loudness domain MMSE estimator) */
843          /* gain2 = [p*sqrt(gain)+(1-p)*sqrt(gain _floor) ]^2 */
844          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)));
845          st->gain2[i]=SQR16_Q15(tmp);
846
847          /* Use this if you want a log-domain MMSE estimator instead */
848          /*st->gain2[i] = pow(st->gain[i], p) * pow(st->gain_floor[i],1.f-p);*/
849          
850       }
851    } else {
852       for (i=N;i<N+M;i++)
853       {
854          float p = FRAC_SCALING_1*st->gain2[i];
855          if (st->gain[i] < st->gain_floor[i])
856             st->gain[i] = st->gain_floor[i];
857          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));
858       }
859       filterbank_compute_psd16(st->bank,st->gain2+N, st->gain2);
860       
861    }
862    
863    if (!st->denoise_enabled)
864    {
865       for (i=0;i<N+M;i++)
866          st->gain2[i]=FRAC_SCALING;
867    }
868    
869    if (st->agc_enabled)
870       speex_compute_agc(st);
871
872    /* Apply computed gain */
873    for (i=1;i<N;i++)
874    {
875       st->ft[2*i-1] *= FRAC_SCALING_1*st->gain2[i];
876       st->ft[2*i] *= FRAC_SCALING_1*st->gain2[i];
877    }
878    st->ft[0] *= FRAC_SCALING_1*st->gain2[0];
879    st->ft[2*N-1] *= FRAC_SCALING_1*st->gain2[N-1];
880
881    /* Inverse FFT with 1/N scaling */
882    spx_ifft(st->fft_lookup, st->ft, st->frame);
883    for (i=0;i<2*N;i++)
884       st->frame[i] = PSHR16(st->frame[i], st->frame_shift);
885
886    {
887       float max_sample=0;
888       for (i=0;i<2*N;i++)
889          if (fabs(st->frame[i])>max_sample)
890             max_sample = fabs(st->frame[i]);
891       if (max_sample>28000.f)
892       {
893          float damp = 28000.f/max_sample;
894          for (i=0;i<2*N;i++)
895             st->frame[i] *= damp;
896       }
897    }
898
899    for (i=0;i<2*N;i++)
900       st->frame[i] = MULT16_16_Q15(st->frame[i], st->window[i]);
901
902    /* Perform overlap and add */
903    for (i=0;i<N3;i++)
904       x[i] = st->outbuf[i] + st->frame[i];
905    for (i=0;i<N4;i++)
906       x[N3+i] = st->frame[N3+i];
907    
908    /* Update outbuf */
909    for (i=0;i<N3;i++)
910       st->outbuf[i] = st->frame[st->frame_size+i];
911
912    if (st->vad_enabled)
913    {
914       if (FRAC_SCALING_1*Pframe > st->speech_prob_start || (st->was_speech && FRAC_SCALING_1*Pframe > st->speech_prob_continue))
915       {
916          st->was_speech=1;
917          return 1;
918       } else
919       {
920          st->was_speech=0;
921          return 0;
922       }
923    } else {
924       return 1;
925    }
926 }
927
928 void speex_preprocess_estimate_update(SpeexPreprocessState *st, spx_int16_t *x, spx_int32_t *echo)
929 {
930    int i;
931    int N = st->ps_size;
932    int N3 = 2*N - st->frame_size;
933    int M;
934    spx_word32_t *ps=st->ps;
935
936    M = st->nbands;
937    st->min_count++;
938    
939    preprocess_analysis(st, x);
940
941    update_noise_prob(st);
942    
943    for (i=1;i<N-1;i++)
944    {
945       if (!st->update_prob[i] || st->ps[i] < PSHR32(st->noise[i],NOISE_SHIFT))
946       {
947          st->noise[i] = .95f*st->noise[i] + .1f*st->ps[i];
948       }
949    }
950
951    for (i=0;i<N3;i++)
952       st->outbuf[i] = MULT16_16_Q15(x[st->frame_size-N3+i],st->window[st->frame_size+i]);
953
954    /* Save old power spectrum */
955    for (i=0;i<N+M;i++)
956       st->old_ps[i] = ps[i];
957
958    for (i=1;i<N;i++)
959       st->reverb_estimate[i] *= st->reverb_decay;
960 }
961
962
963 int speex_preprocess_ctl(SpeexPreprocessState *state, int request, void *ptr)
964 {
965    int i;
966    SpeexPreprocessState *st;
967    st=(SpeexPreprocessState*)state;
968    switch(request)
969    {
970    case SPEEX_PREPROCESS_SET_DENOISE:
971       st->denoise_enabled = (*(int*)ptr);
972       break;
973    case SPEEX_PREPROCESS_GET_DENOISE:
974       (*(int*)ptr) = st->denoise_enabled;
975       break;
976
977    case SPEEX_PREPROCESS_SET_AGC:
978       st->agc_enabled = (*(int*)ptr);
979       break;
980    case SPEEX_PREPROCESS_GET_AGC:
981       (*(int*)ptr) = st->agc_enabled;
982       break;
983
984    case SPEEX_PREPROCESS_SET_AGC_LEVEL:
985       st->agc_level = (*(float*)ptr);
986       if (st->agc_level<1)
987          st->agc_level=1;
988       if (st->agc_level>32768)
989          st->agc_level=32768;
990       break;
991    case SPEEX_PREPROCESS_GET_AGC_LEVEL:
992       (*(float*)ptr) = st->agc_level;
993       break;
994
995    case SPEEX_PREPROCESS_SET_VAD:
996       speex_warning("The VAD has been replaced by a hack pending a complete rewrite");
997       st->vad_enabled = (*(spx_int32_t*)ptr);
998       break;
999    case SPEEX_PREPROCESS_GET_VAD:
1000       (*(spx_int32_t*)ptr) = st->vad_enabled;
1001       break;
1002    
1003    case SPEEX_PREPROCESS_SET_DEREVERB:
1004       st->dereverb_enabled = (*(int*)ptr);
1005       for (i=0;i<st->ps_size;i++)
1006          st->reverb_estimate[i]=0;
1007       break;
1008    case SPEEX_PREPROCESS_GET_DEREVERB:
1009       (*(int*)ptr) = st->dereverb_enabled;
1010       break;
1011
1012    case SPEEX_PREPROCESS_SET_DEREVERB_LEVEL:
1013       st->reverb_level = (*(float*)ptr);
1014       break;
1015    case SPEEX_PREPROCESS_GET_DEREVERB_LEVEL:
1016       (*(float*)ptr) = st->reverb_level;
1017       break;
1018    
1019    case SPEEX_PREPROCESS_SET_DEREVERB_DECAY:
1020       st->reverb_decay = (*(float*)ptr);
1021       break;
1022    case SPEEX_PREPROCESS_GET_DEREVERB_DECAY:
1023       (*(float*)ptr) = st->reverb_decay;
1024       break;
1025
1026    case SPEEX_PREPROCESS_SET_PROB_START:
1027       st->speech_prob_start = (*(int*)ptr) / 100.0;
1028       if ( st->speech_prob_start > 1 || st->speech_prob_start < 0 )
1029          st->speech_prob_start = SPEEX_PROB_START_DEFAULT;
1030       break;
1031    case SPEEX_PREPROCESS_GET_PROB_START:
1032       (*(int*)ptr) = st->speech_prob_start * 100;
1033       break;
1034
1035    case SPEEX_PREPROCESS_SET_PROB_CONTINUE:
1036       st->speech_prob_continue = (*(int*)ptr) / 100.0;
1037       if ( st->speech_prob_continue > 1 || st->speech_prob_continue < 0 )
1038          st->speech_prob_continue = SPEEX_PROB_CONTINUE_DEFAULT;
1039       break;
1040    case SPEEX_PREPROCESS_GET_PROB_CONTINUE:
1041       (*(int*)ptr) = st->speech_prob_continue * 100;
1042       break;
1043
1044    case SPEEX_PREPROCESS_SET_NOISE_SUPPRESS:
1045       st->noise_suppress = -ABS(*(spx_int32_t*)ptr);
1046       break;
1047    case SPEEX_PREPROCESS_GET_NOISE_SUPPRESS:
1048       (*(spx_int32_t*)ptr) = st->noise_suppress;
1049       break;
1050    case SPEEX_PREPROCESS_SET_ECHO_SUPPRESS:
1051       st->echo_suppress = -ABS(*(spx_int32_t*)ptr);
1052       break;
1053    case SPEEX_PREPROCESS_GET_ECHO_SUPPRESS:
1054       (*(spx_int32_t*)ptr) = st->echo_suppress;
1055       break;
1056    case SPEEX_PREPROCESS_SET_ECHO_SUPPRESS_ACTIVE:
1057       st->echo_suppress_active = -ABS(*(spx_int32_t*)ptr);
1058       break;
1059    case SPEEX_PREPROCESS_GET_ECHO_SUPPRESS_ACTIVE:
1060       (*(spx_int32_t*)ptr) = st->echo_suppress_active;
1061       break;
1062    case SPEEX_PREPROCESS_SET_ECHO_STATE:
1063       st->echo_state = (SpeexEchoState*)ptr;
1064       break;
1065    case SPEEX_PREPROCESS_GET_ECHO_STATE:
1066       ptr = (void*)st->echo_state;
1067       break;
1068
1069    default:
1070       speex_warning_int("Unknown speex_preprocess_ctl request: ", request);
1071       return -1;
1072    }
1073    return 0;
1074 }