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