Fixed a wideband regression (innovation wasn't saved in the decoder). Also
[speexdsp.git] / libspeex / nb_celp.c
1 /* Copyright (C) 2002 Jean-Marc Valin 
2    File: nb_celp.c
3
4    Redistribution and use in source and binary forms, with or without
5    modification, are permitted provided that the following conditions
6    are met:
7    
8    - Redistributions of source code must retain the above copyright
9    notice, this list of conditions and the following disclaimer.
10    
11    - Redistributions in binary form must reproduce the above copyright
12    notice, this list of conditions and the following disclaimer in the
13    documentation and/or other materials provided with the distribution.
14    
15    - Neither the name of the Xiph.org Foundation nor the names of its
16    contributors may be used to endorse or promote products derived from
17    this software without specific prior written permission.
18    
19    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
22    A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR
23    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
24    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
25    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
26    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
27    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
28    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
29    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
30 */
31
32 #ifdef HAVE_CONFIG_H
33 #include "config.h"
34 #endif
35
36 #include <math.h>
37 #include "nb_celp.h"
38 #include "lpc.h"
39 #include "lsp.h"
40 #include "ltp.h"
41 #include "quant_lsp.h"
42 #include "cb_search.h"
43 #include "filters.h"
44 #include "stack_alloc.h"
45 #include "vq.h"
46 #include <speex/speex_bits.h>
47 #include "vbr.h"
48 #include "misc.h"
49 #include "math_approx.h"
50 #include <speex/speex_callbacks.h>
51
52 #ifdef VORBIS_PSYCHO
53 #include "vorbis_psy.h"
54 #endif
55
56 #ifndef M_PI
57 #define M_PI           3.14159265358979323846  /* pi */
58 #endif
59
60 #ifndef NULL
61 #define NULL 0
62 #endif
63
64 #define SUBMODE(x) st->submodes[st->submodeID]->x
65
66 /* Default size for the encoder and decoder stack (can be changed at compile time).
67    This does not apply when using variable-size arrays or alloca. */
68 #ifndef NB_ENC_STACK
69 #define NB_ENC_STACK (8000*sizeof(spx_sig_t))
70 #endif
71
72 #ifndef NB_DEC_STACK
73 #define NB_DEC_STACK (4000*sizeof(spx_sig_t))
74 #endif
75
76
77 #ifdef FIXED_POINT
78 const spx_word32_t ol_gain_table[32]={18900, 25150, 33468, 44536, 59265, 78865, 104946, 139653, 185838, 247297, 329081, 437913, 582736, 775454, 1031906, 1373169, 1827293, 2431601, 3235761, 4305867, 5729870, 7624808, 10146425, 13501971, 17967238, 23909222, 31816294, 42338330, 56340132, 74972501, 99766822, 132760927};
79 const spx_word16_t exc_gain_quant_scal3_bound[7]={1841, 3883, 6051, 8062, 10444, 13580, 18560};
80 const spx_word16_t exc_gain_quant_scal3[8]={1002, 2680, 5086, 7016, 9108, 11781, 15380, 21740};
81 const spx_word16_t exc_gain_quant_scal1_bound[1]={14385};
82 const spx_word16_t exc_gain_quant_scal1[2]={11546, 17224};
83
84 #define LSP_MARGIN 16
85 #define LSP_DELTA1 6553
86 #define LSP_DELTA2 1638
87
88 #else
89
90 const float exc_gain_quant_scal3_bound[7]={0.112338, 0.236980, 0.369316, 0.492054, 0.637471, 0.828874, 1.132784};
91 const float exc_gain_quant_scal3[8]={0.061130, 0.163546, 0.310413, 0.428220, 0.555887, 0.719055, 0.938694, 1.326874};
92 const float exc_gain_quant_scal1_bound[1]={0.87798};
93 const float exc_gain_quant_scal1[2]={0.70469, 1.05127};
94
95 #define LSP_MARGIN .002
96 #define LSP_DELTA1 .2
97 #define LSP_DELTA2 .05
98
99 #endif
100
101 #ifdef VORBIS_PSYCHO
102 #define EXTRA_BUFFER 100
103 #else
104 #define EXTRA_BUFFER 0
105 #endif
106
107
108 #define sqr(x) ((x)*(x))
109
110 extern const spx_word16_t lpc_window[];
111
112 void *nb_encoder_init(const SpeexMode *m)
113 {
114    EncState *st;
115    const SpeexNBMode *mode;
116    int i;
117
118    mode=(const SpeexNBMode *)m->mode;
119    st = (EncState*)speex_alloc(sizeof(EncState));
120    if (!st)
121       return NULL;
122 #if defined(VAR_ARRAYS) || defined (USE_ALLOCA)
123    st->stack = NULL;
124 #else
125    st->stack = (char*)speex_alloc_scratch(NB_ENC_STACK);
126 #endif
127    
128    st->mode=m;
129
130    st->frameSize = mode->frameSize;
131    st->nbSubframes=mode->frameSize/mode->subframeSize;
132    st->subframeSize=mode->subframeSize;
133    st->windowSize = st->frameSize+st->subframeSize;
134    st->lpcSize = mode->lpcSize;
135    st->gamma1=mode->gamma1;
136    st->gamma2=mode->gamma2;
137    st->min_pitch=mode->pitchStart;
138    st->max_pitch=mode->pitchEnd;
139    st->lag_factor=mode->lag_factor;
140    st->lpc_floor = mode->lpc_floor;
141   
142    st->submodes=mode->submodes;
143    st->submodeID=st->submodeSelect=mode->defaultSubmode;
144    st->bounded_pitch = 1;
145
146    st->encode_submode = 1;
147 #ifdef EPIC_48K
148    st->lbr_48k=mode->lbr48k;
149 #endif
150
151 #ifdef VORBIS_PSYCHO
152    st->psy = vorbis_psy_init(8000, 256);
153    st->curve = speex_alloc(128*sizeof(float));
154    st->old_curve = speex_alloc(128*sizeof(float));
155    st->psy_window = speex_alloc(256*sizeof(float));
156 #endif
157
158    /* Allocating input buffer */
159    st->winBuf = speex_alloc((st->windowSize-st->frameSize)*sizeof(spx_word16_t));
160    /* Allocating excitation buffer */
161    st->excBuf = speex_alloc((mode->frameSize+mode->pitchEnd+1)*sizeof(spx_sig_t));
162    st->exc = st->excBuf + mode->pitchEnd + 1;
163    st->swBuf = speex_alloc((mode->frameSize+mode->pitchEnd+2)*sizeof(spx_word16_t));
164    st->sw = st->swBuf + mode->pitchEnd + 2;
165
166    st->window= lpc_window;
167    
168    /* Create the window for autocorrelation (lag-windowing) */
169    st->lagWindow = speex_alloc((st->lpcSize+1)*sizeof(spx_word16_t));
170    for (i=0;i<st->lpcSize+1;i++)
171       st->lagWindow[i]=16384*exp(-.5*sqr(2*M_PI*st->lag_factor*i));
172
173    st->autocorr = speex_alloc((st->lpcSize+1)*sizeof(spx_word16_t));
174
175    st->lpc = speex_alloc((st->lpcSize)*sizeof(spx_coef_t));
176    st->interp_lpc = speex_alloc((st->lpcSize)*sizeof(spx_coef_t));
177    st->interp_qlpc = speex_alloc((st->lpcSize)*sizeof(spx_coef_t));
178    st->bw_lpc1 = speex_alloc((st->lpcSize)*sizeof(spx_coef_t));
179    st->bw_lpc2 = speex_alloc((st->lpcSize)*sizeof(spx_coef_t));
180
181    st->lsp = speex_alloc((st->lpcSize)*sizeof(spx_lsp_t));
182    st->qlsp = speex_alloc((st->lpcSize)*sizeof(spx_lsp_t));
183    st->old_lsp = speex_alloc((st->lpcSize)*sizeof(spx_lsp_t));
184    st->old_qlsp = speex_alloc((st->lpcSize)*sizeof(spx_lsp_t));
185    st->interp_lsp = speex_alloc((st->lpcSize)*sizeof(spx_lsp_t));
186    st->interp_qlsp = speex_alloc((st->lpcSize)*sizeof(spx_lsp_t));
187
188    st->first = 1;
189    for (i=0;i<st->lpcSize;i++)
190    {
191       st->lsp[i]=LSP_SCALING*(M_PI*((float)(i+1)))/(st->lpcSize+1);
192    }
193
194    st->mem_sp = speex_alloc((st->lpcSize)*sizeof(spx_mem_t));
195    st->mem_sw = speex_alloc((st->lpcSize)*sizeof(spx_mem_t));
196    st->mem_sw_whole = speex_alloc((st->lpcSize)*sizeof(spx_mem_t));
197    st->mem_exc = speex_alloc((st->lpcSize)*sizeof(spx_mem_t));
198    st->mem_exc2 = speex_alloc((st->lpcSize)*sizeof(spx_mem_t));
199
200    st->pi_gain = speex_alloc((st->nbSubframes)*sizeof(spx_word32_t));
201    st->innov_save = NULL;
202    
203    st->pitch = speex_alloc((st->nbSubframes)*sizeof(int));
204
205    st->vbr = speex_alloc(sizeof(VBRState));
206    vbr_init(st->vbr);
207    st->vbr_quality = 8;
208    st->vbr_enabled = 0;
209    st->vad_enabled = 0;
210    st->dtx_enabled = 0;
211    st->abr_enabled = 0;
212    st->abr_drift = 0;
213
214    st->plc_tuning = 2;
215    st->complexity=2;
216    st->sampling_rate=8000;
217    st->dtx_count=0;
218
219 #ifdef ENABLE_VALGRIND
220    VALGRIND_MAKE_READABLE(st, (st->stack-(char*)st));
221 #endif
222    return st;
223 }
224
225 void nb_encoder_destroy(void *state)
226 {
227    EncState *st=(EncState *)state;
228    /* Free all allocated memory */
229 #if !(defined(VAR_ARRAYS) || defined (USE_ALLOCA))
230    speex_free_scratch(st->stack);
231 #endif
232
233    speex_free (st->winBuf);
234    speex_free (st->excBuf);
235    speex_free (st->interp_qlpc);
236    speex_free (st->qlsp);
237    speex_free (st->old_qlsp);
238    speex_free (st->interp_qlsp);
239    speex_free (st->swBuf);
240
241    speex_free (st->lagWindow);
242    speex_free (st->autocorr);
243    speex_free (st->lpc);
244    speex_free (st->lsp);
245
246    speex_free (st->interp_lpc);
247    speex_free (st->bw_lpc1);
248    speex_free (st->bw_lpc2);
249    speex_free (st->old_lsp);
250    speex_free (st->interp_lsp);
251    speex_free (st->mem_sp);
252    speex_free (st->mem_sw);
253    speex_free (st->mem_sw_whole);
254    speex_free (st->mem_exc);
255    speex_free (st->mem_exc2);
256    speex_free (st->pi_gain);
257    speex_free (st->pitch);
258
259    vbr_destroy(st->vbr);
260    speex_free (st->vbr);
261
262 #ifdef VORBIS_PSYCHO
263    vorbis_psy_destroy(st->psy);
264    speex_free (st->curve);
265    speex_free (st->old_curve);
266    speex_free (st->psy_window);
267 #endif
268
269    /*Free state memory... should be last*/
270    speex_free(st);
271 }
272
273 int nb_encode(void *state, void *vin, SpeexBits *bits)
274 {
275    EncState *st;
276    int i, sub, roots;
277    int ol_pitch;
278    spx_word16_t ol_pitch_coef;
279    spx_word32_t ol_gain;
280    VARDECL(spx_word16_t *ringing);
281    VARDECL(spx_sig_t *target);
282    VARDECL(spx_sig_t *innov);
283    VARDECL(spx_mem_t *mem);
284    char *stack;
285    VARDECL(spx_word16_t *syn_resp);
286    VARDECL(spx_sig_t *real_exc);
287 #ifdef EPIC_48K
288    int pitch_half[2];
289    int ol_pitch_id=0;
290 #endif
291    spx_word16_t *in = vin;
292    spx_word16_t *exc16_alias;
293
294    st=(EncState *)state;
295    stack=st->stack;
296
297    /* Move signals 1 frame towards the past */
298    speex_move(st->excBuf, st->excBuf+st->frameSize, (st->max_pitch+1)*sizeof(spx_sig_t));
299    speex_move(st->swBuf, st->swBuf+st->frameSize, (st->max_pitch+2)*sizeof(spx_word16_t));
300
301    {
302       VARDECL(spx_word16_t *w_sig);
303       ALLOC(w_sig, st->windowSize, spx_word16_t);
304       /* Window for analysis */
305       for (i=0;i<st->windowSize-st->frameSize;i++)
306          w_sig[i] = EXTRACT16(SHR32(MULT16_16(st->winBuf[i],st->window[i]),SIG_SHIFT));
307       for (;i<st->windowSize;i++)
308          w_sig[i] = EXTRACT16(SHR32(MULT16_16(in[i-st->windowSize+st->frameSize],st->window[i]),SIG_SHIFT));
309       /* Compute auto-correlation */
310       _spx_autocorr(w_sig, st->autocorr, st->lpcSize+1, st->windowSize);
311    }
312    st->autocorr[0] = ADD16(st->autocorr[0],MULT16_16_Q15(st->autocorr[0],st->lpc_floor)); /* Noise floor in auto-correlation domain */
313
314    /* Lag windowing: equivalent to filtering in the power-spectrum domain */
315    for (i=0;i<st->lpcSize+1;i++)
316       st->autocorr[i] = MULT16_16_Q14(st->autocorr[i],st->lagWindow[i]);
317
318    /* Levinson-Durbin */
319    _spx_lpc(st->lpc, st->autocorr, st->lpcSize);
320
321    /* LPC to LSPs (x-domain) transform */
322    roots=lpc_to_lsp (st->lpc, st->lpcSize, st->lsp, 15, LSP_DELTA1, stack);
323    /* Check if we found all the roots */
324    if (roots!=st->lpcSize)
325    {
326       /* Search again if we can afford it */
327       if (st->complexity>1)
328          roots = lpc_to_lsp (st->lpc, st->lpcSize, st->lsp, 11, LSP_DELTA2, stack);
329       if (roots!=st->lpcSize) 
330       {
331          /*If we can't find all LSP's, do some damage control and use previous filter*/
332          for (i=0;i<st->lpcSize;i++)
333          {
334             st->lsp[i]=st->old_lsp[i];
335          }
336       }
337    }
338
339
340
341    /* Whole frame analysis (open-loop estimation of pitch and excitation gain) */
342    {
343       if (st->first)
344          for (i=0;i<st->lpcSize;i++)
345             st->interp_lsp[i] = st->lsp[i];
346       else
347          lsp_interpolate(st->old_lsp, st->lsp, st->interp_lsp, st->lpcSize, st->nbSubframes, st->nbSubframes<<1);
348
349       lsp_enforce_margin(st->interp_lsp, st->lpcSize, LSP_MARGIN);
350
351       /* Compute interpolated LPCs (unquantized) for whole frame*/
352       lsp_to_lpc(st->interp_lsp, st->interp_lpc, st->lpcSize,stack);
353
354
355       /*Open-loop pitch*/
356       if (st->complexity>2 || !st->submodes[st->submodeID] || st->vbr_enabled || st->vad_enabled || SUBMODE(forced_pitch_gain) ||
357           SUBMODE(lbr_pitch) != -1)
358       {
359          int nol_pitch[6];
360          spx_word16_t nol_pitch_coef[6];
361          
362          bw_lpc(st->gamma1, st->interp_lpc, st->bw_lpc1, st->lpcSize);
363          bw_lpc(st->gamma2, st->interp_lpc, st->bw_lpc2, st->lpcSize);
364          
365          for (i=0;i<st->windowSize-st->frameSize;i++)
366             st->sw[i] = st->winBuf[i];
367          for (;i<st->frameSize;i++)
368             st->sw[i] = in[i-st->windowSize+st->frameSize];
369          filter_mem16(st->sw, st->bw_lpc1, st->bw_lpc2, st->sw, st->frameSize, st->lpcSize, st->mem_sw_whole);
370
371          open_loop_nbest_pitch(st->sw, st->min_pitch, st->max_pitch, st->frameSize, 
372                                nol_pitch, nol_pitch_coef, 6, stack);
373          ol_pitch=nol_pitch[0];
374          ol_pitch_coef = nol_pitch_coef[0];
375          /*Try to remove pitch multiples*/
376          for (i=1;i<6;i++)
377          {
378 #ifdef FIXED_POINT
379             if ((nol_pitch_coef[i]>MULT16_16_Q15(nol_pitch_coef[0],27853)) && 
380 #else
381             if ((nol_pitch_coef[i]>.85*nol_pitch_coef[0]) && 
382 #endif
383                 (ABS(2*nol_pitch[i]-ol_pitch)<=2 || ABS(3*nol_pitch[i]-ol_pitch)<=3 || 
384                  ABS(4*nol_pitch[i]-ol_pitch)<=4 || ABS(5*nol_pitch[i]-ol_pitch)<=5))
385             {
386                /*ol_pitch_coef=nol_pitch_coef[i];*/
387                ol_pitch = nol_pitch[i];
388             }
389          }
390          /*if (ol_pitch>50)
391            ol_pitch/=2;*/
392          /*ol_pitch_coef = sqrt(ol_pitch_coef);*/
393
394 #ifdef EPIC_48K
395          if (st->lbr_48k)
396          {
397             if (ol_pitch < st->min_pitch+2)
398                ol_pitch = st->min_pitch+2;
399             if (ol_pitch > st->max_pitch-2)
400                ol_pitch = st->max_pitch-2;
401             open_loop_nbest_pitch(st->sw, ol_pitch-2, ol_pitch+2, st->frameSize>>1, 
402                                   &pitch_half[0], nol_pitch_coef, 1, stack);
403             open_loop_nbest_pitch(st->sw+(st->frameSize>>1), pitch_half[0]-1, pitch_half[0]+2, st->frameSize>>1, 
404                                   &pitch_half[1], nol_pitch_coef, 1, stack);
405          }
406 #endif
407       } else {
408          ol_pitch=0;
409          ol_pitch_coef=0;
410       }
411       
412       /*Compute "real" excitation*/
413       /* FIXME: Are we breaking aliasing rules? */
414       exc16_alias = (spx_word16_t*)st->exc;
415       for (i=0;i<st->windowSize-st->frameSize;i++)
416          exc16_alias[i] = st->winBuf[i];
417       for (;i<st->frameSize;i++)
418          exc16_alias[i] = in[i-st->windowSize+st->frameSize];
419       fir_mem16(exc16_alias, st->interp_lpc, exc16_alias, st->frameSize, st->lpcSize, st->mem_exc);
420
421       /* Compute open-loop excitation gain */
422 #ifdef EPIC_48K
423       if (st->lbr_48k)
424       {
425          float ol1=0,ol2=0;
426          float ol_gain2;
427          ol1 = compute_rms(st->exc, st->frameSize>>1);
428          ol2 = compute_rms(st->exc+(st->frameSize>>1), st->frameSize>>1);
429          ol1 *= ol1*(st->frameSize>>1);
430          ol2 *= ol2*(st->frameSize>>1);
431
432          ol_gain2=ol1;
433          if (ol2>ol1)
434             ol_gain2=ol2;
435          ol_gain2 = sqrt(2*ol_gain2*(ol1+ol2))*1.3*(1-.5*GAIN_SCALING_1*GAIN_SCALING_1*ol_pitch_coef*ol_pitch_coef);
436       
437          ol_gain=SHR(sqrt(1+ol_gain2/st->frameSize),SIG_SHIFT);
438
439       } else {
440 #endif
441          spx_word16_t g = compute_rms16(exc16_alias, st->frameSize);
442          if (ol_pitch>0)
443             ol_gain = MULT16_16(g, MULT16_16_Q14(QCONST16(1.1,14),
444                                 spx_sqrt(QCONST32(1.,28)-MULT16_32_Q15(QCONST16(.8,15),SHL32(MULT16_16(ol_pitch_coef,ol_pitch_coef),16)))));
445          else
446             ol_gain = SHL32(EXTEND32(g),SIG_SHIFT);
447 #ifdef EPIC_48K
448       }
449 #endif
450    }
451
452 #ifdef VORBIS_PSYCHO
453    for(i=0;i<256-st->frameSize;i++)
454       st->psy_window[i] = st->psy_window[i+st->frameSize];
455    for(i=0;i<st->frameSize;i++)
456       st->psy_window[256-st->frameSize+i] = in[i];
457    compute_curve(st->psy, st->psy_window, st->curve);
458    /*print_vec(st->curve, 128, "curve");*/
459    if (st->first)
460       for (i=0;i<128;i++)
461          st->old_curve[i] = st->curve[i];
462 #endif
463
464    /*VBR stuff*/
465    if (st->vbr && (st->vbr_enabled||st->vad_enabled))
466    {
467       float lsp_dist=0;
468       for (i=0;i<st->lpcSize;i++)
469          lsp_dist += (st->old_lsp[i] - st->lsp[i])*(st->old_lsp[i] - st->lsp[i]);
470       lsp_dist /= LSP_SCALING*LSP_SCALING;
471       
472       if (st->abr_enabled)
473       {
474          float qual_change=0;
475          if (st->abr_drift2 * st->abr_drift > 0)
476          {
477             /* Only adapt if long-term and short-term drift are the same sign */
478             qual_change = -.00001*st->abr_drift/(1+st->abr_count);
479             if (qual_change>.05)
480                qual_change=.05;
481             if (qual_change<-.05)
482                qual_change=-.05;
483          }
484          st->vbr_quality += qual_change;
485          if (st->vbr_quality>10)
486             st->vbr_quality=10;
487          if (st->vbr_quality<0)
488             st->vbr_quality=0;
489       }
490
491       st->relative_quality = vbr_analysis(st->vbr, in, st->frameSize, ol_pitch, GAIN_SCALING_1*ol_pitch_coef);
492       /*if (delta_qual<0)*/
493       /*  delta_qual*=.1*(3+st->vbr_quality);*/
494       if (st->vbr_enabled) 
495       {
496          int mode;
497          int choice=0;
498          float min_diff=100;
499          mode = 8;
500          while (mode)
501          {
502             int v1;
503             float thresh;
504             v1=(int)floor(st->vbr_quality);
505             if (v1==10)
506                thresh = vbr_nb_thresh[mode][v1];
507             else
508                thresh = (st->vbr_quality-v1)*vbr_nb_thresh[mode][v1+1] + (1+v1-st->vbr_quality)*vbr_nb_thresh[mode][v1];
509             if (st->relative_quality > thresh && 
510                 st->relative_quality-thresh<min_diff)
511             {
512                choice = mode;
513                min_diff = st->relative_quality-thresh;
514             }
515             mode--;
516          }
517          mode=choice;
518          if (mode==0)
519          {
520             if (st->dtx_count==0 || lsp_dist>.05 || !st->dtx_enabled || st->dtx_count>20)
521             {
522                mode=1;
523                st->dtx_count=1;
524             } else {
525                mode=0;
526                st->dtx_count++;
527             }
528          } else {
529             st->dtx_count=0;
530          }
531
532          speex_encoder_ctl(state, SPEEX_SET_MODE, &mode);
533
534          if (st->abr_enabled)
535          {
536             int bitrate;
537             speex_encoder_ctl(state, SPEEX_GET_BITRATE, &bitrate);
538             st->abr_drift+=(bitrate-st->abr_enabled);
539             st->abr_drift2 = .95*st->abr_drift2 + .05*(bitrate-st->abr_enabled);
540             st->abr_count += 1.0;
541          }
542
543       } else {
544          /*VAD only case*/
545          int mode;
546          if (st->relative_quality<2)
547          {
548             if (st->dtx_count==0 || lsp_dist>.05 || !st->dtx_enabled || st->dtx_count>20)
549             {
550                st->dtx_count=1;
551                mode=1;
552             } else {
553                mode=0;
554                st->dtx_count++;
555             }
556          } else {
557             st->dtx_count = 0;
558             mode=st->submodeSelect;
559          }
560          /*speex_encoder_ctl(state, SPEEX_SET_MODE, &mode);*/
561          st->submodeID=mode;
562       } 
563    } else {
564       st->relative_quality = -1;
565    }
566
567    if (st->encode_submode)
568    {
569 #ifdef EPIC_48K
570    if (!st->lbr_48k) {
571 #endif
572
573    /* First, transmit a zero for narrowband */
574    speex_bits_pack(bits, 0, 1);
575
576    /* Transmit the sub-mode we use for this frame */
577    speex_bits_pack(bits, st->submodeID, NB_SUBMODE_BITS);
578
579 #ifdef EPIC_48K
580    }
581 #endif
582    }
583
584    /* If null mode (no transmission), just set a couple things to zero*/
585    if (st->submodes[st->submodeID] == NULL)
586    {
587       for (i=0;i<st->frameSize;i++)
588          st->exc[i]=st->sw[i]=VERY_SMALL;
589
590       for (i=0;i<st->lpcSize;i++)
591          st->mem_sw[i]=0;
592       st->first=1;
593       st->bounded_pitch = 1;
594
595       speex_move(st->winBuf, in+2*st->frameSize-st->windowSize, (st->windowSize-st->frameSize)*sizeof(spx_word16_t));
596
597       /* Clear memory (no need to really compute it) */
598       for (i=0;i<st->lpcSize;i++)
599          st->mem_sp[i] = 0;
600       return 0;
601
602    }
603
604    /* LSP Quantization */
605    if (st->first)
606    {
607       for (i=0;i<st->lpcSize;i++)
608          st->old_lsp[i] = st->lsp[i];
609    }
610
611
612    /*Quantize LSPs*/
613 #if 1 /*0 for unquantized*/
614    SUBMODE(lsp_quant)(st->lsp, st->qlsp, st->lpcSize, bits);
615 #else
616    for (i=0;i<st->lpcSize;i++)
617      st->qlsp[i]=st->lsp[i];
618 #endif
619
620 #ifdef EPIC_48K
621    if (st->lbr_48k) {
622       speex_bits_pack(bits, pitch_half[0]-st->min_pitch, 7);
623       speex_bits_pack(bits, pitch_half[1]-pitch_half[0]+1, 2);
624       
625       {
626          int quant = (int)floor(.5+7.4*GAIN_SCALING_1*ol_pitch_coef);
627          if (quant>7)
628             quant=7;
629          if (quant<0)
630             quant=0;
631          ol_pitch_id=quant;
632          speex_bits_pack(bits, quant, 3);
633          ol_pitch_coef=GAIN_SCALING*0.13514*quant;
634          
635       }
636       {
637          int qe = (int)(floor(.5+2.1*log(ol_gain*1.0/SIG_SCALING)))-2;
638          if (qe<0)
639             qe=0;
640          if (qe>15)
641             qe=15;
642          ol_gain = exp((qe+2)/2.1)*SIG_SCALING;
643          speex_bits_pack(bits, qe, 4);
644       }
645
646    } else {
647 #endif
648
649    /*If we use low bit-rate pitch mode, transmit open-loop pitch*/
650    if (SUBMODE(lbr_pitch)!=-1)
651    {
652       speex_bits_pack(bits, ol_pitch-st->min_pitch, 7);
653    } 
654
655    if (SUBMODE(forced_pitch_gain))
656    {
657       int quant;
658       quant = (int)floor(.5+15*ol_pitch_coef*GAIN_SCALING_1);
659       if (quant>15)
660          quant=15;
661       if (quant<0)
662          quant=0;
663       speex_bits_pack(bits, quant, 4);
664       ol_pitch_coef=GAIN_SCALING*0.066667*quant;
665    }
666    
667    
668    /*Quantize and transmit open-loop excitation gain*/
669 #ifdef FIXED_POINT
670    {
671       int qe = scal_quant32(ol_gain, ol_gain_table, 32);
672       /*ol_gain = exp(qe/3.5)*SIG_SCALING;*/
673       ol_gain = MULT16_32_Q15(28406,ol_gain_table[qe]);
674       speex_bits_pack(bits, qe, 5);
675    }
676 #else
677    {
678       int qe = (int)(floor(.5+3.5*log(ol_gain*1.0/SIG_SCALING)));
679       if (qe<0)
680          qe=0;
681       if (qe>31)
682          qe=31;
683       ol_gain = exp(qe/3.5)*SIG_SCALING;
684       speex_bits_pack(bits, qe, 5);
685    }
686 #endif
687
688
689 #ifdef EPIC_48K
690    }
691 #endif
692
693
694    /* Special case for first frame */
695    if (st->first)
696    {
697       for (i=0;i<st->lpcSize;i++)
698          st->old_qlsp[i] = st->qlsp[i];
699    }
700
701    /* Target signal */
702    ALLOC(target, st->subframeSize, spx_sig_t);
703    ALLOC(innov, st->subframeSize, spx_sig_t);
704    ALLOC(ringing, st->subframeSize, spx_word16_t);
705    ALLOC(syn_resp, st->subframeSize, spx_word16_t);
706    ALLOC(real_exc, st->subframeSize, spx_sig_t);
707    ALLOC(mem, st->lpcSize, spx_mem_t);
708
709    /* Loop on sub-frames */
710    for (sub=0;sub<st->nbSubframes;sub++)
711    {
712       int   offset;
713       spx_word16_t *sw;
714       spx_sig_t *exc;
715       spx_sig_t *innov_save = NULL;
716       int pitch;
717       int response_bound = st->subframeSize;
718 #ifdef EPIC_48K
719       if (st->lbr_48k)
720       {
721          if (sub*2 < st->nbSubframes)
722             ol_pitch = pitch_half[0];
723          else
724             ol_pitch = pitch_half[1];
725       }
726 #endif
727
728       /* Offset relative to start of frame */
729       offset = st->subframeSize*sub;
730       /* Excitation */
731       exc=st->exc+offset;
732       /* Weighted signal */
733       sw=st->sw+offset;
734       /* Pointer for saving innovation */
735       if (st->innov_save)
736          innov_save = st->innov_save+offset;
737       
738       /* LSP interpolation (quantized and unquantized) */
739       lsp_interpolate(st->old_lsp, st->lsp, st->interp_lsp, st->lpcSize, sub, st->nbSubframes);
740       lsp_interpolate(st->old_qlsp, st->qlsp, st->interp_qlsp, st->lpcSize, sub, st->nbSubframes);
741
742       /* Make sure the filters are stable */
743       lsp_enforce_margin(st->interp_lsp, st->lpcSize, LSP_MARGIN);
744       lsp_enforce_margin(st->interp_qlsp, st->lpcSize, LSP_MARGIN);
745
746       /* Compute interpolated LPCs (quantized and unquantized) */
747       lsp_to_lpc(st->interp_lsp, st->interp_lpc, st->lpcSize,stack);
748
749       lsp_to_lpc(st->interp_qlsp, st->interp_qlpc, st->lpcSize, stack);
750
751       /* Compute analysis filter gain at w=pi (for use in SB-CELP) */
752       {
753          spx_word32_t pi_g=LPC_SCALING;
754          for (i=0;i<st->lpcSize;i+=2)
755          {
756             /*pi_g += -st->interp_qlpc[i] +  st->interp_qlpc[i+1];*/
757             pi_g = ADD32(pi_g, SUB32(EXTEND32(st->interp_qlpc[i+1]),EXTEND32(st->interp_qlpc[i])));
758          }
759          st->pi_gain[sub] = pi_g;
760       }
761
762 #ifdef VORBIS_PSYCHO
763       {
764          float curr_curve[128];
765          float fact = ((float)sub+1.0f)/st->nbSubframes;
766          for (i=0;i<128;i++)
767             curr_curve[i] = (1.0f-fact)*st->old_curve[i] + fact*st->curve[i];
768          curve_to_lpc(st->psy, curr_curve, st->bw_lpc1, st->bw_lpc2, 10);
769       }
770 #else
771       /* Compute bandwidth-expanded (unquantized) LPCs for perceptual weighting */
772       bw_lpc(st->gamma1, st->interp_lpc, st->bw_lpc1, st->lpcSize);
773       if (st->gamma2>=0)
774          bw_lpc(st->gamma2, st->interp_lpc, st->bw_lpc2, st->lpcSize);
775       else
776       {
777          st->bw_lpc2[0]=1;
778          for (i=1;i<=st->lpcSize;i++)
779             st->bw_lpc2[i]=0;
780       }
781       /*print_vec(st->bw_lpc1, 10, "bw_lpc");*/
782 #endif
783
784       {
785          /*FIXME: This will break if we change the window size */
786          if (st->windowSize-st->frameSize != st->subframeSize)
787             speex_error("windowSize-frameSize != subframeSize");
788          if (sub==0)
789          {
790             for (i=0;i<st->subframeSize;i++)
791                real_exc[i] = SHL32(EXTEND32(st->winBuf[i]),SIG_SHIFT);
792             for (i=0;i<st->subframeSize;i++)
793                sw[i] = st->winBuf[i];
794          } else {
795             for (i=0;i<st->subframeSize;i++)
796                real_exc[i] = SHL32(EXTEND32(in[i+((sub-1)*st->subframeSize)]),SIG_SHIFT);
797             for (i=0;i<st->subframeSize;i++)
798                sw[i] = in[i+((sub-1)*st->subframeSize)];
799          }
800       }
801       fir_mem2(real_exc, st->interp_qlpc, real_exc, st->subframeSize, st->lpcSize, st->mem_exc2);
802       
803       if (st->complexity==0)
804          response_bound >>= 1;
805       compute_impulse_response(st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, syn_resp, response_bound, st->lpcSize, stack);
806       for (i=response_bound;i<st->subframeSize;i++)
807          syn_resp[i]=VERY_SMALL;
808       
809       /* Compute zero response of A(z/g1) / ( A(z/g2) * A(z) ) */
810       for (i=0;i<st->lpcSize;i++)
811          mem[i]=SHL32(st->mem_sp[i],1);
812       for (i=0;i<st->subframeSize;i++)
813          ringing[i] = VERY_SMALL;
814 #ifdef SHORTCUTS2
815       iir_mem16(ringing, st->interp_qlpc, ringing, response_bound, st->lpcSize, mem);
816       for (i=0;i<st->lpcSize;i++)
817          mem[i]=SHL32(st->mem_sw[i],1);
818       filter_mem16(ringing, st->bw_lpc1, st->bw_lpc2, ringing, response_bound, st->lpcSize, mem);
819       for (i=response_bound;i<st->subframeSize;i++)
820          ringing[i]=0;
821 #else
822       iir_mem16(ringing, st->interp_qlpc, ringing, st->subframeSize, st->lpcSize, mem);
823       for (i=0;i<st->lpcSize;i++)
824          mem[i]=SHL32(st->mem_sw[i],1);
825       filter_mem16(ringing, st->bw_lpc1, st->bw_lpc2, ringing, st->subframeSize, st->lpcSize, mem);
826 #endif
827       
828       /* Compute weighted signal */
829       for (i=0;i<st->lpcSize;i++)
830          mem[i]=st->mem_sw[i];
831       filter_mem16(sw, st->bw_lpc1, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, mem);
832       
833       if (st->complexity==0)
834          for (i=0;i<st->lpcSize;i++)
835             st->mem_sw[i]=mem[i];
836       
837       /* Compute target signal */
838       for (i=0;i<st->subframeSize;i++)
839          target[i]=SHL32(EXTEND32(SUB16(sw[i],PSHR32(ringing[i],1))),SIG_SHIFT);
840
841       /* Reset excitation */
842       for (i=0;i<st->subframeSize;i++)
843          exc[i]=0;
844
845       /* If we have a long-term predictor (otherwise, something's wrong) */
846       if (SUBMODE(ltp_quant))
847       {
848          int pit_min, pit_max;
849          /* Long-term prediction */
850          if (SUBMODE(lbr_pitch) != -1)
851          {
852             /* Low bit-rate pitch handling */
853             int margin;
854             margin = SUBMODE(lbr_pitch);
855             if (margin)
856             {
857                if (ol_pitch < st->min_pitch+margin-1)
858                   ol_pitch=st->min_pitch+margin-1;
859                if (ol_pitch > st->max_pitch-margin)
860                   ol_pitch=st->max_pitch-margin;
861                pit_min = ol_pitch-margin+1;
862                pit_max = ol_pitch+margin;
863             } else {
864                pit_min=pit_max=ol_pitch;
865             }
866          } else {
867             pit_min = st->min_pitch;
868             pit_max = st->max_pitch;
869          }
870          
871          /* Force pitch to use only the current frame if needed */
872          if (st->bounded_pitch && pit_max>offset)
873             pit_max=offset;
874
875 #ifdef EPIC_48K
876          if (st->lbr_48k)
877          {
878             pitch = SUBMODE(ltp_quant)(target, sw, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2,
879                                        exc, SUBMODE(ltp_params), pit_min, pit_max, ol_pitch_coef,
880                                        st->lpcSize, st->subframeSize, bits, stack, 
881                                        exc, syn_resp, st->complexity, ol_pitch_id, st->plc_tuning);
882          } else {
883 #endif
884
885          /* Perform pitch search */
886          pitch = SUBMODE(ltp_quant)(target, sw, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2,
887                                     exc, SUBMODE(ltp_params), pit_min, pit_max, ol_pitch_coef,
888                                     st->lpcSize, st->subframeSize, bits, stack, 
889                                     exc, syn_resp, st->complexity, 0, st->plc_tuning);
890 #ifdef EPIC_48K
891          }
892 #endif
893
894          st->pitch[sub]=pitch;
895       } else {
896          speex_error ("No pitch prediction, what's wrong");
897       }
898
899       /* Quantization of innovation */
900       {
901          spx_word32_t ener=0;
902          spx_word16_t fine_gain;
903
904          for (i=0;i<st->subframeSize;i++)
905             innov[i]=0;
906          
907          for (i=0;i<st->subframeSize;i++)
908             real_exc[i] = SUB32(real_exc[i], exc[i]);
909
910          ener = SHL32(EXTEND32(compute_rms(real_exc, st->subframeSize)),SIG_SHIFT);
911          
912          /*FIXME: Should use DIV32_16 and make sure result fits in 16 bits */
913 #ifdef FIXED_POINT
914          {
915             spx_word32_t f = DIV32(ener,PSHR32(ol_gain,SIG_SHIFT));
916             if (f<=32767)
917                fine_gain = f;
918             else
919                fine_gain = 32767;
920          }
921 #else
922          fine_gain = DIV32_16(ener,PSHR32(ol_gain,SIG_SHIFT));
923 #endif
924          /* Calculate gain correction for the sub-frame (if any) */
925          if (SUBMODE(have_subframe_gain)) 
926          {
927             int qe;
928             if (SUBMODE(have_subframe_gain)==3)
929             {
930                qe = scal_quant(fine_gain, exc_gain_quant_scal3_bound, 8);
931                speex_bits_pack(bits, qe, 3);
932                ener=MULT16_32_Q14(exc_gain_quant_scal3[qe],ol_gain);
933             } else {
934                qe = scal_quant(fine_gain, exc_gain_quant_scal1_bound, 2);
935                speex_bits_pack(bits, qe, 1);
936                ener=MULT16_32_Q14(exc_gain_quant_scal1[qe],ol_gain);               
937             }
938          } else {
939             ener=ol_gain;
940          }
941
942          /*printf ("%f %f\n", ener, ol_gain);*/
943
944          /* Normalize innovation */
945          signal_div(target, target, ener, st->subframeSize);
946
947          /* Quantize innovation */
948          if (SUBMODE(innovation_quant))
949          {
950             /* Codebook search */
951             SUBMODE(innovation_quant)(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, 
952                                       SUBMODE(innovation_params), st->lpcSize, st->subframeSize, 
953                                       innov, syn_resp, bits, stack, st->complexity, SUBMODE(double_codebook));
954             
955             /* De-normalize innovation and update excitation */
956             signal_mul(innov, innov, ener, st->subframeSize);
957
958             for (i=0;i<st->subframeSize;i++)
959                exc[i] = ADD32(exc[i],innov[i]);
960          } else {
961             speex_error("No fixed codebook");
962          }
963
964          if (innov_save)
965          {
966             for (i=0;i<st->subframeSize;i++)
967                innov_save[i] = innov[i];
968          }
969          /* In some (rare) modes, we do a second search (more bits) to reduce noise even more */
970          if (SUBMODE(double_codebook)) {
971             char *tmp_stack=stack;
972             VARDECL(spx_sig_t *innov2);
973             ALLOC(innov2, st->subframeSize, spx_sig_t);
974             for (i=0;i<st->subframeSize;i++)
975                innov2[i]=0;
976             for (i=0;i<st->subframeSize;i++)
977                target[i]=MULT16_32_Q13(QCONST16(2.2,13), target[i]);
978             SUBMODE(innovation_quant)(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, 
979                                       SUBMODE(innovation_params), st->lpcSize, st->subframeSize, 
980                                       innov2, syn_resp, bits, stack, st->complexity, 0);
981             signal_mul(innov2, innov2, MULT16_32_Q15(QCONST16(0.454545,15),ener), st->subframeSize);
982             for (i=0;i<st->subframeSize;i++)
983                exc[i] = ADD32(exc[i],innov2[i]);
984             if (innov_save)
985             {
986                for (i=0;i<st->subframeSize;i++)
987                   innov_save[i] = ADD32(innov_save[i],innov2[i]);
988             }
989             stack = tmp_stack;
990          }
991
992       }
993
994       for (i=0;i<st->subframeSize;i++)
995          sw[i] = PSHR32(exc[i], SIG_SHIFT);
996       /* Final signal synthesis from excitation */
997       iir_mem16(sw, st->interp_qlpc, sw, st->subframeSize, st->lpcSize, st->mem_sp);
998
999       /* Compute weighted signal again, from synthesized speech (not sure it's the right thing) */
1000       if (st->complexity!=0)
1001          filter_mem16(sw, st->bw_lpc1, st->bw_lpc2, sw, st->subframeSize, st->lpcSize, st->mem_sw);
1002       
1003    }
1004
1005    /* Store the LSPs for interpolation in the next frame */
1006    if (st->submodeID>=1)
1007    {
1008       for (i=0;i<st->lpcSize;i++)
1009          st->old_lsp[i] = st->lsp[i];
1010       for (i=0;i<st->lpcSize;i++)
1011          st->old_qlsp[i] = st->qlsp[i];
1012    }
1013
1014 #ifdef VORBIS_PSYCHO
1015    if (st->submodeID>=1)
1016    {
1017       for (i=0;i<128;i++)
1018          st->old_curve[i] = st->curve[i];
1019    }
1020 #endif
1021
1022    if (st->submodeID==1)
1023    {
1024       if (st->dtx_count)
1025          speex_bits_pack(bits, 15, 4);
1026       else
1027          speex_bits_pack(bits, 0, 4);
1028    }
1029
1030    /* The next frame will not be the first (Duh!) */
1031    st->first = 0;
1032    speex_move(st->winBuf, in+2*st->frameSize-st->windowSize, (st->windowSize-st->frameSize)*sizeof(spx_word16_t));
1033
1034    if (SUBMODE(innovation_quant) == noise_codebook_quant || st->submodeID==0)
1035       st->bounded_pitch = 1;
1036    else
1037       st->bounded_pitch = 0;
1038
1039    return 1;
1040 }
1041
1042 #ifdef NEW_ENHANCER
1043 #define PITCH_PERIODS 3
1044 #else
1045 #define PITCH_PERIODS 1
1046 #endif
1047
1048 void *nb_decoder_init(const SpeexMode *m)
1049 {
1050    DecState *st;
1051    const SpeexNBMode *mode;
1052    int i;
1053
1054    mode=(const SpeexNBMode*)m->mode;
1055    st = (DecState *)speex_alloc(sizeof(DecState));
1056    if (!st)
1057       return NULL;
1058 #if defined(VAR_ARRAYS) || defined (USE_ALLOCA)
1059    st->stack = NULL;
1060 #else
1061    st->stack = (char*)speex_alloc_scratch(NB_DEC_STACK);
1062 #endif
1063
1064    st->mode=m;
1065
1066
1067    st->encode_submode = 1;
1068 #ifdef EPIC_48K
1069    st->lbr_48k=mode->lbr48k;
1070 #endif
1071
1072    st->first=1;
1073    /* Codec parameters, should eventually have several "modes"*/
1074    st->frameSize = mode->frameSize;
1075    st->nbSubframes=mode->frameSize/mode->subframeSize;
1076    st->subframeSize=mode->subframeSize;
1077    st->lpcSize = mode->lpcSize;
1078    st->min_pitch=mode->pitchStart;
1079    st->max_pitch=mode->pitchEnd;
1080
1081    st->submodes=mode->submodes;
1082    st->submodeID=mode->defaultSubmode;
1083
1084    st->lpc_enh_enabled=0;
1085
1086
1087    st->excBuf = speex_alloc((st->frameSize + PITCH_PERIODS*st->max_pitch + 1 + 50)*sizeof(spx_sig_t));
1088    st->exc = st->excBuf + PITCH_PERIODS*st->max_pitch + 1;
1089    for (i=0;i<st->frameSize + st->max_pitch + 1;i++)
1090       st->excBuf[i]=0;
1091
1092    st->interp_qlpc = speex_alloc(st->lpcSize*sizeof(spx_coef_t));
1093    st->old_qlsp = speex_alloc(st->lpcSize*sizeof(spx_lsp_t));
1094    st->mem_sp = speex_alloc((5*st->lpcSize)*sizeof(spx_mem_t));
1095    st->comb_mem = speex_alloc(sizeof(CombFilterMem));
1096    comb_filter_mem_init (st->comb_mem);
1097
1098    st->pi_gain = speex_alloc((st->nbSubframes)*sizeof(spx_word32_t));
1099    st->last_pitch = 40;
1100    st->count_lost=0;
1101    st->pitch_gain_buf[0] = st->pitch_gain_buf[1] = st->pitch_gain_buf[2] = 0;
1102    st->pitch_gain_buf_idx = 0;
1103    st->seed = 1000;
1104    
1105    st->sampling_rate=8000;
1106    st->last_ol_gain = 0;
1107
1108    st->user_callback.func = &speex_default_user_handler;
1109    st->user_callback.data = NULL;
1110    for (i=0;i<16;i++)
1111       st->speex_callbacks[i].func = NULL;
1112
1113    st->voc_m1=st->voc_m2=st->voc_mean=0;
1114    st->voc_offset=0;
1115    st->dtx_enabled=0;
1116 #ifdef ENABLE_VALGRIND
1117    VALGRIND_MAKE_READABLE(st, (st->stack-(char*)st));
1118 #endif
1119    return st;
1120 }
1121
1122 void nb_decoder_destroy(void *state)
1123 {
1124    DecState *st;
1125    st=(DecState*)state;
1126    
1127 #if !(defined(VAR_ARRAYS) || defined (USE_ALLOCA))
1128    speex_free_scratch(st->stack);
1129 #endif
1130
1131    speex_free (st->excBuf);
1132    speex_free (st->interp_qlpc);
1133    speex_free (st->old_qlsp);
1134    speex_free (st->mem_sp);
1135    speex_free (st->comb_mem);
1136    speex_free (st->pi_gain);
1137
1138    speex_free(state);
1139 }
1140
1141 #define median3(a, b, c)        ((a) < (b) ? ((b) < (c) ? (b) : ((a) < (c) ? (c) : (a))) : ((c) < (b) ? (b) : ((c) < (a) ? (c) : (a))))
1142
1143 #ifdef FIXED_POINT
1144 const spx_word16_t attenuation[10] = {32767, 31483, 27923, 22861, 17278, 12055, 7764, 4616, 2533, 1283};
1145 #else
1146 const spx_word16_t attenuation[10] = {1., 0.961, 0.852, 0.698, 0.527, 0.368, 0.237, 0.141, 0.077, 0.039};
1147
1148 #endif
1149
1150 static void nb_decode_lost(DecState *st, spx_word16_t *out, char *stack)
1151 {
1152    int i, sub;
1153    int pitch_val;
1154    VARDECL(spx_coef_t *awk1);
1155    VARDECL(spx_coef_t *awk2);
1156    VARDECL(spx_coef_t *awk3);
1157    spx_word16_t pitch_gain;
1158    spx_word16_t fact;
1159    spx_word16_t gain_med;
1160    spx_word16_t innov_gain;
1161    
1162    if (st->count_lost<10)
1163       fact = attenuation[st->count_lost];
1164    else
1165       fact = 0;
1166
1167    gain_med = median3(st->pitch_gain_buf[0], st->pitch_gain_buf[1], st->pitch_gain_buf[2]);
1168    if (gain_med < st->last_pitch_gain)
1169       st->last_pitch_gain = gain_med;
1170    
1171 #ifdef FIXED_POINT
1172    pitch_gain = st->last_pitch_gain;
1173    if (pitch_gain>54)
1174       pitch_gain = 54;
1175    pitch_gain = SHL(pitch_gain, 9);
1176 #else   
1177    pitch_gain = GAIN_SCALING_1*st->last_pitch_gain;
1178    if (pitch_gain>.85)
1179       pitch_gain=.85;
1180 #endif
1181
1182    pitch_gain = MULT16_16_Q15(fact,pitch_gain) + VERY_SMALL;
1183
1184    /* Shift all buffers by one frame */
1185    /*speex_move(st->inBuf, st->inBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(spx_sig_t));*/
1186    speex_move(st->excBuf, st->excBuf+st->frameSize, (PITCH_PERIODS*st->max_pitch + 1)*sizeof(spx_sig_t));
1187
1188    ALLOC(awk1, st->lpcSize, spx_coef_t);
1189    ALLOC(awk2, st->lpcSize, spx_coef_t);
1190    ALLOC(awk3, st->lpcSize, spx_coef_t);
1191
1192    for (sub=0;sub<st->nbSubframes;sub++)
1193    {
1194       int offset;
1195       spx_word16_t *sp;
1196       spx_sig_t *exc;
1197       /* Offset relative to start of frame */
1198       offset = st->subframeSize*sub;
1199       /* Original signal */
1200       sp=out+offset;
1201       /* Excitation */
1202       exc=st->exc+offset;
1203       /* Excitation after post-filter*/
1204
1205       /* Calculate perceptually enhanced LPC filter */
1206       if (st->lpc_enh_enabled)
1207       {
1208          spx_word16_t k1,k2,k3;
1209          if (st->submodes[st->submodeID] != NULL)
1210          {
1211             k1=SUBMODE(lpc_enh_k1);
1212             k2=SUBMODE(lpc_enh_k2);
1213             k3=SUBMODE(lpc_enh_k3);
1214          } else {
1215             k1=k2=.7*GAMMA_SCALING;
1216             k3=.0;
1217          }
1218          bw_lpc(k1, st->interp_qlpc, awk1, st->lpcSize);
1219          bw_lpc(k2, st->interp_qlpc, awk2, st->lpcSize);
1220          bw_lpc(k3, st->interp_qlpc, awk3, st->lpcSize);
1221       }
1222         
1223       /* Make up a plausible excitation */
1224       /* FIXME: THIS CAN BE IMPROVED */
1225       /*if (pitch_gain>.95)
1226         pitch_gain=.95;*/
1227       
1228       /* FIXME: This was rms of innovation (not exc) */
1229       innov_gain = compute_rms(st->exc, st->frameSize);
1230       pitch_val = st->last_pitch + SHR32((spx_int32_t)speex_rand(1+st->count_lost, &st->seed),SIG_SHIFT);
1231       if (pitch_val > st->max_pitch)
1232          pitch_val = st->max_pitch;
1233       if (pitch_val < st->min_pitch)
1234          pitch_val = st->min_pitch;
1235       for (i=0;i<st->subframeSize;i++)
1236       {
1237          exc[i]= MULT16_32_Q15(pitch_gain, (exc[i-pitch_val]+VERY_SMALL)) + 
1238                MULT16_32_Q15(fact, MULT16_32_Q15(SHL(Q15ONE,15)-SHL(MULT16_16(pitch_gain,pitch_gain),1),speex_rand(innov_gain, &st->seed)));
1239       }
1240 #ifdef NEW_ENHANCER
1241       for (i=0;i<st->subframeSize;i++)
1242          sp[i]=PSHR32(exc[i-st->subframeSize],SIG_SHIFT);
1243 #else
1244       for (i=0;i<st->subframeSize;i++)
1245          sp[i]=PSHR32(exc[i],SIG_SHIFT);
1246 #endif 
1247       /* Signal synthesis */
1248       if (st->lpc_enh_enabled)
1249       {
1250          filter_mem16(sp, awk2, awk1, sp, st->subframeSize, st->lpcSize, 
1251                      st->mem_sp+st->lpcSize);
1252          filter_mem16(sp, awk3, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
1253                      st->mem_sp);
1254       } else {
1255          for (i=0;i<st->lpcSize;i++)
1256             st->mem_sp[st->lpcSize+i] = 0;
1257          iir_mem16(sp, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
1258                      st->mem_sp);
1259       }
1260       bw_lpc(QCONST16(.98,15), st->interp_qlpc, st->interp_qlpc, st->lpcSize);
1261    }
1262    
1263    st->first = 0;
1264    st->count_lost++;
1265    st->pitch_gain_buf[st->pitch_gain_buf_idx++] = PSHR(pitch_gain,9);
1266    if (st->pitch_gain_buf_idx > 2) /* rollover */
1267       st->pitch_gain_buf_idx = 0;
1268 }
1269
1270 int nb_decode(void *state, SpeexBits *bits, void *vout)
1271 {
1272    DecState *st;
1273    int i, sub;
1274    int pitch;
1275    spx_word16_t pitch_gain[3];
1276    spx_word32_t ol_gain=0;
1277    int ol_pitch=0;
1278    spx_word16_t ol_pitch_coef=0;
1279    int best_pitch=40;
1280    spx_word16_t best_pitch_gain=0;
1281    int wideband;
1282    int m;
1283    char *stack;
1284    VARDECL(spx_sig_t *innov);
1285    VARDECL(spx_coef_t *ak);
1286    VARDECL(spx_coef_t *awk1);
1287    VARDECL(spx_coef_t *awk2);
1288    VARDECL(spx_coef_t *awk3);
1289    VARDECL(spx_lsp_t *qlsp);
1290    spx_word16_t pitch_average=0;
1291 #ifdef EPIC_48K
1292    int pitch_half[2];
1293    int ol_pitch_id=0;
1294 #endif
1295    spx_word16_t *out = vout;
1296    VARDECL(spx_lsp_t *interp_qlsp);
1297
1298    st=(DecState*)state;
1299    stack=st->stack;
1300
1301    /* Check if we're in DTX mode*/
1302    if (!bits && st->dtx_enabled)
1303    {
1304       st->submodeID=0;
1305    } else 
1306    {
1307       /* If bits is NULL, consider the packet to be lost (what could we do anyway) */
1308       if (!bits)
1309       {
1310          nb_decode_lost(st, out, stack);
1311          return 0;
1312       }
1313
1314       if (st->encode_submode)
1315       {
1316 #ifdef EPIC_48K
1317          if (!st->lbr_48k) {
1318 #endif
1319
1320       /* Search for next narrowband block (handle requests, skip wideband blocks) */
1321       do {
1322          if (speex_bits_remaining(bits)<5)
1323             return -1;
1324          wideband = speex_bits_unpack_unsigned(bits, 1);
1325          if (wideband) /* Skip wideband block (for compatibility) */
1326          {
1327             int submode;
1328             int advance;
1329             advance = submode = speex_bits_unpack_unsigned(bits, SB_SUBMODE_BITS);
1330             speex_mode_query(&speex_wb_mode, SPEEX_SUBMODE_BITS_PER_FRAME, &advance);
1331             if (advance < 0)
1332             {
1333                speex_warning ("Invalid wideband mode encountered. Corrupted stream?");
1334                return -2;
1335             } 
1336             advance -= (SB_SUBMODE_BITS+1);
1337             speex_bits_advance(bits, advance);
1338             
1339             if (speex_bits_remaining(bits)<5)
1340                return -1;
1341             wideband = speex_bits_unpack_unsigned(bits, 1);
1342             if (wideband)
1343             {
1344                advance = submode = speex_bits_unpack_unsigned(bits, SB_SUBMODE_BITS);
1345                speex_mode_query(&speex_wb_mode, SPEEX_SUBMODE_BITS_PER_FRAME, &advance);
1346                if (advance < 0)
1347                {
1348                   speex_warning ("Invalid wideband mode encountered: corrupted stream?");
1349                   return -2;
1350                } 
1351                advance -= (SB_SUBMODE_BITS+1);
1352                speex_bits_advance(bits, advance);
1353                wideband = speex_bits_unpack_unsigned(bits, 1);
1354                if (wideband)
1355                {
1356                   speex_warning ("More than two wideband layers found: corrupted stream?");
1357                   return -2;
1358                }
1359
1360             }
1361          }
1362          if (speex_bits_remaining(bits)<4)
1363             return -1;
1364          /* FIXME: Check for overflow */
1365          m = speex_bits_unpack_unsigned(bits, 4);
1366          if (m==15) /* We found a terminator */
1367          {
1368             return -1;
1369          } else if (m==14) /* Speex in-band request */
1370          {
1371             int ret = speex_inband_handler(bits, st->speex_callbacks, state);
1372             if (ret)
1373                return ret;
1374          } else if (m==13) /* User in-band request */
1375          {
1376             int ret = st->user_callback.func(bits, state, st->user_callback.data);
1377             if (ret)
1378                return ret;
1379          } else if (m>8) /* Invalid mode */
1380          {
1381             speex_warning("Invalid mode encountered: corrupted stream?");
1382             return -2;
1383          }
1384       
1385       } while (m>8);
1386
1387       /* Get the sub-mode that was used */
1388       st->submodeID = m;
1389 #ifdef EPIC_48K
1390          }
1391 #endif
1392       }
1393
1394    }
1395
1396    /* Shift all buffers by one frame */
1397    speex_move(st->excBuf, st->excBuf+st->frameSize, (PITCH_PERIODS*st->max_pitch + 1)*sizeof(spx_sig_t));
1398
1399    /* If null mode (no transmission), just set a couple things to zero*/
1400    if (st->submodes[st->submodeID] == NULL)
1401    {
1402       VARDECL(spx_coef_t *lpc);
1403       ALLOC(lpc, st->lpcSize, spx_coef_t);
1404       bw_lpc(GAMMA_SCALING*.93, st->interp_qlpc, lpc, st->lpcSize);
1405       {
1406          float innov_gain=0;
1407          float pgain=GAIN_SCALING_1*st->last_pitch_gain;
1408          if (pgain>.6)
1409             pgain=.6;
1410          /* This was innov, not exc */
1411          innov_gain = compute_rms(st->exc, st->frameSize);
1412          for (i=0;i<st->frameSize;i++)
1413             st->exc[i]=VERY_SMALL;
1414          speex_rand_vec(innov_gain, st->exc, st->frameSize);
1415       }
1416
1417
1418       st->first=1;
1419
1420       for (i=0;i<st->frameSize;i++)
1421          out[i] = st->exc[i];
1422       /* Final signal synthesis from excitation */
1423       iir_mem16(out, lpc, out, st->frameSize, st->lpcSize, st->mem_sp);
1424
1425       st->count_lost=0;
1426       return 0;
1427    }
1428
1429    ALLOC(qlsp, st->lpcSize, spx_lsp_t);
1430
1431    /* Unquantize LSPs */
1432    SUBMODE(lsp_unquant)(qlsp, st->lpcSize, bits);
1433
1434    /*Damp memory if a frame was lost and the LSP changed too much*/
1435    if (st->count_lost)
1436    {
1437       spx_word16_t fact;
1438       spx_word32_t lsp_dist=0;
1439       for (i=0;i<st->lpcSize;i++)
1440          lsp_dist = ADD32(lsp_dist, EXTEND32(ABS(st->old_qlsp[i] - qlsp[i])));
1441 #ifdef FIXED_POINT
1442       fact = SHR16(19661,SHR32(lsp_dist,LSP_SHIFT+2));      
1443 #else
1444       fact = .6*exp(-.2*lsp_dist);
1445 #endif
1446       for (i=0;i<2*st->lpcSize;i++)
1447          st->mem_sp[i] = MULT16_32_Q15(fact,st->mem_sp[i]);
1448    }
1449
1450
1451    /* Handle first frame and lost-packet case */
1452    if (st->first || st->count_lost)
1453    {
1454       for (i=0;i<st->lpcSize;i++)
1455          st->old_qlsp[i] = qlsp[i];
1456    }
1457
1458 #ifdef EPIC_48K
1459    if (st->lbr_48k) {
1460       pitch_half[0] = st->min_pitch+speex_bits_unpack_unsigned(bits, 7);
1461       pitch_half[1] = pitch_half[0]+speex_bits_unpack_unsigned(bits, 2)-1;
1462
1463       ol_pitch_id = speex_bits_unpack_unsigned(bits, 3);
1464       ol_pitch_coef=GAIN_SCALING*0.13514*ol_pitch_id;
1465
1466       {
1467          int qe;
1468          qe = speex_bits_unpack_unsigned(bits, 4);
1469          ol_gain = SIG_SCALING*exp((qe+2)/2.1),SIG_SHIFT;
1470       }
1471
1472    } else {
1473 #endif
1474
1475    /* Get open-loop pitch estimation for low bit-rate pitch coding */
1476    if (SUBMODE(lbr_pitch)!=-1)
1477    {
1478       ol_pitch = st->min_pitch+speex_bits_unpack_unsigned(bits, 7);
1479    } 
1480    
1481    if (SUBMODE(forced_pitch_gain))
1482    {
1483       int quant;
1484       quant = speex_bits_unpack_unsigned(bits, 4);
1485       ol_pitch_coef=GAIN_SCALING*0.066667*quant;
1486    }
1487    
1488    /* Get global excitation gain */
1489    {
1490       int qe;
1491       qe = speex_bits_unpack_unsigned(bits, 5);
1492 #ifdef FIXED_POINT
1493       ol_gain = MULT16_32_Q15(28406,ol_gain_table[qe]);
1494 #else
1495       ol_gain = SIG_SCALING*exp(qe/3.5);
1496 #endif
1497    }
1498 #ifdef EPIC_48K
1499    }
1500 #endif
1501
1502    ALLOC(ak, st->lpcSize, spx_coef_t);
1503    ALLOC(awk1, st->lpcSize, spx_coef_t);
1504    ALLOC(awk2, st->lpcSize, spx_coef_t);
1505    ALLOC(awk3, st->lpcSize, spx_coef_t);
1506    ALLOC(innov, st->subframeSize, spx_sig_t);
1507
1508    if (st->submodeID==1)
1509    {
1510       int extra;
1511       extra = speex_bits_unpack_unsigned(bits, 4);
1512
1513       if (extra==15)
1514          st->dtx_enabled=1;
1515       else
1516          st->dtx_enabled=0;
1517    }
1518    if (st->submodeID>1)
1519       st->dtx_enabled=0;
1520
1521    /*Loop on subframes */
1522    for (sub=0;sub<st->nbSubframes;sub++)
1523    {
1524       int offset;
1525       spx_sig_t *exc;
1526       spx_word16_t *sp;
1527       spx_sig_t *innov_save = NULL;
1528       spx_word16_t tmp;
1529
1530 #ifdef EPIC_48K
1531       if (st->lbr_48k)
1532       {
1533          if (sub*2 < st->nbSubframes)
1534             ol_pitch = pitch_half[0];
1535          else
1536             ol_pitch = pitch_half[1];
1537       }
1538 #endif
1539
1540       /* Offset relative to start of frame */
1541       offset = st->subframeSize*sub;
1542       /* Excitation */
1543       exc=st->exc+offset;
1544       /* Original signal */
1545       sp=out+offset;
1546       if (st->innov_save)
1547          innov_save = st->innov_save+offset;
1548
1549
1550       /* Reset excitation */
1551       for (i=0;i<st->subframeSize;i++)
1552          exc[i]=0;
1553
1554       /*Adaptive codebook contribution*/
1555       if (SUBMODE(ltp_unquant))
1556       {
1557          int pit_min, pit_max;
1558          /* Handle pitch constraints if any */
1559          if (SUBMODE(lbr_pitch) != -1)
1560          {
1561             int margin;
1562             margin = SUBMODE(lbr_pitch);
1563             if (margin)
1564             {
1565 /* GT - need optimization?
1566                if (ol_pitch < st->min_pitch+margin-1)
1567                   ol_pitch=st->min_pitch+margin-1;
1568                if (ol_pitch > st->max_pitch-margin)
1569                   ol_pitch=st->max_pitch-margin;
1570                pit_min = ol_pitch-margin+1;
1571                pit_max = ol_pitch+margin;
1572 */
1573                pit_min = ol_pitch-margin+1;
1574                if (pit_min < st->min_pitch)
1575                   pit_min = st->min_pitch;
1576                pit_max = ol_pitch+margin;
1577                if (pit_max > st->max_pitch)
1578                   pit_max = st->max_pitch;
1579             } else {
1580                pit_min = pit_max = ol_pitch;
1581             }
1582          } else {
1583             pit_min = st->min_pitch;
1584             pit_max = st->max_pitch;
1585          }
1586
1587
1588 #ifdef EPIC_48K
1589          if (st->lbr_48k)
1590          {
1591              SUBMODE(ltp_unquant)(exc, pit_min, pit_max, ol_pitch_coef, SUBMODE(ltp_params), 
1592                                   st->subframeSize, &pitch, &pitch_gain[0], bits, stack, 
1593                                   st->count_lost, offset, st->last_pitch_gain, ol_pitch_id);
1594          } else {
1595 #endif
1596
1597              SUBMODE(ltp_unquant)(exc, pit_min, pit_max, ol_pitch_coef, SUBMODE(ltp_params), 
1598                                   st->subframeSize, &pitch, &pitch_gain[0], bits, stack, 
1599                                   st->count_lost, offset, st->last_pitch_gain, 0);
1600
1601 #ifdef EPIC_48K
1602          }
1603 #endif
1604
1605          tmp = gain_3tap_to_1tap(pitch_gain);
1606
1607          pitch_average += tmp;
1608          if ((tmp>best_pitch_gain&&ABS(2*best_pitch-pitch)>=3&&ABS(3*best_pitch-pitch)>=4&&ABS(4*best_pitch-pitch)>=5) 
1609               || (tmp>MULT16_16_Q15(QCONST16(.6,15),best_pitch_gain)&&(ABS(best_pitch-2*pitch)<3||ABS(best_pitch-3*pitch)<4||ABS(best_pitch-4*pitch)<5)) 
1610               || (MULT16_16_Q15(QCONST16(.67,15),tmp)>best_pitch_gain&&(ABS(2*best_pitch-pitch)<3||ABS(3*best_pitch-pitch)<4||ABS(4*best_pitch-pitch)<5)) )
1611          {
1612             best_pitch = pitch;
1613             if (tmp > best_pitch_gain)
1614                best_pitch_gain = tmp;
1615          }
1616       } else {
1617          speex_error("No pitch prediction, what's wrong");
1618       }
1619       
1620       /* Unquantize the innovation */
1621       {
1622          int q_energy;
1623          spx_word32_t ener;
1624          
1625          for (i=0;i<st->subframeSize;i++)
1626             innov[i]=0;
1627
1628          /* Decode sub-frame gain correction */
1629          if (SUBMODE(have_subframe_gain)==3)
1630          {
1631             q_energy = speex_bits_unpack_unsigned(bits, 3);
1632             ener = MULT16_32_Q14(exc_gain_quant_scal3[q_energy],ol_gain);
1633          } else if (SUBMODE(have_subframe_gain)==1)
1634          {
1635             q_energy = speex_bits_unpack_unsigned(bits, 1);
1636             ener = MULT16_32_Q14(exc_gain_quant_scal1[q_energy],ol_gain);
1637          } else {
1638             ener = ol_gain;
1639          }
1640                   
1641          if (SUBMODE(innovation_unquant))
1642          {
1643             /*Fixed codebook contribution*/
1644             SUBMODE(innovation_unquant)(innov, SUBMODE(innovation_params), st->subframeSize, bits, stack);
1645          } else {
1646             speex_error("No fixed codebook");
1647          }
1648
1649          /* De-normalize innovation and update excitation */
1650 #ifdef FIXED_POINT
1651          signal_mul(innov, innov, ener, st->subframeSize);
1652 #else
1653          signal_mul(innov, innov, ener, st->subframeSize);
1654 #endif
1655          /*Vocoder mode*/
1656          if (st->submodeID==1) 
1657          {
1658             float g=ol_pitch_coef*GAIN_SCALING_1;
1659
1660             
1661             for (i=0;i<st->subframeSize;i++)
1662                exc[i]=0;
1663             while (st->voc_offset<st->subframeSize)
1664             {
1665                if (st->voc_offset>=0)
1666                   exc[st->voc_offset]=SIG_SCALING*sqrt(1.0*ol_pitch);
1667                st->voc_offset+=ol_pitch;
1668             }
1669             st->voc_offset -= st->subframeSize;
1670
1671             g=.5+2*(g-.6);
1672             if (g<0)
1673                g=0;
1674             if (g>1)
1675                g=1;
1676             for (i=0;i<st->subframeSize;i++)
1677             {
1678                float exci=exc[i];
1679                exc[i]=.8*g*exc[i]*ol_gain/SIG_SCALING + .6*g*st->voc_m1*ol_gain/SIG_SCALING + .5*g*innov[i] - .5*g*st->voc_m2 + (1-g)*innov[i];
1680                st->voc_m1 = exci;
1681                st->voc_m2=innov[i];
1682                st->voc_mean = .95*st->voc_mean + .05*exc[i];
1683                exc[i]-=st->voc_mean;
1684             }
1685          } else {
1686             for (i=0;i<st->subframeSize;i++)
1687                exc[i]=ADD32(exc[i],innov[i]);
1688             /*print_vec(exc, 40, "innov");*/
1689          }
1690          if (innov_save)
1691          {
1692             for (i=0;i<st->subframeSize;i++)
1693                innov_save[i] = innov[i];
1694          }
1695          /* Decode second codebook (only for some modes) */
1696          if (SUBMODE(double_codebook))
1697          {
1698             char *tmp_stack=stack;
1699             VARDECL(spx_sig_t *innov2);
1700             ALLOC(innov2, st->subframeSize, spx_sig_t);
1701             for (i=0;i<st->subframeSize;i++)
1702                innov2[i]=0;
1703             SUBMODE(innovation_unquant)(innov2, SUBMODE(innovation_params), st->subframeSize, bits, stack);
1704             signal_mul(innov2, innov2, MULT16_32_Q15(QCONST16(0.454545,15),ener), st->subframeSize);
1705             for (i=0;i<st->subframeSize;i++)
1706                exc[i] = ADD32(exc[i],innov2[i]);
1707             if (innov_save)
1708             {
1709                for (i=0;i<st->subframeSize;i++)
1710                   innov_save[i] = ADD32(innov_save[i],innov2[i]);
1711             }
1712             stack = tmp_stack;
1713          }
1714
1715       }
1716
1717 #ifndef NEW_ENHANCER
1718       if (st->lpc_enh_enabled && SUBMODE(comb_gain)>0 && !st->count_lost)
1719       {
1720          comb_filter(exc, sp, st->interp_qlpc, st->lpcSize, st->subframeSize,
1721                      pitch, pitch_gain, SUBMODE(comb_gain), st->comb_mem);
1722       } else {
1723          for (i=0;i<st->subframeSize;i++)
1724             sp[i]=PSHR32(exc[i],SIG_SHIFT);
1725       }
1726 #endif      
1727
1728    }
1729    
1730    ALLOC(interp_qlsp, st->lpcSize, spx_lsp_t);
1731
1732 #ifdef NEW_ENHANCER
1733    if (st->lpc_enh_enabled && SUBMODE(comb_gain)>0 && !st->count_lost)
1734    {
1735       multicomb(st->exc-40, out, st->interp_qlpc, st->lpcSize, 2*st->subframeSize, best_pitch, pitch_gain, SUBMODE(comb_gain), stack);
1736       multicomb(st->exc+40, out+80, st->interp_qlpc, st->lpcSize, 2*st->subframeSize, best_pitch, pitch_gain, SUBMODE(comb_gain), stack);
1737    } else {
1738       for (i=0;i<st->frameSize;i++)
1739          out[i]=PSHR32(st->exc[i-40],SIG_SHIFT);
1740    }
1741 #endif
1742    
1743    /* If the last packet was lost, re-scale the excitation to obtain the same energy as encoded in ol_gain */
1744    if (st->count_lost) 
1745    {
1746       spx_word16_t exc_ener;
1747       spx_word32_t gain32;
1748       spx_word16_t gain;
1749       exc_ener = compute_rms (st->exc, st->frameSize);
1750       gain32 = DIV32(ol_gain, ADD16(exc_ener,1));
1751 #ifdef FIXED_POINT
1752       if (gain32 > 32768)
1753          gain32 = 32768;
1754       gain = EXTRACT16(gain32);
1755 #else
1756       if (gain32 > 2)
1757          gain32=2;
1758       gain = gain32;
1759 #endif
1760       for (i=0;i<st->frameSize;i++)
1761       {
1762          st->exc[i] = MULT16_32_Q14(gain, st->exc[i]);
1763 #ifdef NEW_ENHANCER
1764          out[i]=PSHR32(st->exc[i-st->subframeSize],SIG_SHIFT);
1765 #else
1766          out[i]=PSHR32(st->exc[i],SIG_SHIFT);
1767 #endif
1768       }
1769    }
1770
1771    /*Loop on subframes */
1772    for (sub=0;sub<st->nbSubframes;sub++)
1773    {
1774       int offset;
1775       spx_word16_t *sp;
1776       spx_sig_t *exc;
1777       /* Offset relative to start of frame */
1778       offset = st->subframeSize*sub;
1779       /* Original signal */
1780       sp=out+offset;
1781       /* Excitation */
1782       exc=st->exc+offset;
1783
1784       /* LSP interpolation (quantized and unquantized) */
1785       lsp_interpolate(st->old_qlsp, qlsp, interp_qlsp, st->lpcSize, sub, st->nbSubframes);
1786
1787       /* Make sure the LSP's are stable */
1788       lsp_enforce_margin(interp_qlsp, st->lpcSize, LSP_MARGIN);
1789
1790       /* Compute interpolated LPCs (unquantized) */
1791       lsp_to_lpc(interp_qlsp, ak, st->lpcSize, stack);
1792
1793 #ifndef NEW_ENHANCER
1794       for (i=0;i<st->lpcSize;i++)
1795          st->interp_qlpc[i] = ak[i];
1796 #endif
1797
1798       /* Compute enhanced synthesis filter */
1799       if (st->lpc_enh_enabled)
1800       {
1801          bw_lpc(SUBMODE(lpc_enh_k1), st->interp_qlpc, awk1, st->lpcSize);
1802          bw_lpc(SUBMODE(lpc_enh_k2), st->interp_qlpc, awk2, st->lpcSize);
1803          bw_lpc(SUBMODE(lpc_enh_k3), st->interp_qlpc, awk3, st->lpcSize);
1804       }
1805       
1806       /* Compute analysis filter at w=pi */
1807       {
1808          spx_word32_t pi_g=LPC_SCALING;
1809          for (i=0;i<st->lpcSize;i+=2)
1810          {
1811             /*pi_g += -st->interp_qlpc[i] +  st->interp_qlpc[i+1];*/
1812             pi_g = ADD32(pi_g, SUB32(EXTEND32(st->interp_qlpc[i+1]),EXTEND32(st->interp_qlpc[i])));
1813          }
1814          st->pi_gain[sub] = pi_g;
1815       }
1816       
1817       if (st->lpc_enh_enabled)
1818       {
1819          /* Use enhanced LPC filter */
1820          filter_mem16(sp, awk2, awk1, sp, st->subframeSize, st->lpcSize, 
1821                      st->mem_sp+st->lpcSize);
1822          filter_mem16(sp, awk3, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
1823                      st->mem_sp);
1824       } else {
1825          /* Use regular filter */
1826          for (i=0;i<st->lpcSize;i++)
1827             st->mem_sp[st->lpcSize+i] = 0;
1828          iir_mem16(sp, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, 
1829                   st->mem_sp);
1830       }
1831       
1832       for (i=0;i<st->lpcSize;i++)
1833          st->interp_qlpc[i] = ak[i];
1834
1835    }
1836
1837    /*for (i=0;i<st->frameSize;i++)
1838      printf ("%d\n", (int)st->frame[i]);*/
1839
1840    /* Store the LSPs for interpolation in the next frame */
1841    for (i=0;i<st->lpcSize;i++)
1842       st->old_qlsp[i] = qlsp[i];
1843
1844    /* The next frame will not be the first (Duh!) */
1845    st->first = 0;
1846    st->count_lost=0;
1847    st->last_pitch = best_pitch;
1848 #ifdef FIXED_POINT
1849    st->last_pitch_gain = PSHR16(pitch_average,2);
1850 #else
1851    st->last_pitch_gain = .25*pitch_average;   
1852 #endif
1853    st->pitch_gain_buf[st->pitch_gain_buf_idx++] = st->last_pitch_gain;
1854    if (st->pitch_gain_buf_idx > 2) /* rollover */
1855       st->pitch_gain_buf_idx = 0;
1856
1857    st->last_ol_gain = ol_gain;
1858
1859    return 0;
1860 }
1861
1862 int nb_encoder_ctl(void *state, int request, void *ptr)
1863 {
1864    EncState *st;
1865    st=(EncState*)state;     
1866    switch(request)
1867    {
1868    case SPEEX_GET_FRAME_SIZE:
1869       (*(int*)ptr) = st->frameSize;
1870       break;
1871    case SPEEX_SET_LOW_MODE:
1872    case SPEEX_SET_MODE:
1873       st->submodeSelect = st->submodeID = (*(int*)ptr);
1874       break;
1875    case SPEEX_GET_LOW_MODE:
1876    case SPEEX_GET_MODE:
1877       (*(int*)ptr) = st->submodeID;
1878       break;
1879    case SPEEX_SET_VBR:
1880       st->vbr_enabled = (*(int*)ptr);
1881       break;
1882    case SPEEX_GET_VBR:
1883       (*(int*)ptr) = st->vbr_enabled;
1884       break;
1885    case SPEEX_SET_VAD:
1886       st->vad_enabled = (*(int*)ptr);
1887       break;
1888    case SPEEX_GET_VAD:
1889       (*(int*)ptr) = st->vad_enabled;
1890       break;
1891    case SPEEX_SET_DTX:
1892       st->dtx_enabled = (*(int*)ptr);
1893       break;
1894    case SPEEX_GET_DTX:
1895       (*(int*)ptr) = st->dtx_enabled;
1896       break;
1897    case SPEEX_SET_ABR:
1898       st->abr_enabled = (*(int*)ptr);
1899       st->vbr_enabled = 1;
1900       {
1901          int i=10, rate, target;
1902          float vbr_qual;
1903          target = (*(int*)ptr);
1904          while (i>=0)
1905          {
1906             speex_encoder_ctl(st, SPEEX_SET_QUALITY, &i);
1907             speex_encoder_ctl(st, SPEEX_GET_BITRATE, &rate);
1908             if (rate <= target)
1909                break;
1910             i--;
1911          }
1912          vbr_qual=i;
1913          if (vbr_qual<0)
1914             vbr_qual=0;
1915          speex_encoder_ctl(st, SPEEX_SET_VBR_QUALITY, &vbr_qual);
1916          st->abr_count=0;
1917          st->abr_drift=0;
1918          st->abr_drift2=0;
1919       }
1920       
1921       break;
1922    case SPEEX_GET_ABR:
1923       (*(int*)ptr) = st->abr_enabled;
1924       break;
1925    case SPEEX_SET_VBR_QUALITY:
1926       st->vbr_quality = (*(float*)ptr);
1927       break;
1928    case SPEEX_GET_VBR_QUALITY:
1929       (*(float*)ptr) = st->vbr_quality;
1930       break;
1931    case SPEEX_SET_QUALITY:
1932       {
1933          int quality = (*(int*)ptr);
1934          if (quality < 0)
1935             quality = 0;
1936          if (quality > 10)
1937             quality = 10;
1938          st->submodeSelect = st->submodeID = ((const SpeexNBMode*)(st->mode->mode))->quality_map[quality];
1939       }
1940       break;
1941    case SPEEX_SET_COMPLEXITY:
1942       st->complexity = (*(int*)ptr);
1943       if (st->complexity<0)
1944          st->complexity=0;
1945       break;
1946    case SPEEX_GET_COMPLEXITY:
1947       (*(int*)ptr) = st->complexity;
1948       break;
1949    case SPEEX_SET_BITRATE:
1950       {
1951          int i=10, rate, target;
1952          target = (*(int*)ptr);
1953          while (i>=0)
1954          {
1955             speex_encoder_ctl(st, SPEEX_SET_QUALITY, &i);
1956             speex_encoder_ctl(st, SPEEX_GET_BITRATE, &rate);
1957             if (rate <= target)
1958                break;
1959             i--;
1960          }
1961       }
1962       break;
1963    case SPEEX_GET_BITRATE:
1964       if (st->submodes[st->submodeID])
1965          (*(int*)ptr) = st->sampling_rate*SUBMODE(bits_per_frame)/st->frameSize;
1966       else
1967          (*(int*)ptr) = st->sampling_rate*(NB_SUBMODE_BITS+1)/st->frameSize;
1968       break;
1969    case SPEEX_SET_SAMPLING_RATE:
1970       st->sampling_rate = (*(int*)ptr);
1971       break;
1972    case SPEEX_GET_SAMPLING_RATE:
1973       (*(int*)ptr)=st->sampling_rate;
1974       break;
1975    case SPEEX_RESET_STATE:
1976       {
1977          int i;
1978          st->bounded_pitch = 1;
1979          st->first = 1;
1980          for (i=0;i<st->lpcSize;i++)
1981             st->lsp[i]=(M_PI*((float)(i+1)))/(st->lpcSize+1);
1982          for (i=0;i<st->lpcSize;i++)
1983             st->mem_sw[i]=st->mem_sw_whole[i]=st->mem_sp[i]=st->mem_exc[i]=0;
1984          for (i=0;i<st->frameSize+st->max_pitch+1;i++)
1985             st->excBuf[i]=st->swBuf[i]=0;
1986          for (i=0;i<st->windowSize-st->frameSize;i++)
1987             st->winBuf[i]=0;
1988       }
1989       break;
1990    case SPEEX_SET_SUBMODE_ENCODING:
1991       st->encode_submode = (*(int*)ptr);
1992       break;
1993    case SPEEX_GET_SUBMODE_ENCODING:
1994       (*(int*)ptr) = st->encode_submode;
1995       break;
1996    case SPEEX_GET_LOOKAHEAD:
1997       (*(int*)ptr)=(st->windowSize-st->frameSize);
1998       break;
1999    case SPEEX_SET_PLC_TUNING:
2000       st->plc_tuning = (*(int*)ptr);
2001       if (st->plc_tuning>100)
2002          st->plc_tuning=100;
2003       break;
2004    case SPEEX_GET_PLC_TUNING:
2005       (*(int*)ptr)=(st->plc_tuning);
2006       break;
2007    case SPEEX_GET_PI_GAIN:
2008       {
2009          int i;
2010          spx_word32_t *g = (spx_word32_t*)ptr;
2011          for (i=0;i<st->nbSubframes;i++)
2012             g[i]=st->pi_gain[i];
2013       }
2014       break;
2015    case SPEEX_GET_EXC:
2016       {
2017          int i;
2018          spx_sig_t *e = (spx_sig_t*)ptr;
2019          for (i=0;i<st->frameSize;i++)
2020             e[i]=st->exc[i];
2021       }
2022       break;
2023    case SPEEX_GET_RELATIVE_QUALITY:
2024       (*(float*)ptr)=st->relative_quality;
2025       break;
2026    case SPEEX_SET_INNOVATION_SAVE:
2027       st->innov_save = ptr;
2028       break;
2029    default:
2030       speex_warning_int("Unknown nb_ctl request: ", request);
2031       return -1;
2032    }
2033    return 0;
2034 }
2035
2036 int nb_decoder_ctl(void *state, int request, void *ptr)
2037 {
2038    DecState *st;
2039    st=(DecState*)state;
2040    switch(request)
2041    {
2042    case SPEEX_SET_LOW_MODE:
2043    case SPEEX_SET_MODE:
2044       st->submodeID = (*(int*)ptr);
2045       break;
2046    case SPEEX_GET_LOW_MODE:
2047    case SPEEX_GET_MODE:
2048       (*(int*)ptr) = st->submodeID;
2049       break;
2050    case SPEEX_SET_ENH:
2051       st->lpc_enh_enabled = *((int*)ptr);
2052       break;
2053    case SPEEX_GET_ENH:
2054       *((int*)ptr) = st->lpc_enh_enabled;
2055       break;
2056    case SPEEX_GET_FRAME_SIZE:
2057       (*(int*)ptr) = st->frameSize;
2058       break;
2059    case SPEEX_GET_BITRATE:
2060       if (st->submodes[st->submodeID])
2061          (*(int*)ptr) = st->sampling_rate*SUBMODE(bits_per_frame)/st->frameSize;
2062       else
2063          (*(int*)ptr) = st->sampling_rate*(NB_SUBMODE_BITS+1)/st->frameSize;
2064       break;
2065    case SPEEX_SET_SAMPLING_RATE:
2066       st->sampling_rate = (*(int*)ptr);
2067       break;
2068    case SPEEX_GET_SAMPLING_RATE:
2069       (*(int*)ptr)=st->sampling_rate;
2070       break;
2071    case SPEEX_SET_HANDLER:
2072       {
2073          SpeexCallback *c = (SpeexCallback*)ptr;
2074          st->speex_callbacks[c->callback_id].func=c->func;
2075          st->speex_callbacks[c->callback_id].data=c->data;
2076          st->speex_callbacks[c->callback_id].callback_id=c->callback_id;
2077       }
2078       break;
2079    case SPEEX_SET_USER_HANDLER:
2080       {
2081          SpeexCallback *c = (SpeexCallback*)ptr;
2082          st->user_callback.func=c->func;
2083          st->user_callback.data=c->data;
2084          st->user_callback.callback_id=c->callback_id;
2085       }
2086       break;
2087    case SPEEX_RESET_STATE:
2088       {
2089          int i;
2090          for (i=0;i<2*st->lpcSize;i++)
2091             st->mem_sp[i]=0;
2092          for (i=0;i<st->frameSize + st->max_pitch + 1;i++)
2093             st->excBuf[i]=0;
2094       }
2095       break;
2096    case SPEEX_SET_SUBMODE_ENCODING:
2097       st->encode_submode = (*(int*)ptr);
2098       break;
2099    case SPEEX_GET_SUBMODE_ENCODING:
2100       (*(int*)ptr) = st->encode_submode;
2101       break;
2102    case SPEEX_GET_LOOKAHEAD:
2103 #ifdef NEW_ENHANCER
2104       (*(int*)ptr)=st->subframeSize;
2105 #else
2106       (*(int*)ptr)=0;
2107 #endif
2108       break;
2109       case SPEEX_GET_PI_GAIN:
2110       {
2111          int i;
2112          spx_word32_t *g = (spx_word32_t*)ptr;
2113          for (i=0;i<st->nbSubframes;i++)
2114             g[i]=st->pi_gain[i];
2115       }
2116       break;
2117    case SPEEX_GET_EXC:
2118       {
2119          int i;
2120          spx_sig_t *e = (spx_sig_t*)ptr;
2121          for (i=0;i<st->frameSize;i++)
2122             e[i]=st->exc[i];
2123       }
2124       break;
2125    case SPEEX_GET_DTX_STATUS:
2126       *((int*)ptr) = st->dtx_enabled;
2127       break;
2128    case SPEEX_SET_INNOVATION_SAVE:
2129       st->innov_save = ptr;
2130       break;
2131    default:
2132       speex_warning_int("Unknown nb_ctl request: ", request);
2133       return -1;
2134    }
2135    return 0;
2136 }