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