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