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