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