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