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