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