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