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