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